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/CometBotTeleOpCompetition.java

@@ -4,21 +4,27 @@ 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/SpecimenAuto.java

@@ -1,21 +1,16 @@
 package org.firstinspires.ftc.teamcode;
 
-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.hardware.Gamepad;
-import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.util.ElapsedTime;
 
-import org.firstinspires.ftc.robotcore.external.State;
 import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
-import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem;
-import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem;
-import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.DualMotorSliderSubsystem;
-import org.firstinspires.ftc.teamcode.subsystem.HangMotorSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.HangMotorSubsystem;
-import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem;
 
 @Autonomous(name = "Specimen Auto", group = "Development")
 
@@ -48,6 +43,7 @@ public class SpecimenAuto extends OpMode {
          wrist.InitAuto();
          arm.initAuto();
 
+
         follower.setMaxPower(.45);
 
 
@@ -55,42 +51,49 @@ public class SpecimenAuto extends OpMode {
 
     @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();
-                 state = 1;
+                 if(runtime.seconds() > 0.25){state = 1;}
-                 new SleepAction(5);
                  break;
              case 1:
-                state = 2;
+                 //line 1
+                 if(runtime.seconds() > 3){state = 2;}
                  break;
              case 2:
-                 new SleepAction(5);
+                 lift.toSpecimanHangHeight();
-                 state = 3;
+                 if(runtime.seconds() > 3.75){state = 3;}
                  break;
              case 3:
-
+                wrist.toSpecimenHang();
-                 new SleepAction(5);
+                 if(runtime.seconds() > 4){state = 4;}
-                 state = 4;
                  break;
              case 4:
-
+                lift.toSpecimanReleaseHeight();
-                 new SleepAction(5);
+                 if(runtime.seconds() > 4.5){state = 5;}
-                 state = 5;
                  break;
              case 5:
-                 //specimen drop
+                 claw.switchState();
+                 if(runtime.seconds() > 4.65){state = 6;}
                  break;
              case 6:
-                 //path 4
+                 lift.toFloorPosition();
+                 if(runtime.seconds() > 4.75){state = 7;}
                  break;
              case 7:
-                 //specimen pick up
+                 arm.toParkPosition();
+                 wrist.toTravelPosition();
+                 //line 2
+                 if(runtime.seconds() > 0.){state = 8;}
                  break;
              case 8:
-                 //path 5
+                 if(runtime.seconds() > 0.25){state = 9;}
                  break;
              case 9:
                  //specimen drop

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpecimenAutoLineTest.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 = "Specimen Line Test", group = "Competition")
 public class SpecimenAutoLineTest extends OpMode {
     private Telemetry telemetryA;
 
@@ -40,15 +40,15 @@ public class SpecimenAutoLineTest extends OpMode {
                                 new Point(39.500, 60.000, Point.CARTESIAN)
                         )
                 )
-               .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180))
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(0))
-/*                .addPath(
+                .addPath(
                         // Line 2
                         new BezierLine(
                                 new Point(39.500, 60.000, Point.CARTESIAN),
                                 new Point(37.000, 60.000, Point.CARTESIAN)
                         )
                 )
-                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .setConstantHeadingInterpolation(Math.toRadians(0))
                 .addPath(
                         // Line 3
                         new BezierCurve(
@@ -58,7 +58,7 @@ public class SpecimenAutoLineTest extends OpMode {
                                 new Point(58.000, 23.500, Point.CARTESIAN)
                         )
                 )
-                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .setConstantHeadingInterpolation(Math.toRadians(0))
                 .addPath(
                         // Line 4
                         new BezierLine(
@@ -66,7 +66,7 @@ public class SpecimenAutoLineTest extends OpMode {
                                 new Point(14.000, 23.500, Point.CARTESIAN)
                         )
                 )
-                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .setConstantHeadingInterpolation(Math.toRadians(0))
                 .addPath(
                         // Line 5
                         new BezierLine(
@@ -155,7 +155,7 @@ public class SpecimenAutoLineTest extends OpMode {
                                 new Point(37.000, 68.000, Point.CARTESIAN)
                         )
                 )
-                .setConstantHeadingInterpolation(Math.toRadians(180))*/.build();
+                .setConstantHeadingInterpolation(Math.toRadians(180)).build();
         follower.followPath(path);
 
         telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());

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

@@ -1,17 +1,16 @@
 package org.firstinspires.ftc.teamcode.cometbots;
 
-import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR;
 import static org.firstinspires.ftc.teamcode.PedroConstants.MAX_POWER;
 
 import com.qualcomm.robotcore.hardware.Gamepad;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
 import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
