Merge remote-tracking branch 'origin/branch-swiss-cheese' into branch-swiss-cheese

# 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")
 

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 = true;
             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(500);
             dualSlides.update();
         }
+
         if (gamepad1.y) {
             arm.hangBlueberrySkyhook();
             wrist.hangBlueberrySkyhook();
             try {
-                Thread.sleep(1500);
+                Thread.sleep(500);
             } catch (InterruptedException e) {
                 throw new RuntimeException(e);
             }
-            dualSlides.toFixedPosition(2100);
+            goClaw = true;
+            dualSlides.toFixedPosition(2200);
             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,10 +10,10 @@ 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")
 public class CometBotDriveV2 extends OpMode {

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/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,8 +4,10 @@ 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 double armFloor = 0.7;
     public final static double armSubmarine = 0.55;
@@ -13,8 +15,8 @@ public class RobotConstants {
     public final static double armPark = 0.33;
     //value for grabbing the hook Specimen
     public final static double grabBlueberry = 0.69;
-    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,4 +1,4 @@
-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;

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,6 +21,7 @@ public class HangMotorSubsystem {
         return Return;
     }
     public void init(){
+        //init hook, set runmodes
         hang.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
         hang.setDirection(DcMotorSimple.Direction.FORWARD);
         hang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
@@ -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) {