From 1056fe0fc66b16aaf17d0ec778dd6244056e0e4a Mon Sep 17 00:00:00 2001 From: Marley Reeves Date: Sun, 17 Nov 2024 13:36:33 +1000 Subject: [PATCH 1/2] Rotate robot diagrams in localizer files to reduce confusion --- .../pedroPathing/follower/Follower.java | 46 ++++++++++--- .../pedroPathing/localization/Pose.java | 10 ++- .../localizers/OTOSLocalizer.java | 31 +++++---- .../localizers/PinpointLocalizer.java | 30 ++++---- .../localizers/ThreeWheelIMULocalizer.java | 26 +++---- .../localizers/ThreeWheelLocalizer.java | 24 +++---- .../localizers/TwoWheelLocalizer.java | 24 +++---- .../pathGeneration/MathFunctions.java | 52 +++++++++----- .../pathGeneration/PathBuilder.java | 68 ++++++++++++++----- .../pedroPathing/pathGeneration/Point.java | 60 ++++++++++------ .../tuning/FollowerConstants.java | 8 +-- .../tuning/ForwardVelocityTuner.java | 6 +- .../ForwardZeroPowerAccelerationTuner.java | 8 ++- .../LateralZeroPowerAccelerationTuner.java | 10 ++- .../tuning/StrafeVelocityTuner.java | 8 ++- .../teamcode/pedroPathing/util/Drawing.java | 10 +-- 16 files changed, 271 insertions(+), 150 deletions(-) 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 fe9d153..5089eb4 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 @@ -1,19 +1,19 @@ package org.firstinspires.ftc.teamcode.pedroPathing.follower; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.drivePIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.drivePIDFSwitch; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.forwardZeroPowerAcceleration; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.headingPIDFSwitch; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.drivePIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.headingPIDFFeedForward; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.headingPIDFSwitch; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.lateralZeroPowerAcceleration; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorName; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.translationalPIDFFeedForward; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.lateralZeroPowerAcceleration; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.secondaryDrivePIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.secondaryHeadingPIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.secondaryTranslationalPIDFFeedForward; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.translationalPIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.translationalPIDFSwitch; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.useSecondaryDrivePID; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.useSecondaryHeadingPID; @@ -365,6 +365,25 @@ public class Follower { closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), 1); } + /** + * This holds a Point. + * + * @param point the Point to stay at. + * @param heading the heading to face. + */ + public void holdPoint(Point point, double heading) { + holdPoint(new BezierPoint(point), heading); + } + + /** + * This holds a Point. + * + * @param pose the Point (as a Pose) to stay at. + */ + public void holdPoint(Pose pose) { + holdPoint(new Point(pose), pose.getHeading()); + } + /** * This follows a Path. * This also makes the Follower hold the last Point on the Path. @@ -426,15 +445,22 @@ public class Follower { } /** - * This calls an update to the PoseUpdater, which updates the robot's current position estimate. - * This also updates all the Follower's PIDFs, which updates the motor powers. + * Calls an update to the PoseUpdater, which updates the robot's current position estimate. */ - public void update() { + public void updatePose() { poseUpdater.update(); if (drawOnDashboard) { dashboardPoseTracker.update(); } + } + + /** + * This calls an update to the PoseUpdater, which updates the robot's current position estimate. + * This also updates all the Follower's PIDFs, which updates the motor powers. + */ + public void update() { + updatePose(); if (!teleopDrive) { if (currentPath != null) { @@ -515,7 +541,7 @@ public class Follower { * movement, this is the x-axis. * @param lateralDrive determines the lateral drive vector for the robot in teleop. In field centric * movement, this is the y-axis. - * @param heading determines the heading vector for the robot in teleop. + * @param heading determines the heading vector for the robot in teleop. */ public void setTeleOpMovementVectors(double forwardDrive, double lateralDrive, double heading) { setTeleOpMovementVectors(forwardDrive, lateralDrive, heading, true); @@ -528,7 +554,7 @@ public class Follower { * movement, this is the x-axis. * @param lateralDrive determines the lateral drive vector for the robot in teleop. In field centric * movement, this is the y-axis. - * @param heading determines the heading vector for the robot in teleop. + * @param heading determines the heading vector for the robot in teleop. * @param robotCentric sets if the movement will be field or robot centric */ public void setTeleOpMovementVectors(double forwardDrive, double lateralDrive, double heading, boolean robotCentric) { @@ -719,7 +745,7 @@ public class Follower { Vector velocityErrorVector = MathFunctions.addVectors(forwardVelocityError, lateralVelocityError); previousRawDriveError = rawDriveError; - rawDriveError = velocityErrorVector.getMagnitude() * MathFunctions.getSign(MathFunctions.dotProduct(velocityErrorVector, currentPath.getClosestPointTangentVector())); + rawDriveError = velocityErrorVector.getMagnitude() * MathFunctions.getSign(MathFunctions.dotProduct(velocityErrorVector, currentPath.getClosestPointTangentVector())); double projection = 2 * driveErrors[1] - driveErrors[0]; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java index d5ac720..d784c09 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode.pedroPathing.localization; +import androidx.annotation.NonNull; + import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; @@ -44,7 +46,7 @@ public class Pose { * This creates a new Pose with no inputs and 0 for all values. */ public Pose() { - this(0,0,0); + this(0, 0, 0); } /** @@ -208,4 +210,10 @@ public class Pose { public Pose copy() { return new Pose(getX(), getY(), getHeading()); } + + @NonNull + @Override + public String toString() { + return "(" + getX() + ", " + getY() + ", " + Math.toDegrees(getHeading()) + ")"; + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java index e4e1f34..a700e3b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java @@ -21,18 +21,18 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; * * forward on robot is the x positive direction * - * /--------------\ - * | ____ | - * | ---- | - * | || || | - * | || || | ----> left (y positive) - * | | - * | | - * \--------------/ - * | - * | - * V - * forward (x positive) + * forward (x positive) + * △ + * | + * | + * /--------------\ + * | | + * | | + * | || || | + * left (y positive) <--- | || || | + * | ____ | + * | ---- | + * \--------------/ * * @author Anyi Lin - 10158 Scott's Bots * @version 1.0, 7/20/2024 @@ -108,7 +108,10 @@ public class OTOSLocalizer extends Localizer { */ @Override public Pose getPose() { - return MathFunctions.addPoses(startPose, new Pose(otosPose.x, otosPose.y, otosPose.h)); + SparkFunOTOS.Pose2D rawPose = otos.getPosition(); + Pose pose = new Pose(rawPose.x, rawPose.y, rawPose.h); + + return MathFunctions.addPoses(startPose, MathFunctions.rotatePose(pose, startPose.getHeading(), false)); } /** @@ -219,4 +222,4 @@ public class OTOSLocalizer extends Localizer { */ public void resetIMU() { } -} \ No newline at end of file +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java index 151311b..232df9a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java @@ -23,18 +23,18 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; * * forward on robot is the x positive direction * - * /--------------\ - * | ____ | - * | ---- | - * | || | - * | || | ----> left (y positive) - * | | - * | | - * \--------------/ - * | - * | - * V - * forward (x positive) + * forward (x positive) + * △ + * | + * | + * /--------------\ + * | | + * | | + * | || | + * left (y positive) <--- | || | + * | ____ | + * | ---- | + * \--------------/ * With the pinpoint your readings will be used in mm * to use inches ensure to divide your mm value by 25.4 * @author Logan Nash @@ -96,8 +96,10 @@ public class PinpointLocalizer extends Localizer { */ @Override public Pose getPose() { - Pose2D pose = odo.getPosition(); - return new Pose(pose.getX(DistanceUnit.INCH), pose.getY(DistanceUnit.INCH), pose.getHeading(AngleUnit.RADIANS)); + Pose2D rawPose = odo.getPosition(); + Pose pose = new Pose(rawPose.getX(DistanceUnit.INCH), rawPose.getY(DistanceUnit.INCH), rawPose.getHeading(AngleUnit.RADIANS)); + + return MathFunctions.addPoses(startPose, MathFunctions.rotatePose(pose, startPose.getHeading(), false)); } /** diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java index 1e9fd7b..bd3c598 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java @@ -26,18 +26,18 @@ import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; * * forward on robot is the x positive direction * - * /--------------\ - * | ____ | - * | ---- | - * | || || | - * | || || | ----> left (y positive) - * | | - * | | - * \--------------/ - * | - * | - * V - * forward (x positive) + * forward (x positive) + * △ + * | + * | + * /--------------\ + * | | + * | | + * | || || | + * left (y positive) <--- | || || | + * | ____ | + * | ---- | + * \--------------/ * * @author Logan Nash * @author Anyi Lin - 10158 Scott's Bots @@ -313,4 +313,4 @@ public class ThreeWheelIMULocalizer extends Localizer { public void resetIMU() { imu.resetYaw(); } -} \ No newline at end of file +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java index 6814b86..4368dc0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java @@ -23,18 +23,18 @@ import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; * * forward on robot is the x positive direction * - * /--------------\ - * | ____ | - * | ---- | - * | || || | - * | || || | ----> left (y positive) - * | | - * | | - * \--------------/ - * | - * | - * V - * forward (x positive) + * forward (x positive) + * △ + * | + * | + * /--------------\ + * | | + * | | + * | || || | + * left (y positive) <--- | || || | + * | ____ | + * | ---- | + * \--------------/ * * @author Anyi Lin - 10158 Scott's Bots * @version 1.0, 4/2/2024 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java index 62415d6..50d7ec2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java @@ -26,18 +26,18 @@ import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; * * forward on robot is the x positive direction * - * /--------------\ - * | ____ | - * | ---- | - * | || || | - * | || || | ----> left (y positive) - * | | - * | | - * \--------------/ - * | - * | - * V - * forward (x positive) +* forward (x positive) + * △ + * | + * | + * /--------------\ + * | | + * | | + * | || | + * left (y positive) <--- | || | + * | ____ | + * | ---- | + * \--------------/ * * @author Anyi Lin - 10158 Scott's Bots * @version 1.0, 4/2/2024 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java index 0edfbfd..bd4e393 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java @@ -27,16 +27,16 @@ public class MathFunctions { double denom = 1; // this multiplies up the numerator of the nCr function - for (int i = n; i > n-r; i--) { + for (int i = n; i > n - r; i--) { num *= i; } // this multiplies up the denominator of the nCr function - for (int i = 1; i <=r; i++) { + for (int i = 1; i <= r; i++) { denom *= i; } - return num/denom; + return num / denom; } /** @@ -67,7 +67,7 @@ public class MathFunctions { /** * This normalizes an angle to be between 0 and 2 pi radians, inclusive. - * + *

* IMPORTANT NOTE: This method operates in radians. * * @param angleRadians the angle to be normalized. @@ -75,8 +75,8 @@ public class MathFunctions { */ public static double normalizeAngle(double angleRadians) { double angle = angleRadians; - while (angle<0) angle += 2*Math.PI; - while (angle>2*Math.PI) angle -= 2*Math.PI; + while (angle < 0) angle += 2 * Math.PI; + while (angle > 2 * Math.PI) angle -= 2 * Math.PI; return angle; } @@ -88,7 +88,7 @@ public class MathFunctions { * @return returns the smallest angle. */ public static double getSmallestAngleDifference(double one, double two) { - return Math.min(MathFunctions.normalizeAngle(one-two), MathFunctions.normalizeAngle(two-one)); + return Math.min(MathFunctions.normalizeAngle(one - two), MathFunctions.normalizeAngle(two - one)); } /** @@ -98,7 +98,7 @@ public class MathFunctions { * @return returns the turn direction. */ public static double getTurnDirection(double startHeading, double endHeading) { - if (MathFunctions.normalizeAngle(endHeading-startHeading) >= 0 && MathFunctions.normalizeAngle(endHeading-startHeading) <= Math.PI) { + if (MathFunctions.normalizeAngle(endHeading - startHeading) >= 0 && MathFunctions.normalizeAngle(endHeading - startHeading) <= Math.PI) { return 1; // counter clock wise } return -1; // clock wise @@ -112,7 +112,7 @@ public class MathFunctions { * @return returns the distance between the two. */ public static double distance(Pose pose, Point point) { - return Math.sqrt(Math.pow(pose.getX()-point.getX(), 2) + Math.pow(pose.getY()-point.getY(), 2)); + return Math.sqrt(Math.pow(pose.getX() - point.getX(), 2) + Math.pow(pose.getY() - point.getY(), 2)); } /** @@ -123,7 +123,7 @@ public class MathFunctions { * @return returns the distance between the two. */ public static double distance(Pose one, Pose two) { - return Math.sqrt(Math.pow(one.getX()-two.getX(), 2) + Math.pow(one.getY()-two.getY(), 2)); + return Math.sqrt(Math.pow(one.getX() - two.getX(), 2) + Math.pow(one.getY() - two.getY(), 2)); } /** @@ -172,6 +172,22 @@ public class MathFunctions { return new Pose(one.getX() - two.getX(), one.getY() - two.getY(), one.getHeading() - two.getHeading()); } + /** + * This rotates the given pose by the given theta, + * + * @param pose the Pose to rotate. + * @param theta the angle to rotate by. + * @param rotateHeading whether to adjust the Pose heading too. + * @return the rotated Pose. + */ + public static Pose rotatePose(Pose pose, double theta, boolean rotateHeading) { + double x = pose.getX() * Math.cos(theta) - pose.getY() * Math.sin(theta); + double y = pose.getX() * Math.sin(theta) + pose.getY() * Math.cos(theta); + double heading = rotateHeading ? normalizeAngle(pose.getHeading() + theta) : pose.getHeading(); + + return new Pose(x, y, heading); + } + /** * This multiplies a Point by a scalar and returns the result as a Point * @@ -180,7 +196,7 @@ public class MathFunctions { * @return returns the scaled Point. */ public static Point scalarMultiplyPoint(Point point, double scalar) { - return new Point(point.getX()*scalar, point.getY()*scalar, Point.CARTESIAN); + return new Point(point.getX() * scalar, point.getY() * scalar, Point.CARTESIAN); } /** @@ -213,7 +229,7 @@ public class MathFunctions { * @return returns the scaled Vector. */ public static Vector scalarMultiplyVector(Vector vector, double scalar) { - return new Vector(vector.getMagnitude()*scalar, vector.getTheta()); + return new Vector(vector.getMagnitude() * scalar, vector.getTheta()); } /** @@ -227,7 +243,7 @@ public class MathFunctions { if (vector.getMagnitude() == 0) { return new Vector(0.0, vector.getTheta()); } else { - return new Vector(vector.getMagnitude()/Math.abs(vector.getMagnitude()), vector.getTheta()); + return new Vector(vector.getMagnitude() / Math.abs(vector.getMagnitude()), vector.getTheta()); } } @@ -240,7 +256,7 @@ public class MathFunctions { */ public static Vector addVectors(Vector one, Vector two) { Vector returnVector = new Vector(); - returnVector.setOrthogonalComponents(one.getXComponent()+two.getXComponent(), one.getYComponent()+two.getYComponent()); + returnVector.setOrthogonalComponents(one.getXComponent() + two.getXComponent(), one.getYComponent() + two.getYComponent()); return returnVector; } @@ -254,7 +270,7 @@ public class MathFunctions { */ public static Vector subtractVectors(Vector one, Vector two) { Vector returnVector = new Vector(); - returnVector.setOrthogonalComponents(one.getXComponent()-two.getXComponent(), one.getYComponent()-two.getYComponent()); + returnVector.setOrthogonalComponents(one.getXComponent() - two.getXComponent(), one.getYComponent() - two.getYComponent()); return returnVector; } @@ -266,7 +282,7 @@ public class MathFunctions { * @return returns the dot product of the two Vectors. */ public static double dotProduct(Vector one, Vector two) { - return one.getXComponent()*two.getXComponent() + one.getYComponent()*two.getYComponent(); + return one.getXComponent() * two.getXComponent() + one.getYComponent() * two.getYComponent(); } /** @@ -278,7 +294,7 @@ public class MathFunctions { * @return returns the cross product of the two Vectors. */ public static double crossProduct(Vector one, Vector two) { - return one.getXComponent()*two.getYComponent() - one.getYComponent()*two.getXComponent(); + return one.getXComponent() * two.getYComponent() - one.getYComponent() * two.getXComponent(); } /** @@ -286,7 +302,7 @@ public class MathFunctions { * specified accuracy amount. * * @param one first number specified. - * @param two second number specified. + * @param two Second number specified. * @param accuracy the level of accuracy specified. * @return returns if the two numbers are within the specified accuracy of each other. */ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathBuilder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathBuilder.java index 5bfff7d..f044ce7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathBuilder.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathBuilder.java @@ -51,16 +51,47 @@ public class PathBuilder { return this; } + /** + * This adds a default Path defined by a specified BezierCurve to the PathBuilder. + * + * @param controlPoints This is the specified control points that define the BezierCurve. + * @return This returns itself with the updated data. + */ + public PathBuilder addBezierCurve(Point... controlPoints) { + return addPath(new BezierCurve(controlPoints)); + } + + /** + * This adds a default Path defined by a specified BezierCurve to the PathBuilder. + * + * @param controlPoints This is the specified control points that define the BezierCurve. + * @return This returns itself with the updated data. + */ + public PathBuilder addBezierCurve(ArrayList controlPoints) { + return addPath(new BezierCurve(controlPoints)); + } + + /** + * This adds a default Path defined by a specified BezierLine to the PathBuilder. + * + * @param startPoint start point of the line. + * @param endPoint end point of the line. + * @return This returns itself with the updated data. + */ + public PathBuilder addBezierLine(Point startPoint, Point endPoint) { + return addPath(new BezierLine(startPoint, endPoint)); + } + /** * This sets a linear heading interpolation on the last Path added to the PathBuilder. * * @param startHeading The start of the linear heading interpolation. * @param endHeading The end of the linear heading interpolation. - * This will be reached at the end of the Path if no end time is specified. + * This will be reached at the end of the Path if no end time is specified. * @return This returns itself with the updated data. */ public PathBuilder setLinearHeadingInterpolation(double startHeading, double endHeading) { - this.paths.get(paths.size()-1).setLinearHeadingInterpolation(startHeading, endHeading); + this.paths.get(paths.size() - 1).setLinearHeadingInterpolation(startHeading, endHeading); return this; } @@ -69,13 +100,13 @@ public class PathBuilder { * * @param startHeading The start of the linear heading interpolation. * @param endHeading The end of the linear heading interpolation. - * This will be reached at the end of the Path if no end time is specified. + * This will be reached at the end of the Path if no end time is specified. * @param endTime The end time on the Path that the linear heading interpolation will end. - * This value goes from [0, 1] since Bezier curves are parametric functions. + * This value goes from [0, 1] since Bezier curves are parametric functions. * @return This returns itself with the updated data. */ public PathBuilder setLinearHeadingInterpolation(double startHeading, double endHeading, double endTime) { - this.paths.get(paths.size()-1).setLinearHeadingInterpolation(startHeading, endHeading, endTime); + this.paths.get(paths.size() - 1).setLinearHeadingInterpolation(startHeading, endHeading, endTime); return this; } @@ -86,7 +117,7 @@ public class PathBuilder { * @return This returns itself with the updated data. */ public PathBuilder setConstantHeadingInterpolation(double setHeading) { - this.paths.get(paths.size()-1).setConstantHeadingInterpolation(setHeading); + this.paths.get(paths.size() - 1).setConstantHeadingInterpolation(setHeading); return this; } @@ -94,20 +125,21 @@ public class PathBuilder { * This sets a reversed or tangent heading interpolation on the last Path added to the PathBuilder. * * @param set This sets the heading to reversed tangent following if this parameter is true. - * Conversely, this sets a tangent following if this parameter is false. + * Conversely, this sets a tangent following if this parameter is false. * @return This returns itself with the updated data. */ public PathBuilder setReversed(boolean set) { - this.paths.get(paths.size()-1).setReversed(set); + this.paths.get(paths.size() - 1).setReversed(set); return this; } + /** * This sets the heading interpolation to tangential on the last Path added to the PathBuilder. * There really shouldn't be a reason to use this since the default heading interpolation is * tangential but it's here. */ public PathBuilder setTangentHeadingInterpolation() { - this.paths.get(paths.size()-1).setTangentHeadingInterpolation(); + this.paths.get(paths.size() - 1).setTangentHeadingInterpolation(); return this; } @@ -118,7 +150,7 @@ public class PathBuilder { * @return This returns itself with the updated data. */ public PathBuilder setZeroPowerAccelerationMultiplier(double set) { - this.paths.get(paths.size()-1).setZeroPowerAccelerationMultiplier(set); + this.paths.get(paths.size() - 1).setZeroPowerAccelerationMultiplier(set); return this; } @@ -129,7 +161,7 @@ public class PathBuilder { * @return This returns itself with the updated data. */ public PathBuilder setPathEndVelocityConstraint(double set) { - this.paths.get(paths.size()-1).setPathEndVelocityConstraint(set); + this.paths.get(paths.size() - 1).setPathEndVelocityConstraint(set); return this; } @@ -140,7 +172,7 @@ public class PathBuilder { * @return This returns itself with the updated data. */ public PathBuilder setPathEndTranslationalConstraint(double set) { - this.paths.get(paths.size()-1).setPathEndTranslationalConstraint(set); + this.paths.get(paths.size() - 1).setPathEndTranslationalConstraint(set); return this; } @@ -151,7 +183,7 @@ public class PathBuilder { * @return This returns itself with the updated data. */ public PathBuilder setPathEndHeadingConstraint(double set) { - this.paths.get(paths.size()-1).setPathEndHeadingConstraint(set); + this.paths.get(paths.size() - 1).setPathEndHeadingConstraint(set); return this; } @@ -162,7 +194,7 @@ public class PathBuilder { * @return This returns itself with the updated data. */ public PathBuilder setPathEndTValueConstraint(double set) { - this.paths.get(paths.size()-1).setPathEndTValueConstraint(set); + this.paths.get(paths.size() - 1).setPathEndTValueConstraint(set); return this; } @@ -173,7 +205,7 @@ public class PathBuilder { * @return This returns itself with the updated data. */ public PathBuilder setPathEndTimeoutConstraint(double set) { - this.paths.get(paths.size()-1).setPathEndTimeoutConstraint(set); + this.paths.get(paths.size() - 1).setPathEndTimeoutConstraint(set); return this; } @@ -182,12 +214,12 @@ public class PathBuilder { * This callback is set to run at a specified number of milliseconds after the start of the path. * * @param time This sets the number of milliseconds of wait between the start of the Path and - * the calling of the callback. + * the calling of the callback. * @param runnable This sets the code for the callback to run. Use lambda statements for this. * @return This returns itself with the updated data. */ public PathBuilder addTemporalCallback(double time, Runnable runnable) { - this.callbacks.add(new PathCallback(time, runnable, PathCallback.TIME, paths.size()-1)); + this.callbacks.add(new PathCallback(time, runnable, PathCallback.TIME, paths.size() - 1)); return this; } @@ -200,7 +232,7 @@ public class PathBuilder { * @return This returns itself with the updated data. */ public PathBuilder addParametricCallback(double t, Runnable runnable) { - this.callbacks.add(new PathCallback(t, runnable, PathCallback.PARAMETRIC, paths.size()-1)); + this.callbacks.add(new PathCallback(t, runnable, PathCallback.PARAMETRIC, paths.size() - 1)); return this; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java index 0da8df8..95a5f05 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; +import androidx.annotation.NonNull; + import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; /** @@ -31,13 +33,13 @@ public class Point { * This creates a new Point with coordinate inputs and a specified coordinate system. * * @param rOrX Depending on the coordinate system specified, this is either the r or x value. - * In polar coordinates, the r value is the distance from the origin. - * In Cartesian coordinates, the x value is the distance left/right from the origin. + * In polar coordinates, the r value is the distance from the origin. + * In Cartesian coordinates, the x value is the distance left/right from the origin. * @param thetaOrY Depending on the coordinate system specified, this is either the theta or - * y value. - * In polar coordinates, the theta value is the angle from the positive x-axis. - * Increasing theta moves in the counter-clockwise direction. - * In Cartesian coordinates, the y value is the distance up/down from the origin. + * y value. + * In polar coordinates, the theta value is the angle from the positive x-axis. + * Increasing theta moves in the counter-clockwise direction. + * In Cartesian coordinates, the y value is the distance up/down from the origin. * @param identifier this specifies what coordinate system the coordinate inputs are in. */ public Point(double rOrX, double thetaOrY, int identifier) { @@ -53,17 +55,27 @@ public class Point { setCoordinates(pose.getX(), pose.getY(), CARTESIAN); } + /** + * This creates a new Point from a X and Y value. + * + * @param setX the X value. + * @param setY the Y value. + */ + public Point(double setX, double setY) { + setCoordinates(setX, setY, CARTESIAN); + } + /** * This sets the coordinates of the Point using the specified coordinate system. * * @param rOrX Depending on the coordinate system specified, this is either the r or x value. - * In polar coordinates, the r value is the distance from the origin. - * In Cartesian coordinates, the x value is the distance left/right from the origin. + * In polar coordinates, the r value is the distance from the origin. + * In Cartesian coordinates, the x value is the distance left/right from the origin. * @param thetaOrY Depending on the coordinate system specified, this is either the theta or - * y value. - * In polar coordinates, the theta value is the angle from the positive x-axis. - * Increasing theta moves in the counter-clockwise direction. - * In Cartesian coordinates, the y value is the distance up/down from the origin. + * y value. + * In polar coordinates, the theta value is the angle from the positive x-axis. + * Increasing theta moves in the counter-clockwise direction. + * In Cartesian coordinates, the y value is the distance up/down from the origin. * @param identifier this specifies what coordinate system to use when setting values. */ public void setCoordinates(double rOrX, double thetaOrY, int identifier) { @@ -78,9 +90,9 @@ public class Point { theta = setOtherCoordinates[1]; break; default: - if (rOrX<0) { + if (rOrX < 0) { r = -rOrX; - theta = MathFunctions.normalizeAngle(thetaOrY+Math.PI); + theta = MathFunctions.normalizeAngle(thetaOrY + Math.PI); } else { r = rOrX; theta = MathFunctions.normalizeAngle(thetaOrY); @@ -99,7 +111,7 @@ public class Point { * @return returns the distance between the two Points. */ public double distanceFrom(Point otherPoint) { - return Math.sqrt(Math.pow(otherPoint.getX()-x, 2) + Math.pow(otherPoint.getY()-y, 2)); + return Math.sqrt(Math.pow(otherPoint.getX() - x, 2) + Math.pow(otherPoint.getY() - y, 2)); } /** @@ -110,7 +122,7 @@ public class Point { * @return this returns the x and y values, in that order, in an Array of doubles. */ public static double[] polarToCartesian(double r, double theta) { - return new double[] {r * Math.cos(theta), r * Math.sin(theta)}; + return new double[]{r * Math.cos(theta), r * Math.sin(theta)}; } /** @@ -123,17 +135,17 @@ public class Point { public static double[] cartesianToPolar(double x, double y) { if (x == 0) { if (y > 0) { - return new double[] {Math.abs(y), Math.PI/2}; + return new double[]{Math.abs(y), Math.PI / 2}; } else { - return new double[] {Math.abs(y), (3 * Math.PI) / 2}; + return new double[]{Math.abs(y), (3 * Math.PI) / 2}; } } - double r = Math.sqrt(x*x+y*y); - if (x < 0) return new double[] {r, Math.PI+Math.atan(y/x)}; + double r = Math.sqrt(x * x + y * y); + if (x < 0) return new double[]{r, Math.PI + Math.atan(y / x)}; if (y > 0) { return new double[]{r, Math.atan(y / x)}; } else { - return new double[]{r, (2*Math.PI) + Math.atan(y / x)}; + return new double[]{r, (2 * Math.PI) + Math.atan(y / x)}; } } @@ -182,4 +194,10 @@ public class Point { public Point copy() { return new Point(getX(), getY(), CARTESIAN); } + + @NonNull + @Override + public String toString() { + return "(" + getX() + ", " + getY() + ")"; + } } \ No newline at end of file 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 af72609..bd932cd 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 @@ -2,11 +2,11 @@ package org.firstinspires.ftc.teamcode.pedroPathing.tuning; import com.acmerobotics.dashboard.config.Config; -import org.firstinspires.ftc.teamcode.pedroPathing.util.CustomFilteredPIDFCoefficients; -import org.firstinspires.ftc.teamcode.pedroPathing.util.CustomPIDFCoefficients; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; +import org.firstinspires.ftc.teamcode.pedroPathing.util.CustomFilteredPIDFCoefficients; +import org.firstinspires.ftc.teamcode.pedroPathing.util.CustomPIDFCoefficients; import org.firstinspires.ftc.teamcode.pedroPathing.util.KalmanFilterParameters; /** @@ -33,7 +33,7 @@ public class FollowerConstants { private static double xMovement = 81.34056; private static double yMovement = 65.43028; private static double[] convertToPolar = Point.cartesianToPolar(xMovement, -yMovement); - public static Vector frontLeftVector = MathFunctions.normalizeVector(new Vector(convertToPolar[0],convertToPolar[1])); + public static Vector frontLeftVector = MathFunctions.normalizeVector(new Vector(convertToPolar[0], convertToPolar[1])); // Translational PIDF coefficients (don't use integral) @@ -182,7 +182,7 @@ public class FollowerConstants { // the limit at which the heading PIDF switches between the main and secondary heading PIDFs - public static double headingPIDFSwitch = Math.PI/20; + public static double headingPIDFSwitch = Math.PI / 20; // Secondary heading error PIDF coefficients public static CustomPIDFCoefficients secondaryHeadingPIDFCoefficients = new CustomPIDFCoefficients( diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java index e97f78a..ad07d88 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java @@ -40,7 +40,7 @@ import java.util.List; * @version 1.0, 3/13/2024 */ @Config -@Autonomous (name = "Forward Velocity Tuner", group = "Autonomous Pathing Tuning") +@Autonomous(name = "Forward Velocity Tuner", group = "Autonomous Pathing Tuning") public class ForwardVelocityTuner extends OpMode { private ArrayList velocities = new ArrayList<>(); @@ -122,6 +122,10 @@ public class ForwardVelocityTuner extends OpMode { @Override public void loop() { if (gamepad1.cross || gamepad1.a) { + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + motor.setPower(0); + } requestOpModeStop(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardZeroPowerAccelerationTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardZeroPowerAccelerationTuner.java index 9454e13..f7393bd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardZeroPowerAccelerationTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardZeroPowerAccelerationTuner.java @@ -40,7 +40,7 @@ import java.util.List; * @version 1.0, 3/13/2024 */ @Config -@Autonomous (name = "Forward Zero Power Acceleration Tuner", group = "Autonomous Pathing Tuning") +@Autonomous(name = "Forward Zero Power Acceleration Tuner", group = "Autonomous Pathing Tuning") public class ForwardZeroPowerAccelerationTuner extends OpMode { private ArrayList accelerations = new ArrayList<>(); @@ -120,6 +120,10 @@ public class ForwardZeroPowerAccelerationTuner extends OpMode { @Override public void loop() { if (gamepad1.cross || gamepad1.a) { + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + motor.setPower(0); + } requestOpModeStop(); } @@ -149,7 +153,7 @@ public class ForwardZeroPowerAccelerationTuner extends OpMode { for (Double acceleration : accelerations) { average += acceleration; } - average /= (double)accelerations.size(); + average /= (double) accelerations.size(); telemetryA.addData("forward zero power acceleration (deceleration):", average); telemetryA.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java index 505245d..87b1978 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java @@ -40,7 +40,7 @@ import java.util.List; * @version 1.0, 3/13/2024 */ @Config -@Autonomous (name = "Lateral Zero Power Acceleration Tuner", group = "Autonomous Pathing Tuning") +@Autonomous(name = "Lateral Zero Power Acceleration Tuner", group = "Autonomous Pathing Tuning") public class LateralZeroPowerAccelerationTuner extends OpMode { private ArrayList accelerations = new ArrayList<>(); @@ -120,11 +120,15 @@ public class LateralZeroPowerAccelerationTuner extends OpMode { @Override public void loop() { if (gamepad1.cross || gamepad1.a) { + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + motor.setPower(0); + } requestOpModeStop(); } poseUpdater.update(); - Vector heading = new Vector(1.0, poseUpdater.getPose().getHeading() - Math.PI/2); + Vector heading = new Vector(1.0, poseUpdater.getPose().getHeading() - Math.PI / 2); if (!end) { if (!stopping) { if (MathFunctions.dotProduct(poseUpdater.getVelocity(), heading) > VELOCITY) { @@ -149,7 +153,7 @@ public class LateralZeroPowerAccelerationTuner extends OpMode { for (Double acceleration : accelerations) { average += acceleration; } - average /= (double)accelerations.size(); + average /= (double) accelerations.size(); telemetryA.addData("lateral zero power acceleration (deceleration):", average); telemetryA.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java index acd90c0..ad6d22b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java @@ -40,7 +40,7 @@ import java.util.List; * @version 1.0, 3/13/2024 */ @Config -@Autonomous (name = "Strafe Velocity Tuner", group = "Autonomous Pathing Tuning") +@Autonomous(name = "Strafe Velocity Tuner", group = "Autonomous Pathing Tuning") public class StrafeVelocityTuner extends OpMode { private ArrayList velocities = new ArrayList<>(); @@ -120,6 +120,10 @@ public class StrafeVelocityTuner extends OpMode { @Override public void loop() { if (gamepad1.cross || gamepad1.a) { + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + motor.setPower(0); + } requestOpModeStop(); } @@ -132,7 +136,7 @@ public class StrafeVelocityTuner extends OpMode { motor.setPower(0); } } else { - double currentVelocity = Math.abs(MathFunctions.dotProduct(poseUpdater.getVelocity(), new Vector(1, Math.PI/2))); + double currentVelocity = Math.abs(MathFunctions.dotProduct(poseUpdater.getVelocity(), new Vector(1, Math.PI / 2))); velocities.add(currentVelocity); velocities.remove(0); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java index 9a2d956..9af9662 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java @@ -20,11 +20,14 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; * @version 1.0, 4/22/2024 */ public class Drawing { + public static final double ROBOT_RADIUS = 9; + private static TelemetryPacket packet; /** * This draws everything that will be used in the Follower's telemetryDebug() method. This takes * a Follower as an input, so an instance of the DashbaordDrawingHandler class is not needed. + * * @param follower */ public static void drawDebug(Follower follower) { @@ -35,6 +38,7 @@ public class Drawing { } drawPoseHistory(follower.getDashboardPoseTracker(), "#4CAF50"); drawRobot(follower.getPose(), "#4CAF50"); + sendPacket(); } @@ -114,12 +118,10 @@ public class Drawing { * @param t the Point to draw at */ public static void drawRobotOnCanvas(Canvas c, Point t) { - final double ROBOT_RADIUS = 9; - c.setStrokeWidth(1); c.strokeCircle(t.getX(), t.getY(), ROBOT_RADIUS); - Vector halfv = new Vector(0.5*ROBOT_RADIUS, t.getTheta()); + Vector halfv = new Vector(0.5 * ROBOT_RADIUS, t.getTheta()); Vector p1 = MathFunctions.addVectors(halfv, new Vector(t.getR(), t.getTheta())); Vector p2 = MathFunctions.addVectors(p1, halfv); c.strokeLine(p1.getXComponent(), p1.getYComponent(), p2.getXComponent(), p2.getYComponent()); @@ -133,8 +135,6 @@ public class Drawing { * @param t the Pose to draw at */ public static void drawRobotOnCanvas(Canvas c, Pose t) { - final double ROBOT_RADIUS = 9; - c.strokeCircle(t.getX(), t.getY(), ROBOT_RADIUS); Vector v = t.getHeadingVector(); v.setMagnitude(v.getMagnitude() * ROBOT_RADIUS); From 775e7ffbc8ececc68755292c0a80a23e2d12395d Mon Sep 17 00:00:00 2001 From: Marley Reeves <79887575+Marlstar@users.noreply.github.com> Date: Sun, 17 Nov 2024 14:42:31 +1100 Subject: [PATCH 2/2] Clarify translational PIDF tuning process Some quirks in both the process of tuning the translational PIDF and the documentation of it led to confusion for myself and others. - Noted that the telemetry message that the robot will move forward and backward can be ignored for the moment - Noted that the robot should only move when pushed - Noted that the robot will only correct to a line in the original forward direction, not to the starting point --- .../ftc/teamcode/pedroPathing/TUNING.md | 21 ++++++++++++------- 1 file changed, 14 insertions(+), 7 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 bd22f46..e76196f 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 @@ -47,12 +47,19 @@ measurements will be in centimeters. `lateralZeroPowerAcceleration`, respectively. * After this, we will want to tune the translational PID. Go to FTC Dashboard and disable all but - the `useTranslational` checkboxes under the `Follower` tab. Then, run `StraightBackAndForth`. Make - sure you disable the timer on autonomous OpModes. The PID for the translational error is called - `translationalPIDF`. If you need to add a feedforward value, use the `translationalPIDFFeedForward` - since that will add the feedforward in the direction the robot is trying to move, rather than the - feedforward in the PIDF itself, since those will only add the feedforward one way. You can change - the PIDF constants and feedforward values, under the `FollowerConstants` tab in FTC Dashboard. + the `useTranslational` checkboxes under the `Follower` tab. Then, run `StraightBackAndForth`. + Make sure you disable the timer on autonomous OpModes. You will notice in telemetry a message saying + that the robot will travel a distance forward and backward, this will not happen until later, so for + now you can ignore this message. The robot should not move when you run the opmode initally. Instead, + it should correct when you push it away from its starting position. Note that this correction should + happen regardless of the robot's rotation, and that the robot should not rotate itself (if it does, + disable `useHeading` as mentioned prior). Also note that the robot will only correct to an imaginary line + that runs straight forward from the robot's starting position, meaning that it will not correct in the + (original) forward direction. The PID for the translational error is called `translationalPIDF`. + If you need to add a feedforward value, use the `translationalPIDFFeedForward` since that will add + the feedforward in the direction the robot is trying to move, rather than the feedforward in the + PIDF itself, since those will only add the feedforward one way. You can change the PIDF constants + and feedforward values, under the `FollowerConstants` tab in FTC Dashboard. To tune the PID, push the robot off the path and see how corrects. You will want to alternate sides you push to reduce field wear and tear as well as push with varying power and distance. I would recommend tuning the PID so that it is capable of correcting while minimizing oscillations and still @@ -116,4 +123,4 @@ down and all the values pertaining to the secondary PIDs will be there. The two a PID that handles larger errors (the main PID) and a second PID to handle smaller errors (the secondary PID). The main PID should be tuned to move the error within the secondary PID's range without providing too much momentum that could cause an overshoot. The secondary PID should be tuned -to correct within its range quickly and accurately while minimizing oscillations. \ No newline at end of file +to correct within its range quickly and accurately while minimizing oscillations.