SCDS/FTCRoadRunnerRobotics
7
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

simplifying the PID system

Browse Source
This commit is contained in:
brotherhobo
2024-08-06 23:00:02 -04:00
parent 22d38b1289
commit 4a669f770f
4 changed files with 155 additions and 150 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/OVERVIEW.md
View File

@ -68,27 +68,18 @@ calculate the force necessary to keep the robot on the path, and then tune a sca
that force into a corresponding power for the robot. that force into a corresponding power for the robot.
### Translational Correction ### Translational Correction
This is as simple as it sounds: this corrects error in the robot's position only. However, the This is as simple as it sounds: this corrects error in the robot's position only. The robot's translational
robot's translational error is actually used within two different PIDs to move the robot. A large error is corrected with a PID control. The translational correction does not act along the path the
PID and a small PID are used. The large PID is used when error exceeds a certain limit. and the small robot takes, but instead moves the robot back to the closest point on the path.
PID is used when the error is within that limit.
When the robot encounters error, the large PID, if applicable, is used to bring the robot within the
small PID error range without much overshoot to avoid oscillations. Then, the small PID is used to
bring the robot to within acceptable error ranges. The reason for this double PID is to allow for
aggressive correction to bring the robot within tight tolerances, while not correcting too
aggressively at larger ranges, which would've caused oscillations.
### Heading Correction ### Heading Correction
The heading correction operates very similarly to the translational correction, except this corrects The heading correction operates very similarly to the translational correction, except this corrects
the direction the robot is facing. It also uses a small and large PID rather than a single PID, like the direction the robot is facing. The heading correction will turn in the closest direction from the
the translational correction. The heading correction will turn in the closest direction from the
robot's current heading to the target heading. robot's current heading to the target heading.
### Drive Vector ### Drive Vector
The drive vector points in the direction of the tangent of the path and it is responsible for moving The drive vector points in the direction of the tangent of the path and it is responsible for moving
the robot along the path. Similar to the translational and heading corrections, the drive vector the robot along the path. Using basic kinematics equations, we can use the velocity of the robot
uses a large and small PID. Using basic kinematics equations, we can use the velocity of the robot
along the path, the length of path left, and a specified target rate of deceleration to calculate along the path, the length of path left, and a specified target rate of deceleration to calculate
the velocity we should be moving at. Additionally, after finding out the rate of deceleration of the the velocity we should be moving at. Additionally, after finding out the rate of deceleration of the
robot under 0 power, we can compensate for that with another kinematics equation. Combining these robot under 0 power, we can compensate for that with another kinematics equation. Combining these

36
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md
View File

