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 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). 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. 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 `114` in the `FollowerConstants` class under the 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. * @return returns the centripetal force correction vector.
*/ */
public Vector getCentripetalForceCorrection() { public Vector getCentripetalForceCorrection() {
if (!useCentripetal) return new Vector(); return new Vector();
double curvature; // if (!useCentripetal) return new Vector();
if (auto) { // double curvature;
curvature = currentPath.getClosestPointCurvature(); // if (auto) {
} else { // curvature = currentPath.getClosestPointCurvature();
double yPrime = averageVelocity.getYComponent() / averageVelocity.getXComponent(); // } else {
double yDoublePrime = averageAcceleration.getYComponent() / averageVelocity.getXComponent(); // double yPrime = averageVelocity.getYComponent() / averageVelocity.getXComponent();
curvature = (Math.pow(Math.sqrt(1 + Math.pow(yPrime, 2)), 3)) / (yDoublePrime); // 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())); // if (Double.isNaN(curvature)) return new Vector();
return centripetalVector; // 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() { public void init() {
follower = new Follower(hardwareMap); 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))); 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(DISTANCE,DISTANCE, Point.CARTESIAN), new Point(DISTANCE,0, Point.CARTESIAN), new Point(0,0, 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); 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; public static double mass = 10.65942;
// Centripetal force to power scaling // 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; public static double centripetalScaling = 0.001;
// 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)