From 2ba5639bbc9caca3b4076e5b27372e1d29b00182 Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Sat, 30 Mar 2024 22:47:57 -0400 Subject: [PATCH] centripetal force correction fix --- .../ftc/teamcode/pedroPathing/TUNING.md | 1 + .../pedroPathing/follower/Follower.java | 25 ++++++++++--------- .../tuning/CurvedBackAndForth.java | 4 +-- .../tuning/FollowerConstants.java | 2 ++ 4 files changed, 18 insertions(+), 14 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md index cfc1323..7aa7279 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md @@ -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 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java index ca2a607..3cadba6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java @@ -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; } /** diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/CurvedBackAndForth.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/CurvedBackAndForth.java index a93dc28..6ff548f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/CurvedBackAndForth.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/CurvedBackAndForth.java @@ -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); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java index be954e2..bf0c302 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java @@ -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)