From 43c505e292146a759f0a15495c3ca7cc5e944d7f Mon Sep 17 00:00:00 2001 From: Carlos Rivas Date: Tue, 22 Oct 2024 16:26:33 -0700 Subject: [PATCH 1/3] Updated values back to when they worked --- .../ftc/teamcode/pedroPathing/tuning/FollowerConstants.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 766d629..7d01fca 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 @@ -58,10 +58,10 @@ public class FollowerConstants { // Heading error PIDF coefficients public static CustomPIDFCoefficients headingPIDFCoefficients = new CustomPIDFCoefficients( - 2, + 1, 0, - .075, - -.03125); + .1, + 0); // Feed forward constant added on to the heading PIDF public static double headingPIDFFeedForward = 0.01; From 945a77ca495da10a9288d0436339acbc7312fb29 Mon Sep 17 00:00:00 2001 From: robotics1 Date: Tue, 22 Oct 2024 17:28:55 -0700 Subject: [PATCH 2/3] Aditya's sample code --- .../ftc/teamcode/AutoExampleThree.java | 80 ++++++ .../ftc/teamcode/BluebAutoV1.java | 247 ++++++++++++++++++ .../ftc/teamcode/BluenbAutov1.java | 4 + 3 files changed, 331 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoExampleThree.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BluebAutoV1.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BluenbAutov1.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoExampleThree.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoExampleThree.java new file mode 100644 index 0000000..47ca538 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoExampleThree.java @@ -0,0 +1,80 @@ +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 = "AutoExampleThree", group = "Autonomous Pathing Tuning") +public class AutoExampleThree extends OpMode { + private Telemetry telemetryA; + + private Follower follower; + + private PathChain path; + + private final Pose startPose = new Pose(10.0, 40, 90); + + /** + * 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(.4); + + follower.setStartingPose(startPose); + + path = follower.pathBuilder() + .addPath( + // Line 1 + new BezierCurve( + new Point(10.000, 20.000, Point.CARTESIAN), + new Point(29.089, 61.232, Point.CARTESIAN), + new Point(48.054, 19.607, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(90)).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/BluebAutoV1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BluebAutoV1.java new file mode 100644 index 0000000..41e89b8 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BluebAutoV1.java @@ -0,0 +1,247 @@ +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 = "BluebAutoV1", group = "Autonomous Pathing Tuning") +public class BluebAutoV1 extends OpMode { + private Telemetry telemetryA; + + private Follower follower; + + private PathChain path; + + private final Pose startPose = new Pose(7.5, 72, 90); + + /** + * 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(.4); + + follower.setStartingPose(startPose); + + path = follower.pathBuilder() + .addPath( + // Line 1 + new BezierLine( + new Point(7.5, 72, Point.CARTESIAN), + new Point(29.893, 38.250, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(90)) + .addPath( + // Line 2 + new BezierLine( + new Point(29.893, 38.250, Point.CARTESIAN), + new Point(65.250, 32.143, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(90)) + .addPath( + // Line 3 + new BezierLine( + new Point(65.250, 32.143, Point.CARTESIAN), + new Point(61.714, 24.429, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(90)) + .addPath( + // Line 4 + new BezierLine( + new Point(61.714, 24.429, Point.CARTESIAN), + new Point(13.821, 22.821, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(90)) + .addPath( + // Line 5 + new BezierLine( + new Point(13.821, 22.821, Point.CARTESIAN), + new Point(61.714, 24.429, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(90)) + .addPath( + // Line 6 + new BezierLine( + new Point(61.714, 24.429, Point.CARTESIAN), + new Point(60.750, 12.696, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(90)) + .addPath( + // Line 7 + new BezierLine( + new Point(60.750, 12.696, Point.CARTESIAN), + new Point(12.375, 13.179, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(90)) + .addPath( + // Line 8 + new BezierLine( + new Point(12.375, 13.179, Point.CARTESIAN), + new Point(60.750, 12.536, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(90)) + .addPath( + // Line 9 + new BezierLine( + new Point(60.750, 12.536, Point.CARTESIAN), + new Point(60.589, 9.321, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(90)) + .addPath( + // Line 10 + new BezierLine( + new Point(60.589, 9.321, Point.CARTESIAN), + new Point(12.536, 8.357, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(90)) + .addPath( + // Line 11 + new BezierLine( + new Point(12.536, 8.357, Point.CARTESIAN), + new Point(26.679, 8.679, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(90)) + .addPath( + // Line 12 + new BezierLine( + new Point(26.679, 8.679, Point.CARTESIAN), + new Point(22.821, 109.446, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(90)) + .addPath( + // Line 13 + new BezierLine( + new Point(22.821, 109.446, Point.CARTESIAN), + new Point(70.714, 109.446, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(90)) + .addPath( + // Line 14 + new BezierLine( + new Point(70.714, 109.446, Point.CARTESIAN), + new Point(71.036, 120.214, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(90)) + .addPath( + // Line 15 + new BezierLine( + new Point(71.036, 120.214, Point.CARTESIAN), + new Point(22.179, 120.214, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(90)) + .addPath( + // Line 16 + new BezierLine( + new Point(22.179, 120.214, Point.CARTESIAN), + new Point(11.089, 130.821, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(90)) + .addPath( + // Line 17 + new BezierLine( + new Point(11.089, 130.821, Point.CARTESIAN), + new Point(70.714, 112.018, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(90)) + .addPath( + // Line 18 + new BezierLine( + new Point(70.714, 112.018, Point.CARTESIAN), + new Point(70.714, 128.250, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(90)) + .addPath( + // Line 19 + new BezierLine( + new Point(70.714, 128.250, Point.CARTESIAN), + new Point(9.964, 130.018, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(90)) + .addPath( + // Line 20 + new BezierLine( + new Point(9.964, 130.018, Point.CARTESIAN), + new Point(70.554, 130.500, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(90)) + .addPath( + // Line 21 + new BezierLine( + new Point(70.554, 130.500, Point.CARTESIAN), + new Point(70.393, 135.000, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(90)) + .addPath( + // Line 22 + new BezierLine( + new Point(70.393, 135.000, Point.CARTESIAN), + new Point(13.821, 134.839, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(90)).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/BluenbAutov1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BluenbAutov1.java new file mode 100644 index 0000000..5e80228 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BluenbAutov1.java @@ -0,0 +1,4 @@ +package org.firstinspires.ftc.teamcode; + +public class BluenbAutov1 { +} From 6f784936d2318d9a593ce76e5d018533f1f764d2 Mon Sep 17 00:00:00 2001 From: Carlos Rivas Date: Tue, 22 Oct 2024 20:26:15 -0700 Subject: [PATCH 3/3] Add Carlos's file --- .../ftc/teamcode/AutoExampleSeason2025V1.java | 142 ++++++++++++++++++ 1 file changed, 142 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoExampleSeason2025V1.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoExampleSeason2025V1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoExampleSeason2025V1.java new file mode 100644 index 0000000..f6695ba --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoExampleSeason2025V1.java @@ -0,0 +1,142 @@ +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.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 = "AutoExampleSeason2025V1", group = "Autonomous Pathing Tuning") +public class AutoExampleSeason2025V1 extends OpMode { + private Telemetry telemetryA; + + private Follower follower; + + private PathChain path; + + private final Pose startPose = new Pose(15.0, 35, 90); + + /** + * 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(.375); + + follower.setStartingPose(startPose); + + path = follower.pathBuilder() + .addPath( + // Line 1 + new BezierLine( + new Point(15.000, 35.000, Point.CARTESIAN), + new Point(60.000, 35.000, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(90)) + .addPath( + // Line 2 + new BezierLine( + new Point(60.000, 35.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(15.000, 25.000, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(90)) + .addPath( + // Line 4 + new BezierLine( + new Point(15.000, 25.000, Point.CARTESIAN), + new Point(60.000, 25.000, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(90)) + .addPath( + // Line 5 + new BezierLine( + new Point(60.000, 25.000, Point.CARTESIAN), + new Point(60.000, 15.000, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(90)) + .addPath( + // Line 6 + new BezierLine( + new Point(60.000, 15.000, Point.CARTESIAN), + new Point(15.000, 15.000, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(90)) + .addPath( + // Line 7 + new BezierLine( + new Point(15.000, 15.000, Point.CARTESIAN), + new Point(60.000, 15.000, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(90)) + .addPath( + // Line 8 + new BezierLine( + new Point(60.000, 15.000, Point.CARTESIAN), + new Point(60.000, 8.000, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(90)) + .addPath( + // Line 9 + new BezierLine( + new Point(60.000, 8.000, Point.CARTESIAN), + new Point(15.000, 8.000, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(90)).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); + } +}