From a49302482113d0a926329e1feb5cd0e6ca1bcecb Mon Sep 17 00:00:00 2001 From: Carlos Rivas Date: Mon, 11 Nov 2024 20:34:43 -0800 Subject: [PATCH] Committing working Autonomous code --- .../ftc/teamcode/BlueBasketAuto.java | 161 ++++++++++++++++++ 1 file changed, 161 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAuto.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAuto.java new file mode 100644 index 0000000..b5f1bec --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAuto.java @@ -0,0 +1,161 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +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 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 = "BlueBasketAuto", group = "Autonomous Pathing Tuning") +public class BlueBasketAuto extends OpMode { + private Telemetry telemetryA; + + private Follower follower; + + private PathChain path; + + private final Pose startPose = new Pose(11.25, 95.75); + + /** + * 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 BezierLine( + new Point(11.250, 95.750, Point.CARTESIAN), + new Point(37.000, 108.000, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(0)) + .addPath( + // Line 2 + new BezierCurve( + new Point(37.000, 108.000, Point.CARTESIAN), + new Point(73.286, 111.536, Point.CARTESIAN), + new Point(67.821, 120.536, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(0)) + .addPath( + // Line 3 + new BezierLine( + new Point(67.821, 120.536, Point.CARTESIAN), + new Point(28.000, 121.500, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(0)) + .addPath( + // Line 4 + new BezierLine( + new Point(28.000, 121.500, Point.CARTESIAN), + new Point(18.000, 130.179, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(0)) + .addPath( + // Line 5 + new BezierCurve( + new Point(18.000, 130.179, Point.CARTESIAN), + new Point(59.000, 102.500, Point.CARTESIAN), + new Point(68.700, 130.500, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(0)) + .addPath( + // Line 6 + new BezierLine( + new Point(68.700, 130.500, Point.CARTESIAN), + new Point(18.000, 130.339, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(0)) + .addPath( + // Line 7 + new BezierCurve( + new Point(18.000, 130.339, Point.CARTESIAN), + new Point(49.018, 121.179, Point.CARTESIAN), + new Point(63.804, 135.321, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(0)) + .addPath( + // Line 8 + new BezierLine( + new Point(63.804, 135.321, Point.CARTESIAN), + new Point(53.036, 135.161, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(0)) + .addPath( + // Line 9 + new BezierLine( + new Point(53.036, 135.161, Point.CARTESIAN), + new Point(18.643, 135.000, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(0)) + .addPath( + // Line 10 + new BezierLine( + new Point(18.643, 135.000, Point.CARTESIAN), + new Point(72.300, 97.400, Point.CARTESIAN) + ) + ) + .addPath( + // Line 11 + new BezierLine( + new Point(18.643, 135.000, Point.CARTESIAN), + new Point(83.250, 95.464, Point.CARTESIAN) + ) + ) + .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270)).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); + } +}