Skip to content

Commit

Permalink
Revert "hopefully this resolves overshoot"
Browse files Browse the repository at this point in the history 
This reverts commit 356da87.
  • Loading branch information
@BaronClaps
BaronClaps committed Jan 26, 2025
1 parent 07dd645 commit 35bc1c0
Showing 1 changed file with 20 additions and 10 deletions.
30 changes: 20 additions & 10 deletions src/main/java/com/pedropathing/follower/Follower.java
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,3 @@

package com.pedropathing.follower;

import static com.pedropathing.follower.FollowerConstants.automaticHoldEnd;
Expand Down Expand Up @@ -458,6 +457,7 @@ public void holdPoint(Pose pose) {
* @param holdEnd this makes the Follower hold the last Point on the Path.
*/
public void followPath(Path path, boolean holdEnd) {
driveVectorScaler.setMaxPowerScaling(globalMaxPower);
breakFollowing();
holdPositionAtEnd = holdEnd;
isBusy = true;
Expand Down Expand Up @@ -595,8 +595,10 @@ public void update() {
debugLog();
}

if (currentPath.isAtParametricEnd()) {
if (currentPath.isAtParametricEnd() ||
(zeroVelocityDetectedTimer != null && zeroVelocityDetectedTimer.milliseconds() > 500.0)) {
if (followingPathChain && chainIndex < currentPathChain.size() - 1) {

if (logDebug) {
Log.d("Follower_logger", "chainIndex: " + chainIndex + " | Pose: " + getPose());
}
Expand All @@ -616,7 +618,10 @@ public void update() {
reachedParametricPathEndTime = System.currentTimeMillis();
}

if ((System.currentTimeMillis() - reachedParametricPathEndTime > currentPath.getPathEndTimeoutConstraint()) || (poseUpdater.getVelocity().getMagnitude() < currentPath.getPathEndVelocityConstraint() && MathFunctions.distance(poseUpdater.getPose(), closestPose) < currentPath.getPathEndTranslationalConstraint() && MathFunctions.getSmallestAngleDifference(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()) < currentPath.getPathEndHeadingConstraint())) {
if ((System.currentTimeMillis() - reachedParametricPathEndTime > currentPath.getPathEndTimeoutConstraint()) ||
(poseUpdater.getVelocity().getMagnitude() < currentPath.getPathEndVelocityConstraint()
&& MathFunctions.distance(poseUpdater.getPose(), closestPose) < currentPath.getPathEndTranslationalConstraint() &&
MathFunctions.getSmallestAngleDifference(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()) < currentPath.getPathEndHeadingConstraint())) {
if (holdPositionAtEnd) {
holdPositionAtEnd = false;
holdPoint(new BezierPoint(currentPath.getLastControlPoint()), currentPath.getHeadingGoal(1));
Expand All @@ -638,6 +643,7 @@ public void update() {
}
}
}
//RobotLog.d("Follower:: isBusy:" + isBusy);
}
}
} else {
Expand Down Expand Up @@ -858,15 +864,15 @@ public double getDriveVelocityError() {
double forwardVelocity = MathFunctions.dotProduct(forwardHeadingVector, velocity);
double forwardDistanceToGoal = MathFunctions.dotProduct(forwardHeadingVector, distanceToGoalVector);

// Thank you Pear for the bug report on this not reversing
double forwardVelocityGoal = MathFunctions.getSign(forwardDistanceToGoal) * Math.sqrt(Math.abs(-2 * currentPath.getZeroPowerAccelerationMultiplier() * forwardZeroPowerAcceleration * (forwardVelocity < 0 ? -1 : 1) * forwardDistanceToGoal));
double forwardVelocityZeroPowerDecay = forwardVelocity - MathFunctions.getSign(forwardDistanceToGoal) * Math.sqrt(Math.abs(Math.pow(forwardVelocity, 2) + 2 * forwardZeroPowerAcceleration * (forwardVelocity < 0 ? -1 : 1) * forwardDistanceToGoal));
double forwardVelocityGoal = MathFunctions.getSign(forwardDistanceToGoal) * Math.sqrt(Math.abs(-2 * currentPath.getZeroPowerAccelerationMultiplier() * forwardZeroPowerAcceleration * (forwardDistanceToGoal <= 0 ? 1 : -1) * forwardDistanceToGoal));
double forwardVelocityZeroPowerDecay = forwardVelocity - MathFunctions.getSign(forwardDistanceToGoal) * Math.sqrt(Math.abs(Math.pow(forwardVelocity, 2) + 2 * forwardZeroPowerAcceleration * forwardDistanceToGoal));

Vector lateralHeadingVector = new Vector(1.0, poseUpdater.getPose().getHeading() - Math.PI / 2);
double lateralVelocity = MathFunctions.dotProduct(lateralHeadingVector, velocity);
double lateralDistanceToGoal = MathFunctions.dotProduct(lateralHeadingVector, distanceToGoalVector);
double lateralVelocityGoal = MathFunctions.getSign(lateralDistanceToGoal) * Math.sqrt(Math.abs(-2 * currentPath.getZeroPowerAccelerationMultiplier() * lateralZeroPowerAcceleration * (lateralVelocity < 0 ? -1 : 1) * lateralDistanceToGoal));
double lateralVelocityZeroPowerDecay = lateralVelocity - MathFunctions.getSign(lateralDistanceToGoal) * Math.sqrt(Math.abs(Math.pow(lateralVelocity, 2) + 2 * lateralZeroPowerAcceleration * (lateralVelocity < 0 ? -1 : 1) * lateralDistanceToGoal));

double lateralVelocityGoal = MathFunctions.getSign(lateralDistanceToGoal) * Math.sqrt(Math.abs(-2 * currentPath.getZeroPowerAccelerationMultiplier() * lateralZeroPowerAcceleration * (lateralDistanceToGoal <= 0 ? 1 : -1) * lateralDistanceToGoal));
double lateralVelocityZeroPowerDecay = lateralVelocity - MathFunctions.getSign(lateralDistanceToGoal) * Math.sqrt(Math.abs(Math.pow(lateralVelocity, 2) + 2 * lateralZeroPowerAcceleration * lateralDistanceToGoal));

Vector forwardVelocityError = new Vector(forwardVelocityGoal - forwardVelocityZeroPowerDecay - forwardVelocity, forwardHeadingVector.getTheta());
Vector lateralVelocityError = new Vector(lateralVelocityGoal - lateralVelocityZeroPowerDecay - lateralVelocity, lateralHeadingVector.getTheta());
Expand All @@ -886,7 +892,6 @@ public double getDriveVelocityError() {

return driveKalmanFilter.getState();
}

/**
* This returns a Vector in the direction of the robot that contains the heading correction
* as its magnitude. Positive heading correction turns the robot counter-clockwise, and negative
Expand All @@ -901,6 +906,11 @@ public Vector getHeadingVector() {
if (!useHeading) return new Vector();
headingError = MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()) * MathFunctions.getSmallestAngleDifference(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal());
if (Math.abs(headingError) < headingPIDFSwitch && useSecondaryHeadingPID) {
// if(logDebug) {
// Log.d("Follower_logger", "using secondary heading PIDF controller, error: "
// + String.format("%3.3f", Math.toDegrees(headingError)));
//
// }
secondaryHeadingPIDF.updateError(headingError);
headingVector = new Vector(MathFunctions.clamp(secondaryHeadingPIDF.runPIDF() + secondaryHeadingPIDFFeedForward * MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()), -driveVectorScaler.getMaxPowerScaling(), driveVectorScaler.getMaxPowerScaling()), poseUpdater.getPose().getHeading());
return MathFunctions.copyVector(headingVector);
Expand Down Expand Up @@ -1118,7 +1128,7 @@ public void telemetryDebug(Telemetry telemetry) {
}

/**
* This returns the total number of radians the robot has turned. This is calculated by the PoseUpdater.
* This returns the total number of radians the robot has turned.
*
* @return the total heading.
*/
Expand Down

0 comments on commit 35bc1c0

Please sign in to comment.