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.subsystems.ArmSubsystem;
-import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem;
-import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.DualMotorSliderSubsystem;
-import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.HangMotorSubsystem;
-import org.firstinspires.ftc.teamcode.subsystem.HangMotorSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem;
-import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem;
-import org.firstinspires.ftc.teamcode.subsystem.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,114 +86,130 @@ public class CometBotAutoDevelopment {
         openClaw();
         openThumb();
         //hang
-        grabBlueberrySkyhook();
+        hang();
-        hangSkyhook();
-        robotDown();
-        robotUp();
-        stopHook();
         follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x);
         follower.update();
     }
-    private void grabBlueberrySkyhook () {
+    private void openClaw() {
-        if(currentGamepad1.x){
+        if (currentGamepad2.right_bumper && !previousGamepad2.right_bumper) {
-            claw.grabBlueberry();
-            wrist.grabBlueberrySkyhook();
-            arm.grabBlueberrySkyhook();
-        }
-
-        /*if(arm.getState() == ArmSubsystem.ArmState.GRAB_BLUEBERRY_SKYHOOK)
-            claw.closeClaw();*/
-    }
-    private void hangSkyhook() {
-        if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
-            dualSlides.toHangHeight();
-            //check wrist make go throught test files again
-            wrist.hangBlueberrySkyhook();
-            arm.hangBlueberrySkyhook();
-            if (arm.getState() == ArmSubsystem.ArmState.HANG_BLUEBERRY_SKYHOOK) {
-                try {
-                    Thread.sleep(2000);
-                } catch (InterruptedException e) {
-                    throw new RuntimeException(e);
-                }
-            }
-        }
-    }private void openClaw(){
-        if(currentGamepad2.right_bumper && !previousGamepad2.right_bumper){
             claw.switchState();
         }
     }
-    private void openThumb(){
+
-        if(currentGamepad2.left_bumper && !previousGamepad2.left_bumper){
+    private void openThumb() {
+        if (currentGamepad2.left_bumper && !previousGamepad2.left_bumper) {
             claw.switchTState();
+
         }
     }
-    private void armAndWristToFloor(){
+
-        if(currentGamepad2.a && !previousGamepad2.a){
+    private void armAndWristToFloor() {
-            double increment = 0.7 - arm.getPosition();
+        if (currentGamepad2.a && !previousGamepad2.a && wristPickup) {
-            for(int i = 0; i < 3; i ++){
+            if (wrist.getState() != WristSubsystem.WristState.FLOOR) {
-                arm.setPosition(arm.getPosition() + increment);
+                wrist.toFloorPositionTeleop();
-                try {
+            } else if (wristPickup) {
-                    Thread.sleep(50);
+                claw.openClaw();
-                } catch (InterruptedException e) {
+                wrist.toPickupPosition();
-                    throw new RuntimeException(e);
-                }
             }
-            arm.toFloorPosition();
         }
-        if(currentGamepad2.a && !previousGamepad2.a ){
+        if (currentGamepad2.a && !previousGamepad2.a) {
-            wrist.switchState();
+            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

@@ -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 {
 
@@ -54,6 +53,11 @@ public class CometBotTeleopCompetition {
      */
     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);
 
@@ -175,12 +179,38 @@ public class CometBotTeleopCompetition {
      */
     public void toHighBucketScore() {
         if (this.currentGP2.y && !this.previousGP2.y) {
-            lift.toHighBucketPosition();
+            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)
+                if(arm.getPosition() == 0.45);
                     wrist.toBucketPosition();
-                    if(wrist.getPosition() == 0.56)
+                    if(wrist.getPosition() == 0.56);
                         claw.openClaw();
             }
         }

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

@@ -1,10 +1,12 @@
 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;

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/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,26 +5,35 @@ 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 grabBlueberry = 0.56;
-    public final static double armGrabBlueberrySkyhook = 0.1;
+    public final static double armGrabBlueberrySkyhook = 0.045;
-    public final static double wristGrabBlueberrySkyhook = 0.15;
+    public final static double wristGrabBlueberrySkyhook = 0.08;
-    public final static double armHangBlueberrySkyhook = 0.23;
+    public final static double armHangBlueberrySkyhook = 0.18;
-    public final static double wristHangBlueberrySkyhook = 0.7;
+    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 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.475;
+    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.725;
 
@@ -32,7 +41,7 @@ public class RobotConstants {
 
     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;

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,14 +8,14 @@ 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;
 
@@ -64,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);
@@ -109,7 +124,7 @@ public class WristSubsystem {
 
     public void initTeleOp() {
         wrist.resetDeviceConfigurationForOpMode();
-        toPickupPosition();
+        toFloorPositionTeleop();
     }
 
     public void setPosition(double position) {