hang test

# Conflicts:
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HangMotorSubsystem.java

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

@@ -0,0 +1,115 @@
+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;
+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 Competition", group = "Dev")
+public class BlueBasketAuto extends OpMode {
+    private Follower follower;
+    private int state;
+
+    private HighBasketAutoPath1 path1;
+    private HighBasketAutoPath2 path2;
+    private AutoPark pathPark;
+    private SkyHookSubsystem hook;
+    private CometBotTeleopCompetition comp;
+    private ElapsedTime runtime;
+
+    private LiftActionsSubsystem liftActionsSubsystem;
+
+    @Override
+    public void init() {
+        follower = new Follower(hardwareMap);
+        follower.setMaxPower(.75);
+        path1    = new HighBasketAutoPath1();
+        path2    = new HighBasketAutoPath2();
+        pathPark = new AutoPark();
+
+        comp = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2);
+        comp.initCloseClaw();
+        runtime = new ElapsedTime();
+        hook = new SkyHookSubsystem(hardwareMap);
+        state = 0;
+
+    }
+
+
+    @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(telemetry);
+    }
+}
+*/

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

@@ -46,7 +46,7 @@ public class PedroConstants {
     // Robot encoder direction
     public static final double LEFT_ENCODER_DIRECTION = Encoder.FORWARD;
     public static final double RIGHT_ENCODER_DIRECTION = Encoder.REVERSE;
-    public static final double BACK_ENCODER_DIRECTION = Encoder.FORWARD;
+    public static final double BACK_ENCODER_DIRECTION = Encoder.REVERSE;
 
     // Arm config
     public static final String LIFT_SLIDE_LEFT_MOTOR = "lift-slide-left";
@@ -55,6 +55,7 @@ public class PedroConstants {
     public static final String WRIST_SERVO = "wrist-servo";
     public static final String ARM_SERVO = "arm-servo";
     public static final String THUMB_SERVO = "thumb-servo";
+    public static final String HOOK = "skyhook";
 
     /*
         Pedro's parameters
@@ -64,16 +65,16 @@ public class PedroConstants {
     public static final double ROBOT_WEIGHT_IN_KG = 9;
 
     // Maximum velocity of the robot going forward
-    public static final double ROBOT_SPEED_FORWARD = 53.223;
+    public static final double ROBOT_SPEED_FORWARD = 50.02;
 
     // Maximum velocity of the robot going right
-    public static final double ROBOT_SPEED_LATERAL = 41.4081;
+    public static final double ROBOT_SPEED_LATERAL = 36.07;
 
     // Rate of deceleration when power is cut-off when the robot is moving forward
-    public static final double FORWARD_ZERO_POWER_ACCEL = -76.8421;
+    public static final double FORWARD_ZERO_POWER_ACCEL = -91.4557;
 
     // Rate of deceleration when power is cut-off when the robot is moving to the right
-    public static final double LATERAL_ZERO_POWER_ACCEL = -93.4183;
+    public static final double LATERAL_ZERO_POWER_ACCEL = -98.514;
 
     // Determines how fast your robot will decelerate as a factor of how fast your robot will coast to a stop
     public static final double ZERO_POWER_ACCEL_MULT = 4.0;

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

@@ -13,7 +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")
+@Autonomous(name = "Specimen Test", group = "Competition")
 public class PreLoadedBlueBasketAuto extends OpMode {
     private Telemetry telemetryA;
 
@@ -21,7 +21,7 @@ public class PreLoadedBlueBasketAuto extends OpMode {
 
     private PathChain path;
 
-    private final Pose startPose = new Pose(7.875, 89.357);
+    private final Pose startPose = new Pose(8, 55);
 
     @Override
     public void init() {
@@ -35,79 +35,11 @@ public class PreLoadedBlueBasketAuto extends OpMode {
                 .addPath(
                         // Line 1
                         new BezierLine(
-                                new Point(8.036, 89.196, Point.CARTESIAN),
-                                new Point(10.125, 126.804, Point.CARTESIAN)
+                                new Point(8, 55, Point.CARTESIAN),
+                                new Point(24, 21, Point.CARTESIAN)
                         )
                 )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
-                .addPath(
-                        // Line 2
-                        new BezierCurve(
-                                new Point(10.125, 126.804, Point.CARTESIAN),
-                                new Point(37.607, 90.000, Point.CARTESIAN),
-                                new Point(62.357, 119.893, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
-                .addPath(
-                        // Line 3
-                        new BezierCurve(
-                                new Point(62.357, 119.893, Point.CARTESIAN),
-                                new Point(33.750, 112.500, Point.CARTESIAN),
-                                new Point(15.107, 130.661, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
-                .addPath(
-                        // Line 4
-                        new BezierCurve(
-                                new Point(15.107, 130.661, Point.CARTESIAN),
-                                new Point(58.821, 103.018, Point.CARTESIAN),
-                                new Point(59.625, 126.964, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
-                .addPath(
-                        // Line 5
-                        new BezierLine(
-                                new Point(59.625, 126.964, Point.CARTESIAN),
-                                new Point(15.107, 130.339, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
-                .addPath(
-                        // Line 6
-                        new BezierLine(
-                                new Point(15.107, 130.339, Point.CARTESIAN),
-                                new Point(59.625, 126.964, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
-                .addPath(
-                        // Line 7
-                        new BezierLine(
-                                new Point(59.625, 126.964, Point.CARTESIAN),
-                                new Point(57.857, 133.071, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
-                .addPath(
-                        // Line 8
-                        new BezierLine(
-                                new Point(57.857, 133.071, Point.CARTESIAN),
-                                new Point(18.964, 134.679, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
-                .addPath(
-                        // Line 9
-                        new BezierCurve(
-                                new Point(18.964, 134.679, Point.CARTESIAN),
-                                new Point(84.536, 131.786, Point.CARTESIAN),
-                                new Point(80.036, 96.429, Point.CARTESIAN)
-                        )
-                )
-                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270)).build();
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(-180)).build();
         follower.followPath(path);
 
         telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());

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

@@ -0,0 +1,72 @@
+package org.firstinspires.ftc.teamcode;
+
+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.pedroPathing.follower.Follower;
+
+@Autonomous(name = "Specimen Auto", group = "Dev")
+
+public class SpecimenAuto extends OpMode {
+    private Follower follower;
+    private int state;
+    private ElapsedTime runtime;
+
+
+     public void init(){
+        state = 0;
+        follower = new Follower(hardwareMap);
+        follower.setMaxPower(.90);
+        runtime = new ElapsedTime();
+
+     }
+
+    @Override
+    public void loop() {
+         switch(state) {
+             case 0:
+                  //path 1
+                 break;
+             case 1:
+                  //specimen drop
+                 break;
+             case 2:
+                  //path 2
+                 break;
+             case 3:
+                 //specimen pick up
+                 break;
+             case 4:
+                 //path 3
+                 break;
+             case 5:
+                 //specimen drop
+                 break;
+             case 6:
+                 //path 4
+                 break;
+             case 7:
+                 //specimen pick up
+                 break;
+             case 8:
+                 //path 5
+                 break;
+             case 9:
+                 //specimen drop
+                 break;
+             case 10:
+                 //path 6
+                 break;
+             case 11:
+                 //specimen pickup
+                 break;
+             case 12:
+                 //path 7
+                 break;
+             case 13:
+                 //specimen drop
+                 break;
+         }
+    }
+}

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

@@ -0,0 +1,170 @@
+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 = "Specimen Test", group = "Competition")
+public class SpecimenAutoLineTest extends OpMode {
+    private Telemetry telemetryA;
+
+    private Follower follower;
+
+    private PathChain path;
+
+    private final Pose startPose = new Pose(8, 60);
+
+    @Override
+    public void init() {
+        follower = new Follower(hardwareMap);
+
+        follower.setMaxPower(.35);
+
+        follower.setStartingPose(startPose);
+
+
+        path = follower.pathBuilder()
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(8.000, 60.000, Point.CARTESIAN),
+                                new Point(39.500, 60.000, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180))
+                .addPath(
+                        // Line 2
+                        new BezierLine(
+                                new Point(39.500, 60.000, Point.CARTESIAN),
+                                new Point(37.000, 60.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .addPath(
+                        // Line 3
+                        new BezierCurve(
+                                new Point(37.000, 60.000, Point.CARTESIAN),
+                                new Point(16.000, 12.000, Point.CARTESIAN),
+                                new Point(80.000, 54.000, Point.CARTESIAN),
+                                new Point(58.000, 23.500, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .addPath(
+                        // Line 4
+                        new BezierLine(
+                                new Point(58.000, 23.500, Point.CARTESIAN),
+                                new Point(14.000, 23.500, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .addPath(
+                        // Line 5
+                        new BezierLine(
+                                new Point(14.000, 23.500, Point.CARTESIAN),
+                                new Point(58.000, 23.500, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .addPath(
+                        // Line 6
+                        new BezierCurve(
+                                new Point(58.000, 23.500, Point.CARTESIAN),
+                                new Point(64.000, 6.000, Point.CARTESIAN),
+                                new Point(14.000, 12.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .addPath(
+                        // Line 7
+                        new BezierCurve(
+                                new Point(14.000, 12.000, Point.CARTESIAN),
+                                new Point(60.000, 14.000, Point.CARTESIAN),
+                                new Point(60.000, 7.500, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .addPath(
+                        // Line 8
+                        new BezierLine(
+                                new Point(60.000, 7.500, Point.CARTESIAN),
+                                new Point(14.000, 7.500, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .addPath(
+                        // Line 9
+                        new BezierCurve(
+                                new Point(14.000, 7.500, Point.CARTESIAN),
+                                new Point(34.000, 14.250, Point.CARTESIAN),
+                                new Point(19.000, 24.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .addPath(
+                        // Line 10
+                        new BezierCurve(
+                                new Point(19.000, 24.000, Point.CARTESIAN),
+                                new Point(39.500, 24.000, Point.CARTESIAN),
+                                new Point(19.000, 64.000, Point.CARTESIAN),
+                                new Point(39.500, 64.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .addPath(
+                        // Line 11
+                        new BezierLine(
+                                new Point(39.500, 64.000, Point.CARTESIAN),
+                                new Point(37.000, 64.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .addPath(
+                        // Line 12
+                        new BezierCurve(
+                                new Point(37.000, 64.000, Point.CARTESIAN),
+                                new Point(19.000, 64.000, Point.CARTESIAN),
+                                new Point(37.000, 24.000, Point.CARTESIAN),
+                                new Point(19.000, 24.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .addPath(
+                        // Line 13
+                        new BezierCurve(
+                                new Point(19.000, 24.000, Point.CARTESIAN),
+                                new Point(39.500, 24.000, Point.CARTESIAN),
+                                new Point(19.000, 68.000, Point.CARTESIAN),
+                                new Point(39.500, 68.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .addPath(
+                        // Line 14
+                        new BezierLine(
+                                new Point(39.500, 68.000, Point.CARTESIAN),
+                                new Point(37.000, 68.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(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/cometbots/CometBotAutoDevelopment.java

@@ -15,8 +15,12 @@ import com.qualcomm.robotcore.hardware.HardwareMap;
 import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
 import org.firstinspires.ftc.teamcode.states.FieldStates;
+import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
 import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.HangMotorSubsystem;
 import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
 
 public class CometBotAutoDevelopment {
 
@@ -24,7 +28,10 @@ public class CometBotAutoDevelopment {
     Subsystems
      */
     private DualMotorSliderSubsystem dualSlides;
-
+    private ClawSubsystem claw;
+    private WristSubsystem wrist;
+    private ArmSubsystem arm;
+    private HangMotorSubsystem hang;
     /*
     Controllers
      */
@@ -35,10 +42,16 @@ public class CometBotAutoDevelopment {
     public Gamepad previousGamepad1;
     public Gamepad previousGamepad2;
 
+
     private Follower follower;
 
     public CometBotAutoDevelopment(HardwareMap hardwareMap, Gamepad gamepad1, Gamepad gamepad2) {
         dualSlides = new DualMotorSliderSubsystem(hardwareMap);
+        claw = new ClawSubsystem(hardwareMap);
+        arm = new ArmSubsystem(hardwareMap);
+        wrist = new WristSubsystem(hardwareMap);
+        hang = new HangMotorSubsystem(hardwareMap);
+
         this.gamepad1 = gamepad1;
         this.gamepad2 = gamepad2;
         currentGamepad1 = new Gamepad();
@@ -50,6 +63,9 @@ public class CometBotAutoDevelopment {
     
     public void init() {
         dualSlides.init();
+        claw.init();
+        wrist.initTeleOp();
+        arm.initTeleOp();
         follower.setMaxPower(MAX_POWER);
         follower.startTeleopDrive();
     }
@@ -60,25 +76,125 @@ public class CometBotAutoDevelopment {
         previousGamepad2.copy(currentGamepad2);
         currentGamepad2.copy(gamepad2);
 
-        /*
-        Check if dpad_up/down is being pressed for slides
-        */
+        //slides
         dualSlides.update();
         dualSlidesToLowBucketPosition();
         dualSlidesToHighBucketPosition();
-
+        dualSlidesToFloorPosition();
+        //arm
+        armToParkPosition();
+        armAndWristToFloor();
+        armToBucketPosition();
+        //claw
+        openClaw();
+        openThumb();
+        //hang
+        grabBlueberrySkyhook();
+        hangSkyhook();
+        robotDown();
+        robotUp();
+        stopHook();
         follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x);
         follower.update();
     }
+    private void grabBlueberrySkyhook () {
+        if(currentGamepad1.x){
+            claw.grabBlueberry();
+            wrist.grabBlueberrySkyhook();
+            arm.grabBlueberrySkyhook();
+        }
 
-    private void dualSlidesToHighBucketPosition() {
-        if (currentGamepad1.dpad_up && !previousGamepad1.dpad_up) {
-            dualSlides.toHighBucketPosition();
+        /*if(arm.getState() == ArmSubsystem.ArmState.GRAB_BLUEBERRY_SKYHOOK)
+            claw.closeClaw();*/
+    }
+    private void hangSkyhook() {
+        if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
+            dualSlides.toHangHeight();
+            //check wrist make go throught test files again
+            wrist.hangBlueberrySkyhook();
+            arm.hangBlueberrySkyhook();
+            if (arm.getState() == ArmSubsystem.ArmState.HANG_BLUEBERRY_SKYHOOK) {
+                try {
+                    Thread.sleep(2000);
+                } catch (InterruptedException e) {
+                    throw new RuntimeException(e);
+                }
+            }
+        }
+    }private void openClaw(){
+        if(currentGamepad2.right_bumper && !previousGamepad2.right_bumper){
+            claw.switchState();
+        }
+    }
+    private void openThumb(){
+        if(currentGamepad2.left_bumper && !previousGamepad2.left_bumper){
+            claw.switchTState();
+        }
+    }
+    private void armAndWristToFloor(){
+        if(currentGamepad2.a && !previousGamepad2.a){
+            double increment = 0.7 - arm.getPosition();
+            for(int i = 0; i < 3; i ++){
+                arm.setPosition(arm.getPosition() + increment);
+                try {
+                    Thread.sleep(50);
+                } catch (InterruptedException e) {
+                    throw new RuntimeException(e);
+                }
+            }
+            arm.toFloorPosition();
+        }
+        if(currentGamepad2.a && !previousGamepad2.a ){
+            wrist.switchState();
+        }
+
+
+
+    }
+    private void armToBucketPosition(){
+        if(currentGamepad2.dpad_up && !previousGamepad2.dpad_up){
+            arm.toBucketPosition();
+            wrist.toBucketPosition();
+
+        }
+    }
+    private void armToParkPosition(){
+        if(currentGamepad2.x && !previousGamepad2.x){
+            arm.toParkPosition();
+            wrist.toFloorPosition();
         }
     }
 
+    private void dualSlidesToHighBucketPosition() {
+        if (currentGamepad2.y && !previousGamepad2.y) {
+            dualSlides.toHighBucketPosition();
+        }
+    }
+    private void robotUp(){
+        if(currentGamepad1.dpad_up && !previousGamepad1.dpad_up){
+            dualSlides.toHangBelowFloor();
+            dualSlides.update();
+            hang.hangRobot();
+        }
+
+    }
+    private void robotDown(){
+        if(currentGamepad1.dpad_down && !previousGamepad1.dpad_down){
+            hang.robotToFloor();
+        }
+    }
+    private void stopHook(){
+        if(gamepad1.left_bumper){
+            hang.disableMotor();
+        }
+    }
+    private void dualSlidesToFloorPosition() {
+        if (currentGamepad2.dpad_down && !previousGamepad2.dpad_down) {
+            dualSlides.toFloorPosition();
+        }
+    }
     private void dualSlidesToLowBucketPosition() {
-        if (currentGamepad1.dpad_down && !previousGamepad1.dpad_down) {
+        if (currentGamepad2.b && !previousGamepad2.b) {
             dualSlides.toLowBucketPosition();
         }
     }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotDriveV2.java

@@ -0,0 +1,333 @@
+package org.firstinspires.ftc.teamcode.cometbots.paths;
+
+
+import androidx.annotation.NonNull;
+
+import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
+import com.acmerobotics.roadrunner.Action;
+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.pedroPathing.follower.Follower;
+import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
+
+@Autonomous(name = "Auto Test Competition V2", group = "Dev")
+public class CometBotDriveV2 extends OpMode {
+    private Follower follower;
+    private int state;
+
+    private HighBasketPath1 path1;
+    private HighBasketPath2 path2;
+    private HighBasketPath3 path3;
+    private HighBasketPath4 path4;
+    private HighBasketPath5 path5;
+    private HighBasketPath6 path6;
+
+
+    //private CometBotTeleopCompetition comp;
+    private static ElapsedTime runtime;
+    private static boolean initalized = false;
+    private static boolean followingPath = false;
+    private DualMotorSliderSubsystem lift;
+    private ClawSubsystem claw;
+    private WristSubsystem wrist;
+    private ArmSubsystem arm;
+
+    @Override
+    public void init() {
+        follower = new Follower(hardwareMap);
+        follower.setMaxPower(.75);
+
+        path1 = new HighBasketPath1();
+        path2 = new HighBasketPath2();
+        path3 = new HighBasketPath3();
+        path4 = new HighBasketPath4();
+        path5 = new HighBasketPath5();
+        path6 = new HighBasketPath6();
+        lift = new DualMotorSliderSubsystem(hardwareMap);
+        arm = new ArmSubsystem(hardwareMap);
+        wrist = new WristSubsystem(hardwareMap);
+        claw = new ClawSubsystem(hardwareMap);
+
+
+        lift.init();
+        arm.initAuto();
+        wrist.InitAuto();
+        claw.init();
+
+        //comp = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2);
+        // comp.initCloseClaw();
+        runtime = new ElapsedTime();
+    }
+
+
+    public void loop() {
+        telemetry.addData("state", state);
+//        telemetry.addData("arm", arm);
+//        telemetry.addData("lift", lift);
+//        telemetry.addData("wrist", wrist);
+//        telemetry.addData("claw", claw);
+        telemetry.addData("followingPath", followingPath);
+        telemetry.addData("busy", follower.isBusy());
+
+        if (runtime != null) {
+            telemetry.addData("Runtime (seconds)", runtime.seconds());
+        }
+        switch (state) {
+            case 0:
+
+                runtime.reset();
+                moveToPathOneAndHighBucket();
+                break;
+            case 1:
+                doArmThing();
+                state = 2;
+                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:
+                moveToBasketPath5();
+                break;
+            case 9:
+                theArmThingAgain();
+                break;
+            case 10:
+                moveToParkPath6();
+                break;
+            case 11:
+                moveToPark();
+                break;
+            case 99:
+                wrist.toTravelPosition();
+                break;
+            default:
+                telemetry.addLine("default");
+
+        }
+        telemetry.update();
+        lift.update();
+        follower.update();
+
+
+    }
+
+    public void moveToPark() {
+        if (runtime.seconds() > 26) {
+            path6.moveToBasketPath6(follower);
+            state = 99;
+        }
+    }
+
+    private void moveToPathOneAndHighBucket() {
+        path1.moveToPath1(follower);
+        state = 1;
+    }
+    private void moveToBasketPath5() {
+        if(runtime.seconds() > 19){
+            path5.moveToBasketPath5(follower);
+            lift.toHighBucketReverseDrop();
+            wrist.toTravelPosition();
+            state = 9;
+        }
+
+    }
+
+
+    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() {
+
+        claw.thumbOutOfTheWay();
+        wrist.toTravelPosition();
+        lift.toHighBucketReverseDrop();
+        //arm.toReverseBucket();
+        //wrist.toReverseBucket();
+        //claw.openClaw();
+        //wrist.toFloorPosition();
+
+    }
+
+
+    private void theArmThing() {
+        if(lift.getFixedPosition() >= 4280){
+            arm.toReverseBucket();
+            if(runtime.seconds() > 12) {
+                wrist.toReverseBucket();
+            }
+            if(runtime.seconds() > 12.75) {
+                claw.openClaw();
+                state = 6;
+            }
+        }
+    }
+
+    private void theArmThingAgain() {
+        if(lift.getFixedPosition() >= 4280){
+            arm.toReverseBucket();
+            if(runtime.seconds() > 22) {
+                wrist.toReverseBucket();
+            }
+            if(runtime.seconds() > 23.00) {
+                claw.openClaw();
+                state = 10;
+            }
+        }
+    }
+
+
+    private void moveToPathTwoAndPickSampleUp() {
+        if (runtime.seconds() > 3.25 && runtime.seconds() < 4) {
+            wrist.toReverseBucket();
+        }
+        if (runtime.seconds() > 3.5) {
+            claw.openClaw();
+        }
+        if (runtime.seconds() > 4) {
+            wrist.toTravelPosition();
+            lift.toFloorPosition();
+            path2.moveToPath2(follower);
+            state = 3;
+        }
+    }
+
+    //
+    private void moveToBasketPath3() {
+        if (runtime.seconds() > 7.25) {
+            lift.toHighBucketReverseDrop();
+            path3.moveToBasketPath3(follower);
+            state = 5;
+        }
+    }
+
+    private void moveToPickupAgainPath4() {
+        if (runtime.seconds() > 13) {
+            wrist.toTravelPosition();
+        }
+        if (runtime.seconds() > 14) {
+            lift.toFloorPosition();
+            path4.moveToPickupPath4(follower);
+            state = 7;
+        }
+    }
+
+    private void moveToPickupAgainPath5() {
+
+    }
+
+    private void moveToParkPath6() {
+        if (runtime.seconds() > 24.5) {
+            arm.toBucketPosition();
+            wrist.toTravelPosition();
+        }
+        if (runtime.seconds() > 25.) {
+            lift.toFloorPosition();
+            claw.closeClaw();
+            state = 11;
+        }
+    }
+//
+//    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() {
+        wrist.toPickupPosition();
+        arm.toFloorPosition();
+        if (runtime.seconds() > 7){
+            claw.closeClaw();
+            state = 4;
+        }
+    }
+
+    private void doPickUpThingAgain() {
+       wrist.toPickupPosition();
+       arm.toFloorPosition();
+        if (runtime.seconds() > 18){
+            claw.closeClaw();
+            state = 8;
+        }
+    }
+
+
+}
+

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

@@ -0,0 +1,336 @@
+package org.firstinspires.ftc.teamcode.cometbots.paths;
+
+import static org.firstinspires.ftc.teamcode.PedroConstants.MAX_POWER;
+
+import com.acmerobotics.roadrunner.SequentialAction;
+import com.acmerobotics.roadrunner.SleepAction;
+import com.acmerobotics.roadrunner.ftc.Actions;
+import com.qualcomm.robotcore.hardware.Gamepad;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
+import org.firstinspires.ftc.teamcode.states.FieldStates;
+import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
+
+public class CometBotTeleopCompetition {
+
+    /*
+    Subsystems
+     */
+    private MotorsSubsystem motors;
+    private ClawSubsystem claw;
+    private ArmSubsystem arm;
+    private WristSubsystem wrist;
+    private DualMotorSliderSubsystem lift;
+
+    /*
+    Controllers
+     */
+    public Gamepad GP1;
+    public Gamepad GP2;
+    public Gamepad currentGP1;
+    public Gamepad previousGP1;
+    public Gamepad currentGP2;
+    public Gamepad previousGP2;
+
+    /*
+    Pedro/FTC Components
+     */
+    private Follower follower;
+    private Telemetry telemetry;
+
+    /*
+    States
+     */
+    public FieldStates fieldStates;
+
+    /*
+    Configurations
+     */
+    public double currentPower = MAX_POWER;
+
+    public CometBotTeleopCompetition(HardwareMap hardwareMap, Telemetry telemetry, Gamepad gp1, Gamepad gp2) {
+        this.motors = new MotorsSubsystem(hardwareMap, telemetry, .55);
+
+        this.GP1 = gp1;
+        this.GP2 = gp2;
+        this.telemetry = telemetry;
+        this.currentGP1 = new Gamepad();
+        this.currentGP2 = new Gamepad();
+        this.previousGP1 = new Gamepad();
+        this.previousGP2 = new Gamepad();
+        this.fieldStates = new FieldStates();
+        this.follower = new Follower(hardwareMap);
+        claw = new ClawSubsystem(hardwareMap);
+        arm = new ArmSubsystem(hardwareMap);
+        wrist = new WristSubsystem(hardwareMap);
+        lift = new DualMotorSliderSubsystem(hardwareMap);
+    }
+
+    public void init() {
+        this.motors.init();
+        claw.init();
+        arm.initTeleOp();
+        lift.init();
+       ;
+        this.fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
+
+        follower.setMaxPower(MAX_POWER);
+        follower.startTeleopDrive();
+        claw.closeClaw();
+        wrist.initTeleOp();
+    }
+
+    public void initCloseClaw(){
+        this.motors.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();
+        this.toArmParkThenSwitchBetweenSubmarineAndFloorPosition();
+        this.clawControl();
+//        this.decreaseMaxPower();
+//        this.increaseMaxPower();
+
+
+//        Actions.runBlocking(this.lift.toFloorPosition());
+
+        follower.setTeleOpMovementVectors(-this.GP1.left_stick_y, -this.GP1.left_stick_x, -this.GP1.right_stick_x);
+        follower.update();
+        lift.update();
+        this.telemetry.addData("Field State", this.fieldStates.getFieldLocation());
+//        this.telemetry.addData("Claw State", this.claw.getState());
+//        this.telemetry.addData("Claw Position", this.claw.getPosition());
+//        this.telemetry.addData("Wrist State", this.wrist.getState());
+//        this.telemetry.addData("Arm State", this.arm.getState());
+//        this.telemetry.addData("Lift State", this.lift.getState());
+//        this.telemetry.addData("Lift Position", this.lift.getPosition());
+        this.telemetry.addData("MaxPower", MAX_POWER);
+    }
+
+    /*
+    Type: PS4 / Logitech
+    Controller: 1
+    Button: Left Bumper
+    Assumption: Working motor mechanism
+    Action: Decreases maximum speed by -.05
+     */
+//    public void decreaseMaxPower() {
+//        if (this.currentGP1.left_bumper && !this.previousGP1.left_bumper) {
+//            this.currentPower = this.currentPower - .05;
+//            this.follower.setMaxPower(this.currentPower);
+//        }
+//    }
+
+    /*
+    Type: PS4 / Logitech
+    Controller: 1
+    Button: Left Bumper
+    Assumption: Working motor mechanism
+    Action: Increases maximum speed by +.05
+     */
+//    public void increaseMaxPower() {
+//        if (this.currentGP1.right_bumper && !this.previousGP1.right_bumper) {
+//            this.currentPower = this.currentPower + .05;
+//            this.follower.setMaxPower(this.currentPower);
+//        }
+//    }
+//    public void moveSkyHook() {
+//        if (this.currentGP2.dpad_down) {
+//            hook.moveSkyHook(-1.00);
+//        }
+//        else if (this.currentGP2.dpad_up) {
+//            hook.moveSkyHook(1.00);
+//        }
+//        else{
+//            hook.moveSkyHook(0.00);
+//        }
+//
+//    }
+
+    /*
+    Type: PS4 / Logitech
+    Controller: 2
+    Button: TRIANGLE / Y
+    Assumption: Claw is holding specimen, robot is facing buckets ready to score
+    Action: On button press, enter BUCKET state, arm is lifted up, wrist is lifted up and lift
+            raises to high bucket.  Once at high bucket position, move arm forward, wrist forward
+            and open claw to drop specimen into bucket.  Finally, put arm back up and wrist back up,
+            retract lift all the way down to floor position and back to TRAVELING state.
+     */
+    public void toHighBucketScore() {
+        if (this.currentGP2.y && !this.previousGP2.y) {
+            lift.toHighBucketPosition();
+            if (lift.getFixedPosition() >= 4450) {
+                arm.toBucketPosition();
+                if(arm.getPosition() == 0.45)
+                    wrist.toBucketPosition();
+                    if(wrist.getPosition() == 0.56)
+                        claw.openClaw();
+            }
+        }
+    }
+
+    public void highBucketDrop() {
+        Actions.runBlocking(new SequentialAction(
+//                new SleepAction(.5),
+//                this.lift.toHighBucketPosition(),
+//                new SleepAction(.5),
+//                this.arm.toBucketPosition(),
+//                new SleepAction(.5),
+//                this.wrist.toBucketPosition(),
+//                new SleepAction(.5),
+//                this.claw.openClaw(),
+//                new SleepAction(.5),
+//                this.wrist.toFloorPosition(),
+//                new SleepAction(.5),
+//                this.arm.toParkPosition(),
+//                this.lift.toFloorPosition(),
+//                new SleepAction(.5)
+        ));
+    }
+    public void highBucketDropAuto() {
+        Actions.runBlocking(new SequentialAction(
+//                new SleepAction(.1),
+//                this.lift.toHighBucketPosition(),
+//                new SleepAction(.25),
+//                this.arm.toBucketPosition(),
+//                new SleepAction(.25),
+//                this.wrist.toBucketPosition(),
+//                new SleepAction(.25),
+//                this.claw.openClaw(),
+//                new SleepAction(.25),
+//                this.wrist.toFloorPosition(),
+//                new SleepAction(.25),
+//                this.arm.toParkPosition(),
+//                this.lift.toZeroPosition(),
+//                new SleepAction(.25)
+        ));
+    }
+    /*
+    Type: PS4 / Logitech
+    Controller: 2
+    Button: CIRCLE / B
+    Assumption: Claw is holding specimen, robot is facing buckets ready to score
+    Action: On button press, enter BUCKET state, arm is lifted up, wrist is lifted up and lift
+            raises to low bucket.  Once at low bucket position, move arm forward, wrist forward
+            and open claw to drop specimen into bucket.  Finally, put arm back up and wrist back up,
+            retract lift all the way down to floor position and back to TRAVELING state.
+     */
+    public void toLowBucketScore() {
+        if (this.currentGP2.b && !this.previousGP2.b) {
+        lift.toLowBucketPosition();
+            if (lift.getFixedPosition() >= 1700); {
+                arm.toBucketPosition();
+                if(arm.getPosition() == 0.45)
+                    wrist.toBucketPosition();
+                if(wrist.getPosition() == 0.56)
+                    claw.openClaw();
+            }
+        }
+    }
+
+    /*
+    Type: PS4 / Logitech
+    Controller: 2
+    Button: RIGHT BUMPER
+    Assumption: Working claw mechanism
+    Action: On button press, claw switches state from OPEN to CLOSE
+     */
+    public void clawControl() {
+        if (this.currentGP2.right_bumper && !this.previousGP2.right_bumper) {
+            claw.switchState();
+        }
+    }
+
+    /*
+    Type: PS4 / Logitech
+    Controller: 2
+    Button: SQUARE / X
+    Assumption: Working arm mechanism
+    Action: On button press, pulls arm up and wrist up, ideal for traveling the field when
+            holding a specimen in claws
+     */
+    public void toArmParkPosition() {
+        if (this.currentGP2.square && !this.previousGP2.square) {
+            Actions.runBlocking(new SequentialAction(
+//                    this.wrist.toFloorPosition(),
+//                    this.arm.toParkPosition(),
+//                    this.lift.toFloorPosition()
+            ));
+        }
+    }
+
+
+
+
+    /*
+    Type: PS4
+    Controller: 2
+    Button: CROSS / A
+    Assumption: Working claw, arm and wrist mechanisms
+    Action: On button press, if arm is in PARK (toArmParkPosition), drop the arm to SUBMARINE
+            position.  SUBMARINE position means the arm and wrist are parallel to the floor, raised
+            3 INCHES off the ground.  This state is ideal for moving the arm into the SUBMARINE
+            area of the field.
+
+            When arm is in SUBMARINE position, pressing the button again puts the arm and wrist into
+            FLOOR state.  This angles the arm and wrist down so that it is able to pick specimens
+            from within the SUBMARINE floor.
+     */
+    public void toArmParkThenSwitchBetweenSubmarineAndFloorPosition() {
+        if (this.currentGP2.cross && !previousGP2.cross) {
+//            if (this.arm.getState() == ArmActionsSubsystem.ArmState.PARK) {
+//                Actions.runBlocking(
+//                        new SequentialAction(
+//                                this.arm.toSubmarinePosition(),
+//                                this.wrist.toFloorPosition()
+//                        )
+//                );
+//            } else if (this.arm.getState() == ArmActionsSubsystem.ArmState.SUBMARINE && this.wrist.getState() == WristActionsSubsystem.WristState.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()
+//                        )
+//                );
+//            }
+        }
+    }
+
+
+
+
+
+
+
+
+
+
+}
+
+
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotTeleopDriveV2.java

@@ -0,0 +1,24 @@
+package org.firstinspires.ftc.teamcode.cometbots.paths;
+
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+
+@TeleOp(name = "ComeBot Teleop Drive V2", group = "Competition")
+public class CometBotTeleopDriveV2 extends OpMode {
+
+    public CometBotTeleopCompetition runMode;
+
+    @Override
+    public void init() {
+        this.runMode = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2);
+        this.runMode.init();
+
+    }
+
+    @Override
+    public void loop() {
+        this.runMode.update();
+        telemetry.update();
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath1.java

@@ -0,0 +1,39 @@
+package org.firstinspires.ftc.teamcode.cometbots.paths;
+
+
+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 HighBasketPath1 {
+
+    private final Pose startPose = new Pose(10, 89);
+
+    public void moveToPath1(Follower robot) {
+        PathChain pathChain;
+        robot.setStartingPose(startPose);
+        PathBuilder builder = new PathBuilder();
+        builder
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(10.000, 89.000, Point.CARTESIAN),
+                                new Point(20.000, 130.000, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(320));
+        pathChain = builder.build();
+        robot.followPath(pathChain);
+    }
+
+}
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath2.java

@@ -0,0 +1,36 @@
+package org.firstinspires.ftc.teamcode.cometbots.paths;
+
+
+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 HighBasketPath2 {
+
+
+    public void moveToPath2(Follower robot) {
+        PathChain pathChain;
+        PathBuilder builder = new PathBuilder();
+        builder
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(20.000, 130.000, Point.CARTESIAN),
+                                new Point(27.000, 117.000, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(0));
+        pathChain = builder.build();
+        robot.followPath(pathChain);
+    }
+
+}
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath3.java

@@ -0,0 +1,35 @@
+package org.firstinspires.ftc.teamcode.cometbots.paths;
+
+
+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 HighBasketPath3 {
+
+
+    public void moveToBasketPath3(Follower robot) {
+        PathChain pathChain;
+        PathBuilder builder = new PathBuilder();
+        builder
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(26.000, 117.000, Point.CARTESIAN),
+                                new Point(22.000, 132.000, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(320));
+        pathChain = builder.build();
+        robot.followPath(pathChain);
+    }
+
+}
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath4.java

@@ -0,0 +1,35 @@
+package org.firstinspires.ftc.teamcode.cometbots.paths;
+
+
+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 HighBasketPath4 {
+
+
+    public void moveToPickupPath4(Follower robot) {
+        PathChain pathChain;
+        PathBuilder builder = new PathBuilder();
+        builder
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(22.000, 132.000, Point.CARTESIAN),
+                                new Point(27.000, 128.500, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(0));
+        pathChain = builder.build();
+        robot.followPath(pathChain);
+    }
+
+}
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath5.java

@@ -0,0 +1,35 @@
+package org.firstinspires.ftc.teamcode.cometbots.paths;
+
+
+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 HighBasketPath5 {
+
+
+    public void moveToBasketPath5(Follower robot) {
+        PathChain pathChain;
+        PathBuilder builder = new PathBuilder();
+        builder
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(27.000, 127.000, Point.CARTESIAN),
+                                new Point(22.000, 132.000, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(320));
+        pathChain = builder.build();
+        robot.followPath(pathChain);
+    }
+
+}
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath6.java

@@ -0,0 +1,37 @@
+package org.firstinspires.ftc.teamcode.cometbots.paths;
+
+
+import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.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 HighBasketPath6 {
+
+
+    public void moveToBasketPath6(Follower robot) {
+        PathChain pathChain;
+        PathBuilder builder = new PathBuilder();
+        builder
+                .addPath(
+                        // Line 1
+                        new BezierCurve(
+                                new Point(27.000, 128.000, Point.CARTESIAN),
+                                new Point(80.000, 130.000, Point.CARTESIAN),
+                                new Point(100.000, 118.000, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270));
+        pathChain = builder.build();
+        robot.followPath(pathChain);
+    }
+
+}
+

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

@@ -56,7 +56,7 @@ public class ArmTest extends LinearOpMode {
         Gamepad currentGamepad1 = new Gamepad();
         Gamepad previousGamepad1 = new Gamepad();
 
-        arm.init();
+        arm.initTeleOp();
         waitForStart();
         runtime.reset();
 

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

@@ -57,7 +57,7 @@ public class WristTest extends LinearOpMode {
         Gamepad currentGamepad1 = new Gamepad();
         Gamepad previousGamepad1 = new Gamepad();
 
-        wrist.init();
+        wrist.initTeleOp();
         waitForStart();
         runtime.reset();
 

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

@@ -4,32 +4,47 @@ import com.acmerobotics.dashboard.config.Config;
 
 @Config
 public class RobotConstants {
-    public final static double clawClose = 0.85;
+    public final static double clawClose = 0.95;
     public final static double clawOpen = 0.05;
 
-    public final static double armFloor = 0.6375;
+    public final static double armFloor = 0.7;
     public final static double armSubmarine = 0.55;
-    public final static double armPark = 0.1250;
-    public final static double armBucket = 0.25;
-
-    public final static double wristPickup = 0.6;
-    public final static double wristBucket = 0.3;
-    public final static double wristFloor = 0.125;
+    public final static double armReverseBucket = 0.08;
+    public final static double armPark = 0.33;
+    //value for grabbing the hook Specimen
+    public final static double grabBlueberry = 0.35;
+    public final static double armGrabBlueberrySkyhook = 0.1;
+    public final static double wristGrabBlueberrySkyhook = 0.15;
+    public final static double armHangBlueberrySkyhook = 0.23;
+    public final static double wristHangBlueberrySkyhook = 0.7;
+    public final static int slideHangBlueberrySkyhook = 500;
+    public final static int slideBelowFloor = -150;
+    public final static int backwardBucketDrop = 4670;
+    public final static double armBucket = 0.45;
+    public final static double armInit = 0.13;
+    public final static double wristInit = 0.125;
+    public final static double wristPickup = 0.475;
+    public final static double wristBucket = 0.56;
+    public final static double wristFloor = 0.75;
+    public final static double wristBackwardBucket = 0.725;
 
     public final static double wristRung = 0.55;
 
     public final static double wristSpeciemen = 0.1;
 
+    public final static int toBar = 500;
+    public final static double wristtravel = 0.525;
 
+    public final static int toFloor = 0;
     public final static int liftToFloorPos = 350;
     public final static int liftToSubmarinePos = 350;
-    public final static int liftToLowBucketPos = 2650;
+    public final static int liftToLowBucketPos = 1750;
     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 int liftToHighBucketPos = 4500;
     public final static double liftPower = 1;
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java

@@ -31,7 +31,7 @@ public class FollowerConstants {
     public static String rightFrontMotorName = FRONT_RIGHT_MOTOR;
     public static String rightRearMotorName = BACK_RIGHT_MOTOR;
 
-    public static DcMotorSimple.Direction leftFrontMotorDirection = FRONT_RIGHT_MOTOR_DIRECTION;
+    public static DcMotorSimple.Direction leftFrontMotorDirection = FRONT_LEFT_MOTOR_DIRECTION;
     public static DcMotorSimple.Direction rightFrontMotorDirection = FRONT_RIGHT_MOTOR_DIRECTION;
     public static DcMotorSimple.Direction leftRearMotorDirection = BACK_LEFT_MOTOR_DIRECTION;
     public static DcMotorSimple.Direction rightRearMotorDirection = BACK_RIGHT_MOTOR_DIRECTION;
@@ -48,7 +48,7 @@ public class FollowerConstants {
     public static CustomPIDFCoefficients translationalPIDFCoefficients = new CustomPIDFCoefficients(
             .25,
             0,
-            0.0375,
+            0.02,
             0);
 
     // Translational Integral
@@ -75,7 +75,7 @@ public class FollowerConstants {
 
     // Drive PIDF coefficients
     public static CustomFilteredPIDFCoefficients drivePIDFCoefficients = new CustomFilteredPIDFCoefficients(
-            0.00375,
+            0.00235,
             0,
             0.00003,
             0.8,

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/states/BlueBasketAutoStates.java

@@ -0,0 +1,136 @@
+package org.firstinspires.ftc.teamcode.states;
+
+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 Basket Auto States", group = "Competition")
+public class BlueBasketAutoStates extends OpMode {
+    private Telemetry telemetryA;
+
+    private Follower follower;
+
+    private PathChain path;
+
+    private final Pose startPose = new Pose(11.25, 95.75);
+
+    @Override
+    public void init() {
+        follower = new Follower(hardwareMap);
+
+        follower.setMaxPower(.45);
+
+        follower.setStartingPose(startPose);
+
+        path = follower.pathBuilder()
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(11.250, 95.750, Point.CARTESIAN),
+                                new Point(37.000, 108.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 2
+                        new BezierCurve(
+                                new Point(37.000, 108.000, Point.CARTESIAN),
+                                new Point(73.286, 111.536, Point.CARTESIAN),
+                                new Point(67.821, 120.536, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 3
+                        new BezierLine(
+                                new Point(67.821, 120.536, Point.CARTESIAN),
+                                new Point(28.000, 121.500, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 4
+                        new BezierLine(
+                                new Point(28.000, 121.500, Point.CARTESIAN),
+                                new Point(18.000, 130.179, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 5
+                        new BezierCurve(
+                                new Point(18.000, 130.179, Point.CARTESIAN),
+                                new Point(59.000, 102.500, Point.CARTESIAN),
+                                new Point(68.700, 130.500, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 6
+                        new BezierLine(
+                                new Point(68.700, 130.500, Point.CARTESIAN),
+                                new Point(18.000, 130.339, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 7
+                        new BezierCurve(
+                                new Point(18.000, 130.339, Point.CARTESIAN),
+                                new Point(49.018, 121.179, Point.CARTESIAN),
+                                new Point(63.804, 135.321, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 8
+                        new BezierLine(
+                                new Point(63.804, 135.321, Point.CARTESIAN),
+                                new Point(53.036, 135.161, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 9
+                        new BezierLine(
+                                new Point(53.036, 135.161, Point.CARTESIAN),
+                                new Point(18.643, 135.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 10
+                        new BezierLine(
+                                new Point(18.643, 135.000, Point.CARTESIAN),
+                                new Point(72.300, 97.400, Point.CARTESIAN)
+                        )
+                )
+                .addPath(
+                        // Line 11
+                        new BezierLine(
+                                new Point(18.643, 135.000, Point.CARTESIAN),
+                                new Point(83.250, 95.464, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270)).build();
+        follower.followPath(path);
+
+        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+        telemetryA.update();
+    }
+
+    @Override
+    public void loop() {
+        follower.update();
+        follower.telemetryDebug(telemetryA);
+    }
+}

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

@@ -3,7 +3,11 @@ package org.firstinspires.ftc.teamcode.subsystem;
 import static org.firstinspires.ftc.teamcode.PedroConstants.ARM_SERVO;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armBucket;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armFloor;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armGrabBlueberrySkyhook;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armHangBlueberrySkyhook;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armInit;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armPark;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armReverseBucket;
 
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.ServoImplEx;
@@ -11,7 +15,7 @@ import com.qualcomm.robotcore.hardware.ServoImplEx;
 public class ArmSubsystem {
 
     public enum ArmState {
-        PARK, FLOOR, BUCKET, SUBMARINE
+        PARK, FLOOR, BUCKET, SUBMARINE, GRAB_BLUEBERRY_SKYHOOK, HANG_BLUEBERRY_SKYHOOK, INIT, REVERE_BUCKET
     }
 
     private ServoImplEx arm;
@@ -20,7 +24,18 @@ public class ArmSubsystem {
     public ArmSubsystem(HardwareMap hardwareMap) {
         this.arm = hardwareMap.get(ServoImplEx.class, ARM_SERVO);
     }
-
+    public void grabBlueberrySkyhook(){
+        setState(ArmState.GRAB_BLUEBERRY_SKYHOOK);
+        arm.setPosition(armGrabBlueberrySkyhook+ 0.3);
+    }
+    public void hangBlueberrySkyhook(){
+        setState(ArmState.HANG_BLUEBERRY_SKYHOOK);
+        arm.setPosition(armHangBlueberrySkyhook);
+    }
+    public void toReverseBucket(){
+        arm.setPosition(armReverseBucket);
+        setState(ArmState.REVERE_BUCKET);
+    }
     public void toParkPosition() {
         arm.setPosition(armPark);
         setState(ArmState.PARK);
@@ -35,6 +50,10 @@ public class ArmSubsystem {
         arm.setPosition(armBucket);
         setState(ArmState.BUCKET);
     }
+    public void toInitPosition(){
+        arm.setPosition(armInit);
+        setState(ArmState.INIT);
+    }
 
     public void setState(ArmState armState) {
         this.state = armState;
@@ -44,10 +63,14 @@ public class ArmSubsystem {
         return this.state;
     }
 
-    public void init() {
+    public void initTeleOp() {
         arm.resetDeviceConfigurationForOpMode();
         toParkPosition();
     }
+    public void initAuto(){
+        arm.resetDeviceConfigurationForOpMode();
+        toInitPosition();
+    }
 
     public double getPosition() {
         return this.arm.getPosition();

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

@@ -4,6 +4,7 @@ import static org.firstinspires.ftc.teamcode.PedroConstants.CLAW_NAME;
 import static org.firstinspires.ftc.teamcode.PedroConstants.THUMB_SERVO;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.clawClose;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.clawOpen;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.grabBlueberry;
 
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.Servo;
@@ -11,11 +12,11 @@ import com.qualcomm.robotcore.hardware.Servo;
 public class ClawSubsystem {
 
     public enum ClawState {
-        CLOSED, OPEN
+        CLOSED, OPEN, GRAB_BLUEBERRY
     }
 
     public enum ThumbState {
-        DOWN, UP
+        DOWN, UP, AWAY
     }
 
     private Servo claw;
@@ -27,14 +28,19 @@ public class ClawSubsystem {
         claw = hardwareMap.get(Servo.class, CLAW_NAME);
         thumb = hardwareMap.get(Servo.class, THUMB_SERVO);
     }
-
+    public void grabBlueberry(){
+        claw.setPosition(grabBlueberry);
+        state = ClawState.GRAB_BLUEBERRY;
+    }
     public void closeClaw() {
         claw.setPosition(clawClose);
+
         state = ClawState.CLOSED;
     }
 
     public void openClaw() {
         claw.setPosition(clawOpen);
+
         state = ClawState.OPEN;
     }
 
@@ -68,13 +74,18 @@ public class ClawSubsystem {
     }
 
     public void thumbUp() {
-        thumb.setPosition(0.95);
+        thumb.setPosition(0.5);
         tState = ThumbState.UP;
     }
 
     public void thumbDown() {
-        thumb.setPosition(0.05);
+        thumb.setPosition(0.00);
         tState = ThumbState.DOWN;
     }
 
+    public void thumbOutOfTheWay() {
+        thumb.setPosition(0.95);
+        tState = ThumbState.AWAY;
+    }
+
 }

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

@@ -2,9 +2,15 @@ package org.firstinspires.ftc.teamcode.subsystem;
 
 import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_LEFT_MOTOR;
 import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_RIGHT_MOTOR;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.backwardBucketDrop;
+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.slideBelowFloor;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.slideHangBlueberrySkyhook;
 
 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 com.qualcomm.robotcore.util.ElapsedTime;
 
@@ -68,7 +74,9 @@ public class DualMotorSliderSubsystem {
         liftSlideLeft = hardwareMap.get(DcMotorEx.class, LIFT_SLIDE_LEFT_MOTOR);
         liftSlideRight = hardwareMap.get(DcMotorEx.class, LIFT_SLIDE_RIGHT_MOTOR);
     }
-
+    public int getCurrentPosition(){
+        return liftSlideLeft.getCurrentPosition();
+    }
     public void init() {
         /*
         Initialize the motors with the following settings (assuming slide is at the very bottom position):
@@ -81,7 +89,8 @@ public class DualMotorSliderSubsystem {
         liftSlideLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
         liftSlideLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
         liftSlideLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-
+        liftSlideLeft.setDirection(DcMotorSimple.Direction.REVERSE);
+        liftSlideRight.setDirection(DcMotorSimple.Direction.REVERSE);
         liftSlideRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
         liftSlideRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
         liftSlideRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
@@ -121,11 +130,30 @@ public class DualMotorSliderSubsystem {
     }
 
     public void toLowBucketPosition() {
-        setTargetPosition(1500);
+        setTargetPosition(liftToLowBucketPos);
     }
 
     public void toHighBucketPosition() {
-        setTargetPosition(3000);
+        setTargetPosition(liftToHighBucketPos);
     }
 
+    public void toFixedPosition(int value) {
+        setTargetPosition(value);
+    }
+    public void toHangHeight(){
+        setTargetPosition(slideHangBlueberrySkyhook);
+
+    }
+    public void toHangBelowFloor(){
+        setTargetPosition(slideBelowFloor);
+
+    }
+
+    public int getFixedPosition() {
+        return liftSlideLeft.getCurrentPosition();
+    }
+
+    public void toFloorPosition(){setTargetPosition(0);}
+    public void toHighBucketReverseDrop(){setTargetPosition(backwardBucketDrop);}
+
 }

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

@@ -0,0 +1,57 @@
+package org.firstinspires.ftc.teamcode.subsystem;
+
+import static org.firstinspires.ftc.teamcode.PedroConstants.HOOK;
+import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_LEFT_MOTOR;
+import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_RIGHT_MOTOR;
+
+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 static org.firstinspires.ftc.teamcode.configs.RobotConstants.toBar;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.toFloor;
+
+public class HangMotorSubsystem {
+    private DcMotorEx hang;
+
+    public HangMotorSubsystem(HardwareMap hardwareMap) {
+        hang = hardwareMap.get(DcMotorEx.class, HOOK);
+    }
+
+    public int getCurrentPosition(){
+        int Return = hang.getCurrentPosition();
+        return Return;
+    }
+    public void init(){
+        hang.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        hang.setDirection(DcMotorSimple.Direction.FORWARD);
+        hang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        hang.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+    }
+    public void hangRobot(){
+        hang.setTargetPosition(toBar);
+
+    }
+    public void robotToFloor(){
+        hang.setTargetPosition(toFloor);
+    }
+    public void disableMotor(){
+        hang.setPower(0);
+        hang.setMotorDisable();
+
+    }
+    public void setPower(double power){
+        hang.setPower(power);
+    }
+    public void stopMotor(){
+        hang.setPower(0);
+
+
+    }
+    public void toHangPosition(int Position){
+        //write in limits for protection
+        hang.setPower(Position);
+    }
+
+}

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

@@ -0,0 +1,62 @@
+package org.firstinspires.ftc.teamcode.subsystem;
+
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.HangMotorSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
+
+public class HookTest extends OpMode {
+    private HangMotorSubsystem skyhook;
+    private ArmSubsystem arm;
+    private WristSubsystem wrist;
+    private ClawSubsystem claw;
+    private DualMotorSliderSubsystem slides;
+    @Override
+    public void init() {
+        skyhook = new HangMotorSubsystem(hardwareMap);
+        arm = new ArmSubsystem(hardwareMap);
+        wrist = new WristSubsystem(hardwareMap);
+        claw = new ClawSubsystem(hardwareMap);
+        slides = new DualMotorSliderSubsystem(hardwareMap);
+        claw.init();
+        arm.initTeleOp();
+        wrist.initTeleOp();
+        slides.init();
+    }
+
+    @Override
+    public void loop() {
+        telemetry.addData("arm", arm.getState());
+        telemetry.addData("wrist", wrist.getState());
+        telemetry.update();
+        if(gamepad1.a){
+            arm.grabBlueberrySkyhook();
+            telemetry.update();
+            try {
+                Thread.sleep(500);
+            } catch (InterruptedException e) {
+                throw new RuntimeException(e);
+            }
+            //claw Open small amount
+            wrist.grabBlueberrySkyhook();
+            //wrist grab in strange way
+            telemetry.update();
+        }
+        if(gamepad1.b){
+            //after confirmed grab, close claw
+            claw.closeClaw();
+        }
+        if(gamepad1.x){
+            //now slap on bar, first wrist, then arm, then claw then driver must drive away
+        }
+        if(gamepad1.y){
+            //go up
+        }
+
+    }
+
+}

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

@@ -0,0 +1,141 @@
+package org.firstinspires.ftc.teamcode.subsystem;
+
+/* 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.
+ */
+
+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.subsystem.ArmSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
+
+
+@TeleOp(name = "Lift Wrist Arm Test", group = "Debug")
+public class LiftArmWrist extends LinearOpMode {
+
+    private final ElapsedTime runtime = new ElapsedTime();
+
+    @Override
+    public void runOpMode() {
+
+        /*
+         * Instantiate Lift
+         */
+        DualMotorSliderSubsystem lift = new DualMotorSliderSubsystem(hardwareMap);
+        WristSubsystem wrist = new WristSubsystem(hardwareMap);
+        ArmSubsystem arm = new ArmSubsystem(hardwareMap);
+        ClawSubsystem claw = new ClawSubsystem(hardwareMap);
+        HangMotorSubsystem hang = new HangMotorSubsystem(hardwareMap);
+
+
+        /*
+         * Instantiate gamepad state holders
+         */
+        Gamepad currentGamepad1 = new Gamepad();
+        Gamepad previousGamepad1 = new Gamepad();
+
+        lift.init();
+        wrist.initTeleOp();
+        arm.initTeleOp();
+        claw.init();
+        waitForStart();
+        runtime.reset();
+
+
+        // run until the end of the match (driver presses STOP)
+        while (opModeIsActive()) {
+
+            previousGamepad1.copy(currentGamepad1);
+            currentGamepad1.copy(gamepad1);
+
+            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.b && !previousGamepad1.b) {
+                claw.switchState();
+            }
+
+            if (currentGamepad1.dpad_right && !previousGamepad1.dpad_right) {
+                lift.toHighBucketPosition();
+            }
+
+            if (currentGamepad1.dpad_left && !previousGamepad1.dpad_left) {
+                lift.toLowBucketPosition();
+            }
+
+            if (currentGamepad1.y && !previousGamepad1.y) {
+                wrist.setPosition(wrist.getPosition() + .05);
+            }
+
+            if (currentGamepad1.a && !previousGamepad1.a) {
+                wrist.setPosition(wrist.getPosition() - .05);
+            }
+
+
+            if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
+               lift.toHighBucketPosition();
+            }
+
+            if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) {
+                lift.toLowBucketPosition();
+
+            }
+            if(currentGamepad1.left_trigger > 0){
+                hang.setPower((double) currentGamepad1.left_trigger);
+            }
+            if(currentGamepad1.right_trigger > 0){
+                hang.setPower((double) currentGamepad1.right_trigger);
+            }
+
+            lift.update();
+
+            // Show the elapsed game time and wheel power.
+
+            telemetry.addData("Status", "Run Time: " + runtime.toString());
+            telemetry.addData("Lift Drive Position", lift.getFixedPosition());
+            telemetry.addData("Wrist Position", wrist.getPosition());
+            telemetry.addData("Arm Position", arm.getPosition());
+            telemetry.addData("Slide Ticks", lift.getCurrentPosition());
+            telemetry.addData("Hook Ticks", hang.getCurrentPosition());
+            telemetry.update();
+        }
+    }
+}

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

@@ -1,10 +1,15 @@
 package org.firstinspires.ftc.teamcode.subsystem;
 
 import static org.firstinspires.ftc.teamcode.PedroConstants.WRIST_SERVO;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristBackwardBucket;
 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.wristGrabBlueberrySkyhook;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristHangBlueberrySkyhook;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristInit;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristPickup;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristSpeciemen;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristtravel;
 
 import androidx.annotation.NonNull;
 
@@ -17,7 +22,7 @@ import com.qualcomm.robotcore.hardware.ServoImplEx;
 public class WristSubsystem {
 
     public enum WristState {
-        FLOOR, BUCKET, PICKUP, RUNG, SPECIMEN
+        FLOOR, BUCKET, PICKUP, RUNG, SPECIMEN, GRAB_BLUEBERRY_SKYHOOK, HANG_BLUEBERRY_SKYHOOK, INIT, REVERSE_BUCKET, TRAVEL
     }
 
     public ServoImplEx wrist;
@@ -44,7 +49,16 @@ public class WristSubsystem {
             return false;
         }
     }
+    public void grabBlueberrySkyhook(){
+        setState(WristState.GRAB_BLUEBERRY_SKYHOOK);
+        wrist.setPosition(wristGrabBlueberrySkyhook);
+    }
 
+
+    public void hangBlueberrySkyhook(){
+        setState(WristState.HANG_BLUEBERRY_SKYHOOK);
+        wrist.setPosition(wristHangBlueberrySkyhook);
+    }
     public void toFloorPosition() {
         setState(WristState.FLOOR);
         wrist.setPosition(wristFloor);
@@ -59,6 +73,15 @@ public class WristSubsystem {
         setState(WristState.PICKUP);
         wrist.setPosition(wristPickup);
     }
+    public void toInitPosition(){
+        setState(WristState.INIT);
+        wrist.setPosition(wristInit);
+    }
+    public void InitAuto(){
+        wrist.resetDeviceConfigurationForOpMode();
+        toInitPosition();
+
+    }
 
     public void setState(WristState wristState) {
         this.state = wristState;
@@ -71,12 +94,20 @@ public class WristSubsystem {
             toFloorPosition();
         }
     }
+    public void toReverseBucket(){
+        wrist.setPosition(wristBackwardBucket);
+        setState(WristState.REVERSE_BUCKET);
+    }
+    public void toTravelPosition(){
+        wrist.setPosition(wristtravel);
+        setState(WristState.TRAVEL);
+    }
 
     public WristState getState() {
         return this.state;
     }
 
-    public void init() {
+    public void initTeleOp() {
         wrist.resetDeviceConfigurationForOpMode();
         toPickupPosition();
     }