Fine tuning to autonomous prior to switching to new robot

overtime

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAuto.java

@@ -1,158 +1,115 @@
 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.acmerobotics.roadrunner.SleepAction;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.util.ElapsedTime;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.cometbots.CometBotTeleopCompetition;
+import org.firstinspires.ftc.teamcode.configs.RobotConstants;
 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.PathBuilder;
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
+import org.firstinspires.ftc.teamcode.subsystem.AutoPark;
+import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath1;
+import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath2;
+import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.SkyHookSubsystem;
 
-/**
- * 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")
+
+@Autonomous(name = "Auto Test Competition", group = "Dev")
 public class BlueBasketAuto extends OpMode {
-    private Telemetry telemetryA;
-
     private Follower follower;
+    private int state;
 
-    private PathChain path;
+    private HighBasketAutoPath1 path1;
+    private HighBasketAutoPath2 path2;
+    private AutoPark pathPark;
+    private SkyHookSubsystem hook;
+    private CometBotTeleopCompetition comp;
+    private ElapsedTime runtime;
 
-    private final Pose startPose = new Pose(11.25, 95.75);
+    private LiftActionsSubsystem liftActionsSubsystem;
 
-    /**
-     * 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(.75);
+        path1    = new HighBasketAutoPath1();
+        path2    = new HighBasketAutoPath2();
+        pathPark = new AutoPark();
 
-        follower.setMaxPower(.45);
+        comp = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2);
+        comp.initCloseClaw();
+        runtime = new ElapsedTime();
+        hook = new SkyHookSubsystem(hardwareMap);
+        state = 0;
 
-        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() {
+        switch(state) {
+            case 0:
+                telemetry.addData("case0", "case0");
+
+                path1.moveToPath1(follower);
+                state = 1;
+                runtime.reset();
+            case 1:
+                if (runtime.seconds() > 5) {
+                    telemetry.addData("case1", "case1");
+
+                    new SleepAction(.5);
+                    comp.highBucketDropAuto();
+
+                    state = 2;
+                }
+            case 2:
+                if (runtime.seconds() > 15) {
+                    telemetry.addData("case2", "case2");
+
+                   // new SleepAsction(.5);
+                    //path2.moveToPath1(follower);
+
+                    //For next time, add encoder control to skyhook and extend here
+                    //comp.moveSkyHook();
+
+                    //pathPark.moveToPark(follower);
+
+                    state = 3;
+                }
+            case 3:
+                if (runtime.seconds() > 15) {
+                    telemetry.addData("case3", "case3");
+                    hook.toLevel1Position();
+
+
+                    state = 4;
+                }
+            case 4:
+                if (runtime.seconds() > 15) {
+                    telemetry.addData("case3", "case3");
+                    hook.toLevel1Position();
+
+
+                    state = 4;
+                }
+                //System.out.println("default");
+                //telemetry.addData("default", "default");
+                //telemetry.update();
+        }
+        telemetry.update();
         follower.update();
-        follower.telemetryDebug(telemetryA);
+        //follower.telemetryDebug(telemetry);
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop1.java

@@ -0,0 +1,56 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+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.PathChain;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
+
+@Autonomous(name = "Auto Test1", group = "Dev")
+public class BlueBasketAutoWithDrop1 extends OpMode {
+    private Telemetry telemetryA;
+
+    private Follower follower;
+
+    private PathChain path;
+
+    private final Pose startPose = new Pose(8, 89);
+
+    @Override
+    public void init() {
+        follower = new Follower(hardwareMap);
+
+        follower.setMaxPower(.75);
+
+        follower.setStartingPose(startPose);
+
+        path = follower.pathBuilder()
+                .addPath(
+                        // Line 1
+                        new BezierCurve(
+                                new Point(8.000, 89.000, Point.CARTESIAN),
+                                new Point(24.000, 96.000, Point.CARTESIAN),
+                                new Point(16.000, 128.000, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135)).build();
+
+        follower.followPath(path);
+
+        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+        telemetryA.update();
+    }
+
+
+    @Override
+    public void loop() {
+        follower.update();
+        follower.telemetryDebug(telemetryA);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop2.java

@@ -0,0 +1,57 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+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.PathChain;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
+
+@Autonomous(name = "Auto Test2", group = "Dev")
+public class BlueBasketAutoWithDrop2 extends OpMode {
+    private Telemetry telemetryA;
+
+    private Follower follower;
+
+    private PathChain path;
+
+    private final Pose startPose = new Pose(16, 128);
+
+    @Override
+    public void init() {
+        follower = new Follower(hardwareMap);
+
+        follower.setMaxPower(.75);
+
+        follower.setStartingPose(startPose);
+
+        path = follower.pathBuilder()
+
+                .addPath(
+                        // Line 1
+                        new BezierCurve(
+                                new Point(16.000, 128.000, Point.CARTESIAN),
+                                new Point(88.000, 140.000, Point.CARTESIAN),
+                                new Point(83.250, 99.000, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(270)).build();
+
+        follower.followPath(path);
+
+        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+        telemetryA.update();
+    }
+
+
+    @Override
+    public void loop() {
+        follower.update();
+        follower.telemetryDebug(telemetryA);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueNonBasketAuto.java

@@ -0,0 +1,161 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+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;
+
+@Autonomous(name = "Blue Non Basket Auto", group = "Competition")
+public class BlueNonBasketAuto extends OpMode {
+    private Telemetry telemetryA;
+
+    private Follower follower;
+
+    private PathChain path;
+
+    private PathChain path2;
+
+    private final Pose startPose = new Pose(8.000, 55.000);
+
+    @Override
+    public void init() {
+        follower = new Follower(hardwareMap);
+
+        follower.setMaxPower(.6);
+
+        follower.setStartingPose(startPose);
+
+        path = follower.pathBuilder()
+                .addPath(
+                        // Line 1
+                        new BezierCurve(
+                                new Point(8.000, 55.000, Point.CARTESIAN),
+                                new Point(27.482, 33.750, Point.CARTESIAN),
+                                new Point(62.357, 33.107, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0)).build();
+        follower.followPath(path);
+        
+        path2 = follower.pathBuilder()
+                .addPath(
+                        // Line 2
+                        new BezierLine(
+                                new Point(62.357, 33.107, Point.CARTESIAN),
+                                new Point(62.000, 27.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 3
+                        new BezierLine(
+                                new Point(62.000, 27.000, Point.CARTESIAN),
+                                new Point(10.000, 27.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 4
+                        new BezierLine(
+                                new Point(10.000, 27.000, Point.CARTESIAN),
+                                new Point(61.875, 27.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 5
+                        new BezierLine(
+                                new Point(61.875, 27.000, Point.CARTESIAN),
+                                new Point(61.714, 17.357, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 6
+                        new BezierLine(
+                                new Point(61.714, 17.357, Point.CARTESIAN),
+                                new Point(14.464, 17.357, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 7
+                        new BezierLine(
+                                new Point(14.464, 17.357, Point.CARTESIAN),
+                                new Point(61.714, 17.357, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 8
+                        new BezierLine(
+                                new Point(61.714, 17.357, Point.CARTESIAN),
+                                new Point(61.554, 8.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 9
+                        new BezierLine(
+                                new Point(61.554, 8.000, Point.CARTESIAN),
+                                new Point(12.536, 8.196, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 10
+                        new BezierCurve(
+                                new Point(12.536, 8.196, Point.CARTESIAN),
+                                new Point(52.071, 19.929, Point.CARTESIAN),
+                                new Point(50.786, 33.750, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 11
+                        new BezierCurve(
+                                new Point(50.786, 33.750, Point.CARTESIAN),
+                                new Point(2.571, 39.375, Point.CARTESIAN),
+                                new Point(20.732, 78.911, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 12
+                        new BezierCurve(
+                                new Point(20.732, 78.911, Point.CARTESIAN),
+                                new Point(24.429, 111.054, Point.CARTESIAN),
+                                new Point(46.929, 121.018, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 13
+                        new BezierCurve(
+                                new Point(46.929, 121.018, Point.CARTESIAN),
+                                new Point(68.143, 116.357, Point.CARTESIAN),
+                                new Point(63.000, 97.714, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270)).build();
+
+        follower.followPath(path2);
+
+        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+        telemetryA.update();
+    }
+
+    @Override
+    public void loop() {
+        follower.update();
+        follower.telemetryDebug(telemetryA);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ComeBotDriveDevV2.java

@@ -0,0 +1,290 @@
+package org.firstinspires.ftc.teamcode;
+
+import static com.qualcomm.hardware.lynx.commands.core.LynxSetMotorPIDFControlLoopCoefficientsCommand.InternalMotorControlAlgorithm.Max;
+
+import androidx.annotation.NonNull;
+
+import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
+import com.acmerobotics.roadrunner.Action;
+import com.acmerobotics.roadrunner.SequentialAction;
+import com.acmerobotics.roadrunner.SleepAction;
+import com.acmerobotics.roadrunner.ftc.Actions;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.teamcode.cometbots.CometBotTeleopCompetition;
+import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
+import org.firstinspires.ftc.teamcode.subsystem.AutoPark;
+import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath1;
+import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath2;
+import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath3;
+import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath4;
+import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath5;
+import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath6;
+import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.SkyHookSubsystem;
+
+
+@Autonomous(name = "Auto Test Competition V2", group = "Dev")
+public class ComeBotDriveDevV2 extends OpMode {
+    private Follower follower;
+    private int state;
+
+    private HighBasketAutoPath1 path1;
+    private HighBasketAutoPath2 path2;
+
+    private HighBasketAutoPath3 path3;
+    private HighBasketAutoPath4 path4;
+    private HighBasketAutoPath5 path5;
+    private HighBasketAutoPath6 path6;
+
+    private AutoPark pathPark;
+    private SkyHookSubsystem hook;
+    private CometBotTeleopCompetition comp;
+    private static ElapsedTime runtime;
+    private static boolean initalized = false;
+    private static boolean followingPath = false;
+    private LiftActionsSubsystem liftActionsSubsystem;
+
+    @Override
+    public void init() {
+        follower = new Follower(hardwareMap);
+        follower.setMaxPower(.75);
+        path1 = new HighBasketAutoPath1();
+        path2 = new HighBasketAutoPath2();
+        path3 = new HighBasketAutoPath3();
+        path4 = new HighBasketAutoPath4();
+        path5 = new HighBasketAutoPath5();
+        path6 = new HighBasketAutoPath6();
+
+        pathPark = new AutoPark();
+
+        comp = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2);
+        comp.initCloseClaw();
+        hook = new SkyHookSubsystem(hardwareMap);
+        state = 0;
+    }
+
+
+    public void loop() {
+        telemetry.addData("state", state);
+        telemetry.addData("followingPath", followingPath);
+        if (runtime != null) {
+            telemetry.addData("Runtime (seconds)", runtime.seconds());
+        }
+        switch (state) {
+            case 0:
+                moveToPathOneAndHighBucket();
+                break;
+            case 1:
+                doArmThing();
+                break;
+            case 2:
+                moveToPathTwoAndPickSampleUp();
+                break;
+            case 3:
+                doPickUpThing();
+                break;
+            case 4:
+                moveToBasketPath3();
+                break;
+            case 5:
+                theArmThing();
+                break;
+            case 6:
+                moveToPickupAgainPath4();
+                break;
+            case 7:
+                doPickUpThingAgain();
+                break;
+            case 8:
+                moveToPickupAgainPath5();
+                break;
+            case 9:
+                theArmThingAgain();
+                break;
+            case 10:
+                //moveToParkPath6();
+                break;
+            case 11:
+
+
+        }
+        telemetry.update();
+        follower.update();
+    }
+
+    private void moveToPathOneAndHighBucket() {
+        if (!followingPath) {
+            runtime = new ElapsedTime();
+            path1.moveToPath1(follower);
+            followingPath = true;
+        }
+        if (runtime != null) {
+            telemetry.addData("Runtime (seconds)", runtime.seconds());
+            if (follower.atParametricEnd() || runtime.seconds() > 4) {
+                state = 1;
+                followingPath = false;
+            }
+        }
+    }
+
+    public class SetStateAction implements Action {
+
+        private int value;
+
+        public SetStateAction(int value) {
+            this.value = value;
+        }
+
+        @Override
+        public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+            state = value;
+            return false;
+        }
+    }
+
+    private Action setStateValue(int value) {
+        return new SetStateAction(value);
+    }
+
+    private void doArmThing() {
+        comp.highBucketDropAuto();
+        state = 2;
+    }
+
+    private void theArmThing() {
+        telemetry.addData("busy?", follower.isBusy());
+        telemetry.addData("end?", follower.atParametricEnd());
+        if (follower.atParametricEnd()){
+            follower.breakFollowing();
+            comp.highBucketDropAuto();
+            state = 6;
+        }
+//        follower.breakFollowing();
+    }
+
+    private void theArmThingAgain() {
+        follower.breakFollowing();
+        comp.highBucketDropAuto();
+        state = 10;
+    }
+
+    private void moveToPathTwoAndPickSampleUp() {
+        if (!followingPath) {
+            path2.moveToPath2(follower);
+            followingPath = true;
+        }
+        if (runtime != null) {
+            telemetry.addData("Runtime (seconds)", runtime.seconds());
+            if (follower.atParametricEnd() || runtime.seconds() > 22.0) {
+                state = 3;
+                followingPath = false;
+            }
+        }
+    }
+
+    private void moveToPickupAgainPath4() {
+        if (!followingPath) {
+            path4.moveToPickupAgainPath4(follower);
+            followingPath = true;
+        }
+        if (runtime != null) {
+            telemetry.addData("Runtime (seconds)", runtime.seconds());
+            if (follower.atParametricEnd() || runtime.seconds() > 27.0) {
+                state = 7;
+                followingPath = false;
+            }
+        }
+    }
+
+    private void moveToPickupAgainPath5() {
+        if (!followingPath) {
+            path5.moveToPickupAgainPath5(follower);
+            followingPath = true;
+        }
+        if (runtime != null) {
+            telemetry.addData("Runtime (seconds)", runtime.seconds());
+            if (follower.atParametricEnd() || runtime.seconds() > 36.0) {
+                state = 9;
+                followingPath = false;
+            }
+        }
+    }
+    private void moveToParkPath6() {
+        if (!followingPath) {
+            path6.moveToParkPath6(follower);
+            followingPath = true;
+        }
+        if (runtime != null) {
+            telemetry.addData("Runtime (seconds)", runtime.seconds());
+            if (follower.atParametricEnd() || runtime.seconds() > 48.0) {
+                state = 11;
+                followingPath = false;
+            }
+        }
+    }
+
+    private void moveToBasketPath3() {
+        if (!followingPath) {
+            path3.moveToBasketPath3(follower);
+            followingPath = true;
+        }
+        if (runtime != null) {
+            telemetry.addData("Runtime (seconds)", runtime.seconds());
+            if (follower.atParametricEnd() || runtime.seconds() > 10.0) {
+                state = 5;
+                followingPath = false;
+            }
+        }
+    }
+
+    private void thePickUpAuto() {
+        Actions.runBlocking(new SequentialAction(
+                new SleepAction(.1),
+                comp.claw.openClaw(),
+                new SleepAction(.2),
+                comp.wrist.toPickupPosition(),
+                new SleepAction(.2),
+                comp.arm.toSubmarinePosition(),
+                new SleepAction(.5),
+                comp.claw.closeClaw(),
+                new SleepAction(.3),
+                comp.wrist.toFloorPosition(),
+                new SleepAction(.2),
+                comp.arm.toParkPosition(),
+                new SleepAction(.2)
+        ));
+    }
+    private void thePickUp() {
+        Actions.runBlocking(new SequentialAction(
+                new SleepAction(.5),
+                comp.wrist.toPickupPosition(),
+                new SleepAction(.5),
+                comp.arm.toSubmarinePosition(),
+                new SleepAction(.7),
+                comp.claw.closeClaw(),
+                new SleepAction(.7),
+                comp.wrist.toFloorPosition(),
+                new SleepAction(.5),
+                comp.arm.toParkPosition(),
+                new SleepAction(.5)
+        ));
+    }
+
+    private void doPickUpThing() {
+        follower.breakFollowing();
+        thePickUpAuto();
+        state = 4;
+    }
+
+    private void doPickUpThingAgain() {
+        follower.breakFollowing();
+        thePickUpAuto();
+        state = 8;
+    }
+
+
+}
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotAuto.java

@@ -1,35 +0,0 @@
-//package org.firstinspires.ftc.teamcode;
-//
-//import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-//import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-//
-//import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
-//import org.firstinspires.ftc.teamcode.runmodes.Auto;
-//
-//
-//@Autonomous(name = "CometBot Auto", group = "Competition")
-//public class CometBotAuto extends OpMode {
-//    public Auto auto;
-//
-//    @Override
-//    public void init() {
-//        auto = new Auto(hardwareMap, telemetry, new Follower(hardwareMap));
-//    }
-//
-//    @Override
-//    public void start() {
-//        auto.start();
-//    }
-//
-//    @Override
-//    public void loop() {
-//        auto.update();
-//        telemetry.addData("Arm State", auto.arm.getState());
-//        telemetry.addData("Arm Position", auto.arm.getPosition());
-//        telemetry.addData("Claw State", auto.claw.getState());
-//        telemetry.addData("Wrist State", auto.wrist.getState());
-//        telemetry.addData("Wrist Position", auto.wrist.getPosition());
-//        telemetry.update();
-//    }
-//
-//}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotDevAuto.java

@@ -3,16 +3,16 @@ package org.firstinspires.ftc.teamcode;
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
-import org.firstinspires.ftc.teamcode.runmodes.DevTeleopRunModeCompetition;
+import org.firstinspires.ftc.teamcode.cometbots.CometBotAutoDevelopment;
 
-@TeleOp(name = "Dev Teleop RR Actions", group = "Debug")
-public class DevTeleop extends OpMode {
+@TeleOp(name = "CometBot Auto", group = "Development")
+public class CometBotDevAuto extends OpMode {
 
-    public DevTeleopRunModeCompetition runMode;
+    public CometBotAutoDevelopment runMode;
 
     @Override
     public void init() {
-        this.runMode = new DevTeleopRunModeCompetition(hardwareMap, telemetry, gamepad1, gamepad2);
+        this.runMode = new CometBotAutoDevelopment(hardwareMap, telemetry, gamepad1, gamepad2);
         this.runMode.init();
     }
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotDrive.java

@@ -1,11 +1,9 @@
 package org.firstinspires.ftc.teamcode;
 
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
-import org.firstinspires.ftc.teamcode.runmodes.CometBotTeleopCompetition;
-import org.firstinspires.ftc.teamcode.runmodes.DevTeleopRunModeCompetition;
+import org.firstinspires.ftc.teamcode.cometbots.CometBotTeleopCompetition;
 
 @TeleOp(name = "ComeBot Drive", group = "Competition")
 public class CometBotDrive extends OpMode {

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemix.java

@@ -1,97 +0,0 @@
-//package org.firstinspires.ftc.teamcode;
-//
-//import com.qualcomm.robotcore.eventloop.opmode.Disabled;
-//import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-//import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-//import com.qualcomm.robotcore.hardware.Gamepad;
-//
-//import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
-//import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
-//import org.firstinspires.ftc.teamcode.subsystem.LiftSubsystem;
-//import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem;
-//import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
-//
-//@TeleOp(name = "Dev Teleop Remix", group = "Debug")
-//@Disabled
-//public class DevTeleOpRemix extends OpMode {
-//
-//    public ClawSubsystem claw;
-//    public ArmSubsystem arm;
-//    public WristSubsystem wrist;
-//    public LiftSubsystem lift;
-//    public MotorsSubsystem motors;
-//
-//    public Gamepad currentGamepad1;
-//    public Gamepad previousGamepad1;
-//    public Gamepad currentGamepad2;
-//    public Gamepad previousGamepad2;
-//
-//    public double power = .6;
-//
-//    @Override
-//    public void init() {
-//
-//        claw = new ClawSubsystem(hardwareMap, telemetry);
-//        arm = new ArmSubsystem(hardwareMap, telemetry);
-//        wrist = new WristSubsystem(hardwareMap, telemetry);
-//        lift = new LiftSubsystem(hardwareMap, telemetry);
-//        motors = new MotorsSubsystem(hardwareMap, telemetry, power);
-//
-//        claw.init();
-//        arm.init();
-//        wrist.init();
-//        lift.init();
-//        motors.init();
-//
-//        currentGamepad1 = new Gamepad();
-//        previousGamepad1 = new Gamepad();
-//        currentGamepad2 = new Gamepad();
-//        previousGamepad2 = new Gamepad();
-//    }
-//
-//    public void theDrop(ArmSubsystem arm, WristSubsystem wrist) {
-//        if (currentGamepad1.a && !previousGamepad1.a) {
-//            wrist.toFloorPosition();
-//            arm.toFloorPosition();
-//        }
-//    }
-//
-//    public void thePickup(ClawSubsystem claw) {
-//        if (currentGamepad1.x && !previousGamepad1.x) {
-//            claw.switchState();
-//        }
-//    }
-//
-//    public void theLift(ArmSubsystem arm, WristSubsystem wrist) {
-//        if (currentGamepad1.b && !previousGamepad1.b) {
-//            arm.toParkPosition();
-//            wrist.toBucketPosition();
-//        }
-//    }
-//
-//    public void theLowBucketScore(LiftSubsystem lift, WristSubsystem wrist, ArmSubsystem arm) {
-//        if (currentGamepad1.y && !previousGamepad1.y) {
-//            lift.toLowBucket();
-//            wrist.toBucketPosition();
-//        }
-//    }
-//
-//    @Override
-//    public void loop() {
-//
-//        previousGamepad1.copy(currentGamepad1);
-//        currentGamepad1.copy(gamepad1);
-//
-//        previousGamepad2.copy(currentGamepad2);
-//        currentGamepad2.copy(gamepad2);
-//
-//        theDrop(arm, wrist);
-//        thePickup(claw);
-//        theLift(arm, wrist);
-//        theLowBucketScore(lift, wrist, arm);
-//
-//        motors.calculateTrajectory(gamepad1);
-//
-//    }
-//
-//}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemixDeux.java

@@ -1,107 +0,0 @@
-//package org.firstinspires.ftc.teamcode;
-//
-//import com.qualcomm.robotcore.eventloop.opmode.Disabled;
-//import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-//import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-//import com.qualcomm.robotcore.hardware.Gamepad;
-//
-//import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
-//import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
-//import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
-//import org.firstinspires.ftc.teamcode.subsystem.LiftSubsystem;
-//import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem;
-//import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
-//
-//@TeleOp(name = "Dev Teleop Remix Deux", group = "Debug")
-//@Disabled
-//public class DevTeleOpRemixDeux extends OpMode {
-//
-//    private Follower follower;
-//
-//    public ClawSubsystem claw;
-//    public ArmSubsystem arm;
-//    public WristSubsystem wrist;
-//    public LiftSubsystem lift;
-//    public MotorsSubsystem motors;
-//
-//    public Gamepad currentGamepad1;
-//    public Gamepad previousGamepad1;
-//    public Gamepad currentGamepad2;
-//    public Gamepad previousGamepad2;
-//
-//    public double power = .6;
-//
-//    @Override
-//    public void init() {
-//
-//        follower = new Follower(hardwareMap);
-//
-//        claw = new ClawSubsystem(hardwareMap, telemetry);
-//        arm = new ArmSubsystem(hardwareMap, telemetry);
-//        wrist = new WristSubsystem(hardwareMap, telemetry);
-//        motors = new MotorsSubsystem(hardwareMap, telemetry);
-//        lift = new LiftSubsystem(hardwareMap, telemetry);
-//
-//        claw.init();
-//        arm.init();
-//        wrist.init();
-//        lift.init();
-//        motors.init();
-//
-//        currentGamepad1 = new Gamepad();
-//        previousGamepad1 = new Gamepad();
-//        currentGamepad2 = new Gamepad();
-//        previousGamepad2 = new Gamepad();
-//
-//        follower.setMaxPower(this.power);
-//        follower.startTeleopDrive();
-//
-//    }
-//
-//    public void theDrop(ArmSubsystem arm, WristSubsystem wrist) {
-//        if (currentGamepad1.a && !previousGamepad1.a) {
-//            wrist.toFloorPosition();
-//            arm.toFloorPosition();
-//        }
-//    }
-//
-//    public void thePickup(ClawSubsystem claw) {
-//        if (currentGamepad1.x && !previousGamepad1.x) {
-//            claw.switchState();
-//        }
-//    }
-//
-//    public void theLift(ArmSubsystem arm, WristSubsystem wrist) {
-//        if (currentGamepad1.b && !previousGamepad1.b) {
-//            arm.toParkPosition();
-//            wrist.toBucketPosition();
-//        }
-//    }
-//
-//    public void theLowBucketScore(LiftSubsystem lift, WristSubsystem wrist, ArmSubsystem arm) {
-//        if (currentGamepad1.y && !previousGamepad1.y) {
-//            lift.toLowBucket();
-//            wrist.toBucketPosition();
-//        }
-//    }
-//
-//    @Override
-//    public void loop() {
-//
-//        previousGamepad1.copy(currentGamepad1);
-//        currentGamepad1.copy(gamepad1);
-//
-//        previousGamepad2.copy(currentGamepad2);
-//        currentGamepad2.copy(gamepad2);
-//
-//        theDrop(arm, wrist);
-//        thePickup(claw);
-//        theLift(arm, wrist);
-//        theLowBucketScore(lift, wrist, arm);
-//
-//        follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x);
-//        follower.update();
-//
-//    }
-//
-//}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/NetAuto.java

@@ -0,0 +1,27 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.acmerobotics.roadrunner.Action;
+import com.acmerobotics.roadrunner.ftc.Actions;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+
+import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
+import org.firstinspires.ftc.teamcode.subsystem.AutoLine1;
+
+@Autonomous(name = "BlueNetAuto", group = "Dev")
+public class NetAuto extends OpMode {
+
+    public Follower follower;
+
+    @Override
+    public void init() {
+        follower = new Follower(hardwareMap);
+    }
+
+    @Override
+    public void loop() {
+        follower.update();
+
+        follower.telemetryDebug(telemetry);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java

@@ -10,10 +10,10 @@ public class PedroConstants {
     /*
         Motor configuration names
     */