@ -10,7 +10,7 @@ One last thing to note is that Pedro Pathing operates in inches and radians.
## Tuning ## Tuning
* To start with, we need the mass of the robot in kg. This is used for the centripetal force * To start with, we need the mass of the robot in kg. This is used for the centripetal force
correction, and the mass should be put on line `123` in the `FollowerConstants` class under the correction, and the mass should be put on line `80` in the `FollowerConstants` class under the
`tuning` package. `tuning` package.
* Next, we need to find the preferred mecanum drive vectors. The rollers on mecanum wheels point at a * Next, we need to find the preferred mecanum drive vectors. The rollers on mecanum wheels point at a
@ -36,27 +36,23 @@ One last thing to note is that Pedro Pathing operates in inches and radians.
which point it will display the deceleration in telemetry. This robot will need to drift to a stop which point it will display the deceleration in telemetry. This robot will need to drift to a stop
to properly work, and the higher the velocity the greater the drift distance, so make sure you have to properly work, and the higher the velocity the greater the drift distance, so make sure you have
enough room. Once you're done, put the zero power acceleration for the enough room. Once you're done, put the zero power acceleration for the
`Forward Zero Power Acceleration Tuner` on line `130` in the `FollowerConstants` class and the zero `Forward Zero Power Acceleration Tuner` on line `88` in the `FollowerConstants` class and the zero
power acceleration for the `Lateral Zero Power Acceleration Tuner` on line `134` in the power acceleration for the `Lateral Zero Power Acceleration Tuner` on line `92` in the
`FollowerConstants` class. `FollowerConstants` class.
* After this, we will want to tune the translational PIDs. Go to FTC Dashboard and disable all but * After this, we will want to tune the translational PIDs. Go to FTC Dashboard and disable all but
the `useTranslational` checkboxes under the `Follower` tab. Then, run `StraightBackAndForth`. Make the `useTranslational` checkboxes under the `Follower` tab. Then, run `StraightBackAndForth`. Make
sure you disable the timer on autonomous OpModes. There are two different PIDs for translational sure you disable the timer on autonomous OpModes. The PID for the translational error is called
error, the `smallTranslationalPIDF` and `largeTranslationalPIDF`. If error is larger than a certain `translationalPIDF`. If you need to add a feedforward value, use the `translationalPIDFFeedForward`
amount, the `largeTranslationalPIDF` will be used, and if error is smaller than that amount the since that will add the feedforward in the direction the robot is trying to move, rather than the
`smallTranslationalPIDF` will be used. If you need to add a feedforward value, use the feedforward in the PIDF itself, since those will only add the feedforward one way. You can change
`smallTranslationalPIDFFeedForward` and `largeTranslationalPIDFFeedForward` since those will add the the PIDF constants and feedforward values, under the `FollowerConstants` tab in FTC Dashboard.
feedforward in the direction the robot is trying to move, rather than the feedforward in the PIDFs To tune the PID, push the robot off the path and see how corrects. You will want to alternate sides
themselves, since those will only add the feedforward one way. You can change the amount of error you push to reduce field wear and tear as well as push with varying power and distance. I would
required to switch PIDFs, as well as the PIDF constants and feedforward values, under the recommend tuning the PID so that it is capable of correcting while minimizing oscillations and still
`FollowerConstants` tab in FTC Dashboard. To tune the PIDs, push the robot off the path and see how achieving a satisfactory level of accuracy. Overall, try to tune for fewer oscillations rather than
corrects. You will want to alternate sides you push to reduce field wear and tear as well as push higher speeds or perfect accuracy, since this will make the robot run more smoothly under actual
with varying power and distance. I would recommend tuning the large PID first, and tuning it so that pathing conditions.
the PID is capable of correcting to the point where the PIDs switch with little momentum. Then, tune
the small PID to minimize oscillations while still achieving a satisfactory level of accuracy.
Overall, try to tune for fewer oscillations rather than higher speeds or perfect accuracy, since
this will make the robot run more smoothly under actual pathing conditions.
* Next, we will tune the heading PIDs. The process is essentially the same as above, except you will * Next, we will tune the heading PIDs. The process is essentially the same as above, except you will
want to only enable `useHeading` under `Follower` on FTC Dashboard, as well as turn the robot from want to only enable `useHeading` under `Follower` on FTC Dashboard, as well as turn the robot from
@ -68,7 +64,7 @@ One last thing to note is that Pedro Pathing operates in inches and radians.
`zeroPowerAccelerationMultiplier`. This determines how fast your robot will decelerate as a factor `zeroPowerAccelerationMultiplier`. This determines how fast your robot will decelerate as a factor
of how fast your robot will coast to a stop. Honestly, this is up to you. I personally used 4, but of how fast your robot will coast to a stop. Honestly, this is up to you. I personally used 4, but
what works best for you is most important. Higher numbers will cause a faster brake, but increase what works best for you is most important. Higher numbers will cause a faster brake, but increase
oscillations at the end. Lower numbers will do the opposite. This can be found on line `143` in oscillations at the end. Lower numbers will do the opposite. This can be found on line `101` in
`FollowerConstants`. There are once again two PIDs for the drive vector, but these PIDs are much, `FollowerConstants`. There are once again two PIDs for the drive vector, but these PIDs are much,
much more sensitive than the others. For reference, my P values were in the hundredths and much more sensitive than the others. For reference, my P values were in the hundredths and
thousandths place values, and my D values were in the hundred thousandths and millionths place thousandths place values, and my D values were in the hundred thousandths and millionths place
@ -95,7 +91,7 @@ One last thing to note is that Pedro Pathing operates in inches and radians.
* Finally, we will want to tune the centripetal force correction. This is a pretty simple tune. Open * Finally, we will want to tune the centripetal force correction. This is a pretty simple tune. Open
up FTC Dashboard and enable everything under the `Follower` tab. Then, run `CurvedBackAndForth` up FTC Dashboard and enable everything under the `Follower` tab. Then, run `CurvedBackAndForth`
and turn off its timer. If you notice the robot is correcting towards the inside of the curve and turn off its timer. If you notice the robot is correcting towards the inside of the curve
as/after running a path, then increase `centripetalScaling`, which can be found on line `126` of as/after running a path, then increase `centripetalScaling`, which can be found on line `83` of
`FollowerConstants`. If the robot is correcting towards the outside of the curve, then decrease `FollowerConstants`. If the robot is correcting towards the outside of the curve, then decrease
`centripetalScaling`. `centripetalScaling`.

