Skip to content
This repository has been archived by the owner on Dec 30, 2024. It is now read-only.

Commit

Permalink
centripetal correction
Browse files Browse the repository at this point in the history
  • Loading branch information
@AnyiLin
AnyiLin committed Aug 11, 2024
1 parent 2c28cda commit 87f4240
Showing 1 changed file with 41 additions and 39 deletions.
80 changes: 41 additions & 39 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -433,54 +433,56 @@ public void update() {
}

if (!teleopDrive) {
if (holdingPosition) {
closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), 1);
if (currentPath != null) {
if (holdingPosition) {
closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), 1);

drivePowers = driveVectorScaler.getDrivePowers(MathFunctions.scalarMultiplyVector(getTranslationalCorrection(), holdPointTranslationalScaling), MathFunctions.scalarMultiplyVector(getHeadingVector(), holdPointHeadingScaling), new Vector(), poseUpdater.getPose().getHeading());

limitDrivePowers();

for (int i = 0; i < motors.size(); i++) {
motors.get(i).setPower(drivePowers[i]);
}
} else {
if (isBusy) {
closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), BEZIER_CURVE_BINARY_STEP_LIMIT);

if (followingPathChain) updateCallbacks();

drivePowers = driveVectorScaler.getDrivePowers(getCorrectiveVector(), getHeadingVector(), getDriveVector(), poseUpdater.getPose().getHeading());
drivePowers = driveVectorScaler.getDrivePowers(MathFunctions.scalarMultiplyVector(getTranslationalCorrection(), holdPointTranslationalScaling), MathFunctions.scalarMultiplyVector(getHeadingVector(), holdPointHeadingScaling), new Vector(), poseUpdater.getPose().getHeading());

limitDrivePowers();

for (int i = 0; i < motors.size(); i++) {
motors.get(i).setPower(drivePowers[i]);
}
}
if (currentPath.isAtParametricEnd()) {
if (followingPathChain && chainIndex < currentPathChain.size() - 1) {
// Not at last path, keep going
breakFollowing();
pathStartTimes[chainIndex] = System.currentTimeMillis();
isBusy = true;
followingPathChain = true;
chainIndex++;
currentPath = currentPathChain.getPath(chainIndex);
} else {
if (isBusy) {
closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), BEZIER_CURVE_BINARY_STEP_LIMIT);
} else {
// At last path, run some end detection stuff
// set isBusy to false if at end
if (!reachedParametricPathEnd) {
reachedParametricPathEnd = true;
reachedParametricPathEndTime = System.currentTimeMillis();

if (followingPathChain) updateCallbacks();

drivePowers = driveVectorScaler.getDrivePowers(getCorrectiveVector(), getHeadingVector(), getDriveVector(), poseUpdater.getPose().getHeading());

limitDrivePowers();

for (int i = 0; i < motors.size(); i++) {
motors.get(i).setPower(drivePowers[i]);
}
}
if (currentPath.isAtParametricEnd()) {
if (followingPathChain && chainIndex < currentPathChain.size() - 1) {
// Not at last path, keep going
breakFollowing();
pathStartTimes[chainIndex] = System.currentTimeMillis();
isBusy = true;
followingPathChain = true;
chainIndex++;
currentPath = currentPathChain.getPath(chainIndex);
closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), BEZIER_CURVE_BINARY_STEP_LIMIT);
} else {
// At last path, run some end detection stuff
// set isBusy to false if at end
if (!reachedParametricPathEnd) {
reachedParametricPathEnd = true;
reachedParametricPathEndTime = System.currentTimeMillis();
}

if ((System.currentTimeMillis() - reachedParametricPathEndTime > currentPath.getPathEndTimeoutConstraint()) || (poseUpdater.getVelocity().getMagnitude() < currentPath.getPathEndVelocityConstraint() && MathFunctions.distance(poseUpdater.getPose(), closestPose) < currentPath.getPathEndTranslationalConstraint() && MathFunctions.getSmallestAngleDifference(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()) < currentPath.getPathEndHeadingConstraint())) {
if (holdPositionAtEnd) {
holdPositionAtEnd = false;
holdPoint(new BezierPoint(currentPath.getLastControlPoint()), currentPath.getHeadingGoal(1));
} else {
breakFollowing();
if ((System.currentTimeMillis() - reachedParametricPathEndTime > currentPath.getPathEndTimeoutConstraint()) || (poseUpdater.getVelocity().getMagnitude() < currentPath.getPathEndVelocityConstraint() && MathFunctions.distance(poseUpdater.getPose(), closestPose) < currentPath.getPathEndTranslationalConstraint() && MathFunctions.getSmallestAngleDifference(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()) < currentPath.getPathEndHeadingConstraint())) {
if (holdPositionAtEnd) {
holdPositionAtEnd = false;
holdPoint(new BezierPoint(currentPath.getLastControlPoint()), currentPath.getHeadingGoal(1));
} else {
breakFollowing();
}
}
}
}
Expand Down Expand Up @@ -851,7 +853,7 @@ public Vector getCentripetalForceCorrection() {
curvature = (yDoublePrime) / (Math.pow(Math.sqrt(1 + Math.pow(yPrime, 2)), 3));
}
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;
}

Expand Down

0 comments on commit 87f4240

Please sign in to comment.