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

centripetal force correction fix

Browse Source
This commit is contained in:
brotherhobo
2024-03-30 22:47:57 -04:00
parent 3af80e243b
commit 2ba5639bbc
4 changed files with 18 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -7,6 +7,7 @@ localizer tuned before starting tuning Pedro Pathing, since Pedro Pathing needs
robot is at all times. Additionally, using [FTC Dashboard](http://192.168.43.1:8080/dash) will help
a lot in tuning, and we have a slightly scuffed Desmos path visualizer [here](https://www.desmos.com/calculator/3so1zx0hcd).
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 `114` in the `FollowerConstants` class under the

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

@ -757,18 +757,19 @@ public class Follower {
* @return returns the centripetal force correction vector.
*/
public Vector getCentripetalForceCorrection() {
if (!useCentripetal) return new Vector();
double curvature;
if (auto) {
curvature = currentPath.getClosestPointCurvature();
} else {
double yPrime = averageVelocity.getYComponent() / averageVelocity.getXComponent();
double yDoublePrime = averageAcceleration.getYComponent() / averageVelocity.getXComponent();
curvature = (Math.pow(Math.sqrt(1 + Math.pow(yPrime, 2)), 3)) / (yDoublePrime);
}
if (Double.isNaN(curvature)) return new Vector();
centripetalVector = new Vector(MathFunctions.clamp(FollowerConstants.centripetalScaling * FollowerConstants.mass * Math.pow(MathFunctions.dotProduct(poseUpdater.getVelocity(), MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), 2) * curvature, -1, 1), currentPath.getClosestPointTangentVector().getTheta() + -Math.PI / 2 * MathFunctions.getSign(currentPath.getClosestPointNormalVector().getTheta()));
return centripetalVector;
return new Vector();
// if (!useCentripetal) return new Vector();
// double curvature;
// if (auto) {
// curvature = currentPath.getClosestPointCurvature();
// } else {
// double yPrime = averageVelocity.getYComponent() / averageVelocity.getXComponent();
// double yDoublePrime = averageAcceleration.getYComponent() / averageVelocity.getXComponent();
// curvature = (Math.pow(Math.sqrt(1 + Math.pow(yPrime, 2)), 3)) / (yDoublePrime);
// }
// if (Double.isNaN(curvature)) return new Vector();
// centripetalVector = new Vector(MathFunctions.clamp(Math.abs(FollowerConstants.centripetalScaling * FollowerConstants.mass * Math.pow(MathFunctions.dotProduct(poseUpdater.getVelocity(), MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), 2) * curvature), -1, 1), currentPath.getClosestPointTangentVector().getTheta() + Math.PI / 2 * MathFunctions.getSign(currentPath.getClosestPointNormalVector().getTheta()));
// return centripetalVector;
}
/**

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/CurvedBackAndForth.java
View File

@ -47,8 +47,8 @@ public class CurvedBackAndForth extends OpMode {
public void init() {
follower = new Follower(hardwareMap);
forwards = new Path(new BezierCurve(new Point(0,0, Point.CARTESIAN), new Point(DISTANCE,0, Point.CARTESIAN), new Point(DISTANCE,DISTANCE, Point.CARTESIAN)));
backwards = new Path(new BezierCurve(new Point(DISTANCE,DISTANCE, Point.CARTESIAN), new Point(DISTANCE,0, Point.CARTESIAN), new Point(0,0, Point.CARTESIAN)));
forwards = new Path(new BezierCurve(new Point(0,0, Point.CARTESIAN), new Point(Math.abs(DISTANCE),0, Point.CARTESIAN), new Point(Math.abs(DISTANCE),DISTANCE, Point.CARTESIAN)));
backwards = new Path(new BezierCurve(new Point(Math.abs(DISTANCE),DISTANCE, Point.CARTESIAN), new Point(Math.abs(DISTANCE),0, Point.CARTESIAN), new Point(0,0, Point.CARTESIAN)));
backwards.setReversed(true);

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

@ -114,6 +114,8 @@ public class FollowerConstants {
public static double mass = 10.65942;
// Centripetal force to power scaling
// todo: there are currently issues with the centripetal force correction, so just don't use it for now
// i will fix these in another commit soon
public static double centripetalScaling = 0.001;
// Acceleration of the drivetrain when power is cut in inches/second^2 (should be negative)