115
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java
View File

@ -3,14 +3,17 @@ package org.firstinspires.ftc.teamcode.pedroPathing.follower;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.drivePIDFSwitch; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.drivePIDFSwitch;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.forwardZeroPowerAcceleration; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.forwardZeroPowerAcceleration;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.headingPIDFSwitch; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.headingPIDFSwitch;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.largeDrivePIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.drivePIDFFeedForward;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.largeHeadingPIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.headingPIDFFeedForward;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.largeTranslationalPIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.translationalPIDFFeedForward;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.lateralZeroPowerAcceleration; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.lateralZeroPowerAcceleration;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.smallDrivePIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.secondaryDrivePIDFFeedForward;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.smallHeadingPIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.secondaryHeadingPIDFFeedForward;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.smallTranslationalPIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.secondaryTranslationalPIDFFeedForward;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.translationalPIDFSwitch; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.translationalPIDFSwitch;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.useSecondaryDrivePID;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.useSecondaryHeadingPID;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.useSecondaryTranslationalPID;
import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
@ -87,8 +90,8 @@ public class Follower {
private boolean teleopDrive; private boolean teleopDrive;
private double maxPower = 1; private double maxPower = 1;
private double previousSmallTranslationalIntegral; private double previousSecondaryTranslationalIntegral;
private double previousLargeTranslationalIntegral; private double previousTranslationalIntegral;
private double holdPointTranslationalScaling = FollowerConstants.holdPointTranslationalScaling; private double holdPointTranslationalScaling = FollowerConstants.holdPointTranslationalScaling;
private double holdPointHeadingScaling = FollowerConstants.holdPointHeadingScaling; private double holdPointHeadingScaling = FollowerConstants.holdPointHeadingScaling;
public double driveError; public double driveError;
@ -99,16 +102,14 @@ public class Follower {
private double[] drivePowers; private double[] drivePowers;
private double[] teleopDriveValues; private double[] teleopDriveValues;
private Vector[] teleOpMovementVectors = new Vector[]{new Vector(), new Vector(), new Vector()};
private ArrayList<Vector> velocities = new ArrayList<>(); private ArrayList<Vector> velocities = new ArrayList<>();
private ArrayList<Vector> accelerations = new ArrayList<>(); private ArrayList<Vector> accelerations = new ArrayList<>();
private Vector averageVelocity; private Vector averageVelocity;
private Vector averagePreviousVelocity; private Vector averagePreviousVelocity;
private Vector averageAcceleration; private Vector averageAcceleration;
private Vector smallTranslationalIntegralVector; private Vector secondaryTranslationalIntegralVector;
private Vector largeTranslationalIntegralVector; private Vector translationalIntegralVector;
private Vector teleopDriveVector; private Vector teleopDriveVector;
private Vector teleopHeadingVector; private Vector teleopHeadingVector;
public Vector driveVector; public Vector driveVector;
@ -117,14 +118,14 @@ public class Follower {
public Vector centripetalVector; public Vector centripetalVector;
public Vector correctiveVector; public Vector correctiveVector;
private PIDFController smallTranslationalPIDF = new PIDFController(FollowerConstants.smallTranslationalPIDFCoefficients); private PIDFController secondaryTranslationalPIDF = new PIDFController(FollowerConstants.secondaryTranslationalPIDFCoefficients);
private PIDFController smallTranslationalIntegral = new PIDFController(FollowerConstants.smallTranslationalIntegral); private PIDFController secondaryTranslationalIntegral = new PIDFController(FollowerConstants.secondaryTranslationalIntegral);
private PIDFController largeTranslationalPIDF = new PIDFController(FollowerConstants.largeTranslationalPIDFCoefficients); private PIDFController translationalPIDF = new PIDFController(FollowerConstants.translationalPIDFCoefficients);
private PIDFController largeTranslationalIntegral = new PIDFController(FollowerConstants.largeTranslationalIntegral); private PIDFController translationalIntegral = new PIDFController(FollowerConstants.translationalIntegral);
private PIDFController smallHeadingPIDF = new PIDFController(FollowerConstants.smallHeadingPIDFCoefficients); private PIDFController secondaryHeadingPIDF = new PIDFController(FollowerConstants.secondaryHeadingPIDFCoefficients);
private PIDFController largeHeadingPIDF = new PIDFController(FollowerConstants.largeHeadingPIDFCoefficients); private PIDFController headingPIDF = new PIDFController(FollowerConstants.headingPIDFCoefficients);
private FilteredPIDFController smallDrivePIDF = new FilteredPIDFController(FollowerConstants.smallDrivePIDFCoefficients); private FilteredPIDFController secondaryDrivePIDF = new FilteredPIDFController(FollowerConstants.secondaryDrivePIDFCoefficients);
private FilteredPIDFController largeDrivePIDF = new FilteredPIDFController(FollowerConstants.largeDrivePIDFCoefficients); private FilteredPIDFController drivePIDF = new FilteredPIDFController(FollowerConstants.drivePIDFCoefficients);
private KalmanFilter driveKalmanFilter = new KalmanFilter(FollowerConstants.driveKalmanFilterParameters); private KalmanFilter driveKalmanFilter = new KalmanFilter(FollowerConstants.driveKalmanFilterParameters);
private double[] driveErrors; private double[] driveErrors;
@ -597,18 +598,18 @@ public class Follower {
holdingPosition = false; holdingPosition = false;
isBusy = false; isBusy = false;
reachedParametricPathEnd = false; reachedParametricPathEnd = false;
smallDrivePIDF.reset(); secondaryDrivePIDF.reset();
largeDrivePIDF.reset(); drivePIDF.reset();
smallHeadingPIDF.reset(); secondaryHeadingPIDF.reset();
largeHeadingPIDF.reset(); headingPIDF.reset();
smallTranslationalPIDF.reset(); secondaryTranslationalPIDF.reset();
smallTranslationalIntegral.reset(); secondaryTranslationalIntegral.reset();
smallTranslationalIntegralVector = new Vector(); secondaryTranslationalIntegralVector = new Vector();
previousSmallTranslationalIntegral = 0; previousSecondaryTranslationalIntegral = 0;
largeTranslationalPIDF.reset(); translationalPIDF.reset();
largeTranslationalIntegral.reset(); translationalIntegral.reset();
largeTranslationalIntegralVector = new Vector(); translationalIntegralVector = new Vector();
previousLargeTranslationalIntegral = 0; previousTranslationalIntegral = 0;
driveVector = new Vector(); driveVector = new Vector();
headingVector = new Vector(); headingVector = new Vector();
translationalVector = new Vector(); translationalVector = new Vector();
@ -665,14 +666,14 @@ public class Follower {
driveError = getDriveVelocityError(); driveError = getDriveVelocityError();
if (Math.abs(driveError) < drivePIDFSwitch) { if (Math.abs(driveError) < drivePIDFSwitch && useSecondaryDrivePID) {
smallDrivePIDF.updateError(driveError); secondaryDrivePIDF.updateError(driveError);
driveVector = new Vector(MathFunctions.clamp(smallDrivePIDF.runPIDF() + smallDrivePIDFFeedForward * MathFunctions.getSign(driveError), -1, 1), currentPath.getClosestPointTangentVector().getTheta()); driveVector = new Vector(MathFunctions.clamp(secondaryDrivePIDF.runPIDF() + secondaryDrivePIDFFeedForward * MathFunctions.getSign(driveError), -1, 1), currentPath.getClosestPointTangentVector().getTheta());
return MathFunctions.copyVector(driveVector); return MathFunctions.copyVector(driveVector);
} }
largeDrivePIDF.updateError(driveError); drivePIDF.updateError(driveError);
driveVector = new Vector(MathFunctions.clamp(largeDrivePIDF.runPIDF() + largeDrivePIDFFeedForward * MathFunctions.getSign(driveError), -1, 1), currentPath.getClosestPointTangentVector().getTheta()); driveVector = new Vector(MathFunctions.clamp(drivePIDF.runPIDF() + drivePIDFFeedForward * MathFunctions.getSign(driveError), -1, 1), currentPath.getClosestPointTangentVector().getTheta());
return MathFunctions.copyVector(driveVector); return MathFunctions.copyVector(driveVector);
} }
@ -739,13 +740,13 @@ public class Follower {
public Vector getHeadingVector() { public Vector getHeadingVector() {
if (!useHeading) return new Vector(); if (!useHeading) return new Vector();
headingError = MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()) * MathFunctions.getSmallestAngleDifference(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()); headingError = MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()) * MathFunctions.getSmallestAngleDifference(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal());
if (Math.abs(headingError) < headingPIDFSwitch) { if (Math.abs(headingError) < headingPIDFSwitch && useSecondaryHeadingPID) {
smallHeadingPIDF.updateError(headingError); secondaryHeadingPIDF.updateError(headingError);
headingVector = new Vector(MathFunctions.clamp(smallHeadingPIDF.runPIDF() + smallHeadingPIDFFeedForward * MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()), -1, 1), poseUpdater.getPose().getHeading()); headingVector = new Vector(MathFunctions.clamp(secondaryHeadingPIDF.runPIDF() + secondaryHeadingPIDFFeedForward * MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()), -1, 1), poseUpdater.getPose().getHeading());
return MathFunctions.copyVector(headingVector); return MathFunctions.copyVector(headingVector);
} }
largeHeadingPIDF.updateError(headingError); headingPIDF.updateError(headingError);
headingVector = new Vector(MathFunctions.clamp(largeHeadingPIDF.runPIDF() + largeHeadingPIDFFeedForward * MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()), -1, 1), poseUpdater.getPose().getHeading()); headingVector = new Vector(MathFunctions.clamp(headingPIDF.runPIDF() + headingPIDFFeedForward * MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()), -1, 1), poseUpdater.getPose().getHeading());
return MathFunctions.copyVector(headingVector); return MathFunctions.copyVector(headingVector);
} }
@ -789,26 +790,26 @@ public class Follower {
if (!(currentPath.isAtParametricEnd() || currentPath.isAtParametricStart())) { if (!(currentPath.isAtParametricEnd() || currentPath.isAtParametricStart())) {
translationalVector = MathFunctions.subtractVectors(translationalVector, new Vector(MathFunctions.dotProduct(translationalVector, MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), currentPath.getClosestPointTangentVector().getTheta())); translationalVector = MathFunctions.subtractVectors(translationalVector, new Vector(MathFunctions.dotProduct(translationalVector, MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), currentPath.getClosestPointTangentVector().getTheta()));
smallTranslationalIntegralVector = MathFunctions.subtractVectors(smallTranslationalIntegralVector, new Vector(MathFunctions.dotProduct(smallTranslationalIntegralVector, MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), currentPath.getClosestPointTangentVector().getTheta())); secondaryTranslationalIntegralVector = MathFunctions.subtractVectors(secondaryTranslationalIntegralVector, new Vector(MathFunctions.dotProduct(secondaryTranslationalIntegralVector, MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), currentPath.getClosestPointTangentVector().getTheta()));
largeTranslationalIntegralVector = MathFunctions.subtractVectors(largeTranslationalIntegralVector, new Vector(MathFunctions.dotProduct(largeTranslationalIntegralVector, MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), currentPath.getClosestPointTangentVector().getTheta())); translationalIntegralVector = MathFunctions.subtractVectors(translationalIntegralVector, new Vector(MathFunctions.dotProduct(translationalIntegralVector, MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), currentPath.getClosestPointTangentVector().getTheta()));
} }
if (MathFunctions.distance(poseUpdater.getPose(), closestPose) < translationalPIDFSwitch) { if (MathFunctions.distance(poseUpdater.getPose(), closestPose) < translationalPIDFSwitch && useSecondaryTranslationalPID) {
smallTranslationalIntegral.updateError(translationalVector.getMagnitude()); secondaryTranslationalIntegral.updateError(translationalVector.getMagnitude());
smallTranslationalIntegralVector = MathFunctions.addVectors(smallTranslationalIntegralVector, new Vector(smallTranslationalIntegral.runPIDF() - previousSmallTranslationalIntegral, translationalVector.getTheta())); secondaryTranslationalIntegralVector = MathFunctions.addVectors(secondaryTranslationalIntegralVector, new Vector(secondaryTranslationalIntegral.runPIDF() - previousSecondaryTranslationalIntegral, translationalVector.getTheta()));
previousSmallTranslationalIntegral = smallTranslationalIntegral.runPIDF(); previousSecondaryTranslationalIntegral = secondaryTranslationalIntegral.runPIDF();
smallTranslationalPIDF.updateError(translationalVector.getMagnitude()); secondaryTranslationalPIDF.updateError(translationalVector.getMagnitude());
translationalVector.setMagnitude(smallTranslationalPIDF.runPIDF() + smallTranslationalPIDFFeedForward); translationalVector.setMagnitude(secondaryTranslationalPIDF.runPIDF() + secondaryTranslationalPIDFFeedForward);
translationalVector = MathFunctions.addVectors(translationalVector, smallTranslationalIntegralVector); translationalVector = MathFunctions.addVectors(translationalVector, secondaryTranslationalIntegralVector);
} else { } else {
largeTranslationalIntegral.updateError(translationalVector.getMagnitude()); translationalIntegral.updateError(translationalVector.getMagnitude());
largeTranslationalIntegralVector = MathFunctions.addVectors(largeTranslationalIntegralVector, new Vector(largeTranslationalIntegral.runPIDF() - previousLargeTranslationalIntegral, translationalVector.getTheta())); translationalIntegralVector = MathFunctions.addVectors(translationalIntegralVector, new Vector(translationalIntegral.runPIDF() - previousTranslationalIntegral, translationalVector.getTheta()));
previousLargeTranslationalIntegral = largeTranslationalIntegral.runPIDF(); previousTranslationalIntegral = translationalIntegral.runPIDF();
largeTranslationalPIDF.updateError(translationalVector.getMagnitude()); translationalPIDF.updateError(translationalVector.getMagnitude());
translationalVector.setMagnitude(largeTranslationalPIDF.runPIDF() + largeTranslationalPIDFFeedForward); translationalVector.setMagnitude(translationalPIDF.runPIDF() + translationalPIDFFeedForward);
translationalVector = MathFunctions.addVectors(translationalVector, largeTranslationalIntegralVector); translationalVector = MathFunctions.addVectors(translationalVector, translationalIntegralVector);
} }
translationalVector.setMagnitude(MathFunctions.clamp(translationalVector.getMagnitude(), 0, 1)); translationalVector.setMagnitude(MathFunctions.clamp(translationalVector.getMagnitude(), 0, 1));

135
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java
View File

@ -29,102 +29,60 @@ public class FollowerConstants {
private static double[] convertToPolar = Point.cartesianToPolar(xMovement, -yMovement); private static double[] convertToPolar = Point.cartesianToPolar(xMovement, -yMovement);
public static Vector frontLeftVector = MathFunctions.normalizeVector(new Vector(convertToPolar[0],convertToPolar[1])); public static Vector frontLeftVector = MathFunctions.normalizeVector(new Vector(convertToPolar[0],convertToPolar[1]));
// Large translational PIDF coefficients
public static CustomPIDFCoefficients largeTranslationalPIDFCoefficients = new CustomPIDFCoefficients( // Translational PIDF coefficients (don't use integral)
public static CustomPIDFCoefficients translationalPIDFCoefficients = new CustomPIDFCoefficients(
0.1, 0.1,
0, 0,
0, 0,
0); 0);
// Feed forward constant added on to the large translational PIDF // Translational Integral
public static double largeTranslationalPIDFFeedForward = 0.015; public static CustomPIDFCoefficients translationalIntegral = new CustomPIDFCoefficients(
// Large translational Integral
public static CustomPIDFCoefficients largeTranslationalIntegral = new CustomPIDFCoefficients(
0, 0,
0, 0,
0, 0,
0); 0);
// the limit at which the heading PIDF switches between the large and small translational PIDFs // Feed forward constant added on to the translational PIDF
public static double translationalPIDFSwitch = 3; public static double translationalPIDFFeedForward = 0.015;
// Small translational PIDF coefficients
public static CustomPIDFCoefficients smallTranslationalPIDFCoefficients = new CustomPIDFCoefficients(
0.3,
0,
0.01,
0);
// Small translational Integral value // Heading error PIDF coefficients
public static CustomPIDFCoefficients smallTranslationalIntegral = new CustomPIDFCoefficients( public static CustomPIDFCoefficients headingPIDFCoefficients = new CustomPIDFCoefficients(
0,
0,
0,
0);
// Feed forward constant added on to the small translational PIDF
public static double smallTranslationalPIDFFeedForward = 0.015;
// Large heading error PIDF coefficients
public static CustomPIDFCoefficients largeHeadingPIDFCoefficients = new CustomPIDFCoefficients(
1, 1,
0, 0,
0, 0,
0); 0);
// Feed forward constant added on to the large heading PIDF // Feed forward constant added on to the heading PIDF
public static double largeHeadingPIDFFeedForward = 0.01; public static double headingPIDFFeedForward = 0.01;
// the limit at which the heading PIDF switches between the large and small heading PIDFs
public static double headingPIDFSwitch = Math.PI/20;
// Small heading error PIDF coefficients // Drive PIDF coefficients
public static CustomPIDFCoefficients smallHeadingPIDFCoefficients = new CustomPIDFCoefficients( public static CustomFilteredPIDFCoefficients drivePIDFCoefficients = new CustomFilteredPIDFCoefficients(
5,
0,
0.08,
0);
// Feed forward constant added on to the small heading PIDF
public static double smallHeadingPIDFFeedForward = 0.01;
// Large drive PIDF coefficients
public static CustomFilteredPIDFCoefficients largeDrivePIDFCoefficients = new CustomFilteredPIDFCoefficients(
0.025, 0.025,
0, 0,
0.00001, 0.00001,
0.6, 0.6,
0); 0);
// Feed forward constant added on to the large drive PIDF // Feed forward constant added on to the drive PIDF
public static double largeDrivePIDFFeedForward = 0.01; public static double drivePIDFFeedForward = 0.01;
// the limit at which the heading PIDF switches between the large and small drive PIDFs
public static double drivePIDFSwitch = 20;
// Small drive PIDF coefficients
public static CustomFilteredPIDFCoefficients smallDrivePIDFCoefficients = new CustomFilteredPIDFCoefficients(
0.02,
0,
0.000005,
0.6,
0);
// Feed forward constant added on to the small drive PIDF
public static double smallDrivePIDFFeedForward = 0.01;
// Kalman filter parameters for the drive error Kalman filter // Kalman filter parameters for the drive error Kalman filter
public static KalmanFilterParameters driveKalmanFilterParameters = new KalmanFilterParameters( public static KalmanFilterParameters driveKalmanFilterParameters = new KalmanFilterParameters(
6, 6,
1); 1);
// Mass of robot in kilograms // Mass of robot in kilograms
public static double mass = 10.65942; public static double mass = 10.65942;
// Centripetal force to power scaling // Centripetal force to power scaling
public static double centripetalScaling = 0.0005; public static double centripetalScaling = 0.0005;
// Acceleration of the drivetrain when power is cut in inches/second^2 (should be negative) // Acceleration of the drivetrain when power is cut in inches/second^2 (should be negative)
// if not negative, then the robot thinks that its going to go faster under 0 power // if not negative, then the robot thinks that its going to go faster under 0 power
public static double forwardZeroPowerAcceleration = -34.62719; public static double forwardZeroPowerAcceleration = -34.62719;
@ -142,6 +100,7 @@ public class FollowerConstants {
// This can be set individually for each Path, but this is the default. // This can be set individually for each Path, but this is the default.
public static double zeroPowerAccelerationMultiplier = 4; public static double zeroPowerAccelerationMultiplier = 4;
// When the robot is at the end of its current Path or PathChain and the velocity goes below // When the robot is at the end of its current Path or PathChain and the velocity goes below
// this value, then end the Path. This is in inches/second. // this value, then end the Path. This is in inches/second.
// This can be custom set for each Path. // This can be custom set for each Path.
@ -185,4 +144,62 @@ public class FollowerConstants {
// accuracy, and this increases at an exponential rate. However, more steps also does take more // accuracy, and this increases at an exponential rate. However, more steps also does take more
// time. // time.
public static int BEZIER_CURVE_BINARY_STEP_LIMIT = 10; public static int BEZIER_CURVE_BINARY_STEP_LIMIT = 10;
// These activate / deactivate the secondary PIDs. These take over at errors under a set limit for
// the translational, heading, and drive PIDs.
public static boolean useSecondaryTranslationalPID = false;
public static boolean useSecondaryHeadingPID = false;
public static boolean useSecondaryDrivePID = false;
// the limit at which the translational PIDF switches between the main and secondary translational PIDFs,
// if the secondary PID is active
public static double translationalPIDFSwitch = 3;
// Secondary translational PIDF coefficients (don't use integral)
public static CustomPIDFCoefficients secondaryTranslationalPIDFCoefficients = new CustomPIDFCoefficients(
0.3,
0,
0.01,
0);
// Secondary translational Integral value
public static CustomPIDFCoefficients secondaryTranslationalIntegral = new CustomPIDFCoefficients(
0,
0,
0,
0);
// Feed forward constant added on to the small translational PIDF
public static double secondaryTranslationalPIDFFeedForward = 0.015;
// the limit at which the heading PIDF switches between the main and secondary heading PIDFs
public static double headingPIDFSwitch = Math.PI/20;
// Secondary heading error PIDF coefficients
public static CustomPIDFCoefficients secondaryHeadingPIDFCoefficients = new CustomPIDFCoefficients(
5,
0,
0.08,
0);
// Feed forward constant added on to the secondary heading PIDF
public static double secondaryHeadingPIDFFeedForward = 0.01;
// the limit at which the heading PIDF switches between the main and secondary drive PIDFs
public static double drivePIDFSwitch = 20;
// Secondary drive PIDF coefficients
public static CustomFilteredPIDFCoefficients secondaryDrivePIDFCoefficients = new CustomFilteredPIDFCoefficients(
0.02,
0,
0.000005,
0.6,
0);
// Feed forward constant added on to the secondary drive PIDF
public static double secondaryDrivePIDFFeedForward = 0.01;
} }