-    public static final String FRONT_LEFT_MOTOR = "Drive front lt";
-    public static final String BACK_LEFT_MOTOR = "Drive back lt";
-    public static final String FRONT_RIGHT_MOTOR = "Drive front rt";
-    public static final String BACK_RIGHT_MOTOR = "Drive back rt";
+    public static final String FRONT_LEFT_MOTOR = "front-left";
+    public static final String BACK_LEFT_MOTOR = "back-left";
+    public static final String FRONT_RIGHT_MOTOR = "front-right";
+    public static final String BACK_RIGHT_MOTOR = "back-right";
 
     /*
         Motor directions
@@ -23,10 +23,15 @@ public class PedroConstants {
     public static final Direction FRONT_RIGHT_MOTOR_DIRECTION = Direction.FORWARD;
     public static final Direction BACK_RIGHT_MOTOR_DIRECTION = Direction.FORWARD;
 
+    /*
+        Centricity : true is robot-centric movement; false if field-centric movement
+     */
+    public static final boolean CENTRICITY = true;
+
     /*
         Motor Max Power
      */
-    public static final double MAX_POWER = .75;
+    public static final double MAX_POWER = .8;
 
     /*
         IMU
@@ -40,9 +45,9 @@ public class PedroConstants {
     /*
         Dead wheels
      */
