Fine tuning to autonomous prior to switching to new robot

overtime

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

@@ -2,10 +2,15 @@ package org.firstinspires.ftc.teamcode;
 
 import com.acmerobotics.dashboard.FtcDashboard;
 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;
@@ -13,184 +18,98 @@ 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;
 
-@Autonomous(name = "Auto Test", group = "Dev")
+
+@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(8, 65);
+    private LiftActionsSubsystem liftActionsSubsystem;
 
     @Override
     public void init() {
         follower = new Follower(hardwareMap);
-
         follower.setMaxPower(.75);
+        path1    = new HighBasketAutoPath1();
+        path2    = new HighBasketAutoPath2();
+        pathPark = new AutoPark();
 
-        follower.setStartingPose(startPose);
+        comp = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2);
+        comp.initCloseClaw();
+        runtime = new ElapsedTime();
+        hook = new SkyHookSubsystem(hardwareMap);
+        state = 0;
 
-        path = follower.pathBuilder()
-                .addPath(
-                        // Line 1
-                        new BezierLine(
-                                new Point(8.000, 65.000, Point.CARTESIAN),
-                                new Point(33.000, 65.000, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
-                .addPath(
-                        // Line 2
-                        new BezierLine(
-                                new Point(33.000, 65.000, Point.CARTESIAN),
-                                new Point(31.000, 65.000, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
-                .addPath(
-                        // Line 3
-                        new BezierCurve(
-                                new Point(31.000, 65.000, Point.CARTESIAN),
-                                new Point(26.000, 32.000, Point.CARTESIAN),
-                                new Point(60.000, 34.000, Point.CARTESIAN)
-                        )
-                )
-                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180))
-                .addPath(
-                        // Line 4
-                        new BezierLine(
-                                new Point(60.000, 34.000, Point.CARTESIAN),
-                                new Point(60.000, 24.000, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(180))
-                .addPath(
-                        // Line 5
-                        new BezierLine(
-                                new Point(60.000, 24.000, Point.CARTESIAN),
-                                new Point(14.500, 24.000, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(180))
-                .addPath(
-                        // Line 6
-                        new BezierLine(
-                                new Point(14.500, 24.000, Point.CARTESIAN),
-                                new Point(18.000, 24.000, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(180))
-                .addPath(
-                        // Line 1
-                        new BezierCurve(
-                                new Point(18.000, 24.000, Point.CARTESIAN),
-                                new Point(18.000, 67.000, Point.CARTESIAN),
-                                new Point(31.000, 67.000, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 8
-                        new BezierLine(
-                                new Point(31.000, 67.500, Point.CARTESIAN),
-                                new Point(33.000, 67.500, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
-                .addPath(
-                        // Line 9
-                        new BezierLine(
-                                new Point(33.000, 67.500, Point.CARTESIAN),
-                                new Point(31.000, 67.500, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
-                .addPath(
-                        // Line 10
-                        new BezierCurve(
-                                new Point(31.000, 67.500, Point.CARTESIAN),
-                                new Point(26.000, 32.000, Point.CARTESIAN),
-                                new Point(60.000, 34.000, Point.CARTESIAN)
-                        )
-                )
-                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180))
-                .addPath(
-                        // Line 11
-                        new BezierLine(
-                                new Point(60.000, 34.000, Point.CARTESIAN),
-                                new Point(60.000, 12.000, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(180))
-                .addPath(
-                        // Line 12
-                        new BezierLine(
-                                new Point(60.000, 12.000, Point.CARTESIAN),
-                                new Point(12.000, 12.000, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(180))
-                .addPath(
-                        // Line 13
-                        new BezierLine(
-                                new Point(12.000, 12.000, Point.CARTESIAN),
-                                new Point(20.000, 12.000, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(180))
-                .addPath(
-                        // Line 14
-                        new BezierLine(
-                                new Point(20.000, 12.000, Point.CARTESIAN),
-                                new Point(20.000, 24.000, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(180))
-                .addPath(
-                        // Line 15
-                        new BezierLine(
-                                new Point(20.000, 24.000, Point.CARTESIAN),
-                                new Point(14.000, 24.000, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(180))
-                .addPath(
-                        // Line 16
-                        new BezierCurve(
-                                new Point(14.000, 24.000, Point.CARTESIAN),
-                                new Point(14.000, 70.000, Point.CARTESIAN),
-                                new Point(31.000, 70.000, Point.CARTESIAN)
-                        )
-                )
-                .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(0))
-                .addPath(
-                        // Line 17
-                        new BezierLine(
-                                new Point(31.000, 70.000, Point.CARTESIAN),
-                                new Point(33.000, 70.000, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
-                .addPath(
-                        // Line 18
-                        new BezierLine(
-                                new Point(33.000, 70.000, Point.CARTESIAN),
-                                new Point(31.000, 70.000, Point.CARTESIAN)
-                        )
-                )
-                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180)).build();
-        follower.followPath(path);
-
-        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
-        telemetryA.update();
     }
 
 
     @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/BlueBasketAutoWithDrop.java

@@ -1,195 +0,0 @@
-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 = "Auto Test", group = "Dev")
-public class BlueBasketAutoWithDrop extends OpMode {
-    private Telemetry telemetryA;
-
-    private Follower follower;
-
-    private PathChain path;
-
-    private final Pose startPose = new Pose(8, 65);
-
-    @Override
-    public void init() {
-        follower = new Follower(hardwareMap);
-
-        follower.setMaxPower(.75);
-
-        follower.setStartingPose(startPose);
-
-        path = follower.pathBuilder()
-                .addPath(
-                        // Line 1
-                        new BezierLine(
-                                new Point(8.000, 65.000, Point.CARTESIAN),
-                                new Point(33.000, 65.000, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
-                .addPath(
-                        // Line 2
-                        new BezierLine(
-                                new Point(33.000, 65.000, Point.CARTESIAN),
-                                new Point(31.000, 65.000, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
-                .addPath(
-                        // Line 3
-                        new BezierCurve(
-                                new Point(31.000, 65.000, Point.CARTESIAN),
-                                new Point(26.000, 32.000, Point.CARTESIAN),
-                                new Point(60.000, 34.000, Point.CARTESIAN)
-                        )
-                )
-                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180))
-                .addPath(
-                        // Line 4
-                        new BezierLine(
-                                new Point(60.000, 34.000, Point.CARTESIAN),
-                                new Point(60.000, 24.000, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(180))
-                .addPath(
-                        // Line 5
-                        new BezierLine(
-                                new Point(60.000, 24.000, Point.CARTESIAN),
-                                new Point(14.500, 24.000, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(180))
-                .addPath(
-                        // Line 6
-                        new BezierLine(
-                                new Point(14.500, 24.000, Point.CARTESIAN),
-                                new Point(18.000, 24.000, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(180))
-                .addPath(
-                        // Line 1
-                        new BezierCurve(
-                                new Point(18.000, 24.000, Point.CARTESIAN),
-                                new Point(18.000, 67.000, Point.CARTESIAN),
-                                new Point(31.000, 67.000, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 8
-                        new BezierLine(
-                                new Point(31.000, 67.500, Point.CARTESIAN),
-                                new Point(33.000, 67.500, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
-                .addPath(
-                        // Line 9
-                        new BezierLine(
-                                new Point(33.000, 67.500, Point.CARTESIAN),
-                                new Point(31.000, 67.500, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
-                .addPath(
-                        // Line 10
-                        new BezierCurve(
-                                new Point(31.000, 67.500, Point.CARTESIAN),
-                                new Point(26.000, 32.000, Point.CARTESIAN),
-                                new Point(60.000, 34.000, Point.CARTESIAN)
-                        )
-                )
-                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180))
-                .addPath(
-                        // Line 11
-                        new BezierLine(
-                                new Point(60.000, 34.000, Point.CARTESIAN),
-                                new Point(60.000, 12.000, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(180))
-                .addPath(
-                        // Line 12
-                        new BezierLine(
-                                new Point(60.000, 12.000, Point.CARTESIAN),
-                                new Point(12.000, 12.000, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(180))
-                .addPath(
-                        // Line 13
-                        new BezierLine(
-                                new Point(12.000, 12.000, Point.CARTESIAN),
-                                new Point(20.000, 12.000, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(180))
-                .addPath(
-                        // Line 14
-                        new BezierLine(
-                                new Point(20.000, 12.000, Point.CARTESIAN),
-                                new Point(20.000, 24.000, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(180))
-                .addPath(
-                        // Line 15
-                        new BezierLine(
-                                new Point(20.000, 24.000, Point.CARTESIAN),
-                                new Point(14.000, 24.000, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(180))
-                .addPath(
-                        // Line 16
-                        new BezierCurve(
-                                new Point(14.000, 24.000, Point.CARTESIAN),
-                                new Point(14.000, 70.000, Point.CARTESIAN),
-                                new Point(31.000, 70.000, Point.CARTESIAN)
-                        )
-                )
-                .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(0))
-                .addPath(
-                        // Line 17
-                        new BezierLine(
-                                new Point(31.000, 70.000, Point.CARTESIAN),
-                                new Point(33.000, 70.000, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
-                .addPath(
-                        // Line 18
-                        new BezierLine(
-                                new Point(33.000, 70.000, Point.CARTESIAN),
-                                new Point(31.000, 70.000, Point.CARTESIAN)
-                        )
-                )
-                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180)).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/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/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/PedroConstants.java

@@ -31,7 +31,7 @@ public class PedroConstants {
     /*
         Motor Max Power
      */
-    public static final double MAX_POWER = .675;
+    public static final double MAX_POWER = .8;
 
     /*
         IMU

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

@@ -88,12 +88,24 @@ public class CometBotTeleopCompetition {
         follower.startTeleopDrive();
     }
 
+    public void initCloseClaw(){
+        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 update() {
         this.previousGP1.copy(currentGP1);
         this.currentGP1.copy(this.GP1);
         this.previousGP2.copy(currentGP2);
         this.currentGP2.copy(this.GP2);
-
+        this.moveSkyHook();
         this.toHighBucketScore();
         this.toLowBucketScore();
         this.toArmParkPosition();
@@ -101,8 +113,7 @@ public class CometBotTeleopCompetition {
         this.clawControl();
         this.decreaseMaxPower();
         this.increaseMaxPower();
-        this.raiseSkyHook();
-        this.lowerSkyHook();
+
 
         Actions.runBlocking(this.lift.toFloorPosition());
 
@@ -146,6 +157,18 @@ public class CometBotTeleopCompetition {
             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);
+        }
+
+    }
 
     /*
     Type: PS4 / Logitech
@@ -162,6 +185,14 @@ public class CometBotTeleopCompetition {
             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(),
@@ -178,12 +209,25 @@ public class CometBotTeleopCompetition {
                 this.lift.toFloorPosition(),
                 new SleepAction(.5)
         ));
-                fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
-                this.follower.startTeleopDrive();
     }
+    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
@@ -252,6 +296,9 @@ public class CometBotTeleopCompetition {
         }
     }
 
+
+
+
     /*
     Type: PS4
     Controller: 2
@@ -294,18 +341,16 @@ public class CometBotTeleopCompetition {
         }
     }
 
-    public void lowerSkyHook() {
-        hook.raiseHook(currentGP2.left_trigger);
+
+
+
+
+
+
+
 
 
     }
 
 
-    public void raiseSkyHook() {
-        hook.lowerHook(currentGP2.right_trigger * 2.0);
-
-
-    }
-
-}
 

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

@@ -115,11 +115,13 @@ 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);
+                }
             }
 
 

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

@@ -49,7 +49,6 @@ 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.AutoLine2;
 import org.firstinspires.ftc.teamcode.subsystem.ClawActionsSubsystem;
 import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
 import org.firstinspires.ftc.teamcode.subsystem.WristActionsSubsystem;

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

@@ -4,8 +4,8 @@ import com.acmerobotics.dashboard.config.Config;
 
 @Config
 public class RobotConstants {
-    public final static double clawOpen = 0.5;
-    public final static double clawClose = 0.05;
+    public final static double clawClose = 0.8;
+    public final static double clawOpen = 0.15;
 
     public final static double armFloor = 0.7;
     public final static double armSubmarine = 0.55;
@@ -20,8 +20,7 @@ public class RobotConstants {
     public final static double wristSpeciemen = 0.1;
 
 
-    public final static int liftToFloorPos = 350;
-    public final static int liftToSubmarinePos = 350;
+    public final static int liftToFloorPos = 500;
     public final static int liftToLowBucketPos = 2650;
     public final static int liftToHighRung = 2100;
     public final static int dropToHighRung = 1675;

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/subsystem/AutoLine2.java

@@ -1,40 +0,0 @@
-package org.firstinspires.ftc.teamcode.subsystem;
-
-import androidx.annotation.NonNull;
-
-import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
-import com.acmerobotics.roadrunner.Action;
-
-import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
-import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
-
-
-/*
-    AutoLine# - This file does something of a path......
-
- */
-public class AutoLine2 {
-
-
-    private PathChain pathChain;
-
-    private PathChain goToStore;
-
-    private Pose startPose = new Pose(37.5, 72);
-
-    public AutoLine2(Follower robot) {
-        robot.setStartingPose(startPose);
-        pathChain  = robot.pathBuilder().addPath(
-                new BezierLine(
-                        new Point(37.500, 72.000, Point.CARTESIAN),
-                        new Point(36.000, 72.000, Point.CARTESIAN)
-                )
-        ).setConstantHeadingInterpolation(Math.toRadians(0)).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

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

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

@@ -6,12 +6,8 @@ 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.liftToSubmarinePos;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighRung;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighRungAttach;
-import static org.firstinspires.ftc.teamcode.configs.RobotConstants.dropToHighRung;
-
-
 
 
 import androidx.annotation.NonNull;
@@ -60,7 +56,7 @@ public class LiftActionsSubsystem {
     }
 
     public Action toFloorPosition() {
-        return new MoveToPosition(liftToSubmarinePos, LiftState.FLOOR);
+        return new MoveToPosition(liftToFloorPos, LiftState.FLOOR);
     }
 
     public Action toHighRung() {
@@ -72,12 +68,15 @@ public class LiftActionsSubsystem {
     }
 
 
-
+    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);
 

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

@@ -6,49 +6,47 @@ 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 DcMotor hook;
-
-    public enum SkyHookState {
-        UP, DOWN, STOP
-    }
-
-    private SkyHookState skyHookState;
+    public DcMotorEx hook;
 
     public SkyHookSubsystem(HardwareMap hardwareMap) {
-        hook = hardwareMap.get(DcMotor.class, SKYHOOK_NAME);
+        hook = hardwareMap.get(DcMotorEx.class, SKYHOOK_NAME);
+
     }
 
-
-
-   public void raiseHook(double power){
-        this.hook.setPower(power);
-        this.hook.setDirection(DcMotorSimple.Direction.FORWARD);
-        this.setState(SkyHookState.UP);
+    public void moveSkyHook(int position){
+        hook.setTargetPosition(position);
+        hook.setMode(DcMotor.RunMode.RUN_TO_POSITION);
     }
-    public void lowerHook(double power){
-        this.hook.setPower(power);
-        this.hook.setDirection(DcMotorSimple.Direction.REVERSE);
-        this.setState(SkyHookState.DOWN);
+
+    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_WITHOUT_ENCODER);
+        hook.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        hook.setTargetPosition(0);
     }
 
-    private void setState(SkyHookState liftState) {
-        this.skyHookState = liftState;
+    public void toParkPosition() {
+        moveSkyHook(0);
     }
 
-    public SkyHookState getState() {
-        return this.skyHookState;
+    public void toLevel1Position() {
+        moveSkyHook(1500);
     }
 
-
+    public int getHookPosition() {
+        return hook.getCurrentPosition();
+    }
 
 }