-import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem;
-import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem;
-import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.DualMotorSliderSubsystem;
-import org.firstinspires.ftc.teamcode.subsystem.HangMotorSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.HangMotorSubsystem;
-import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem;
 
 public class CometBotTeleOpDevelopment {
 
@@ -32,9 +31,10 @@ public class CometBotTeleOpDevelopment {
     public Gamepad currentGamepad2;
     public Gamepad previousGamepad1;
     public Gamepad previousGamepad2;
-    public boolean grabBlock;
-
 
+    public boolean wristPickup;
+    public boolean armParked;
+    public boolean goClaw;
     private Follower follower;
 
     public CometBotTeleOpDevelopment(HardwareMap hardwareMap, Gamepad gamepad1, Gamepad gamepad2) {
@@ -43,7 +43,7 @@ public class CometBotTeleOpDevelopment {
         arm = new ArmSubsystem(hardwareMap);
         wrist = new WristSubsystem(hardwareMap);
         skyhook = new HangMotorSubsystem(hardwareMap);
-        grabBlock = false;
+
         this.gamepad1 = gamepad1;
         this.gamepad2 = gamepad2;
         currentGamepad1 = new Gamepad();
@@ -51,6 +51,7 @@ public class CometBotTeleOpDevelopment {
         previousGamepad1 = new Gamepad();
         previousGamepad2 = new Gamepad();
         follower = new Follower(hardwareMap);
+
     }
 
     public void init() {
@@ -61,6 +62,9 @@ public class CometBotTeleOpDevelopment {
         skyhook.init();
         follower.setMaxPower(MAX_POWER);
         follower.startTeleopDrive();
+        wristPickup = false;
+        armParked = true;
+        goClaw = false;
     }
 
     public void update() {
@@ -82,15 +86,10 @@ public class CometBotTeleOpDevelopment {
         openClaw();
         openThumb();
         //hang
-
         hang();
         follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x);
         follower.update();
     }
-
-
-
-
     private void openClaw() {
         if (currentGamepad2.right_bumper && !previousGamepad2.right_bumper) {
             claw.switchState();
@@ -100,58 +99,68 @@ public class CometBotTeleOpDevelopment {
     private void openThumb() {
         if (currentGamepad2.left_bumper && !previousGamepad2.left_bumper) {
             claw.switchTState();
+
         }
     }
 
     private void armAndWristToFloor() {
+        if (currentGamepad2.a && !previousGamepad2.a && wristPickup) {
+            if (wrist.getState() != WristSubsystem.WristState.FLOOR) {
+                wrist.toFloorPositionTeleop();
+            } else if (wristPickup) {
+                claw.openClaw();
+                wrist.toPickupPosition();
+            }
+        }
         if (currentGamepad2.a && !previousGamepad2.a) {
             arm.toFloorPositionTeleOp();
-            if (arm.getState() == ArmSubsystem.ArmState.FLOOR) {
+
-                wrist.switchState();
+            wristPickup = true;
-            } else if (arm.getState() != ArmSubsystem.ArmState.FLOOR) {
+            armParked = false;
-                wrist.toPickupPosition();
-            } else {
-                wrist.toFloorPosition();
-            }
         }
     }
 
     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) {
+            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 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.13);
+            arm.setPosition(0.15);
             arm.grabBlueberrySkyhook();
 
             //claw Open small amount
@@ -161,51 +170,46 @@ public class CometBotTeleOpDevelopment {
         if(gamepad1.b) {
             //confirm grab
             claw.closeClaw();
-            grabBlock = true;
+
         }
 
-        if (gamepad1.x && grabBlock) {
+        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(300);
+            dualSlides.toFixedPosition(200);
             dualSlides.update();
         }
+
         if (gamepad1.y) {
-            arm.hangBlueberrySkyhook();
             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();
-            try {
+            if(goClaw == true) {
-                Thread.sleep(1000);
+                try {
-            } catch (InterruptedException e) {
+                    Thread.sleep(500);
-                throw new RuntimeException(e);
+                } catch (InterruptedException e) {
+                    throw new RuntimeException(e);
+                }
+                dualSlides.toFloorPosition();
+                dualSlides.update();
+                arm.toParkPosition();
+                wrist.toPickupPosition();
             }
-            dualSlides.toFloorPosition();
-            claw.closeClaw();
-            arm.toParkPosition();
-            wrist.toFloorPosition();
         }
-        skyhook.setPower(0);
+        skyhook.setPowerForward(-gamepad2.right_stick_y);
+
 
-        /*if (gamepad1.dpad_down) {
-            skyhook.disableMotor();
-            skyhook.setPower(1);
-        }*/
-        if(gamepad1.right_trigger  > 0){
-            skyhook.setPower(true, 1);
 
-        }
-        if(gamepad1.left_trigger  > 0){
-            skyhook.setPower(false, 1);
 
-        }
     }
 
 }

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

@@ -10,12 +10,12 @@ import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 import com.qualcomm.robotcore.util.ElapsedTime;
 
 import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
-import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem;
-import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem;
-import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.DualMotorSliderSubsystem;
-import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
+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;
@@ -118,7 +118,7 @@ public class CometBotDriveV2 extends OpMode {
                 moveToPark();
                 break;
             case 99:
-                wrist.toTravelPosition();
+                wrist.toFloorPosition();
                 break;
             default:
                 telemetry.addLine("default");
@@ -192,7 +192,7 @@ public class CometBotDriveV2 extends OpMode {
                 wrist.toReverseBucket();
             }
             if(runtime.seconds() > 12.75) {
-                claw.openClaw();
+                claw.autoOpenClaw();
                 state = 6;
             }
         }
@@ -205,7 +205,7 @@ public class CometBotDriveV2 extends OpMode {
                 wrist.toReverseBucket();
             }
             if(runtime.seconds() > 23.00) {
-                claw.openClaw();
+                claw.autoOpenClaw();
                 state = 10;
             }
         }
@@ -217,7 +217,7 @@ public class CometBotDriveV2 extends OpMode {
             wrist.toReverseBucket();
         }
         if (runtime.seconds() > 3.5) {
-            claw.openClaw();
+            claw.autoOpenClaw();
         }
         if (runtime.seconds() > 4) {
             wrist.toTravelPosition();
@@ -247,9 +247,7 @@ public class CometBotDriveV2 extends OpMode {
         }
     }
 
-    private void moveToPickupAgainPath5() {
 
-    }
 
     private void moveToParkPath6() {
         if (runtime.seconds() > 24.5) {

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

@@ -3,7 +3,6 @@ package org.firstinspires.ftc.teamcode.cometbots.paths;
 import static org.firstinspires.ftc.teamcode.PedroConstants.MAX_POWER;
 
 import com.acmerobotics.roadrunner.SequentialAction;
-import com.acmerobotics.roadrunner.SleepAction;
 import com.acmerobotics.roadrunner.ftc.Actions;
 import com.qualcomm.robotcore.hardware.Gamepad;
 import com.qualcomm.robotcore.hardware.HardwareMap;
@@ -11,11 +10,11 @@ import com.qualcomm.robotcore.hardware.HardwareMap;
 import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
 import org.firstinspires.ftc.teamcode.states.FieldStates;
-import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem;
-import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem;
-import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.DualMotorSliderSubsystem;
-import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.MotorsSubsystem;
-import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem;
 
 public class CometBotTeleopCompetition {
 

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/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/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

@@ -4,17 +4,19 @@ import com.acmerobotics.dashboard.config.Config;
 
 @Config
 public class RobotConstants {
-    public final static double clawClose = 1;
+    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.69;
+    public final static double grabBlueberry = 0.56;
-    public final static double armGrabBlueberrySkyhook = 0.05;
+    public final static double armGrabBlueberrySkyhook = 0.045;
-    public final static double wristGrabBlueberrySkyhook = 0.1;
+    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;
@@ -26,7 +28,7 @@ public class RobotConstants {
     public final static double armBucket = 0.45;
     public final static double armSpecimen = 0.155;
     public final static double armInit = 0.13;
-    public final static double wristInit = 0.125;
+    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;

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;

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;
@@ -43,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; }
 
@@ -79,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):
@@ -91,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);
@@ -108,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);
+
     }
 
     /*
@@ -124,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;
@@ -142,6 +156,27 @@ public class DualMotorSliderSubsystem {
     public void toFixedPosition(int value) {
         setTargetPosition(value);
     }
+
+    /**
+     * 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);

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;
@@ -23,10 +21,11 @@ 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(){
@@ -43,7 +42,12 @@ public class HangMotorSubsystem {
         hang.setPower(0);
 
     }
-    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(){

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

@@ -1,15 +1,9 @@
-package org.firstinspires.ftc.teamcode.subsystem;
+package org.firstinspires.ftc.teamcode.subsystems;
 
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-import com.qualcomm.robotcore.hardware.HardwareMap;
 
-import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
+@TeleOp(name = "Hook Test", group = "Debug")
-import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
-import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem;
-import org.firstinspires.ftc.teamcode.subsystem.HangMotorSubsystem;
-import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
-@TeleOp(name = "Hook Test")
 public class HookTest extends OpMode {
 
     private HangMotorSubsystem skyhook;
@@ -81,10 +75,19 @@ public class HookTest extends OpMode {
         if (gamepad1.dpad_up && claw.getState() == ClawSubsystem.ClawState.OPEN) {
             skyhook.hangRobot();
         }
-        skyhook.setPower(0);
+        if (gamepad1.left_trigger > 0) {
-        if (gamepad1.dpad_down) {
             skyhook.disableMotor();
-            skyhook.setPower(1);
+            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 {
@@ -118,12 +110,7 @@ public class LiftArmWrist extends LinearOpMode {
                 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,7 +8,6 @@ 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;
@@ -17,7 +16,6 @@ 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;
 
@@ -68,7 +66,7 @@ public class WristSubsystem {
 
     public void toFloorPositionTeleop() {
         setState(WristState.FLOOR);
-        wrist.setPosition(.425);
+        wrist.setPosition(0.69);
     }
 
     public void toBucketPosition() {
@@ -126,7 +124,7 @@ public class WristSubsystem {
 
     public void initTeleOp() {
         wrist.resetDeviceConfigurationForOpMode();
-        toPickupPosition();
+        toFloorPositionTeleop();
     }
 
     public void setPosition(double position) {