-    public static final String LEFT_ENCODER = "encoder left";
-    public static final String RIGHT_ENCODER = "encoder right";
-    public static final String BACK_ENCODER = "encoder back";
+    public static final String RIGHT_ENCODER = "back-right";
+    public static final String BACK_ENCODER = "front-right";
+    public static final String LEFT_ENCODER = "front-left";
 
     /*
         Dead wheel directions
@@ -71,6 +76,11 @@ public class PedroConstants {
      */
     public static final String WRIST_NAME = "wrist-servo";
 
+    /*
+        Skyhook configuration name
+     */
+    public static final String SKYHOOK_NAME = "skyhook";
+
     /*
         Pedro's parameters
     */

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PreLoadedBlueBasketAuto.java

@@ -0,0 +1,124 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+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;
+
+
+@Autonomous(name = "Pre Loaded Blue Basket Auto", group = "Competition")
+public class PreLoadedBlueBasketAuto extends OpMode {
+    private Telemetry telemetryA;
+
+    private Follower follower;
+
+    private PathChain path;
+
+    private final Pose startPose = new Pose(7.875, 89.357);
+
+    @Override
+    public void init() {
+        follower = new Follower(hardwareMap);
+
+        follower.setMaxPower(.45);
+
+        follower.setStartingPose(startPose);
+
+        path = follower.pathBuilder()
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(8, 89, Point.CARTESIAN),
+                                new Point(10.125, 126.804, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 2
+                        new BezierCurve(
+                                new Point(10.125, 126.804, Point.CARTESIAN),
+                                new Point(37.607, 90.000, Point.CARTESIAN),
+                                new Point(62.357, 119.893, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 3
+                        new BezierCurve(
+                                new Point(62.357, 119.893, Point.CARTESIAN),
+                                new Point(33.750, 112.500, Point.CARTESIAN),
+                                new Point(15.107, 130.661, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 4
+                        new BezierCurve(
+                                new Point(15.107, 130.661, Point.CARTESIAN),
+                                new Point(58.821, 103.018, Point.CARTESIAN),
+                                new Point(59.625, 126.964, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 5
+                        new BezierLine(
+                                new Point(59.625, 126.964, Point.CARTESIAN),
+                                new Point(15.107, 130.339, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 6
+                        new BezierLine(
+                                new Point(15.107, 130.339, Point.CARTESIAN),
+                                new Point(59.625, 126.964, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 7
+                        new BezierLine(
+                                new Point(59.625, 126.964, Point.CARTESIAN),
+                                new Point(57.857, 131.071, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 8
+                        new BezierLine(
+                                new Point(57.857, 131.071, Point.CARTESIAN),
+                                new Point(18.964, 131.679, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 9
+                        new BezierCurve(
+                                new Point(18.964, 134.679, Point.CARTESIAN),
+                                new Point(84.536, 131.786, Point.CARTESIAN),
+                                new Point(80.036, 96.429, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270)).build();
+        follower.followPath(path);
+
+        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+        telemetryA.update();
+    }
+
+    @Override
+    public void loop() {
+        follower.update();
+        follower.telemetryDebug(telemetryA);
+
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotAutoCompetition.java

@@ -1,5 +1,10 @@
-//package org.firstinspires.ftc.teamcode.runmodes;
-//
+package org.firstinspires.ftc.teamcode.cometbots;
+
+public class CometBotAutoCompetition {
+
+}
+
+
 //import com.qualcomm.robotcore.hardware.HardwareMap;
 //
 //import org.firstinspires.ftc.robotcore.external.Telemetry;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotAutoDevelopment.java

@@ -1,5 +1,6 @@
-package org.firstinspires.ftc.teamcode.runmodes;
+package org.firstinspires.ftc.teamcode.cometbots;
 
+import com.acmerobotics.roadrunner.Action;
 import com.acmerobotics.roadrunner.SequentialAction;
 import com.acmerobotics.roadrunner.SleepAction;
 import com.acmerobotics.roadrunner.ftc.Actions;
@@ -10,12 +11,13 @@ import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
 import org.firstinspires.ftc.teamcode.states.FieldStates;
 import org.firstinspires.ftc.teamcode.subsystem.ArmActionsSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.AutoLine1;
 import org.firstinspires.ftc.teamcode.subsystem.ClawActionsSubsystem;
 import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
 import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem;
 import org.firstinspires.ftc.teamcode.subsystem.WristActionsSubsystem;
 
-public class DevTeleopRunModeCompetition {
+public class CometBotAutoDevelopment {
 
     /*
     Subsystems
@@ -40,7 +42,12 @@ public class DevTeleopRunModeCompetition {
 
     private Follower follower;
 
-    public DevTeleopRunModeCompetition(HardwareMap hardwareMap, Telemetry telemetry, Gamepad gp1, Gamepad gp2) {
+    /*
+        Actions - Path
+     */
+    private AutoLine1 myFirstPath;
+
+    public CometBotAutoDevelopment(HardwareMap hardwareMap, Telemetry telemetry, Gamepad gp1, Gamepad gp2) {
         this.motors = new MotorsSubsystem(hardwareMap, telemetry, .55);
         this.claw = new ClawActionsSubsystem(hardwareMap);
         this.arm = new ArmActionsSubsystem(hardwareMap);
@@ -92,6 +99,7 @@ public class DevTeleopRunModeCompetition {
         this.telemetry.addData("Lift Position", this.lift.getPosition());
     }
 
+
     /*
     Controller: 1
     Button: A

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotTeleopCompetition.java

@@ -1,5 +1,6 @@
-package org.firstinspires.ftc.teamcode.runmodes;
+package org.firstinspires.ftc.teamcode.cometbots;
 
+import static org.firstinspires.ftc.teamcode.PedroConstants.CENTRICITY;
 import static org.firstinspires.ftc.teamcode.PedroConstants.MAX_POWER;
 
 import com.acmerobotics.roadrunner.SequentialAction;
@@ -15,6 +16,7 @@ import org.firstinspires.ftc.teamcode.subsystem.ArmActionsSubsystem;
 import org.firstinspires.ftc.teamcode.subsystem.ClawActionsSubsystem;
 import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
 import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.SkyHookSubsystem;
 import org.firstinspires.ftc.teamcode.subsystem.WristActionsSubsystem;
 
 public class CometBotTeleopCompetition {
@@ -27,6 +29,7 @@ public class CometBotTeleopCompetition {
     public ArmActionsSubsystem arm;
     public WristActionsSubsystem wrist;
     public LiftActionsSubsystem lift;
+    public SkyHookSubsystem hook;
 
     /*
     Controllers
@@ -37,10 +40,22 @@ public class CometBotTeleopCompetition {
     public Gamepad previousGP1;
     public Gamepad currentGP2;
     public Gamepad previousGP2;
+
+    /*
+    Pedro/FTC Components
+     */
+    private Follower follower;
     private Telemetry telemetry;
+
+    /*
+    States
+     */
     public FieldStates fieldStates;
 
-    private Follower follower;
+    /*
+    Configurations
+     */
+    public double currentPower = MAX_POWER;
 
     public CometBotTeleopCompetition(HardwareMap hardwareMap, Telemetry telemetry, Gamepad gp1, Gamepad gp2) {
         this.motors = new MotorsSubsystem(hardwareMap, telemetry, .55);
@@ -48,6 +63,8 @@ public class CometBotTeleopCompetition {
         this.arm = new ArmActionsSubsystem(hardwareMap);
         this.wrist = new WristActionsSubsystem(hardwareMap);
         this.lift = new LiftActionsSubsystem(hardwareMap);
+        this.hook = new SkyHookSubsystem(hardwareMap);
+
         this.GP1 = gp1;
         this.GP2 = gp2;
         this.telemetry = telemetry;
@@ -61,6 +78,19 @@ public class CometBotTeleopCompetition {
 
     public void init() {
         this.motors.init();
+        this.hook.init();
+        this.claw.init();
+        this.arm.init();
+        this.wrist.init();
+        this.lift.init();
+        this.fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
+        follower.setMaxPower(MAX_POWER);
+        follower.startTeleopDrive();
+    }
+
+    public void initCloseClaw(){
+        this.motors.init();
+        this.hook.init();
         this.claw.init();
         this.arm.init();
         this.wrist.init();
@@ -75,14 +105,19 @@ public class CometBotTeleopCompetition {
         this.currentGP1.copy(this.GP1);
         this.previousGP2.copy(currentGP2);
         this.currentGP2.copy(this.GP2);
-
+        this.moveSkyHook();
         this.toHighBucketScore();
         this.toLowBucketScore();
         this.toArmParkPosition();
         this.toArmParkThenSwitchBetweenSubmarineAndFloorPosition();
         this.clawControl();
+        this.decreaseMaxPower();
+        this.increaseMaxPower();
 
-        follower.setTeleOpMovementVectors(-this.GP1.left_stick_y, -this.GP1.left_stick_x, -this.GP1.right_stick_x);
+
+        Actions.runBlocking(this.lift.toFloorPosition());
+
+        follower.setTeleOpMovementVectors(-this.GP1.left_stick_y, -this.GP1.left_stick_x, -this.GP1.right_stick_x, CENTRICITY);
         follower.update();
 
         this.telemetry.addData("Field State", this.fieldStates.getFieldLocation());
@@ -92,6 +127,47 @@ public class CometBotTeleopCompetition {
         this.telemetry.addData("Arm State", this.arm.getState());
         this.telemetry.addData("Lift State", this.lift.getState());
         this.telemetry.addData("Lift Position", this.lift.getPosition());
+        this.telemetry.addData("MaxPower", MAX_POWER);
+    }
+
+    /*
+    Type: PS4 / Logitech
+    Controller: 1
+    Button: Left Bumper
+    Assumption: Working motor mechanism
+    Action: Decreases maximum speed by -.05
+     */
+    public void decreaseMaxPower() {
+        if (this.currentGP1.left_bumper && !this.previousGP1.left_bumper) {
+            this.currentPower = this.currentPower - .05;
+            this.follower.setMaxPower(this.currentPower);
+        }
+    }
+
+    /*
+    Type: PS4 / Logitech
+    Controller: 1
+    Button: Left Bumper
+    Assumption: Working motor mechanism
+    Action: Increases maximum speed by +.05
+     */
+    public void increaseMaxPower() {
+        if (this.currentGP1.right_bumper && !this.previousGP1.right_bumper) {
+            this.currentPower = this.currentPower + .05;
+            this.follower.setMaxPower(this.currentPower);
+        }
+    }
+    public void moveSkyHook() {
+        if (this.currentGP2.dpad_down) {
+            hook.moveSkyHook(-1.00);
+        }
+        else if (this.currentGP2.dpad_up) {
+            hook.moveSkyHook(1.00);
+        }
+        else{
+            hook.moveSkyHook(0.00);
+        }
+
     }
 
     /*
@@ -106,27 +182,52 @@ public class CometBotTeleopCompetition {
      */
     public void toHighBucketScore() {
         if (this.currentGP2.triangle && !this.previousGP2.triangle) {
-            fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET);
-            Actions.runBlocking(new SequentialAction(
-                    this.wrist.toFloorPosition(),
-                    this.arm.toParkPosition(),
-                    this.lift.toHighBucketPosition(),
-                    new SleepAction(.5),
-                    this.arm.toBucketPosition(),
-                    new SleepAction(.5),
-                    this.wrist.toBucketPosition(),
-                    new SleepAction(.5),
-                    this.claw.openClaw(),
-                    new SleepAction(.5),
-                    this.wrist.toFloorPosition(),
-                    new SleepAction(.5),
-                    this.arm.toParkPosition(),
-                    this.lift.toFloorPosition()
-            ));
-            fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
+            if (this.arm.getState() == ArmActionsSubsystem.ArmState.PARK) {
+                this.follower.breakFollowing();
+                fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET);
+                highBucketDrop();
+                fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
+                this.follower.startTeleopDrive();
+            }
         }
     }
 
+    public void highBucketDrop() {
+        Actions.runBlocking(new SequentialAction(
+                new SleepAction(.5),
+                this.lift.toHighBucketPosition(),
+                new SleepAction(.5),
+                this.arm.toBucketPosition(),
+                new SleepAction(.5),
+                this.wrist.toBucketPosition(),
+                new SleepAction(.5),
+                this.claw.openClaw(),
+                new SleepAction(.5),
+                this.wrist.toFloorPosition(),
+                new SleepAction(.5),
+                this.arm.toParkPosition(),
+                this.lift.toFloorPosition(),
+                new SleepAction(.5)
+        ));
+    }
+    public void highBucketDropAuto() {
+        Actions.runBlocking(new SequentialAction(
+                new SleepAction(.1),
+                this.lift.toHighBucketPosition(),
+                new SleepAction(.25),
+                this.arm.toBucketPosition(),
+                new SleepAction(.25),
+                this.wrist.toBucketPosition(),
+                new SleepAction(.25),
+                this.claw.openClaw(),
+                new SleepAction(.25),
+                this.wrist.toFloorPosition(),
+                new SleepAction(.25),
+                this.arm.toParkPosition(),
+                this.lift.toZeroPosition(),
+                new SleepAction(.25)
+        ));
+    }
     /*
     Type: PS4 / Logitech
     Controller: 2
@@ -139,24 +240,28 @@ public class CometBotTeleopCompetition {
      */
     public void toLowBucketScore() {
         if (this.currentGP2.circle && !this.previousGP2.circle) {
-            fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET);
-            Actions.runBlocking(new SequentialAction(
-                    this.wrist.toFloorPosition(),
-                    this.arm.toParkPosition(),
-                    this.lift.toLowBucketPosition(),
-                    new SleepAction(.5),
-                    this.arm.toBucketPosition(),
-                    new SleepAction(.5),
-                    this.wrist.toBucketPosition(),
-                    new SleepAction(.5),
-                    this.claw.openClaw(),
-                    new SleepAction(.5),
-                    this.wrist.toFloorPosition(),
-                    new SleepAction(.5),
-                    this.arm.toParkPosition(),
-                    this.lift.toFloorPosition()
-            ));
-            fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
+            if (this.arm.getState() == ArmActionsSubsystem.ArmState.PARK) {
+                this.follower.breakFollowing();
+                fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET);
+                Actions.runBlocking(new SequentialAction(
+                        new SleepAction(.5),
+                        this.lift.toLowBucketPosition(),
+                        new SleepAction(.5),
+                        this.arm.toBucketPosition(),
+                        new SleepAction(.5),
+                        this.wrist.toBucketPosition(),
+                        new SleepAction(.5),
+                        this.claw.openClaw(),
+                        new SleepAction(.5),
+                        this.wrist.toFloorPosition(),
+                        new SleepAction(.5),
+                        this.arm.toParkPosition(),
+                        this.lift.toFloorPosition(),
+                        new SleepAction(.5)
+                ));
+                fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
+                this.follower.startTeleopDrive();
+            }
         }
     }
 
@@ -183,10 +288,17 @@ public class CometBotTeleopCompetition {
      */
     public void toArmParkPosition() {
         if (this.currentGP2.square && !this.previousGP2.square) {
-            Actions.runBlocking(this.arm.toParkPosition());
+            Actions.runBlocking(new SequentialAction(
+                    this.wrist.toFloorPosition(),
+                    this.arm.toParkPosition(),
+                    this.lift.toFloorPosition()
+            ));
         }
     }
 
+
+
+
     /*
     Type: PS4
     Controller: 2
@@ -210,17 +322,18 @@ public class CometBotTeleopCompetition {
                                 this.wrist.toFloorPosition()
                         )
                 );
-            } else if (this.arm.getState() == ArmActionsSubsystem.ArmState.SUBMARINE) {
+            } else if (this.arm.getState() == ArmActionsSubsystem.ArmState.SUBMARINE && this.wrist.getState() == WristActionsSubsystem.WristState.FLOOR) {
                 Actions.runBlocking(
                         new SequentialAction(
-                                this.arm.toFloorPosition(),
-                                this.wrist.toFloorPosition()
+//                                this.lift.toFloorPosition(),
+                                this.arm.toSubmarinePosition(),
+                                this.wrist.toPickupPosition()
                         )
                 );
-            } else if (this.arm.getState() == ArmActionsSubsystem.ArmState.FLOOR) {
+            } else if (this.wrist.getState() == WristActionsSubsystem.WristState.PICKUP) {
                 Actions.runBlocking(
                         new SequentialAction(
-                                this.arm.toSubmarinePosition(),
+                                //  this.arm.toSubmarinePosition(),
                                 this.wrist.toFloorPosition()
                         )
                 );
@@ -228,4 +341,16 @@ public class CometBotTeleopCompetition {
         }
     }
 
-}
+
+
+
+
+
+
+
+
+
+    }
+
+
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotTeleopDevelopment.java

@@ -1,5 +1,9 @@
-//package org.firstinspires.ftc.teamcode.runmodes;
-//
+package org.firstinspires.ftc.teamcode.cometbots;
+
+public class CometBotTeleopDevelopment {
+
+}
+
 //import com.qualcomm.robotcore.hardware.Gamepad;
 //import com.qualcomm.robotcore.hardware.HardwareMap;
 //

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/SensorIMUOrthogonal.java

@@ -1,172 +0,0 @@
-/* Copyright (c) 2022 FIRST. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without modification,
- * are permitted (subject to the limitations in the disclaimer below) provided that
- * the following conditions are met:
- *
- * Redistributions of source code must retain the above copyright notice, this list
- * of conditions and the following disclaimer.
- *
- * Redistributions in binary form must reproduce the above copyright notice, this
- * list of conditions and the following disclaimer in the documentation and/or
- * other materials provided with the distribution.
- *
- * Neither the name of FIRST nor the names of its contributors may be used to endorse or
- * promote products derived from this software without specific prior written permission.
- *
- * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
- * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
- * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
- * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
- * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
- * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
- * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-
-package org.firstinspires.ftc.teamcode.cometbots;
-
-import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER;
-import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION;
-import static org.firstinspires.ftc.teamcode.PedroConstants.IMU_LOGO_FACING_DIRECTION;
-import static org.firstinspires.ftc.teamcode.PedroConstants.IMU_USB_FACING_DIRECTION;
-import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER;
-import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER_DIRECTION;
-import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER;
-import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER_DIRECTION;
-
-import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-import com.qualcomm.robotcore.hardware.DcMotorEx;
-import com.qualcomm.robotcore.hardware.IMU;
-
-import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
-import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
-import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
-import org.firstinspires.ftc.teamcode.PedroConstants;
-import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
-
-/*
- * This OpMode shows how to use the new universal IMU interface. This
- * interface may be used with the BNO055 IMU or the BHI260 IMU. It assumes that an IMU is configured
- * on the robot with the name "imu".
- *
- * The sample will display the current Yaw, Pitch and Roll of the robot.<br>
- * With the correct orientation parameters selected, pitch/roll/yaw should act as follows:
- *   Pitch value should INCREASE as the robot is tipped UP at the front. (Rotation about X) <br>
- *   Roll value should INCREASE as the robot is tipped UP at the left side. (Rotation about Y) <br>
- *   Yaw value should INCREASE as the robot is rotated Counter Clockwise. (Rotation about Z) <br>
- *
- * The yaw can be reset (to zero) by pressing the Y button on the gamepad (Triangle on a PS4 controller)
- *
- * This specific sample assumes that the Hub is mounted on one of the three orthogonal planes
- * (X/Y, X/Z or Y/Z) and that the Hub has only been rotated in a range of 90 degree increments.
- *
- * Note: if your Hub is mounted on a surface angled at some non-90 Degree multiple (like 30) look at
- *       the alternative SensorIMUNonOrthogonal sample in this folder.
- *
- * This "Orthogonal" requirement means that:
- *
- * 1) The Logo printed on the top of the Hub can ONLY be pointing in one of six directions:
- *    FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT.
- *
- * 2) The USB ports can only be pointing in one of the same six directions:<br>
- *    FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT.
- *
- * So, To fully define how your Hub is mounted to the robot, you must simply specify:<br>
- *    logoFacingDirection<br>
- *    usbFacingDirection
- *
- * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
- * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list.
- *
- * Finally, choose the two correct parameters to define how your Hub is mounted and edit this OpMode
- * to use those parameters.
- */
-@TeleOp(name = "Sensor: IMU Orthogonal", group = "Sensor")
-@Disabled   // Comment this out to add to the OpMode list
-public class SensorIMUOrthogonal extends LinearOpMode {
-    // The IMU sensor object
-    IMU imu;
-    private Encoder leftEncoder;
-    private Encoder rightEncoder;
-    private Encoder strafeEncoder;
-
-    //----------------------------------------------------------------------------------------------
-    // Main logic
-    //----------------------------------------------------------------------------------------------
-
-    @Override
-    public void runOpMode() throws InterruptedException {
-
-        // Retrieve and initialize the IMU.
-        // This sample expects the IMU to be in a REV Hub and named "imu".
-        imu = hardwareMap.get(IMU.class, PedroConstants.IMU);
-
-        // TODO: replace these with your encoder ports
-        leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER));
-        rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, RIGHT_ENCODER));
-        strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_ENCODER));
-
-        // TODO: reverse any encoders necessary
-        leftEncoder.setDirection(LEFT_ENCODER_DIRECTION);
-        rightEncoder.setDirection(RIGHT_ENCODER_DIRECTION);
-        strafeEncoder.setDirection(BACK_ENCODER_DIRECTION);
-
-        /* Define how the hub is mounted on the robot to get the correct Yaw, Pitch and Roll values.
-         *
-         * Two input parameters are required to fully specify the Orientation.
-         * The first parameter specifies the direction the printed logo on the Hub is pointing.
-         * The second parameter specifies the direction the USB connector on the Hub is pointing.
-         * All directions are relative to the robot, and left/right is as-viewed from behind the robot.
-         *
-         * If you are using a REV 9-Axis IMU, you can use the Rev9AxisImuOrientationOnRobot class instead of the
-         * RevHubOrientationOnRobot class, which has an I2cPortFacingDirection instead of a UsbFacingDirection.
-         */
-
-        /* The next two lines define Hub orientation.
-         * The Default Orientation (shown) is when a hub is mounted horizontally with the printed logo pointing UP and the USB port pointing FORWARD.
-         *
-         * To Do:  EDIT these two lines to match YOUR mounting configuration.
-         */
-        RevHubOrientationOnRobot.LogoFacingDirection logoDirection = IMU_LOGO_FACING_DIRECTION;
-        RevHubOrientationOnRobot.UsbFacingDirection usbDirection = IMU_USB_FACING_DIRECTION;
-
-        RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(logoDirection, usbDirection);
-
-        // Now initialize the IMU with this mounting orientation
-        // Note: if you choose two conflicting directions, this initialization will cause a code exception.
-        imu.initialize(new IMU.Parameters(orientationOnRobot));
-
-        // Loop and update the dashboard
-        while (!isStopRequested()) {
-
-            telemetry.addData("Hub orientation", "Logo=%s   USB=%s\n ", logoDirection, usbDirection);
-
-            // Check to see if heading reset is requested
-            if (gamepad1.y) {
-                telemetry.addData("Yaw", "Resetting\n");
-                imu.resetYaw();
-            } else {
-                telemetry.addData("Yaw", "Press Y (triangle) on Gamepad to reset\n");
-            }
-
-            // Retrieve Rotational Angles and Velocities
-            YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles();
-            AngularVelocity angularVelocity = imu.getRobotAngularVelocity(AngleUnit.DEGREES);
-
-            telemetry.addData("Yaw (Z)", "%.2f Deg. (Heading)", orientation.getYaw(AngleUnit.DEGREES));
-            telemetry.addData("Pitch (X)", "%.2f Deg.", orientation.getPitch(AngleUnit.DEGREES));
-            telemetry.addData("Roll (Y)", "%.2f Deg.\n", orientation.getRoll(AngleUnit.DEGREES));
-            telemetry.addData("Yaw (Z) velocity", "%.2f Deg/Sec", angularVelocity.zRotationRate);
-            telemetry.addData("Pitch (X) velocity", "%.2f Deg/Sec", angularVelocity.xRotationRate);
-            telemetry.addData("Roll (Y) velocity", "%.2f Deg/Sec", angularVelocity.yRotationRate);
-            telemetry.update();
-        }
-    }
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ClawTest.java

@@ -58,6 +58,7 @@ public class ClawTest extends LinearOpMode {
         Gamepad previousGamepad1 = new Gamepad();
 
         waitForStart();
+        claw.init();
         runtime.reset();
 
         // run until the end of the match (driver presses STOP)

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/LiftWristArmTest.java

@@ -29,6 +29,9 @@
 
 package org.firstinspires.ftc.teamcode.cometbots.tests;
 
+import com.acmerobotics.roadrunner.SequentialAction;
+import com.acmerobotics.roadrunner.SleepAction;
+import com.acmerobotics.roadrunner.ftc.Actions;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.hardware.Gamepad;
@@ -37,6 +40,8 @@ import com.qualcomm.robotcore.util.ElapsedTime;
 import org.firstinspires.ftc.teamcode.subsystem.ArmActionsSubsystem;
 import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
 import org.firstinspires.ftc.teamcode.subsystem.WristActionsSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.ClawActionsSubsystem;
+
 
 @TeleOp(name = "Lift Wrist Arm Test", group = "Debug")
 public class LiftWristArmTest extends LinearOpMode {
@@ -52,6 +57,8 @@ public class LiftWristArmTest extends LinearOpMode {
         LiftActionsSubsystem lift = new LiftActionsSubsystem(hardwareMap);
         WristActionsSubsystem wrist = new WristActionsSubsystem(hardwareMap);
         ArmActionsSubsystem arm = new ArmActionsSubsystem(hardwareMap);
+        ClawActionsSubsystem claw = new ClawActionsSubsystem(hardwareMap);
+
 
         /*
          * Instantiate gamepad state holders
@@ -62,6 +69,7 @@ public class LiftWristArmTest extends LinearOpMode {
         lift.init();
         wrist.init();
         arm.init();
+        claw.init();
         waitForStart();
         runtime.reset();
 
@@ -79,6 +87,24 @@ public class LiftWristArmTest extends LinearOpMode {
             if (currentGamepad1.dpad_down && !previousGamepad1.dpad_down) {
                 arm.setPosition(arm.getPosition() - .05);
             }
+            if (currentGamepad1.circle && !previousGamepad1.circle) {
+                claw.switchState();
+            }
+            if (currentGamepad1.dpad_left && !previousGamepad1.dpad_left) {
+                Actions.runBlocking(
+                        new SequentialAction(
+                                lift.toHighRung(),
+                                arm.toBucketPosition(),
+                                wrist.toRungPosition(),
+                                new SleepAction(0.5),
+                                lift.toHighRungAttach(),
+                                new SleepAction(0.5),
+                                claw.openClaw())
+                );
+
+                //Delete open claw
+            }
+
 
             if (currentGamepad1.triangle && !previousGamepad1.triangle) {
                 wrist.setPosition(wrist.getPosition() + .05);
@@ -89,13 +115,16 @@ public class LiftWristArmTest extends LinearOpMode {
             }
 
             if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
-                lift.setPosition(lift.getPosition() + 175);
+                lift.setPosition(lift.getPosition() + 100);
             }
 
             if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) {
-                lift.setPosition(lift.getPosition() - 25);
+                if (lift.getPosition() > 1000) {
+                    lift.setPosition(lift.getPosition()-1000);
+                }
             }
 
+
             // Show the elapsed game time and wheel power.
             telemetry.addData("Status", "Run Time: " + runtime.toString());
             telemetry.addData("Lift Drive Position", lift.getPosition());

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/SkyhookTest.java

@@ -0,0 +1,88 @@
+/* Copyright (c) 2021 FIRST. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted (subject to the limitations in the disclaimer below) provided that
+ * the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this list
+ * of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice, this
+ * list of conditions and the following disclaimer in the documentation and/or
+ * other materials provided with the distribution.
+ *
+ * Neither the name of FIRST nor the names of its contributors may be used to endorse or
+ * promote products derived from this software without specific prior written permission.
+ *
+ * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+ * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+package org.firstinspires.ftc.teamcode.cometbots.tests;
+
+import com.acmerobotics.roadrunner.ftc.Actions;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.Gamepad;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.SkyHookSubsystem;
+
+@TeleOp(name = "Skyhook Test", group = "Debug")
+public class SkyhookTest extends LinearOpMode {
+
+    private final ElapsedTime runtime = new ElapsedTime();
+
+    @Override
+    public void runOpMode() {
+
+        /*
+         * Instantiate Lift
+         */
+        SkyHookSubsystem hook = new SkyHookSubsystem(hardwareMap);
+
+        /*
+         * Instantiate gamepad state holders
+         */
+
+        Gamepad currentGamepad2 = new Gamepad();
+        Gamepad previousGamepad2 = new Gamepad();
+
+
+        hook.init();
+        waitForStart();
+        runtime.reset();
+
+
+        // run until the end of the match (driver presses STOP)
+        while (opModeIsActive()) {
+
+            previousGamepad2.copy(currentGamepad2);
+            currentGamepad2.copy(gamepad2);
+
+            if (currentGamepad2.dpad_up) {
+                hook.moveSkyHook(-1.00);;
+            } else if (currentGamepad2.dpad_down) {
+                hook.moveSkyHook(1.00);
+            } else {
+                hook.moveSkyHook(0.00);
+            }
+
+
+            // Show the elapsed game time and wheel power.
+            telemetry.addData("Status", "Run Time: " + runtime.toString());
+            telemetry.addData("Skyhook Position", hook.getHookPosition());
+            telemetry.update();
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/SpecimenTest.java

@@ -0,0 +1,196 @@
+/* Copyright (c) 2021 FIRST. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted (subject to the limitations in the disclaimer below) provided that
+ * the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this list
+ * of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice, this
+ * list of conditions and the following disclaimer in the documentation and/or
+ * other materials provided with the distribution.
+ *
+ * Neither the name of FIRST nor the names of its contributors may be used to endorse or
+ * promote products derived from this software without specific prior written permission.
+ *
+ * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+ * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+package org.firstinspires.ftc.teamcode.cometbots.tests;
+
+import androidx.annotation.NonNull;
+
+import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
+import com.acmerobotics.roadrunner.Action;
+import com.acmerobotics.roadrunner.SequentialAction;
+import com.acmerobotics.roadrunner.SleepAction;
+import com.acmerobotics.roadrunner.ftc.Actions;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.Gamepad;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+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.Path;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
+import org.firstinspires.ftc.teamcode.subsystem.ArmActionsSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.AutoLine1;
+import org.firstinspires.ftc.teamcode.subsystem.ClawActionsSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.WristActionsSubsystem;
+
+
+@TeleOp(name = "SpecimenTest", group = "Debug")
+public class SpecimenTest extends LinearOpMode {
+
+    private final ElapsedTime runtime = new ElapsedTime();
+
+    private PathChain path;
+
+    private Follower robot;
+
+    private final Pose startPose = new Pose(36, 72);
+
+
+
+
+    public AutoLine1 firstPath = new AutoLine1();
+
+    @Override
+    public void runOpMode() {
+
+
+        /*
+         * Instantiate Lift
+         */
+        LiftActionsSubsystem lift = new LiftActionsSubsystem(hardwareMap);
+        WristActionsSubsystem wrist = new WristActionsSubsystem(hardwareMap);
+        ArmActionsSubsystem arm = new ArmActionsSubsystem(hardwareMap);
+        ClawActionsSubsystem claw = new ClawActionsSubsystem(hardwareMap);
+        Follower robot = new Follower(hardwareMap);
+        firstPath.moveToPath1(robot);
+
+        /*
+          Robot stuff
+         */
+        robot.setStartingPose(startPose);
+        path = robot.pathBuilder().addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(37.500, 72.000, Point.CARTESIAN),
+                                new Point(36.000, 72.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0)).build();
+
+        /*
+         * Instantiate gamepad state holders
+         */
+        Gamepad currentGamepad1 = new Gamepad();
+        Gamepad previousGamepad1 = new Gamepad();
+        Gamepad currentGamepad2 = new Gamepad();
+        Gamepad previousGamepad2 = new Gamepad();
+
+        lift.init();
+        wrist.init();
+        arm.init();
+        claw.init();
+        robot.setMaxPower(.40);
+        robot.followPath(path);
+
+        waitForStart();
+        runtime.reset();
+
+
+        // run until the end of the match (driver presses STOP)
+        while (opModeIsActive()) {
+
+            previousGamepad1.copy(currentGamepad1);
+            currentGamepad1.copy(gamepad1);
+
+            previousGamepad2.copy(currentGamepad2);
+            currentGamepad2.copy(gamepad2);
+
+
+            robot.setTeleOpMovementVectors(-currentGamepad1.left_stick_y, -currentGamepad1.left_stick_x, -currentGamepad1.right_stick_x);
+            //robot.update();
+
+            if (currentGamepad1.dpad_up && !previousGamepad1.dpad_up) {
+                arm.setPosition(arm.getPosition() + .05);
+            }
+
+            if (currentGamepad1.dpad_down && !previousGamepad1.dpad_down) {
+                arm.setPosition(arm.getPosition() - .05);
+            }
+            if (currentGamepad1.circle && !previousGamepad1.circle) {
+                claw.switchState();
+            }
+            if (currentGamepad1.dpad_right && !previousGamepad1.dpad_right) {
+                robot.update();
+                Actions.runBlocking(
+                        new SequentialAction(
+                                wrist.toFloorPosition(),
+                                lift.toHighRung(),
+                                wrist.toSpeciemenBar(),
+                                lift.dropToHighRung()
+               //                 reverseMoveToPath(robot),
+                 //               claw.openClaw(),
+                   //             wrist.toFloorPosition(),
+                     //           lift.toFloorPosition()
+                        )
+                );
+            }
+
+
+            if (currentGamepad1.triangle && !previousGamepad1.triangle) {
+                wrist.setPosition(wrist.getPosition() + .05);
+            }
+
+            if (currentGamepad1.cross && !previousGamepad1.cross) {
+                wrist.setPosition(wrist.getPosition() - .05);
+            }
+
+            if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
+                lift.setPosition(lift.getPosition() + 175);
+            }
+
+            if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) {
+                lift.setPosition(lift.getPosition() - 25);
+            }
+
+            if (currentGamepad2.x && !previousGamepad2.x) {
+                while (true) {
+                    robot.update();
+                    if (!robot.isBusy()) {
+                        robot.breakFollowing();
+                        robot.startTeleopDrive();
+                        break;
+                    }
+                }
+            }
+
+
+            // Show the elapsed game time and wheel power.
+            telemetry.addData("Status", "Run Time: " + runtime.toString());
+            telemetry.addData("Lift Drive Position", lift.getPosition());
+            telemetry.addData("Wrist Position", wrist.getPosition());
+            telemetry.addData("Arm Position", arm.getPosition());
+
+            telemetry.update();
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java

@@ -4,20 +4,30 @@ import com.acmerobotics.dashboard.config.Config;
 
 @Config
 public class RobotConstants {
-    public final static double clawClose = 1.00;
-    public final static double clawOpen = 0.05;
+    public final static double clawClose = 0.8;
+    public final static double clawOpen = 0.15;
 
-    public final static double armFloor = 0.45;
-    public final static double armSubmarine = 0.375;
+    public final static double armFloor = 0.7;
+    public final static double armSubmarine = 0.55;
     public final static double armPark = 0.0;
-    public final static double armBucket = 0.15;
+    public final static double armBucket = 0.2;
 
-    public final static double wristFloor = 0.7;
-    public final static double wristBucket = 0.35;
+    public final static double wristFloor = 0.55;
+    public final static double wristBucket = 0.25;
+    public final static double wristRung = 0.55;
 
-    public final static int liftToFloorPos = 0;
-    public final static int liftToSubmarinePos = 250;
-    public final static int liftToLowBucketPos = 2250;
-    public final static int liftToHighBucketPos = 3850;
-    public final static double liftPower = .625;
+    public final static double wristPickup = 0.1;
+    public final static double wristSpeciemen = 0.1;
+
+
+    public final static int liftToFloorPos = 500;
+    public final static int liftToLowBucketPos = 2650;
+    public final static int liftToHighRung = 2100;
+    public final static int dropToHighRung = 1675;
+
+    public final static int liftToHighRungAttach = 1050;
+
+
+    public final static int liftToHighBucketPos = 4900;
+    public final static double liftPower = 1;
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java

@@ -929,31 +929,31 @@ public class Follower {
      *                  method will use to output the debug data.
      */
     public void telemetryDebug(MultipleTelemetry telemetry) {
-        telemetry.addData("follower busy", isBusy());
-        telemetry.addData("heading error", headingError);
-        telemetry.addData("heading vector magnitude", headingVector.getMagnitude());
-        telemetry.addData("corrective vector magnitude", correctiveVector.getMagnitude());
-        telemetry.addData("corrective vector heading", correctiveVector.getTheta());
-        telemetry.addData("translational error magnitude", getTranslationalError().getMagnitude());
-        telemetry.addData("translational error direction", getTranslationalError().getTheta());
-        telemetry.addData("translational vector magnitude", translationalVector.getMagnitude());
-        telemetry.addData("translational vector heading", translationalVector.getMagnitude());
-        telemetry.addData("centripetal vector magnitude", centripetalVector.getMagnitude());
-        telemetry.addData("centripetal vector heading", centripetalVector.getTheta());
-        telemetry.addData("drive error", driveError);
-        telemetry.addData("drive vector magnitude", driveVector.getMagnitude());
-        telemetry.addData("drive vector heading", driveVector.getTheta());
-        telemetry.addData("x", getPose().getX());
-        telemetry.addData("y", getPose().getY());
-        telemetry.addData("heading", getPose().getHeading());
-        telemetry.addData("total heading", poseUpdater.getTotalHeading());
-        telemetry.addData("velocity magnitude", getVelocity().getMagnitude());
-        telemetry.addData("velocity heading", getVelocity().getTheta());
-        driveKalmanFilter.debug(telemetry);
-        telemetry.update();
-        if (drawOnDashboard) {
-            Drawing.drawDebug(this);
-        }
+//        telemetry.addData("follower busy", isBusy());
+//        telemetry.addData("heading error", headingError);
+//        telemetry.addData("heading vector magnitude", headingVector.getMagnitude());
+//        telemetry.addData("corrective vector magnitude", correctiveVector.getMagnitude());
+//        telemetry.addData("corrective vector heading", correctiveVector.getTheta());
+//        telemetry.addData("translational error magnitude", getTranslationalError().getMagnitude());
+//        telemetry.addData("translational error direction", getTranslationalError().getTheta());
+//        telemetry.addData("translational vector magnitude", translationalVector.getMagnitude());
+//        telemetry.addData("translational vector heading", translationalVector.getMagnitude());
+//        telemetry.addData("centripetal vector magnitude", centripetalVector.getMagnitude());
+//        telemetry.addData("centripetal vector heading", centripetalVector.getTheta());
+//        telemetry.addData("drive error", driveError);
+//        telemetry.addData("drive vector magnitude", driveVector.getMagnitude());
+//        telemetry.addData("drive vector heading", driveVector.getTheta());
+//        telemetry.addData("x", getPose().getX());
+//        telemetry.addData("y", getPose().getY());
+//        telemetry.addData("heading", getPose().getHeading());
+//        telemetry.addData("total heading", poseUpdater.getTotalHeading());
+//        telemetry.addData("velocity magnitude", getVelocity().getMagnitude());
+//        telemetry.addData("velocity heading", getVelocity().getTheta());
+//        driveKalmanFilter.debug(telemetry);
+//        telemetry.update();
+//        if (drawOnDashboard) {
+//            Drawing.drawDebug(this);
+//        }
     }
 
     /**

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathBuilder.java

@@ -14,6 +14,7 @@ import java.util.ArrayList;
  * @version 1.0, 3/11/2024
  */
 public class PathBuilder {
+    public PathBuilder setTangentHeadingInterpolation;
     private ArrayList<Path> paths = new ArrayList<>();
 
     private ArrayList<PathCallback> callbacks = new ArrayList<>();

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Teleop.java

@@ -1,89 +0,0 @@
-//package org.firstinspires.ftc.teamcode.runmodes;
-//
-//import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR;
-//import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION;
-//import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR;
-//import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR_DIRECTION;
-//import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR;
-//import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION;
-//import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR;
-//import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION;
-//
-//import com.qualcomm.robotcore.hardware.DcMotor;
-//import com.qualcomm.robotcore.hardware.DcMotorEx;
-//import com.qualcomm.robotcore.hardware.Gamepad;
-//import com.qualcomm.robotcore.hardware.HardwareMap;
-//
-//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.subsystem.ClawSubsystem;
-//import org.firstinspires.ftc.teamcode.util.action.RunAction;
-//
-//public class Teleop {
-//
-//    private ClawSubsystem claw;
-//    private Follower follower;
-//    private DcMotorEx leftFront;
-//    private DcMotorEx leftRear;
-//    private DcMotorEx rightFront;
-//    private DcMotorEx rightRear;
-//    private Telemetry telemetry;
-//
-//    private Gamepad gamepad1;
-//    private Gamepad currentGamepad1;
-//    private Gamepad previousGamepad1;
-//
-//    public Teleop(HardwareMap hardwareMap, Telemetry telemetry, Follower follower, Gamepad gamepad1) {
-//
-//        claw = new ClawSubsystem(hardwareMap, telemetry);
-//        initMotors(hardwareMap);
-//
-//        this.follower = follower;
-//
-//        this.telemetry = telemetry;
-//        this.gamepad1 = gamepad1;
-//
-//        this.currentGamepad1 = new Gamepad();
-//        this.previousGamepad1 = new Gamepad();
-//    }
-//
-//    public void start() {
-//        claw.start();
-//        follower.startTeleopDrive();
-//    }
-//
-//    public void update() {
-//        previousGamepad1.copy(currentGamepad1);
-//        currentGamepad1.copy(gamepad1);
-//
-//        if (currentGamepad1.a && !previousGamepad1.a)
-//            claw.switchState();
-//
-//        follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x);
-//        follower.update();
-//
-//        telemetry.addData("X", follower.getPose().getX());
-//        telemetry.addData("Y", follower.getPose().getY());
-//        telemetry.addData("Heading", Math.toDegrees(follower.getPose().getHeading()));
-//        telemetry.addData("Claw State", claw.getState());
-//        telemetry.update();
-//    }
-//
-//    private void initMotors(HardwareMap hardwareMap) {
-//        rightFront = hardwareMap.get(DcMotorEx.class, FRONT_RIGHT_MOTOR);
-//        rightRear = hardwareMap.get(DcMotorEx.class, BACK_RIGHT_MOTOR);
-//        leftFront = hardwareMap.get(DcMotorEx.class, FRONT_LEFT_MOTOR);
-//        leftRear = hardwareMap.get(DcMotorEx.class, BACK_LEFT_MOTOR);
-//
-//        rightFront.setDirection(FRONT_RIGHT_MOTOR_DIRECTION);
-//        rightRear.setDirection(BACK_RIGHT_MOTOR_DIRECTION);
-//        leftFront.setDirection(FRONT_LEFT_MOTOR_DIRECTION);
-//        leftRear.setDirection(BACK_LEFT_MOTOR_DIRECTION);
-//
-//        leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-//        leftRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-//        rightRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-//        rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-//    }
-//}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine1.java

@@ -0,0 +1,36 @@
+package org.firstinspires.ftc.teamcode.subsystem;
+
+
+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.PathBuilder;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
+
+
+/*
+    AutoLine# - This file does something of a path......
+
+ */
+public class AutoLine1 {
+
+    private Pose startPose = new Pose(36, 72);
+
+    public void moveToPath1(Follower robot) {
+        PathChain pathChain;
+        robot.setStartingPose(startPose);
+        PathBuilder builder = new PathBuilder();
+        builder
+                .addPath(
+                        new BezierCurve(
+                                new Point(36.000, 72.000, Point.CARTESIAN),
+                                new Point(37.500, 72.000, Point.CARTESIAN)
+                        )
+                );
+        pathChain = builder.build();
+        robot.followPath(pathChain);
+    }
+
+}
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoPark.java

@@ -0,0 +1,36 @@
+package org.firstinspires.ftc.teamcode.subsystem;
+
+
+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.PathBuilder;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
+
+
+/*
+    AutoLine# - This file does something of a path......
+
+ */
+public class AutoPark {
+
+
+    public void moveToPark(Follower robot) {
+        PathChain pathChain;
+        PathBuilder builder = new PathBuilder();
+        builder
+                .addPath(
+                        new BezierCurve(
+                                new Point(18.000, 126.000, Point.CARTESIAN),
+                                new Point(85.000, 132.750, Point.CARTESIAN),
+                                new Point(84.000, 97.000, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(135));
+        pathChain = builder.build();
+        robot.followPath(pathChain);
+    }
+
+}
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawActionsSubsystem.java

@@ -72,7 +72,7 @@ public class ClawActionsSubsystem {
     }
 
     public void start() {
-        Actions.runBlocking(closeClaw());
+        Actions.runBlocking(openClaw());
     }
 
     public double getPosition() {

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath1.java

@@ -0,0 +1,38 @@
+package org.firstinspires.ftc.teamcode.subsystem;
+
+
+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.PathBuilder;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
+
+
+/*
+    AutoLine# - This file does something of a path......
+
+ */
+public class HighBasketAutoPath1 {
+
+    private final Pose startPose = new Pose(8, 89);
+
+    public void moveToPath1(Follower robot) {
+        PathChain pathChain;
+        robot.setStartingPose(startPose);
+        PathBuilder builder = new PathBuilder();
+        builder
+                .addPath(
+                        new BezierCurve(
+                                new Point(8.000, 89.000, Point.CARTESIAN),
+                                new Point(24.000, 96.000, Point.CARTESIAN),
+                                new Point(18.000, 126.000, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135));
+        pathChain = builder.build();
+        robot.followPath(pathChain);
+    }
+
+}
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath2.java

@@ -0,0 +1,36 @@
+package org.firstinspires.ftc.teamcode.subsystem;
+
+
+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.PathBuilder;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
+
+
+/*
+    AutoLine# - This file does something of a path......
+
+ */
+public class HighBasketAutoPath2 {
+    
+    public void moveToPath2(Follower robot) {
+        PathChain pathChain;
+        PathBuilder builder = new PathBuilder();
+        builder
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(18.000, 126.000, Point.CARTESIAN),
+                                new Point(25.500, 120.000, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0));
+        pathChain = builder.build();
+        robot.followPath(pathChain);
+    }
+
+}
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath3.java

@@ -0,0 +1,35 @@
+package org.firstinspires.ftc.teamcode.subsystem;
+
+
+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.PathBuilder;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
+
+
+/*
+    AutoLine# - This file does something of a path......
+
+ */
+public class HighBasketAutoPath3 {
+
+    public void moveToBasketPath3(Follower robot) {
+        PathChain pathChain;
+        PathBuilder builder = new PathBuilder();
+        builder
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(29.000, 120.000, Point.CARTESIAN),
+                                new Point(18.000, 126.000, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135));
+        pathChain = builder.build();
+        robot.followPath(pathChain);
+    }
+
+}
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath4.java

@@ -0,0 +1,34 @@
+package org.firstinspires.ftc.teamcode.subsystem;
+
+
+import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
+
+
+/*
+    AutoLine# - This file does something of a path......
+
+ */
+public class HighBasketAutoPath4 {
+    
+    public void moveToPickupAgainPath4(Follower robot) {
+        PathChain pathChain;
+        PathBuilder builder = new PathBuilder();
+        builder
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(18.000, 126.000, Point.CARTESIAN),
+                                new Point(25.000, 129.900, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0));
+        pathChain = builder.build();
+        robot.followPath(pathChain);
+    }
+
+}
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath5.java

@@ -0,0 +1,34 @@
+package org.firstinspires.ftc.teamcode.subsystem;
+
+
+import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
+
+
+/*
+    AutoLine# - This file does something of a path......
+
+ */
+public class HighBasketAutoPath5 {
+    
+    public void moveToPickupAgainPath5(Follower robot) {
+        PathChain pathChain;
+        PathBuilder builder = new PathBuilder();
+        builder
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(24.000, 131.000, Point.CARTESIAN),
+                                new Point(18.000, 126.000, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135));
+        pathChain = builder.build();
+        robot.followPath(pathChain);
+    }
+
+}
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath6.java

@@ -0,0 +1,50 @@
+package org.firstinspires.ftc.teamcode.subsystem;
+
+
+import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
+
+
+/*
+    AutoLine# - This file does something of a path......
+
+ */
+public class HighBasketAutoPath6 {
+    
+    public void moveToParkPath6(Follower robot) {
+        PathChain pathChain;
+        PathBuilder builder = new PathBuilder();
+        builder
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(18.000, 126.000, Point.CARTESIAN),
+                                new Point(18.000, 126.000, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(270))
+                .addPath(
+                        // Line 2
+                        new BezierLine(
+                                new Point(18.000, 126.000, Point.CARTESIAN),
+                                new Point(81.000, 125.000, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(270), Math.toRadians(270))
+                .addPath(
+                        // Line 3
+                        new BezierLine(
+                                new Point(81.000, 125.000, Point.CARTESIAN),
+                                new Point(81.000, 100.000, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(270), Math.toRadians(270));
+        pathChain = builder.build();
+        robot.followPath(pathChain);
+    }
+
+}
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftActionsSubsystem.java

@@ -1,10 +1,14 @@
 package org.firstinspires.ftc.teamcode.subsystem;
 
 import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_NAME;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.dropToHighRung;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftPower;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToFloorPos;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighBucketPos;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToLowBucketPos;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighRung;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighRungAttach;
+
 
 import androidx.annotation.NonNull;
 
@@ -20,7 +24,7 @@ public class LiftActionsSubsystem {
     public DcMotor lift;
 
     public enum LiftState {
-        FLOOR, LOW_BUCKET, HIGH_BUCKET, FLOAT, SUBMARINE
+        FLOOR, LOW_BUCKET, HIGH_BUCKET, FLOAT, SUBMARINE, HIGH_RUNG, HIGH_RUNG_DROP
     }
 
     private LiftState liftState;
@@ -55,8 +59,27 @@ public class LiftActionsSubsystem {
         return new MoveToPosition(liftToFloorPos, LiftState.FLOOR);
     }
 
+    public Action toHighRung() {
+        return new MoveToPosition(liftToHighRung, LiftState.HIGH_RUNG);
+    }
+
+    public Action dropToHighRung() {
+        return new MoveToPosition(dropToHighRung, LiftState.HIGH_RUNG_DROP);
+    }
+
+
+    public Action toZeroPosition() {
+        return new MoveToPosition(0, LiftState.FLOOR);
+    }
+
+
+    public Action toHighRungAttach() {
+        return new MoveToPosition(liftToHighRungAttach, LiftState.HIGH_RUNG);
+    }
+
     public Action toLowBucketPosition() {
         return new MoveToPosition(liftToLowBucketPos, LiftState.LOW_BUCKET);
+
     }
 
     public Action toHighBucketPosition() {

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/SkyHookSubsystem.java

@@ -0,0 +1,52 @@
+package org.firstinspires.ftc.teamcode.subsystem;
+
+import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER;
+import static org.firstinspires.ftc.teamcode.PedroConstants.SKYHOOK_NAME;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftPower;
+
+import com.acmerobotics.roadrunner.Action;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
+
+public class SkyHookSubsystem {
+
+    public DcMotorEx hook;
+
+    public SkyHookSubsystem(HardwareMap hardwareMap) {
+        hook = hardwareMap.get(DcMotorEx.class, SKYHOOK_NAME);
+
+    }
+
+    public void moveSkyHook(int position){
+        hook.setTargetPosition(position);
+        hook.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+    }
+
+    public void moveSkyHook(double power) {
+        hook.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        hook.setPower(power);
+    }
+
+    public void init() {
+        hook.setPower(0);
+        hook.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        hook.setTargetPosition(0);
+    }
+
+    public void toParkPosition() {
+        moveSkyHook(0);
+    }
+
+    public void toLevel1Position() {
+        moveSkyHook(1500);
+    }
+
+    public int getHookPosition() {
+        return hook.getCurrentPosition();
+    }
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristActionsSubsystem.java

@@ -1,7 +1,13 @@
 package org.firstinspires.ftc.teamcode.subsystem;
 
+import static org.firstinspires.ftc.teamcode.PedroConstants.WRIST_NAME;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristBucket;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristFloor;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristPickup;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristRung;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristSpeciemen;
+
+
 
 import androidx.annotation.NonNull;
 
@@ -14,14 +20,14 @@ import com.qualcomm.robotcore.hardware.ServoImplEx;
 public class WristActionsSubsystem {
 
     public enum WristState {
-        FLOOR, BUCKET
+        FLOOR, BUCKET, PICKUP, RUNG, SPECIMEN
     }
 
     public ServoImplEx wrist;
     public WristState state;
 
     public WristActionsSubsystem(HardwareMap hardwareMap) {
-        this.wrist = hardwareMap.get(ServoImplEx.class, "wrist-servo");
+        this.wrist = hardwareMap.get(ServoImplEx.class, WRIST_NAME);
     }
 
     public class MoveToPosition implements Action {
@@ -42,7 +48,14 @@ public class WristActionsSubsystem {
         }
     }
 
-    public Action toFloorPosition() {
+    public Action   toFloorPosition() {
+        return new MoveToPosition(wristFloor, WristState.FLOOR);
+    }
+    public Action toSpeciemenBar() {
+        return new MoveToPosition(wristSpeciemen, WristState.SPECIMEN);
+    }
+
+    public Action toRungPosition() {
         return new MoveToPosition(wristFloor, WristState.FLOOR);
     }
 
@@ -50,6 +63,11 @@ public class WristActionsSubsystem {
         return new MoveToPosition(wristBucket, WristState.BUCKET);
     }
 
+    public Action toPickupPosition() {
+        return new MoveToPosition(wristPickup, WristState.PICKUP);
+    }
+
+
     public void setState(WristState wristState) {
         this.state = wristState;
     }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/Action.java

@@ -1,11 +0,0 @@
-package org.firstinspires.ftc.teamcode.util.action;
-
-import com.acmerobotics.dashboard.canvas.Canvas;
-import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
-
-public interface Action {
-    boolean run(TelemetryPacket p);
-
-    default void preview(Canvas fieldOverlay) {
-    }
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/Actions.java

@@ -1,22 +0,0 @@
-package org.firstinspires.ftc.teamcode.util.action;
-
-import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.canvas.Canvas;
-import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
-
-public class Actions {
-
-    public static void runBlocking(Action a) {
-        FtcDashboard dash = FtcDashboard.getInstance();
-        Canvas c = new Canvas();
-        a.preview(c);
-
-        boolean b = true;
-        while (b && !Thread.currentThread().isInterrupted()) {
-            TelemetryPacket p = new TelemetryPacket();
-            p.fieldOverlay().getOperations().addAll(c.getOperations());
-            b = a.run(p);
-            dash.sendTelemetryPacket(p);
-        }
-    }
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/FieldConstants.java

@@ -1,39 +0,0 @@
-package org.firstinspires.ftc.teamcode.util.action;
-
-import com.acmerobotics.dashboard.config.Config;
-
-import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
-
-@Config
-public class FieldConstants {
-
-    public enum RobotStart {
-        BLUE_BUCKET,
-        BLUE_OBSERVATION,
-        RED_BUCKET,
-        RED_OBSERVATION
-    }
-
-    public static final Pose blueBucketStartPose = new Pose(8, 79.5, Math.toRadians(180));
-    public static final Pose blueObservationStartPose = new Pose(8, 36, Math.toRadians(180));
-    public static final Pose redBucketStartPose = new Pose(144-8, 79.5, 0);
-    public static final Pose redObservationStartPose = new Pose(144-8, 36, 0);
-
-    // Blue Preload Poses
-    public static final Pose blueBucketPreloadPose = new Pose(34.5, 79.5, Math.toRadians(180));
-
-    // Blue Bucket Sample Poses
-    public static final Pose blueBucketLeftSamplePose = new Pose(34.75, 113.5, Math.toRadians(66));
-    public static final Pose blueBucketLeftSampleControlPose = new Pose(32, 108);
-    public static final Pose blueBucketMidSamplePose = new Pose(33, 125.5, Math.toRadians(73));
-    public static final Pose blueBucketMidSampleControlPose = new Pose(47.5, 110);
-    public static final Pose blueBucketRightSamplePose = new Pose(33, 133, Math.toRadians(74));
-    public static final Pose blueBucketRightSampleControlPose = new Pose(46, 101);
-
-    public static final Pose blueBucketScorePose = new Pose(16, 128, Math.toRadians(-45));
-
-    public static final Pose blueBucketParkPose = new Pose(65, 97.75, Math.toRadians(90));
-    public static final Pose blueBucketParkControlPose = new Pose(60.25, 123.5);
-
-
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/RunAction.java

@@ -1,30 +0,0 @@
-package org.firstinspires.ftc.teamcode.util.action;
-
-import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
-
-public class RunAction implements Action {
-
-    private final Runnable runnable;
-    private Runnable callback;
-
-    public RunAction(Runnable runnable) {
-        this.runnable = runnable;
-    }
-
-    public void runAction() {
-        runnable.run();
-        if (callback != null) {
-            callback.run();
-        }
-    }
-
-    public void setCallback(Runnable callback) {
-        this.callback = callback;
-    }
-
-    // Adapter to make Action compatible with the Action interface
-    public boolean run(TelemetryPacket p) {
-        runAction();
-        return false; // Regular actions complete after one execution
-    }
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/SequentialAction.java

@@ -1,43 +0,0 @@
-package org.firstinspires.ftc.teamcode.util.action;
-
-import com.acmerobotics.dashboard.canvas.Canvas;
-import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
-
-import java.util.ArrayList;
-import java.util.Arrays;
-import java.util.List;
-
-public class SequentialAction implements Action {
-    private List<Action> actions;
-
-    public SequentialAction(List<Action> actions) {
-        this.actions = new ArrayList<>(actions);
-    }
-
-    public SequentialAction(Action... actions) {
-        this(Arrays.asList(actions));
-    }
-
-    @Override
-    public boolean run(TelemetryPacket p) {
-        if (actions.isEmpty()) {
-            return false;
-        }
-
-        if (actions.get(0).run(p)) {
-            return true;
-        } else {
-            actions.remove(0);
-            return run(p);
-        }
-    }
-
-    @Override
-    public void preview(Canvas fieldOverlay) {
-        for (Action a : actions) {
-            a.preview(fieldOverlay);
-        }
-    }
-
-
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/SleepAction.java

@@ -1,31 +0,0 @@
-package org.firstinspires.ftc.teamcode.util.action;
-
-import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
-
-public class SleepAction implements Action {
-    private double dt;
-    private double beginTs = -1.0;
-
-    public SleepAction(double dt) {
-        this.dt = dt;
-    }
-
-    public static double now() {
-        return System.nanoTime() * 1e-9;
-    }
-
-    @Override
-    public boolean run(TelemetryPacket p) {
-        double t;
-        if (beginTs < 0) {
-            beginTs = now();
-            t = 0.0;
-        } else {
-            t = now() - beginTs;
-        }
-        boolean output = t < dt;
-        System.out.println(t + ":" + now() + ":" + beginTs + ":" + output);
-
-        return t < dt;
-    }
-}