Getting parking to work again as path 6b. Original path during competition was not working properly.

# Conflicts:
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotTeleOpDevelopment.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java

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

@@ -1,24 +0,0 @@
-package org.firstinspires.ftc.teamcode;
-
-import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-
-import org.firstinspires.ftc.teamcode.cometbots.CometBotAutoDevelopment;
-
-@TeleOp(name = "CometBot Auto v2", group = "Development")
-public class CometBotDevAuto extends OpMode {
-
-    public CometBotAutoDevelopment runMode;
-
-    @Override
-    public void init() {
-        runMode = new CometBotAutoDevelopment(hardwareMap, gamepad1, gamepad2);
-        runMode.init();
-    }
-
-    @Override
-    public void loop() {
-        runMode.update();
-        telemetry.update();
-    }
-}

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

@@ -0,0 +1,33 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.cometbots.CometBotTeleOpDevelopment;
+import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.DualMotorSliderSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.HangMotorSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem;
+@TeleOp(name = "CometBot Drive V2.137", group = "Competition")
+public class CometBotTeleOpCompetition extends OpMode {
+    public DualMotorSliderSubsystem slide;
+    public CometBotTeleOpDevelopment runMode;
+
+    @Override
+    public void init() {
+        runMode = new CometBotTeleOpDevelopment(hardwareMap, gamepad1, gamepad2);
+        slide = new DualMotorSliderSubsystem(hardwareMap);
+        runMode.init();
+    }
+
+    @Override
+    public void loop() {
+        runMode.update();
+
+        telemetry.update();
+    }
+    public void stop(){
+
+    }
+}

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 = "Specimen Test", group = "Competition")
+@Autonomous(name = "PreLoadedBlue", group = "Competition")
 public class PreLoadedBlueBasketAuto extends OpMode {
     private Telemetry telemetryA;
 
@@ -27,7 +27,7 @@ public class PreLoadedBlueBasketAuto extends OpMode {
     public void init() {
         follower = new Follower(hardwareMap);
 
-        follower.setMaxPower(.45);
+        follower.setMaxPower(.20);
 
         follower.setStartingPose(startPose);
 

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

@@ -0,0 +1,117 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.acmerobotics.roadrunner.SleepAction;
+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.subsystems.ArmSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.DualMotorSliderSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.HangMotorSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem;
+
+@Autonomous(name = "Specimen Auto", group = "Development")
+
+public class SpecimenAuto extends OpMode {
+    private Follower follower;
+    private int state;
+    private ElapsedTime runtime;
+
+    private DualMotorSliderSubsystem lift;
+    private ClawSubsystem claw;
+    private WristSubsystem wrist;
+    private ArmSubsystem arm;
+    private HangMotorSubsystem hang;
+
+
+
+    @Override
+     public void init(){
+         lift = new DualMotorSliderSubsystem(hardwareMap);
+         claw = new ClawSubsystem(hardwareMap);
+         arm = new ArmSubsystem(hardwareMap);
+         wrist = new WristSubsystem(hardwareMap);
+         hang = new HangMotorSubsystem(hardwareMap);
+         follower = new Follower(hardwareMap);
+         runtime = new ElapsedTime();
+
+         state = 0;
+         lift.init();
+         claw.init();
+         wrist.InitAuto();
+         arm.initAuto();
+
+
+        follower.setMaxPower(.45);
+
+
+     }
+
+    @Override
+    public void loop() {
+        if(runtime != null){
+            telemetry.addData("Runtime (seconds)", runtime.seconds());
+        }
+        telemetry.addData("state", state);
+         switch(state) {
+             case 0:
+                 runtime.reset();
+                 wrist.toSpecimenPrep();
+                 arm.toSpecimenPrep();
+                 if(runtime.seconds() > 0.25){state = 1;}
+                 break;
+             case 1:
+                 //line 1
+                 if(runtime.seconds() > 3){state = 2;}
+                 break;
+             case 2:
+                 lift.toSpecimanHangHeight();
+                 if(runtime.seconds() > 3.75){state = 3;}
+                 break;
+             case 3:
+                wrist.toSpecimenHang();
+                 if(runtime.seconds() > 4){state = 4;}
+                 break;
+             case 4:
+                lift.toSpecimanReleaseHeight();
+                 if(runtime.seconds() > 4.5){state = 5;}
+                 break;
+             case 5:
+                 claw.switchState();
+                 if(runtime.seconds() > 4.65){state = 6;}
+                 break;
+             case 6:
+                 lift.toFloorPosition();
+                 if(runtime.seconds() > 4.75){state = 7;}
+                 break;
+             case 7:
+                 arm.toParkPosition();
+                 wrist.toTravelPosition();
+                 //line 2
+                 if(runtime.seconds() > 0.){state = 8;}
+                 break;
+             case 8:
+                 if(runtime.seconds() > 0.25){state = 9;}
+                 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;
+         }
+         new SleepAction(5);
+    }
+
+}

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 Line Test", group = "Competition")
+public class SpecimenAutoLineTest extends OpMode {
+    private Telemetry telemetryA;
+
+    private Follower follower;
+
+    private PathChain path;
+
+    private final Pose startPose = new Pose(8, 55);
+
+    @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, 55.000, Point.CARTESIAN),
+                                new Point(39.500, 60.000, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(0))
+                .addPath(
+                        // Line 2
+                        new BezierLine(
+                                new Point(39.500, 60.000, Point.CARTESIAN),
+                                new Point(37.000, 60.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .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(0))
+                .addPath(
+                        // Line 4
+                        new BezierLine(
+                                new Point(58.000, 23.500, Point.CARTESIAN),
+                                new Point(14.000, 23.500, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .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/CometBotTeleOpDevelopment.java

@@ -2,27 +2,17 @@ package org.firstinspires.ftc.teamcode.cometbots;
 
 import static org.firstinspires.ftc.teamcode.PedroConstants.MAX_POWER;
 
-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.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.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;
+import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.DualMotorSliderSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.HangMotorSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem;
 
-public class CometBotAutoDevelopment {
+public class CometBotTeleOpDevelopment {
 
     /*
     Subsystems
@@ -31,7 +21,7 @@ public class CometBotAutoDevelopment {
     private ClawSubsystem claw;
     private WristSubsystem wrist;
     private ArmSubsystem arm;
-    private HangMotorSubsystem hang;
+    private HangMotorSubsystem skyhook;
     /*
     Controllers
      */
@@ -42,15 +32,17 @@ public class CometBotAutoDevelopment {
     public Gamepad previousGamepad1;
     public Gamepad previousGamepad2;
 
-
+    public boolean wristPickup;
+    public boolean armParked;
+    public boolean goClaw;
     private Follower follower;
 
-    public CometBotAutoDevelopment(HardwareMap hardwareMap, Gamepad gamepad1, Gamepad gamepad2) {
+    public CometBotTeleOpDevelopment(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);
+        skyhook = new HangMotorSubsystem(hardwareMap);
 
         this.gamepad1 = gamepad1;
         this.gamepad2 = gamepad2;
@@ -59,15 +51,20 @@ public class CometBotAutoDevelopment {
         previousGamepad1 = new Gamepad();
         previousGamepad2 = new Gamepad();
         follower = new Follower(hardwareMap);
+
     }
-    
+
     public void init() {
         dualSlides.init();
         claw.init();
         wrist.initTeleOp();
         arm.initTeleOp();
+        skyhook.init();
         follower.setMaxPower(MAX_POWER);
         follower.startTeleopDrive();
+        wristPickup = false;
+        armParked = true;
+        goClaw = false;
     }
 
     public void update() {
@@ -89,119 +86,130 @@ public class CometBotAutoDevelopment {
         openClaw();
         openThumb();
         //hang
-        grabBlueberrySkyhook();
-        hangSkyhook();
-        robotDown();
-        robotUp();
-        stopHook();
+        hang();
         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();
-        }
-        if(arm.getState() == ArmSubsystem.ArmState.GRAB_BLUEBERRY_SKYHOOK)
-            claw.init();
-    }
-    private void hangSkyhook (){
-        dualSlides.toHangHeight();
-        wrist.hangBlueberrySkyhook();
-        try {
-            Thread.sleep(1000);
-        } catch (InterruptedException e) {
-            throw new RuntimeException(e);
-        }
-        arm.hangBlueberrySkyhook();
-        if(arm.getState() == ArmSubsystem.ArmState.HANG_BLUEBERRY_SKYHOOK){
-            try {
-                Thread.sleep(1000);
-            } catch (InterruptedException e) {
-                throw new RuntimeException(e);
-            }claw.closeClaw();
-        }
-
-    }
-
-    private void openClaw(){
-        if(currentGamepad2.right_bumper && !previousGamepad2.right_bumper){
+    private void openClaw() {
+        if (currentGamepad2.right_bumper && !previousGamepad2.right_bumper) {
             claw.switchState();
         }
     }
-    private void openThumb(){
-        if(currentGamepad2.left_bumper && !previousGamepad2.left_bumper){
+
+    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);
-                }
+    private void armAndWristToFloor() {
+        if (currentGamepad2.a && !previousGamepad2.a && wristPickup) {
+            if (wrist.getState() != WristSubsystem.WristState.FLOOR) {
+                wrist.toFloorPositionTeleop();
+            } else if (wristPickup) {
+                claw.openClaw();
+                wrist.toPickupPosition();
             }
-            arm.toFloorPosition();
         }
-        if(currentGamepad2.a && !previousGamepad2.a ){
-            wrist.switchState();
+        if (currentGamepad2.a && !previousGamepad2.a) {
+            arm.toFloorPositionTeleOp();
+
+            wristPickup = true;
+            armParked = false;
         }
-
-
-
     }
-    private void armToBucketPosition(){
-        if(currentGamepad2.dpad_up && !previousGamepad2.dpad_up){
+
+    private void armToBucketPosition() {
+        if (currentGamepad2.dpad_up && !previousGamepad2.dpad_up) {
+            armParked = false;
             arm.toBucketPosition();
             wrist.toBucketPosition();
+            wristPickup = false;
 
         }
     }
-    private void armToParkPosition(){
-        if(currentGamepad2.x && !previousGamepad2.x){
+
+    private void armToParkPosition() {
+        if (currentGamepad2.x && !previousGamepad2.x) {
+            armParked = true;
             arm.toParkPosition();
-            wrist.toFloorPosition();
+            wrist.toFloorPositionTeleop();
+            wristPickup = false;
+
         }
     }
 
     private void dualSlidesToHighBucketPosition() {
-        if (currentGamepad2.y && !previousGamepad2.y) {
+        if (currentGamepad2.y && !previousGamepad2.y && armParked) {
             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) {
+        if (currentGamepad2.dpad_down && !previousGamepad2.dpad_down && armParked) {
             dualSlides.toFloorPosition();
         }
     }
+
     private void dualSlidesToLowBucketPosition() {
-        if (currentGamepad2.b && !previousGamepad2.b) {
+        if (currentGamepad2.b && !previousGamepad2.b && armParked) {
             dualSlides.toLowBucketPosition();
         }
+    }
+    private void hang(){
+        if (gamepad1.a) {
+            claw.grabBlueberry();
+            arm.setPosition(0.15);
+            arm.grabBlueberrySkyhook();
+
+            //claw Open small amount
+            wrist.grabBlueberrySkyhook();
+            //wrist grab in strange way
+        }
+        if(gamepad1.b) {
+            //confirm grab
+            claw.closeClaw();
+
+        }
+
+        if (gamepad1.x && claw.getState() == ClawSubsystem.ClawState.CLOSED) {
+            //now slap on bar, first wrist, then arm, then claw then driver must drive away
+            dualSlides.toFixedPosition(200);
+            dualSlides.update();
+        }
+
+        if (gamepad1.y) {
+            wrist.hangBlueberrySkyhook();
+            arm.hangBlueberrySkyhook();
+            try {
+                Thread.sleep(1500);
+            } catch (InterruptedException e) {
+                throw new RuntimeException(e);
+            }
+            goClaw = true;
+            dualSlides.toFixedPosition(2100);
+            dualSlides.update();
+        }
+        if (gamepad1.right_bumper) {
+            claw.openClaw();
+            if(goClaw == true) {
+                try {
+                    Thread.sleep(500);
+                } catch (InterruptedException e) {
+                    throw new RuntimeException(e);
+                }
+                dualSlides.toFloorPosition();
+                dualSlides.update();
+                arm.toParkPosition();
+                wrist.toPickupPosition();
+            }
+        }
+        skyhook.setPowerForward(-gamepad2.right_stick_y);
+
+
+
+
     }
 
 }

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

@@ -1,37 +1,21 @@
 package org.firstinspires.ftc.teamcode.cometbots.paths;
 
-import static com.qualcomm.hardware.lynx.commands.core.LynxSetMotorPIDFControlLoopCoefficientsCommand.InternalMotorControlAlgorithm.Max;
 
 import androidx.annotation.NonNull;
 
 import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
 import com.acmerobotics.roadrunner.Action;
-import com.acmerobotics.roadrunner.SequentialAction;
-import com.acmerobotics.roadrunner.SleepAction;
-import com.acmerobotics.roadrunner.ftc.Actions;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 import com.qualcomm.robotcore.util.ElapsedTime;
 
-//import org.firstinspires.ftc.teamcode.cometbots.CometBotTeleopCompetition;
-//import org.firstinspires.ftc.teamcode.cometbots.paths.HighBasketPath1;
 import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
-//import org.firstinspires.ftc.teamcode.subsystem.AutoPark;
-import org.firstinspires.ftc.teamcode.cometbots.paths.HighBasketPath1;
-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;
-//import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath2;
-//import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath3;
-//import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath4;
-//import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath5;
-//import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath6;
-//import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
-//import org.firstinspires.ftc.teamcode.subsystem.SkyHookSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.DualMotorSliderSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem;
 
-
-@Autonomous(name = "Auto Test Competition V2", group = "Dev")
+@Autonomous(name = "Auto Competition V2", group = "A")
 public class CometBotDriveV2 extends OpMode {
     private Follower follower;
     private int state;
@@ -39,7 +23,9 @@ public class CometBotDriveV2 extends OpMode {
     private HighBasketPath1 path1;
     private HighBasketPath2 path2;
     private HighBasketPath3 path3;
-
+    private HighBasketPath4 path4;
+    private HighBasketPath5 path5;
+    private HighBasketPath6 path6;
 
 
     //private CometBotTeleopCompetition comp;
@@ -59,33 +45,42 @@ public class CometBotDriveV2 extends OpMode {
         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();
-
+        // 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:
@@ -96,46 +91,68 @@ public class CometBotDriveV2 extends OpMode {
                 moveToPathTwoAndPickSampleUp();
                 break;
             case 3:
-//                doPickUpThing();
-                state = 4;
+                doPickUpThing();
                 break;
             case 4:
-               // moveToBasketPath3();
+                moveToBasketPath3();
                 break;
-//            case 5:
-//                theArmThing();
-//                break;
-//            case 6:
-//                moveToPickupAgainPath4();
-//                break;
-//            case 7:
-//                doPickUpThingAgain();
-//                break;
-//            case 8:
-//                moveToPickupAgainPath5();
-//                break;
-//            case 9:
-//                //theArmThingAgain();
-//                break;
-//            case 10:
-//                //moveToParkPath6();
-//                break;
-//            case 11:
-
+            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.toFloorPosition();
+                break;
+            default:
+                telemetry.addLine("default");
 
         }
         telemetry.update();
-
+        lift.update();
         follower.update();
 
 
     }
 
-    private void moveToPathOneAndHighBucket() {
-            path1.moveToPath1(follower);
-            state = 1;
+    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;
@@ -143,6 +160,7 @@ public class CometBotDriveV2 extends OpMode {
         public SetStateAction(int value) {
             this.value = value;
         }
+
         @Override
         public boolean run(@NonNull TelemetryPacket telemetryPacket) {
             state = value;
@@ -156,87 +174,92 @@ public class CometBotDriveV2 extends OpMode {
 
     private void doArmThing() {
 
+        claw.thumbOutOfTheWay();
+        wrist.toTravelPosition();
         lift.toHighBucketReverseDrop();
-        arm.toBucketPosition();
-        wrist.toBucketPosition();
-        claw.openClaw();
-        wrist.toFloorPosition();
+        //arm.toReverseBucket();
+        //wrist.toReverseBucket();
+        //claw.openClaw();
+        //wrist.toFloorPosition();
 
-        state = 2;
     }
 
 
-//    private void theArmThing() {
-//        telemetry.addData("busy?", follower.isBusy());
-//        telemetry.addData("end?", follower.atParametricEnd());
-//        if (follower.atParametricEnd()){
-//            follower.breakFollowing();
-//            comp.highBucketDropAuto();
-//            state = 6;
-//        }
-//        follower.breakFollowing();
-//    }
+    private void theArmThing() {
+        if(lift.getFixedPosition() >= 4280){
+            arm.toReverseBucket();
+            if(runtime.seconds() > 12) {
+                wrist.toReverseBucket();
+            }
+            if(runtime.seconds() > 12.75) {
+                claw.autoOpenClaw();
+                state = 6;
+            }
+        }
+    }
+
+    private void theArmThingAgain() {
+        if(lift.getFixedPosition() >= 4280){
+            arm.toReverseBucket();
+            if(runtime.seconds() > 22) {
+                wrist.toReverseBucket();
+            }
+            if(runtime.seconds() > 23.00) {
+                claw.autoOpenClaw();
+                state = 10;
+            }
+        }
+    }
 
-//    private void theArmThingAgain() {
-//        follower.breakFollowing();
-//        comp.highBucketDropAuto();
-//        state = 10;
-//    }
 
     private void moveToPathTwoAndPickSampleUp() {
-        if (!follower.isBusy()) {
+        if (runtime.seconds() > 3.25 && runtime.seconds() < 4) {
+            wrist.toReverseBucket();
+        }
+        if (runtime.seconds() > 3.5) {
+            claw.autoOpenClaw();
+        }
+        if (runtime.seconds() > 4) {
+            wrist.toTravelPosition();
+            lift.toFloorPosition();
             path2.moveToPath2(follower);
             state = 3;
         }
     }
-//
+
+    //
     private void moveToBasketPath3() {
-        if (!follower.isBusy()) {
+        if (runtime.seconds() > 7.25) {
+            lift.toHighBucketReverseDrop();
             path3.moveToBasketPath3(follower);
             state = 5;
         }
     }
 
-//    private void moveToPickupAgainPath4() {
-//        if (!followingPath) {
-//            path4.moveToPickupAgainPath4(follower);
-//            followingPath = true;
-//        }
-//        if (runtime != null) {
-//            telemetry.addData("Runtime (seconds)", runtime.seconds());
-//            if (follower.atParametricEnd() || runtime.seconds() > 27.0) {
-//                state = 7;
-//                followingPath = false;
-//            }
-//        }
-//    }
-//
-//    private void moveToPickupAgainPath5() {
-//        if (!followingPath) {
-//            path5.moveToPickupAgainPath5(follower);
-//            followingPath = true;
-//        }
-//        if (runtime != null) {
-//            telemetry.addData("Runtime (seconds)", runtime.seconds());
-//            if (follower.atParametricEnd() || runtime.seconds() > 36.0) {
-//                state = 9;
-//                followingPath = false;
-//            }
-//        }
-//    }
-//    private void moveToParkPath6() {
-//        if (!followingPath) {
-//            path6.moveToParkPath6(follower);
-//            followingPath = true;
-//        }
-//        if (runtime != null) {
-//            telemetry.addData("Runtime (seconds)", runtime.seconds());
-//            if (follower.atParametricEnd() || runtime.seconds() > 48.0) {
-//                state = 11;
-//                followingPath = false;
-//            }
-//        }
-//    }
+    private void moveToPickupAgainPath4() {
+        if (runtime.seconds() > 13) {
+            wrist.toTravelPosition();
+        }
+        if (runtime.seconds() > 14) {
+            lift.toFloorPosition();
+            path4.moveToPickupPath4(follower);
+            state = 7;
+        }
+    }
+
+
+
+    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) {
@@ -285,18 +308,24 @@ public class CometBotDriveV2 extends OpMode {
 //        ));
 //    }
 //
-//    private void doPickUpThing() {
-//        follower.breakFollowing();
-//        thePickUpAuto();
-//        state = 4;
-//    }
-//
-//    private void doPickUpThingAgain() {
-//        follower.breakFollowing();
-//        thePickUpAuto();
-//        state = 8;
-//    }
-//
-//
+    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,366 @@
+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.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.subsystems.ClawSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.DualMotorSliderSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.MotorsSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.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;
+
+    /*
+    Misc
+     */
+    private int hbCounter = 0;
+
+    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) {
+            switch(hbCounter) {
+                case 0:
+                    lift.toHighBucketPosition();
+                    hbCounter = 1;
+                    break;
+                case 1:
+                    wrist.toBucketPosition();
+                    arm.toBucketPosition();
+                    hbCounter = 2;
+                    break;
+                case 2:
+                    claw.openClaw();
+                    hbCounter = 3;
+                    break;
+                case 3:
+                    claw.closeClaw();
+                    wrist.toTravelPosition();
+                    arm.toParkPosition();
+                    hbCounter = 4;
+                    break;
+                case 4:
+                    lift.toFloorPosition();
+                    break;
+                default:
+                    break;
+            }
+            hbCounter = 1;
+            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,26 @@
+package org.firstinspires.ftc.teamcode.cometbots.paths;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+
+@TeleOp(name = "ComeBot Teleop Drive V2", group = "Competition")
+@Disabled
+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/CopyofCometBotDriveV2.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.subsystems.ArmSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.DualMotorSliderSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem;
+
+@Autonomous(name = "Copy of Auto Competition V2", group = "A")
+public class CopyofCometBotDriveV2 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 HighBasketPath6b 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 HighBasketPath6b();
+        //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.toFloorPosition();
+                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.autoOpenClaw();
+                state = 6;
+            }
+        }
+    }
+
+    private void theArmThingAgain() {
+        if(lift.getFixedPosition() >= 4280){
+            arm.toReverseBucket();
+            if(runtime.seconds() > 22) {
+                wrist.toReverseBucket();
+            }
+            if(runtime.seconds() > 23.00) {
+                claw.autoOpenClaw();
+                state = 10;
+            }
+        }
+    }
+
+
+    private void moveToPathTwoAndPickSampleUp() {
+        if (runtime.seconds() > 3.25 && runtime.seconds() < 4) {
+            wrist.toReverseBucket();
+        }
+        if (runtime.seconds() > 3.5) {
+            claw.autoOpenClaw();
+        }
+        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 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/HighBasketPath2.java

@@ -24,7 +24,7 @@ public class HighBasketPath2 {
                         // Line 1
                         new BezierLine(
                                 new Point(20.000, 130.000, Point.CARTESIAN),
-                                new Point(26.000, 117.000, Point.CARTESIAN)
+                                new Point(27.000, 117.000, Point.CARTESIAN)
                         )
                 )
                 .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(0));

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

@@ -23,7 +23,7 @@ public class HighBasketPath3 {
                         // Line 1
                         new BezierLine(
                                 new Point(26.000, 117.000, Point.CARTESIAN),
-                                new Point(10.000, 89.00, Point.CARTESIAN)
+                                new Point(22.000, 132.000, Point.CARTESIAN)
                         )
                 )
                 .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(320));

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/paths/HighBasketPath6b.java

@@ -0,0 +1,44 @@
+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 HighBasketPath6b {
+
+
+    public void moveToBasketPath6(Follower robot) {
+        PathChain pathChain;
+        PathBuilder builder = new PathBuilder();
+        builder
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(27.000, 128.000, Point.CARTESIAN),
+                                new Point(92.448, 125.671, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(315), Math.toRadians(270))
+                .addPath(
+                        // Line 2
+                        new BezierLine(
+                                new Point(92.448, 125.671, Point.CARTESIAN),
+                                new Point(93.000, 94.000, Point.CARTESIAN)
+                        )
+                )
+                .setTangentHeadingInterpolation();
+        pathChain = builder.build();
+        robot.followPath(pathChain);
+    }
+
+}
+

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

@@ -34,7 +34,7 @@ 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.subsystems.ArmSubsystem;
 
 @TeleOp(name = "Arm Test v2", group = "Debug")
 public class ArmTest extends LinearOpMode {

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

@@ -34,7 +34,7 @@ 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.ClawSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem;
 
 
 @TeleOp(name = "Claw Test v2", group = "Debug")

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

@@ -19,7 +19,7 @@ public class DualMotorSliderTest extends LinearOpMode {
 
     private DcMotorEx liftSlideLeft;
     private DcMotorEx liftSlideRight;
-    public static double kp = 0.0015, ki = 0, kd = 0;
+    public static double kp = 0.002, ki = 0, kd = 0;
     private double lastError = 0;
     private double integralSum = 0;
     public static int targetPosition = 0;

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

@@ -29,13 +29,12 @@
 
 package org.firstinspires.ftc.teamcode.cometbots.tests;
 
-import com.acmerobotics.roadrunner.ftc.Actions;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.hardware.Gamepad;
 import com.qualcomm.robotcore.util.ElapsedTime;
 
-import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem;
 
 @TeleOp(name = "Wrist Test v2", group = "Debug")
 public class WristTest extends LinearOpMode {

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

@@ -5,34 +5,45 @@ import com.acmerobotics.dashboard.config.Config;
 @Config
 public class RobotConstants {
     public final static double clawClose = 0.95;
-    public final static double clawOpen = 0.05;
+    public final static double clawOpen = 0.35;
+    public final static int autoClawOpen = 0;
+
 
     public final static double armFloor = 0.7;
     public final static double armSubmarine = 0.55;
     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.15;
-    public final static double wristGrabBlueberrySkyhook = 0.15;
-    public final static double armHangBlueberrySkyhook = 0.23;
-    public final static double wristHangBlueberrySkyhook = 0.7;
+    public final static double grabBlueberry = 0.56;
+    public final static double armGrabBlueberrySkyhook = 0.045;
+    public final static double wristGrabBlueberrySkyhook = 0.08;
+    public final static double armHangBlueberrySkyhook = 0.18;
+    public final static double wristHangBlueberrySkyhook = 0;
     public final static int slideHangBlueberrySkyhook = 500;
+
     public final static int slideBelowFloor = -150;
+    public final static int slideSpecimanHang = 900;
+    public final static int slideSpecimanRelease = 200;
     public final static int backwardBucketDrop = 4670;
     public final static double armBucket = 0.45;
-    public final static double armInit = 0.135;
-    public final static double wristInit = 0.25;
-    public final static double wristPickup = 0.475;
+    public final static double armSpecimen = 0.155;
+    public final static double armInit = 0.13;
+    public final static double wristInit = 0;
+    public final static double wristPickup = 0.425;
     public final static double wristBucket = 0.56;
+    public final static double wristSpecimenPrep = 0.63;
+    public final static double wristSpecimenHang = 0.53;
+
     public final static double wristFloor = 0.75;
-    public final static double wristBackwardBucket = 0.775;
+    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 int toBar = -7000;
+    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;
@@ -43,6 +54,6 @@ public class RobotConstants {
     public final static int liftToHighRungAttach = 1050;
 
 
-    public final static int liftToHighBucketPos = 4000;
+    public final static int liftToHighBucketPos = 4500;
     public final static double liftPower = 1;
 }

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

@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.subsystem;
+package org.firstinspires.ftc.teamcode.subsystems;
 
 import static org.firstinspires.ftc.teamcode.PedroConstants.ARM_SERVO;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armBucket;
@@ -8,6 +8,7 @@ import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armHangBlueb
 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 static org.firstinspires.ftc.teamcode.configs.RobotConstants.armSpecimen;
 
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.ServoImplEx;
@@ -15,7 +16,7 @@ import com.qualcomm.robotcore.hardware.ServoImplEx;
 public class ArmSubsystem {
 
     public enum ArmState {
-        PARK, FLOOR, BUCKET, SUBMARINE, GRAB_BLUEBERRY_SKYHOOK, HANG_BLUEBERRY_SKYHOOK, INIT, REVERE_BUCKET
+        PARK, FLOOR, BUCKET, SUBMARINE, GRAB_BLUEBERRY_SKYHOOK, HANG_BLUEBERRY_SKYHOOK, INIT, REVERE_BUCKET, SPECIMEN
     }
 
     private ServoImplEx arm;
@@ -45,11 +46,19 @@ public class ArmSubsystem {
         arm.setPosition(armFloor);
         setState(ArmState.FLOOR);
     }
+    public void toFloorPositionTeleOp() {
+        arm.setPosition(.68);
+        setState(ArmState.FLOOR);
+    }
 
     public void toBucketPosition() {
         arm.setPosition(armBucket);
         setState(ArmState.BUCKET);
     }
+    public void toSpecimenPrep() {
+        arm.setPosition(armSpecimen);
+        setState(ArmState.SPECIMEN);
+    }
     public void toInitPosition(){
         arm.setPosition(armInit);
         setState(ArmState.INIT);

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

@@ -1,9 +1,10 @@
-package org.firstinspires.ftc.teamcode.subsystem;
+package org.firstinspires.ftc.teamcode.subsystems;
 
 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.autoClawOpen;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.grabBlueberry;
 
 import com.qualcomm.robotcore.hardware.HardwareMap;
@@ -12,7 +13,7 @@ import com.qualcomm.robotcore.hardware.Servo;
 public class ClawSubsystem {
 
     public enum ClawState {
-        CLOSED, OPEN, GRAB_BLUEBERRY
+        CLOSED, OPEN, GRAB_BLUEBERRY, AUTO_OPEN
     }
 
     public enum ThumbState {
@@ -32,6 +33,7 @@ public class ClawSubsystem {
         claw.setPosition(grabBlueberry);
         state = ClawState.GRAB_BLUEBERRY;
     }
+
     public void closeClaw() {
         claw.setPosition(clawClose);
 
@@ -43,6 +45,11 @@ public class ClawSubsystem {
 
         state = ClawState.OPEN;
     }
+    public void autoOpenClaw() {
+        claw.setPosition(autoClawOpen);
+
+        state = ClawState.AUTO_OPEN;
+    }
 
     public ClawState getState() {
         return state;

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

@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.subsystem;
+package org.firstinspires.ftc.teamcode.subsystems;
 
 import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_LEFT_MOTOR;
 import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_RIGHT_MOTOR;
@@ -7,6 +7,8 @@ import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighBu
 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 static org.firstinspires.ftc.teamcode.configs.RobotConstants.slideSpecimanHang;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.slideSpecimanRelease;
 
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
@@ -41,7 +43,10 @@ public class DualMotorSliderSubsystem {
                                           a "getter" to get the target position value.
     */
     public void setTargetPosition(int value) {
-        targetPosition = value;
+        if(value >= liftToHighBucketPos)
+            targetPosition = liftToHighBucketPos;
+        else if(targetPosition <= liftToHighBucketPos)
+            targetPosition = value;
     }
     private int getTargetPosition() { return targetPosition; }
 
@@ -54,7 +59,7 @@ public class DualMotorSliderSubsystem {
     It's the only value we set because the variable ki and kd deal with how to handle when we're off the path.
     Since we're going straight, we don't need to worry about.
     */
-    public final static double kp = 0.0015, ki = 0, kd = 0;
+    public final static double kp = 0.002, ki = 0, kd = 0;
 
     /*
     lastError/integralSum/timer - These 3 variables are placeholders in determining how much
@@ -77,6 +82,9 @@ public class DualMotorSliderSubsystem {
     public int getCurrentPosition(){
         return liftSlideLeft.getCurrentPosition();
     }
+    public double getCurrentPower(){
+        return liftSlideLeft.getPower();
+    }
     public void init() {
         /*
         Initialize the motors with the following settings (assuming slide is at the very bottom position):
@@ -89,7 +97,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);
+        //rotation is different because spools spin opposite directions now
+        liftSlideLeft.setDirection(DcMotorSimple.Direction.FORWARD);
         liftSlideRight.setDirection(DcMotorSimple.Direction.REVERSE);
         liftSlideRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
         liftSlideRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
@@ -106,8 +115,12 @@ public class DualMotorSliderSubsystem {
     */
     public void update() {
         double power = calculatePower();
+        if(power < 0.1 && power > 0.05){
+            power = 0.12;
+        }
         liftSlideLeft.setPower(power);
         liftSlideRight.setPower(power);
+
     }
 
     /*
@@ -122,6 +135,9 @@ public class DualMotorSliderSubsystem {
     */
     private double calculatePower() {
         double error = getTargetPosition() - liftSlideLeft.getCurrentPosition();
+        if(error < 20 && error > 0){
+            error = 20;
+        }else if(error > -20 && error < 0)
         integralSum += error * timer.seconds();
         double derivative = (error - lastError) / timer.seconds();
         lastError = error;
@@ -140,15 +156,43 @@ public class DualMotorSliderSubsystem {
     public void toFixedPosition(int value) {
         setTargetPosition(value);
     }
-    public void toHangHeight(){
-        setTargetPosition(slideHangBlueberrySkyhook);
 
+    /**
+     * go to a specified position (measure in CM)
+     * Measured from the top of the left Slide
+     *At the floor, measurement is 34 cm
+     * @param centimeters height you want to go to, lowest is 34
+     * UNFINISHED
+     */
+    public void toCentimeterMeasurement(int centimeters){
+        int ticksPerCm = 60;
+        int HeightWhenAtFloor = 34;
+        if(centimeters > 0) {
+            liftSlideLeft.setTargetPosition((centimeters+ 34)* ticksPerCm);
+            liftSlideRight.setTargetPosition((centimeters+ 34) * ticksPerCm);
+        }
     }
+    public int getCM(){
+        int ticksPerCm = 60;
+        int HeightWhenAtFloor = 34;
+        return 34 +(liftSlideLeft.getCurrentPosition()/ticksPerCm);
+    }
+    public void toHangHeight(){setTargetPosition(slideHangBlueberrySkyhook);}
     public void toHangBelowFloor(){
         setTargetPosition(slideBelowFloor);
 
     }
 
+    public void toSpecimanHangHeight(){
+        setTargetPosition(slideSpecimanHang);
+    }
+
+    public void toSpecimanReleaseHeight(){
+        setTargetPosition(slideSpecimanRelease);
+    }
+
+
+
     public int getFixedPosition() {
         return liftSlideLeft.getCurrentPosition();
     }

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

@@ -1,8 +1,6 @@
-package org.firstinspires.ftc.teamcode.subsystem;
+package org.firstinspires.ftc.teamcode.subsystems;
 
 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;
@@ -15,10 +13,7 @@ public class HangMotorSubsystem {
     private DcMotorEx hang;
 
     public HangMotorSubsystem(HardwareMap hardwareMap) {
-
         hang = hardwareMap.get(DcMotorEx.class, HOOK);
-
-
     }
 
     public int getCurrentPosition(){
@@ -26,25 +21,33 @@ public class HangMotorSubsystem {
         return Return;
     }
     public void init(){
-        hang.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        //init hook, set runmodes
+        //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);
+        hang.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
 
     }
     public void hangRobot(){
+        hang.setMode(DcMotor.RunMode.RUN_TO_POSITION);
         hang.setTargetPosition(toBar);
-
+        while(hang.isBusy()){
+            hang.setPower(1);
+        }
     }
     public void robotToFloor(){
         hang.setTargetPosition(toFloor);
     }
     public void disableMotor(){
         hang.setPower(0);
-        hang.setMotorDisable();
 
     }
-    public void setPower(double power){
+    public void setPowerForward(double power){
+        hang.setDirection(DcMotorSimple.Direction.FORWARD);
+        hang.setPower(power);
+    }
+    public void setPowerReverse(double power){
+        hang.setDirection(DcMotorSimple.Direction.REVERSE);
         hang.setPower(power);
     }
     public void stopMotor(){
@@ -56,5 +59,12 @@ public class HangMotorSubsystem {
         //write in limits for protection
         hang.setPower(Position);
     }
+    public void setPower(boolean forward, double power){
+        if(forward)
+        hang.setDirection(DcMotorSimple.Direction.FORWARD);
+        else if(!forward)
+            hang.setDirection(DcMotorSimple.Direction.REVERSE);
+        hang.setPower(power);
+    }
 
 }

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

@@ -0,0 +1,97 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+@TeleOp(name = "Hook Test", group = "Debug")
+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();
+        claw.thumbUp();
+    }
+
+    @Override
+    public void loop() {
+        telemetry.addData("hook", skyhook.getCurrentPosition());
+        telemetry.addData("arm", arm.getState());
+        telemetry.addData("wrist", wrist.getState());
+        telemetry.addData("slides", slides.getCurrentPosition());
+        telemetry.update();
+        slides.update();
+
+
+        if (gamepad1.a) {
+            claw.grabBlueberry();
+            arm.setPosition(0.15);
+            arm.grabBlueberrySkyhook();
+            telemetry.update();
+            //claw Open small amount
+            wrist.grabBlueberrySkyhook();
+            //wrist grab in strange way
+            telemetry.update();
+
+
+            }
+        if(gamepad1.b) {
+            //confirm grab
+            claw.closeClaw();
+
+        }
+
+        if (gamepad1.x) {
+            //now slap on bar, first wrist, then arm, then claw then driver must drive away
+            slides.toFixedPosition(500);
+            slides.update();
+        }
+        if (gamepad1.y) {
+            arm.hangBlueberrySkyhook();
+            wrist.hangBlueberrySkyhook();
+            try {
+                Thread.sleep(500);
+            } catch (InterruptedException e) {
+                throw new RuntimeException(e);
+            }
+            slides.toFixedPosition(1800);
+            slides.update();
+        }
+        if (gamepad1.right_bumper) {
+            claw.openClaw();
+        }
+        if (gamepad1.dpad_up && claw.getState() == ClawSubsystem.ClawState.OPEN) {
+            skyhook.hangRobot();
+        }
+        if (gamepad1.left_trigger > 0) {
+            skyhook.disableMotor();
+            skyhook.setPowerReverse(1.0);
+        }
+        else if(gamepad1.left_trigger == 0){
+            skyhook.setPowerReverse(0.0);
+        }
+        if (gamepad1.right_trigger > 0) {
+            skyhook.disableMotor();
+            skyhook.setPowerForward(1.0);
+        }
+        else if(gamepad1.right_trigger == 0){
+            skyhook.setPowerForward(0.0);
+        }
+
+
+
+    }
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Inspection.java

@@ -0,0 +1,58 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+import static org.firstinspires.ftc.teamcode.PedroConstants.MAX_POWER;
+
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.DualMotorSliderSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.HangMotorSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem;
+@TeleOp(name = "Inspection", group = "Debug")
+public class Inspection extends OpMode {
+    private DualMotorSliderSubsystem dualSlides;
+    private ClawSubsystem claw;
+    private WristSubsystem wrist;
+    private ArmSubsystem arm;
+    private HangMotorSubsystem skyhook;
+
+    @Override
+    public void init() {
+        dualSlides = new DualMotorSliderSubsystem(hardwareMap);
+        claw = new ClawSubsystem(hardwareMap);
+        arm = new ArmSubsystem(hardwareMap);
+        wrist = new WristSubsystem(hardwareMap);
+        skyhook = new HangMotorSubsystem(hardwareMap);
+        dualSlides.init();
+        claw.init();
+        wrist.initTeleOp();
+        arm.initTeleOp();
+        skyhook.init();
+    }
+
+    @Override
+    public void loop() {
+        telemetry.addData("a: ", "init");
+        telemetry.addData("b: ", "armFloor/WristFloor");
+        telemetry.addData("y: ", "GrabSkyhook");
+        if(gamepad1.a){
+            wrist.InitAuto();
+            arm.initAuto();
+        }
+        if(gamepad1.b){
+            arm.toFloorPosition();
+            wrist.toFloorPositionTeleop();
+        }
+        if (gamepad1.y) {
+            claw.grabBlueberry();
+            arm.setPosition(0.15);
+            arm.grabBlueberrySkyhook();
+
+            //claw Open small amount
+            wrist.grabBlueberrySkyhook();
+            //wrist grab in strange way
+        }
+    }
+}

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

@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.subsystem;
+package org.firstinspires.ftc.teamcode.subsystems;
 
 /* Copyright (c) 2021 FIRST. All rights reserved.
  *
@@ -29,19 +29,11 @@ package org.firstinspires.ftc.teamcode.subsystem;
  * 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 {
@@ -94,11 +86,11 @@ public class LiftArmWrist extends LinearOpMode {
             }
 
             if (currentGamepad1.dpad_right && !previousGamepad1.dpad_right) {
-                lift.toHighBucketPosition();
+                lift.toSpecimanHangHeight();
             }
 
             if (currentGamepad1.dpad_left && !previousGamepad1.dpad_left) {
-                lift.toLowBucketPosition();
+                lift.toSpecimanReleaseHeight();
             }
 
             if (currentGamepad1.y && !previousGamepad1.y) {
@@ -111,19 +103,14 @@ public class LiftArmWrist extends LinearOpMode {
 
 
             if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
-               lift.toHighBucketPosition();
+               lift.setTargetPosition(lift.getCurrentPosition() + 250);
             }
 
             if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) {
-                lift.toLowBucketPosition();
+                lift.setTargetPosition(lift.getCurrentPosition() - 250);
 
             }
-            if(currentGamepad1.left_trigger > 0){
-                hang.setPower((double) currentGamepad1.left_trigger);
-            }
-            if(currentGamepad1.right_trigger > 0){
-                hang.setPower((double) currentGamepad1.right_trigger);
-            }
+
 
             lift.update();
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/MotorsSubsystem.java

@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.subsystem;
+package org.firstinspires.ftc.teamcode.subsystems;
 
 import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR;
 import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION;
@@ -10,12 +10,7 @@ import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR;
 import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION;
 import static org.firstinspires.ftc.teamcode.PedroConstants.MAX_POWER;
 
-import androidx.annotation.NonNull;
-
-import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
-import com.acmerobotics.roadrunner.Action;
 import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.Gamepad;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;

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

@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.subsystem;
+package org.firstinspires.ftc.teamcode.subsystems;
 
 import static org.firstinspires.ftc.teamcode.PedroConstants.WRIST_SERVO;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristBackwardBucket;
@@ -8,20 +8,21 @@ import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristGrabBlu
 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.wristSpecimenHang;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristSpecimenPrep;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristtravel;
 
 import androidx.annotation.NonNull;
 
 import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
 import com.acmerobotics.roadrunner.Action;
-import com.acmerobotics.roadrunner.ftc.Actions;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.ServoImplEx;
 
 public class WristSubsystem {
 
     public enum WristState {
-        FLOOR, BUCKET, PICKUP, RUNG, SPECIMEN, GRAB_BLUEBERRY_SKYHOOK, HANG_BLUEBERRY_SKYHOOK, INIT, REVERSE_BUCKET
+        FLOOR, BUCKET, PICKUP, RUNG, SPECIMEN, GRAB_BLUEBERRY_SKYHOOK, HANG_BLUEBERRY_SKYHOOK, INIT, REVERSE_BUCKET, TRAVEL
     }
 
     public ServoImplEx wrist;
@@ -63,11 +64,26 @@ public class WristSubsystem {
         wrist.setPosition(wristFloor);
     }
 
+    public void toFloorPositionTeleop() {
+        setState(WristState.FLOOR);
+        wrist.setPosition(0.69);
+    }
+
     public void toBucketPosition() {
         setState(WristState.BUCKET);
         wrist.setPosition(wristBucket);
     }
 
+    public void toSpecimenPrep() {
+        setState(WristState.SPECIMEN);
+        wrist.setPosition(wristSpecimenPrep);
+    }
+
+    public void toSpecimenHang() {
+        setState(WristState.SPECIMEN);
+        wrist.setPosition(wristSpecimenHang);
+    }
+
     public void toPickupPosition() {
         setState(WristState.PICKUP);
         wrist.setPosition(wristPickup);
@@ -97,13 +113,18 @@ public class WristSubsystem {
         wrist.setPosition(wristBackwardBucket);
         setState(WristState.REVERSE_BUCKET);
     }
+    public void toTravelPosition(){
+        wrist.setPosition(wristtravel);
+        setState(WristState.TRAVEL);
+    }
+
     public WristState getState() {
         return this.state;
     }
 
     public void initTeleOp() {
         wrist.resetDeviceConfigurationForOpMode();
-        toPickupPosition();
+        toFloorPositionTeleop();
     }
 
     public void setPosition(double position) {