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

centripetal correction

Browse Source
This commit is contained in:
brotherhobo
2024-08-11 15:50:08 -04:00
parent 2c28cda121
commit 87f42407f7
1 changed files with 42 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -433,6 +433,7 @@ public class Follower {
} }
if (!teleopDrive) { if (!teleopDrive) {
if (currentPath != null) {
if (holdingPosition) { if (holdingPosition) {
closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), 1); closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), 1);
@ -486,6 +487,7 @@ public class Follower {
} }
} }
} }
}
} else { } else {
velocities.add(poseUpdater.getVelocity()); velocities.add(poseUpdater.getVelocity());
velocities.remove(velocities.get(velocities.size() - 1)); velocities.remove(velocities.get(velocities.size() - 1));
@ -851,7 +853,7 @@ public class Follower {
curvature = (yDoublePrime) / (Math.pow(Math.sqrt(1 + Math.pow(yPrime, 2)), 3)); curvature = (yDoublePrime) / (Math.pow(Math.sqrt(1 + Math.pow(yPrime, 2)), 3));
} }
if (Double.isNaN(curvature)) return new Vector(); 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())); centripetalVector = new Vector(MathFunctions.clamp(FollowerConstants.centripetalScaling * FollowerConstants.mass * Math.pow(MathFunctions.dotProduct(poseUpdater.getVelocity(), MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), 2) * Math.abs(curvature), -1, 1), currentPath.getClosestPointTangentVector().getTheta() + Math.PI / 2 * MathFunctions.getSign(currentPath.getClosestPointNormalVector().getTheta()));
return centripetalVector; return centripetalVector;
} }