diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoTest.java new file mode 100644 index 0000000..7f5fea7 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoTest.java @@ -0,0 +1,91 @@ +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.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 = "Test", group = "Autonomous Pathing Tuning") +public class AutoTest extends OpMode { + private Telemetry telemetryA; + + + private Follower follower; + + private PathChain test; + + @Override + public void init() { + follower = new Follower(hardwareMap); + + test = follower.pathBuilder() + .addPath( + new BezierLine( + new Point(8.000, 60.000, Point.CARTESIAN), + new Point(18.000, 60.000, Point.CARTESIAN) + ) + ) + + .addPath( + // Line 2 + new BezierCurve( + new Point(18.000, 60.000, Point.CARTESIAN), + new Point(18.000, 23.000, Point.CARTESIAN), + new Point(48.000, 23.000, Point.CARTESIAN) + ) + ) + + .addPath( + // Line 3 + new BezierLine( + new Point(48.000, 23.000, Point.CARTESIAN), + new Point(60.000, 36.000, Point.CARTESIAN) + ) + ) + + .addPath( + // Line 4 + new BezierLine( + new Point(60.000, 36.000, Point.CARTESIAN), + new Point(60.000, 49.000, Point.CARTESIAN) + ) + ).build(); + + + follower.followPath(test); + + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetryA.update(); + } + + + @Override + public void loop() { + follower.update(); + if (follower.atParametricEnd()) { + follower.followPath(test); + } + + follower.telemetryDebug(telemetryA); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedAuto.java new file mode 100644 index 0000000..ef7ea4f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedAuto.java @@ -0,0 +1,99 @@ +package org.firstinspires.ftc.teamcode; + + + +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; +public class RedAuto { + + + public class GeneratedPath { + + public GeneratedPath() { + PathBuilder builder = new PathBuilder(); + + builder + .addPath( + // Line 1 + new BezierLine( + new Point(131.499, 58.931, Point.CARTESIAN), + new Point(131.986, 18.183, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 2 + new BezierLine( + new Point(131.986, 18.183, Point.CARTESIAN), + new Point(90.264, 40.911, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 3 + new BezierLine( + new Point(90.264, 40.911, Point.CARTESIAN), + new Point(83.445, 26.300, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 4 + new BezierLine( + new Point(83.445, 26.300, Point.CARTESIAN), + new Point(136.207, 14.286, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 5 + new BezierLine( + new Point(136.207, 14.286, Point.CARTESIAN), + new Point(81.497, 24.352, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 6 + new BezierLine( + new Point(81.497, 24.352, Point.CARTESIAN), + new Point(82.634, 12.988, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 7 + new BezierLine( + new Point(82.634, 12.988, Point.CARTESIAN), + new Point(133.935, 11.364, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 8 + new BezierLine( + new Point(133.935, 11.364, Point.CARTESIAN), + new Point(82.309, 11.689, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 9 + new BezierLine( + new Point(82.309, 11.689, Point.CARTESIAN), + new Point(83.445, 2.598, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 10 + new BezierLine( + new Point(83.445, 2.598, Point.CARTESIAN), + new Point(132.149, 10.390, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation(); + } + } +}