Fixed naming convention

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

@@ -10,10 +10,11 @@ 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;
 
-@Autonomous(name = "Blue Basket Auto", group = "Competition")
+@Autonomous(name = "Auto Test", group = "Dev")
 public class BlueBasketAuto extends OpMode {
     private Telemetry telemetryA;
 
@@ -21,13 +22,13 @@ public class BlueBasketAuto extends OpMode {
 
     private PathChain path;
 
-    private final Pose startPose = new Pose(11.25, 95.75);
+    private final Pose startPose = new Pose(8, 65);
 
     @Override
     public void init() {
         follower = new Follower(hardwareMap);
 
-        follower.setMaxPower(.45);
+        follower.setMaxPower(.75);
 
         follower.setStartingPose(startPose);
 
@@ -35,99 +36,158 @@ public class BlueBasketAuto extends OpMode {
                 .addPath(
                         // Line 1
                         new BezierLine(
-                                new Point(11.250, 95.750, Point.CARTESIAN),
+                                new Point(8.000, 65.000, Point.CARTESIAN),
-                                new Point(37.000, 108.000, Point.CARTESIAN)
+                                new Point(33.000, 65.000, Point.CARTESIAN)
                         )
                 )
                 .setConstantHeadingInterpolation(Math.toRadians(0))
                 .addPath(
                         // Line 2
-                        new BezierCurve(
+                        new BezierLine(
-                                new Point(37.000, 108.000, Point.CARTESIAN),
+                                new Point(33.000, 65.000, Point.CARTESIAN),
-                                new Point(73.286, 111.536, Point.CARTESIAN),
+                                new Point(31.000, 65.000, Point.CARTESIAN)
-                                new Point(67.821, 120.536, Point.CARTESIAN)
                         )
                 )
                 .setConstantHeadingInterpolation(Math.toRadians(0))
                 .addPath(
                         // Line 3
-                        new BezierLine(
+                        new BezierCurve(
-                                new Point(67.821, 120.536, Point.CARTESIAN),
+                                new Point(31.000, 65.000, Point.CARTESIAN),
-                                new Point(28.000, 121.500, Point.CARTESIAN)
+                                new Point(26.000, 32.000, Point.CARTESIAN),
+                                new Point(60.000, 34.000, Point.CARTESIAN)
                         )
                 )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180))
                 .addPath(
                         // Line 4
                         new BezierLine(
-                                new Point(28.000, 121.500, Point.CARTESIAN),
+                                new Point(60.000, 34.000, Point.CARTESIAN),
-                                new Point(18.000, 130.179, Point.CARTESIAN)
+                                new Point(60.000, 24.000, Point.CARTESIAN)
                         )
                 )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .setConstantHeadingInterpolation(Math.toRadians(180))
                 .addPath(
                         // Line 5
-                        new BezierCurve(
+                        new BezierLine(
-                                new Point(18.000, 130.179, Point.CARTESIAN),
+                                new Point(60.000, 24.000, Point.CARTESIAN),
-                                new Point(59.000, 102.500, Point.CARTESIAN),
+                                new Point(14.500, 24.000, Point.CARTESIAN)
-                                new Point(68.700, 130.500, Point.CARTESIAN)
                         )
                 )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .setConstantHeadingInterpolation(Math.toRadians(180))
                 .addPath(
                         // Line 6
                         new BezierLine(
-                                new Point(68.700, 130.500, Point.CARTESIAN),
+                                new Point(14.500, 24.000, Point.CARTESIAN),
-                                new Point(18.000, 130.339, Point.CARTESIAN)
+                                new Point(18.000, 24.000, Point.CARTESIAN)
                         )
                 )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .setConstantHeadingInterpolation(Math.toRadians(180))
                 .addPath(
-                        // Line 7
+                        // Line 1
                         new BezierCurve(
-                                new Point(18.000, 130.339, Point.CARTESIAN),
+                                new Point(18.000, 24.000, Point.CARTESIAN),
-                                new Point(49.018, 121.179, Point.CARTESIAN),
+                                new Point(18.000, 67.000, Point.CARTESIAN),
-                                new Point(63.804, 135.321, Point.CARTESIAN)
+                                new Point(31.000, 67.000, Point.CARTESIAN)
                         )
                 )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .setTangentHeadingInterpolation()
                 .addPath(
                         // Line 8
                         new BezierLine(
-                                new Point(63.804, 135.321, Point.CARTESIAN),
+                                new Point(31.000, 67.500, Point.CARTESIAN),
-                                new Point(53.036, 135.161, Point.CARTESIAN)
+                                new Point(33.000, 67.500, Point.CARTESIAN)
                         )
                 )
                 .setConstantHeadingInterpolation(Math.toRadians(0))
                 .addPath(
                         // Line 9
                         new BezierLine(
-                                new Point(53.036, 135.161, Point.CARTESIAN),
+                                new Point(33.000, 67.500, Point.CARTESIAN),
-                                new Point(18.643, 135.000, Point.CARTESIAN)
+                                new Point(31.000, 67.500, Point.CARTESIAN)
                         )
                 )
                 .setConstantHeadingInterpolation(Math.toRadians(0))
                 .addPath(
                         // Line 10
-                        new BezierLine(
+                        new BezierCurve(
-                                new Point(18.643, 135.000, Point.CARTESIAN),
+                                new Point(31.000, 67.500, Point.CARTESIAN),
-                                new Point(72.300, 97.400, 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(18.643, 135.000, Point.CARTESIAN),
+                                new Point(60.000, 34.000, Point.CARTESIAN),
-                                new Point(83.250, 95.464, Point.CARTESIAN)
+                                new Point(60.000, 12.000, Point.CARTESIAN)
                         )
                 )
-                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270)).build();
+                .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();

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

@@ -0,0 +1,195 @@
+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/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/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 FRONT_LEFT_MOTOR = "front-left";
-    public static final String BACK_LEFT_MOTOR = "Drive back lt";
+    public static final String BACK_LEFT_MOTOR = "back-left";
-    public static final String FRONT_RIGHT_MOTOR = "Drive front rt";
+    public static final String FRONT_RIGHT_MOTOR = "front-right";
-    public static final String BACK_RIGHT_MOTOR = "Drive back rt";
+    public static final String BACK_RIGHT_MOTOR = "back-right";
 
     /*
         Motor directions
@@ -26,12 +26,12 @@ public class PedroConstants {
     /*
         Centricity : true is robot-centric movement; false if field-centric movement
      */
-    public static final boolean CENTRICITY = false;
+    public static final boolean CENTRICITY = true;
 
     /*
         Motor Max Power
      */
-    public static final double MAX_POWER = .50;
+    public static final double MAX_POWER = .675;
 
     /*
         IMU
@@ -45,9 +45,9 @@ public class PedroConstants {
     /*
         Dead wheels
      */
-    public static final String LEFT_ENCODER = "encoder left";
+    public static final String RIGHT_ENCODER = "back-right";
-    public static final String RIGHT_ENCODER = "encoder right";
+    public static final String BACK_ENCODER = "front-right";
-    public static final String BACK_ENCODER = "encoder back";
+    public static final String LEFT_ENCODER = "front-left";
 
     /*
         Dead wheel directions
@@ -76,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

@@ -13,6 +13,7 @@ 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;
@@ -35,7 +36,7 @@ public class PreLoadedBlueBasketAuto extends OpMode {
                 .addPath(
                         // Line 1
                         new BezierLine(
-                                new Point(8.036, 89.196, Point.CARTESIAN),
+                                new Point(8, 89, Point.CARTESIAN),
                                 new Point(10.125, 126.804, Point.CARTESIAN)
                         )
                 )
@@ -87,15 +88,15 @@ public class PreLoadedBlueBasketAuto extends OpMode {
                         // Line 7
                         new BezierLine(
                                 new Point(59.625, 126.964, Point.CARTESIAN),
-                                new Point(57.857, 133.071, Point.CARTESIAN)
+                                new Point(57.857, 131.071, Point.CARTESIAN)
                         )
                 )
                 .setConstantHeadingInterpolation(Math.toRadians(0))
                 .addPath(
                         // Line 8
                         new BezierLine(
-                                new Point(57.857, 133.071, Point.CARTESIAN),
+                                new Point(57.857, 131.071, Point.CARTESIAN),
-                                new Point(18.964, 134.679, Point.CARTESIAN)
+                                new Point(18.964, 131.679, Point.CARTESIAN)
                         )
                 )
                 .setConstantHeadingInterpolation(Math.toRadians(0))
@@ -118,5 +119,6 @@ public class PreLoadedBlueBasketAuto extends OpMode {
     public void loop() {
         follower.update();
         follower.telemetryDebug(telemetryA);
+
     }
 }

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

@@ -1,5 +1,6 @@
 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,6 +11,7 @@ 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;
@@ -40,6 +42,11 @@ public class CometBotAutoDevelopment {
 
     private Follower follower;
 
+    /*
+        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);
@@ -92,6 +99,7 @@ public class CometBotAutoDevelopment {
         this.telemetry.addData("Lift Position", this.lift.getPosition());
     }
 
+
     /*
     Controller: 1
     Button: A

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

@@ -104,6 +104,8 @@ public class CometBotTeleopCompetition {
         this.raiseSkyHook();
         this.lowerSkyHook();
 
+        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();
 
@@ -244,7 +246,8 @@ public class CometBotTeleopCompetition {
         if (this.currentGP2.square && !this.previousGP2.square) {
             Actions.runBlocking(new SequentialAction(
                     this.wrist.toFloorPosition(),
-                    this.arm.toParkPosition()
+                    this.arm.toParkPosition(),
+                    this.lift.toFloorPosition()
             ));
         }
     }
@@ -272,37 +275,37 @@ 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()
-                        )
-                );
-            } else if (this.arm.getState() == ArmActionsSubsystem.ArmState.FLOOR) {
                 Actions.runBlocking(
                         new SequentialAction(
+//                                this.lift.toFloorPosition(),
                                 this.arm.toSubmarinePosition(),
+                                this.wrist.toPickupPosition()
+                        )
+                );
+            } else if (this.wrist.getState() == WristActionsSubsystem.WristState.PICKUP) {
+                Actions.runBlocking(
+                        new SequentialAction(
+                                //  this.arm.toSubmarinePosition(),
                                 this.wrist.toFloorPosition()
                         )
                 );
             }
         }
     }
-    public void raiseSkyHook() {
-        if (this.currentGP2.dpad_right && !previousGP2.dpad_right) {
-            hook.raiseHook();
 
-
-        }
-
-    }
     public void lowerSkyHook() {
-        if (this.currentGP2.dpad_left && !previousGP2.dpad_left) {
+        hook.raiseHook(currentGP2.left_trigger);
-            hook.lowerHook();
+
+
+    }
+
+
+    public void raiseSkyHook() {
+        hook.lowerHook(currentGP2.right_trigger * 2.0);
 
 
     }
 
 }
-}
+

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);
@@ -96,6 +122,7 @@ public class LiftWristArmTest extends LinearOpMode {
                 lift.setPosition(lift.getPosition() - 25);
             }
 
+
             // 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/SpecimenTest.java

@@ -0,0 +1,197 @@
+/* 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.AutoLine2;
+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

@@ -10,14 +10,25 @@ public class RobotConstants {
     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.3;
+    public final static double wristFloor = 0.55;
-    public final static double wristBucket = 0;
+    public final static double wristBucket = 0.25;
+    public final static double wristRung = 0.55;
 
-    public final static int liftToFloorPos = 0;
+    public final static double wristPickup = 0.1;
-    public final static int liftToSubmarinePos = 250;
+    public final static double wristSpeciemen = 0.1;
-    public final static int liftToLowBucketPos = 2250;
+
-    public final static int liftToHighBucketPos = 3850;
+
-    public final static double liftPower = .625;
+    public final static int liftToFloorPos = 350;
+    public final static int liftToSubmarinePos = 350;
+    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/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/AutoLine2.java

@@ -0,0 +1,40 @@
+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/ClawActionsSubsystem.java

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

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

@@ -1,10 +1,18 @@
 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.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;
 
@@ -20,7 +28,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;
@@ -52,11 +60,27 @@ public class LiftActionsSubsystem {
     }
 
     public Action toFloorPosition() {
-        return new MoveToPosition(liftToFloorPos, LiftState.FLOOR);
+        return new MoveToPosition(liftToSubmarinePos, 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 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

@@ -1,6 +1,7 @@
 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;
@@ -19,21 +20,22 @@ public class SkyHookSubsystem {
     private SkyHookState skyHookState;
 
     public SkyHookSubsystem(HardwareMap hardwareMap) {
-        hook = hardwareMap.get(DcMotor.class, LEFT_ENCODER);
+        hook = hardwareMap.get(DcMotor.class, SKYHOOK_NAME);
     }
 
 
 
-   public void raiseHook(){
+   public void raiseHook(double power){
-        this.hook.setPower(1.00);
+        this.hook.setPower(power);
         this.hook.setDirection(DcMotorSimple.Direction.FORWARD);
         this.setState(SkyHookState.UP);
    }
-    public void lowerHook(){
+    public void lowerHook(double power){
-        this.hook.setPower(1.00);
+        this.hook.setPower(power);
         this.hook.setDirection(DcMotorSimple.Direction.REVERSE);
         this.setState(SkyHookState.DOWN);
     }
+
     public void init() {
         hook.setPower(0);
         hook.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);

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 {
@@ -45,11 +51,23 @@ public class WristActionsSubsystem {
     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);
+    }
 
     public Action toBucketPosition() {
         return new MoveToPosition(wristBucket, WristState.BUCKET);
     }
 
+    public Action toPickupPosition() {
+        return new MoveToPosition(wristPickup, WristState.PICKUP);
+    }
+
+
     public void setState(WristState wristState) {
         this.state = wristState;
     }