simplifying the PID system
This commit is contained in:
brotherhobo
@ -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.
|
||||
|
||||
### Translational Correction
|
||||
This is as simple as it sounds: this corrects error in the robot's position only. However, the
|
||||
robot's translational error is actually used within two different PIDs to move the robot. A large
|
||||
PID and a small PID are used. The large PID is used when error exceeds a certain limit. and the small
|
||||
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.
|
||||
This is as simple as it sounds: this corrects error in the robot's position only. The robot's translational
|
||||
error is corrected with a PID control. The translational correction does not act along the path the
|
||||
robot takes, but instead moves the robot back to the closest point on the path.
|
||||
|
||||
### Heading Correction
|
||||
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 translational correction. The heading correction will turn in the closest direction from the
|
||||
the direction the robot is facing. The heading correction will turn in the closest direction from the
|
||||
robot's current heading to the target heading.
|
||||
|
||||
### Drive Vector
|
||||
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
|
||||
uses a large and small PID. Using basic kinematics equations, we can use the velocity of the robot
|
||||
the robot along the path. 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
|
||||
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
|
||||
|
||||
@ -10,7 +10,7 @@ One last thing to note is that Pedro Pathing operates in inches and radians.
|
||||
|
||||
## Tuning
|
||||
* 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.
|
||||
|
||||
* 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
|
||||
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
|
||||
`Forward Zero Power Acceleration Tuner` on line `130` in the `FollowerConstants` class and the zero
|
||||
power acceleration for the `Lateral Zero Power Acceleration Tuner` on line `134` in the
|
||||
`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 `92` in the
|
||||
`FollowerConstants` class.
|
||||
|
||||
* 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
|
||||
sure you disable the timer on autonomous OpModes. There are two different PIDs for translational
|
||||
error, the `smallTranslationalPIDF` and `largeTranslationalPIDF`. If error is larger than a certain
|
||||
amount, the `largeTranslationalPIDF` will be used, and if error is smaller than that amount the
|
||||
`smallTranslationalPIDF` will be used. If you need to add a feedforward value, use the
|
||||
`smallTranslationalPIDFFeedForward` and `largeTranslationalPIDFFeedForward` since those will add the
|
||||
feedforward in the direction the robot is trying to move, rather than the feedforward in the PIDFs
|
||||
themselves, since those will only add the feedforward one way. You can change the amount of error
|
||||
required to switch PIDFs, as well as the PIDF constants and feedforward values, under the
|
||||
`FollowerConstants` tab in FTC Dashboard. To tune the PIDs, push the robot off the path and see how
|
||||
corrects. You will want to alternate sides you push to reduce field wear and tear as well as push
|
||||
with varying power and distance. I would recommend tuning the large PID first, and tuning it so that
|
||||
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.
|
||||
sure you disable the timer on autonomous OpModes. The PID for the translational error is called
|
||||
`translationalPIDF`. If you need to add a feedforward value, use the `translationalPIDFFeedForward`
|
||||
since that will add the feedforward in the direction the robot is trying to move, rather than the
|
||||
feedforward in the PIDF itself, since those will only add the feedforward one way. You can change
|
||||
the PIDF constants and feedforward values, under the `FollowerConstants` tab in FTC Dashboard.
|
||||
To tune the PID, push the robot off the path and see how corrects. You will want to alternate sides
|
||||
you push to reduce field wear and tear as well as push with varying power and distance. I would
|
||||
recommend tuning the PID so that it is capable of correcting while minimizing oscillations and 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
|
||||
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
|
||||
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
|
||||
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,
|
||||
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
|
||||
@ -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
|
||||
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
|
||||
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
|
||||
`centripetalScaling`.
|
||||
|
||||
|
||||
@ -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.forwardZeroPowerAcceleration;
|
||||
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.largeHeadingPIDFFeedForward;
|
||||
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.largeTranslationalPIDFFeedForward;
|
||||
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.drivePIDFFeedForward;
|
||||
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.headingPIDFFeedForward;
|
||||
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.smallDrivePIDFFeedForward;
|
||||
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.smallHeadingPIDFFeedForward;
|
||||
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.smallTranslationalPIDFFeedForward;
|
||||
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.secondaryDrivePIDFFeedForward;
|
||||
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.secondaryHeadingPIDFFeedForward;
|
||||
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.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.telemetry.MultipleTelemetry;
|
||||
@ -87,8 +90,8 @@ public class Follower {
|
||||
private boolean teleopDrive;
|
||||
|
||||
private double maxPower = 1;
|
||||
private double previousSmallTranslationalIntegral;
|
||||
private double previousLargeTranslationalIntegral;
|
||||
private double previousSecondaryTranslationalIntegral;
|
||||
private double previousTranslationalIntegral;
|
||||
private double holdPointTranslationalScaling = FollowerConstants.holdPointTranslationalScaling;
|
||||
private double holdPointHeadingScaling = FollowerConstants.holdPointHeadingScaling;
|
||||
public double driveError;
|
||||
@ -99,16 +102,14 @@ public class Follower {
|
||||
private double[] drivePowers;
|
||||
private double[] teleopDriveValues;
|
||||
|
||||
private Vector[] teleOpMovementVectors = new Vector[]{new Vector(), new Vector(), new Vector()};
|
||||
|
||||
private ArrayList<Vector> velocities = new ArrayList<>();
|
||||
private ArrayList<Vector> accelerations = new ArrayList<>();
|
||||
|
||||
private Vector averageVelocity;
|
||||
private Vector averagePreviousVelocity;
|
||||
private Vector averageAcceleration;
|
||||
private Vector smallTranslationalIntegralVector;
|
||||
private Vector largeTranslationalIntegralVector;
|
||||
private Vector secondaryTranslationalIntegralVector;
|
||||
private Vector translationalIntegralVector;
|
||||
private Vector teleopDriveVector;
|
||||
private Vector teleopHeadingVector;
|
||||
public Vector driveVector;
|
||||
@ -117,14 +118,14 @@ public class Follower {
|
||||
public Vector centripetalVector;
|
||||
public Vector correctiveVector;
|
||||
|
||||
private PIDFController smallTranslationalPIDF = new PIDFController(FollowerConstants.smallTranslationalPIDFCoefficients);
|
||||
private PIDFController smallTranslationalIntegral = new PIDFController(FollowerConstants.smallTranslationalIntegral);
|
||||
private PIDFController largeTranslationalPIDF = new PIDFController(FollowerConstants.largeTranslationalPIDFCoefficients);
|
||||
private PIDFController largeTranslationalIntegral = new PIDFController(FollowerConstants.largeTranslationalIntegral);
|
||||
private PIDFController smallHeadingPIDF = new PIDFController(FollowerConstants.smallHeadingPIDFCoefficients);
|
||||
private PIDFController largeHeadingPIDF = new PIDFController(FollowerConstants.largeHeadingPIDFCoefficients);
|
||||
private FilteredPIDFController smallDrivePIDF = new FilteredPIDFController(FollowerConstants.smallDrivePIDFCoefficients);
|
||||
private FilteredPIDFController largeDrivePIDF = new FilteredPIDFController(FollowerConstants.largeDrivePIDFCoefficients);
|
||||
private PIDFController secondaryTranslationalPIDF = new PIDFController(FollowerConstants.secondaryTranslationalPIDFCoefficients);
|
||||
private PIDFController secondaryTranslationalIntegral = new PIDFController(FollowerConstants.secondaryTranslationalIntegral);
|
||||
private PIDFController translationalPIDF = new PIDFController(FollowerConstants.translationalPIDFCoefficients);
|
||||
private PIDFController translationalIntegral = new PIDFController(FollowerConstants.translationalIntegral);
|
||||
private PIDFController secondaryHeadingPIDF = new PIDFController(FollowerConstants.secondaryHeadingPIDFCoefficients);
|
||||
private PIDFController headingPIDF = new PIDFController(FollowerConstants.headingPIDFCoefficients);
|
||||
private FilteredPIDFController secondaryDrivePIDF = new FilteredPIDFController(FollowerConstants.secondaryDrivePIDFCoefficients);
|
||||
private FilteredPIDFController drivePIDF = new FilteredPIDFController(FollowerConstants.drivePIDFCoefficients);
|
||||
|
||||
private KalmanFilter driveKalmanFilter = new KalmanFilter(FollowerConstants.driveKalmanFilterParameters);
|
||||
private double[] driveErrors;
|
||||
@ -597,18 +598,18 @@ public class Follower {
|
||||
holdingPosition = false;
|
||||
isBusy = false;
|
||||
reachedParametricPathEnd = false;
|
||||
smallDrivePIDF.reset();
|
||||
largeDrivePIDF.reset();
|
||||
smallHeadingPIDF.reset();
|
||||
largeHeadingPIDF.reset();
|
||||
smallTranslationalPIDF.reset();
|
||||
smallTranslationalIntegral.reset();
|
||||
smallTranslationalIntegralVector = new Vector();
|
||||
previousSmallTranslationalIntegral = 0;
|
||||
largeTranslationalPIDF.reset();
|
||||
largeTranslationalIntegral.reset();
|
||||
largeTranslationalIntegralVector = new Vector();
|
||||
previousLargeTranslationalIntegral = 0;
|
||||
secondaryDrivePIDF.reset();
|
||||
drivePIDF.reset();
|
||||
secondaryHeadingPIDF.reset();
|
||||
headingPIDF.reset();
|
||||
secondaryTranslationalPIDF.reset();
|
||||
secondaryTranslationalIntegral.reset();
|
||||
secondaryTranslationalIntegralVector = new Vector();
|
||||
previousSecondaryTranslationalIntegral = 0;
|
||||
translationalPIDF.reset();
|
||||
translationalIntegral.reset();
|
||||
translationalIntegralVector = new Vector();
|
||||
previousTranslationalIntegral = 0;
|
||||
driveVector = new Vector();
|
||||
headingVector = new Vector();
|
||||
translationalVector = new Vector();
|
||||
@ -665,14 +666,14 @@ public class Follower {
|
||||
|
||||
driveError = getDriveVelocityError();
|
||||
|
||||
if (Math.abs(driveError) < drivePIDFSwitch) {
|
||||
smallDrivePIDF.updateError(driveError);
|
||||
driveVector = new Vector(MathFunctions.clamp(smallDrivePIDF.runPIDF() + smallDrivePIDFFeedForward * MathFunctions.getSign(driveError), -1, 1), currentPath.getClosestPointTangentVector().getTheta());
|
||||
if (Math.abs(driveError) < drivePIDFSwitch && useSecondaryDrivePID) {
|
||||
secondaryDrivePIDF.updateError(driveError);
|
||||
driveVector = new Vector(MathFunctions.clamp(secondaryDrivePIDF.runPIDF() + secondaryDrivePIDFFeedForward * MathFunctions.getSign(driveError), -1, 1), currentPath.getClosestPointTangentVector().getTheta());
|
||||
return MathFunctions.copyVector(driveVector);
|
||||
}
|
||||
|
||||
largeDrivePIDF.updateError(driveError);
|
||||
driveVector = new Vector(MathFunctions.clamp(largeDrivePIDF.runPIDF() + largeDrivePIDFFeedForward * MathFunctions.getSign(driveError), -1, 1), currentPath.getClosestPointTangentVector().getTheta());
|
||||
drivePIDF.updateError(driveError);
|
||||
driveVector = new Vector(MathFunctions.clamp(drivePIDF.runPIDF() + drivePIDFFeedForward * MathFunctions.getSign(driveError), -1, 1), currentPath.getClosestPointTangentVector().getTheta());
|
||||
return MathFunctions.copyVector(driveVector);
|
||||
}
|
||||
|
||||
@ -739,13 +740,13 @@ public class Follower {
|
||||
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) {
|
||||
smallHeadingPIDF.updateError(headingError);
|
||||
headingVector = new Vector(MathFunctions.clamp(smallHeadingPIDF.runPIDF() + smallHeadingPIDFFeedForward * MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()), -1, 1), poseUpdater.getPose().getHeading());
|
||||
if (Math.abs(headingError) < headingPIDFSwitch && useSecondaryHeadingPID) {
|
||||
secondaryHeadingPIDF.updateError(headingError);
|
||||
headingVector = new Vector(MathFunctions.clamp(secondaryHeadingPIDF.runPIDF() + secondaryHeadingPIDFFeedForward * MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()), -1, 1), poseUpdater.getPose().getHeading());
|
||||
return MathFunctions.copyVector(headingVector);
|
||||
}
|
||||
largeHeadingPIDF.updateError(headingError);
|
||||
headingVector = new Vector(MathFunctions.clamp(largeHeadingPIDF.runPIDF() + largeHeadingPIDFFeedForward * MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()), -1, 1), poseUpdater.getPose().getHeading());
|
||||
headingPIDF.updateError(headingError);
|
||||
headingVector = new Vector(MathFunctions.clamp(headingPIDF.runPIDF() + headingPIDFFeedForward * MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()), -1, 1), poseUpdater.getPose().getHeading());
|
||||
return MathFunctions.copyVector(headingVector);
|
||||
}
|
||||
|
||||
@ -789,26 +790,26 @@ public class Follower {
|
||||
if (!(currentPath.isAtParametricEnd() || currentPath.isAtParametricStart())) {
|
||||
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()));
|
||||
largeTranslationalIntegralVector = MathFunctions.subtractVectors(largeTranslationalIntegralVector, new Vector(MathFunctions.dotProduct(largeTranslationalIntegralVector, MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), currentPath.getClosestPointTangentVector().getTheta()));
|
||||
secondaryTranslationalIntegralVector = MathFunctions.subtractVectors(secondaryTranslationalIntegralVector, new Vector(MathFunctions.dotProduct(secondaryTranslationalIntegralVector, 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) {
|
||||
smallTranslationalIntegral.updateError(translationalVector.getMagnitude());
|
||||
smallTranslationalIntegralVector = MathFunctions.addVectors(smallTranslationalIntegralVector, new Vector(smallTranslationalIntegral.runPIDF() - previousSmallTranslationalIntegral, translationalVector.getTheta()));
|
||||
previousSmallTranslationalIntegral = smallTranslationalIntegral.runPIDF();
|
||||
if (MathFunctions.distance(poseUpdater.getPose(), closestPose) < translationalPIDFSwitch && useSecondaryTranslationalPID) {
|
||||
secondaryTranslationalIntegral.updateError(translationalVector.getMagnitude());
|
||||
secondaryTranslationalIntegralVector = MathFunctions.addVectors(secondaryTranslationalIntegralVector, new Vector(secondaryTranslationalIntegral.runPIDF() - previousSecondaryTranslationalIntegral, translationalVector.getTheta()));
|
||||
previousSecondaryTranslationalIntegral = secondaryTranslationalIntegral.runPIDF();
|
||||
|
||||
smallTranslationalPIDF.updateError(translationalVector.getMagnitude());
|
||||
translationalVector.setMagnitude(smallTranslationalPIDF.runPIDF() + smallTranslationalPIDFFeedForward);
|
||||
translationalVector = MathFunctions.addVectors(translationalVector, smallTranslationalIntegralVector);
|
||||
secondaryTranslationalPIDF.updateError(translationalVector.getMagnitude());
|
||||
translationalVector.setMagnitude(secondaryTranslationalPIDF.runPIDF() + secondaryTranslationalPIDFFeedForward);
|
||||
translationalVector = MathFunctions.addVectors(translationalVector, secondaryTranslationalIntegralVector);
|
||||
} else {
|
||||
largeTranslationalIntegral.updateError(translationalVector.getMagnitude());
|
||||
largeTranslationalIntegralVector = MathFunctions.addVectors(largeTranslationalIntegralVector, new Vector(largeTranslationalIntegral.runPIDF() - previousLargeTranslationalIntegral, translationalVector.getTheta()));
|
||||
previousLargeTranslationalIntegral = largeTranslationalIntegral.runPIDF();
|
||||
translationalIntegral.updateError(translationalVector.getMagnitude());
|
||||
translationalIntegralVector = MathFunctions.addVectors(translationalIntegralVector, new Vector(translationalIntegral.runPIDF() - previousTranslationalIntegral, translationalVector.getTheta()));
|
||||
previousTranslationalIntegral = translationalIntegral.runPIDF();
|
||||
|
||||
largeTranslationalPIDF.updateError(translationalVector.getMagnitude());
|
||||
translationalVector.setMagnitude(largeTranslationalPIDF.runPIDF() + largeTranslationalPIDFFeedForward);
|
||||
translationalVector = MathFunctions.addVectors(translationalVector, largeTranslationalIntegralVector);
|
||||
translationalPIDF.updateError(translationalVector.getMagnitude());
|
||||
translationalVector.setMagnitude(translationalPIDF.runPIDF() + translationalPIDFFeedForward);
|
||||
translationalVector = MathFunctions.addVectors(translationalVector, translationalIntegralVector);
|
||||
}
|
||||
|
||||
translationalVector.setMagnitude(MathFunctions.clamp(translationalVector.getMagnitude(), 0, 1));
|
||||
|
||||
@ -29,102 +29,60 @@ public class FollowerConstants {
|
||||
private static double[] convertToPolar = Point.cartesianToPolar(xMovement, -yMovement);
|
||||
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,
|
||||
0,
|
||||
0);
|
||||
|
||||
// Feed forward constant added on to the large translational PIDF
|
||||
public static double largeTranslationalPIDFFeedForward = 0.015;
|
||||
|
||||
// Large translational Integral
|
||||
public static CustomPIDFCoefficients largeTranslationalIntegral = new CustomPIDFCoefficients(
|
||||
// Translational Integral
|
||||
public static CustomPIDFCoefficients translationalIntegral = new CustomPIDFCoefficients(
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0);
|
||||
|
||||
// the limit at which the heading PIDF switches between the large and small translational PIDFs
|
||||
public static double translationalPIDFSwitch = 3;
|
||||
// Feed forward constant added on to the translational PIDF
|
||||
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
|
||||
public static CustomPIDFCoefficients smallTranslationalIntegral = 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(
|
||||
// Heading error PIDF coefficients
|
||||
public static CustomPIDFCoefficients headingPIDFCoefficients = new CustomPIDFCoefficients(
|
||||
1,
|
||||
0,
|
||||
0,
|
||||
0);
|
||||
|
||||
// Feed forward constant added on to the large heading PIDF
|
||||
public static double largeHeadingPIDFFeedForward = 0.01;
|
||||
// Feed forward constant added on to the heading PIDF
|
||||
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
|
||||
public static CustomPIDFCoefficients smallHeadingPIDFCoefficients = new CustomPIDFCoefficients(
|
||||
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(
|
||||
// Drive PIDF coefficients
|
||||
public static CustomFilteredPIDFCoefficients drivePIDFCoefficients = new CustomFilteredPIDFCoefficients(
|
||||
0.025,
|
||||
0,
|
||||
0.00001,
|
||||
0.6,
|
||||
0);
|
||||
|
||||
// Feed forward constant added on to the large drive PIDF
|
||||
public static double largeDrivePIDFFeedForward = 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;
|
||||
// Feed forward constant added on to the drive PIDF
|
||||
public static double drivePIDFFeedForward = 0.01;
|
||||
|
||||
// Kalman filter parameters for the drive error Kalman filter
|
||||
public static KalmanFilterParameters driveKalmanFilterParameters = new KalmanFilterParameters(
|
||||
6,
|
||||
1);
|
||||
|
||||
|
||||
// Mass of robot in kilograms
|
||||
public static double mass = 10.65942;
|
||||
|
||||
// Centripetal force to power scaling
|
||||
public static double centripetalScaling = 0.0005;
|
||||
|
||||
|
||||
// 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
|
||||
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.
|
||||
public static double zeroPowerAccelerationMultiplier = 4;
|
||||
|
||||
|
||||
// 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 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
|
||||
// time.
|
||||
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;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user