diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AsherPathV1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AsherPathV1.java index d797ab9..ba648ec 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AsherPathV1.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AsherPathV1.java @@ -26,7 +26,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; * @version 1.0, 3/12/2024 */ @Config -@Autonomous(name = "AutoExampleThree", group = "Autonomous Pathing Tuning") +@Autonomous(name = "AsherPathV1", group = "Autonomous Pathing Tuning") public class AsherPathV1 extends OpMode { private Telemetry telemetryA; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoExample.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoExample.java index b9108c5..7383dc9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoExample.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoExample.java @@ -33,7 +33,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; * @version 1.0, 3/12/2024 */ @Config -@Autonomous(name = "AutoExample", group = "Autonomous Pathing Tuning") +@Autonomous(name = "AutoExample - Straight Path", group = "Autonomous Pathing Tuning") public class AutoExample extends OpMode { private Telemetry telemetryA; @@ -41,7 +41,7 @@ public class AutoExample extends OpMode { private PathChain path; - private final Pose startPose = new Pose(12.0, 11, 90); + private final Pose startPose = new Pose(0.0, 20.0, 0); /** * This initializes the Follower and creates the PathChain for the "circle". Additionally, this @@ -59,46 +59,11 @@ public class AutoExample extends OpMode { .addPath( // Line 1 new BezierLine( - new Point(12.804, 11.223, Point.CARTESIAN), - new Point(12.804, 42.362, Point.CARTESIAN) + new Point(0.000, 20.000, Point.CARTESIAN), + new Point(50.000, 20.000, Point.CARTESIAN) ) ) - .setTangentHeadingInterpolation() - .addPath( - // Line 2 - new BezierCurve( - new Point(12.804, 42.362, Point.CARTESIAN), - new Point(11.381, 57.379, Point.CARTESIAN), - new Point(31.614, 56.588, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 3 - new BezierLine( - new Point(31.614, 56.588, Point.CARTESIAN), - new Point(51.214, 56.746, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 4 - new BezierCurve( - new Point(51.214, 56.746, Point.CARTESIAN), - new Point(64.334, 58.643, Point.CARTESIAN), - new Point(61.172, 45.524, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 5 - new BezierCurve( - new Point(61.172, 45.524, Point.CARTESIAN), - new Point(36.198, 26.239, Point.CARTESIAN), - new Point(19.759, 11.065, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() + .setConstantHeadingInterpolation(Math.toRadians(0)) .build(); follower.followPath(path); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoExampleFour.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoExampleFour.java new file mode 100644 index 0000000..0f13231 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoExampleFour.java @@ -0,0 +1,106 @@ +package org.firstinspires.ftc.teamcode; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; + +/** + * This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite + * a circle, but some Bezier curves that have control points set essentially in a square. However, + * it turns enough to tune your centripetal force correction and some of your heading. Some lag in + * heading is to be expected. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/12/2024 + */ +@Config +@Autonomous(name = "AutoExample - 2 Curves/2 Lines", group = "Autonomous Pathing Tuning") +public class AutoExampleFour extends OpMode { + private Telemetry telemetryA; + + private Follower follower; + + private PathChain path; + + private final Pose startPose = new Pose(12,60, 0); + + /** + * This initializes the Follower and creates the PathChain for the "circle". Additionally, this + * initializes the FTC Dashboard telemetry. + */ + @Override + public void init() { + follower = new Follower(hardwareMap); + + follower.setMaxPower(.45); + + follower.setStartingPose(startPose); + + path = follower.pathBuilder() + .addPath( + // Line 1 + new BezierCurve( + new Point(12.000, 60.000, Point.CARTESIAN), + new Point(60.000, 60.000, Point.CARTESIAN), + new Point(60.000, 12.000, Point.CARTESIAN) + ) + ) + .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(-90)) + .addPath( + // Line 2 + new BezierLine( + new Point(60.000, 12.000, Point.CARTESIAN), + new Point(40.000, 12.000, Point.CARTESIAN) + ) + ) + .setLinearHeadingInterpolation(Math.toRadians(-90), Math.toRadians(-90)) + .addPath( + // Line 3 + new BezierCurve( + new Point(40.000, 12.000, Point.CARTESIAN), + new Point(35.000, 35.000, Point.CARTESIAN), + new Point(12.000, 35.000, Point.CARTESIAN) + ) + ) + .setLinearHeadingInterpolation(Math.toRadians(-90), Math.toRadians(-90)) + .addPath( + // Line 4 + new BezierLine( + new Point(12.000, 35.000, Point.CARTESIAN), + new Point(12.000, 60.000, Point.CARTESIAN) + ) + ) + .setLinearHeadingInterpolation(Math.toRadians(-90), Math.toRadians(0)) + .build(); + + follower.followPath(path); + + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetryA.update(); + } + + /** + * This runs the OpMode, updating the Follower as well as printing out the debug statements to + * the Telemetry, as well as the FTC Dashboard. + */ + @Override + public void loop() { + follower.update(); + if (follower.atParametricEnd()) { + follower.followPath(path); + } + follower.telemetryDebug(telemetryA); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoExampleThree.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoExampleThree.java index bbebecf..3089353 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoExampleThree.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoExampleThree.java @@ -26,7 +26,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; * @version 1.0, 3/12/2024 */ @Config -@Autonomous(name = "AutoExampleThree", group = "Autonomous Pathing Tuning") +@Autonomous(name = "AutoExample - Curve and Line", group = "Autonomous Pathing Tuning") public class AutoExampleThree extends OpMode { private Telemetry telemetryA; @@ -34,7 +34,7 @@ public class AutoExampleThree extends OpMode { private PathChain path; - private final Pose startPose = new Pose(7.467869222096955, 59.74295377677565); + private final Pose startPose = new Pose(10,45, 0); /** * This initializes the Follower and creates the PathChain for the "circle". Additionally, this @@ -52,21 +52,21 @@ public class AutoExampleThree extends OpMode { .addPath( // Line 1 new BezierCurve( - new Point(7.468, 59.743, Point.CARTESIAN), - new Point(67.860, 12.014, Point.CARTESIAN), - new Point(101.790, 36.041, Point.CARTESIAN), - new Point(68.347, 22.891, Point.CARTESIAN) + new Point(10.000, 45.000, Point.CARTESIAN), + new Point(45.000, 45.000, Point.CARTESIAN), + new Point(50.000, 20.000, Point.CARTESIAN) ) ) - .setTangentHeadingInterpolation() + .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(-90)) .addPath( // Line 2 new BezierLine( - new Point(68.347, 22.891, Point.CARTESIAN), - new Point(13.637, 24.352, Point.CARTESIAN) + new Point(50.000, 20.000, Point.CARTESIAN), + new Point(10.000, 20.000, Point.CARTESIAN) ) ) - .setTangentHeadingInterpolation().build(); + .setLinearHeadingInterpolation(Math.toRadians(-90), Math.toRadians(-90)) + .build(); follower.followPath(path); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoExampleTwo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoExampleTwo.java index b905cd3..86bbf69 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoExampleTwo.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoExampleTwo.java @@ -26,7 +26,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; * @version 1.0, 3/12/2024 */ @Config -@Autonomous(name = "AutoExampleTwo", group = "Autonomous Pathing Tuning") +@Autonomous(name = "AutoExample - Simple Curve", group = "Autonomous Pathing Tuning") public class AutoExampleTwo extends OpMode { private Telemetry telemetryA; @@ -34,7 +34,7 @@ public class AutoExampleTwo extends OpMode { private PathChain path; - private final Pose startPose = new Pose(10.0, 40, 90); + private final Pose startPose = new Pose(10.0, 45, 0); /** * This initializes the Follower and creates the PathChain for the "circle". Additionally, this @@ -51,28 +51,14 @@ public class AutoExampleTwo extends OpMode { path = follower.pathBuilder() .addPath( // Line 1 - new BezierLine( - new Point(10.000, 40.000, Point.CARTESIAN), - new Point(60.000, 40.000, Point.CARTESIAN) + new BezierCurve( + new Point(10.000, 45.000, Point.CARTESIAN), + new Point(45.000, 45.000, Point.CARTESIAN), + new Point(50.000, 20.000, Point.CARTESIAN) ) ) - .setConstantHeadingInterpolation(Math.toRadians(90)) - .addPath( - // Line 2 - new BezierLine( - new Point(60.000, 40.000, Point.CARTESIAN), - new Point(60.000, 25.000, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(90)) - .addPath( - // Line 3 - new BezierLine( - new Point(60.000, 25.000, Point.CARTESIAN), - new Point(10.000, 25.000, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(90)).build(); + .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(-90)) + .build(); follower.followPath(path); 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 c7fa481..5abe96a 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 @@ -71,8 +71,9 @@ measurements will be in centimeters. of how fast your robot will coast to a stop. Honestly, this is up to you. I personally used 4, but what works best for you is most important. Higher numbers will cause a faster brake, but increase oscillations at the end. Lower numbers will do the opposite. This can be found on line `107` in - `FollowerConstants`, named `zeroPowerAccelerationMultiplier`. The drive PID is much, much more -* sensitive than the others. For reference, + `FollowerConstants`, named `zeroPowerAccelerationMultiplier`. + +* The drive PID is much, much more sensitive than the others. For reference, my P values were in the hundredths and thousandths place values, and my D values were in the hundred thousandths and millionths place values. To tune this, enable `useDrive`, `useHeading`, and `useTranslational` in the `Follower` dropdown in FTC Dashboard. Next, run `StraightBackAndForth` 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 8f93ac1..1563b9d 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 @@ -81,7 +81,7 @@ public class FollowerConstants { // Kalman filter parameters for the drive error Kalman filter public static KalmanFilterParameters driveKalmanFilterParameters = new KalmanFilterParameters( 6, - 1); + 3); // Mass of robot in kilograms