commit fixed arm bugs

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.subsystem.ClawSubsystem;
-import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem;
-import org.firstinspires.ftc.teamcode.subsystem.HangMotorSubsystem;
-import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.DualMotorSliderSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.HangMotorSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem;
 
 @Autonomous(name = "Specimen Auto", group = "Development")
 
@@ -67,16 +62,17 @@ public class SpecimenAuto extends OpMode {
                 state = 2;
                  break;
              case 2:
+                 lift.toSpecimanHangHeight();
                  new SleepAction(5);
                  state = 3;
                  break;
              case 3:
-
+                 wrist.toSpecimenHang();
                  new SleepAction(5);
                  state = 4;
                  break;
              case 4:
-
+                 lift.toSpecimanReleaseHeight();
                  new SleepAction(5);
                  state = 5;
                  break;

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.subsystem.ClawSubsystem;
-import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem;
-import org.firstinspires.ftc.teamcode.subsystem.HangMotorSubsystem;
-import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.DualMotorSliderSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.HangMotorSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem;
 
 public class 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();
-            } else if (arm.getState() != ArmSubsystem.ArmState.FLOOR) {
-                wrist.toPickupPosition();
-            } else {
-                wrist.toFloorPosition();
-            }
+
+            wristPickup = true;
+            armParked = false;
         }
     }
 
     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 {
-                Thread.sleep(1000);
-            } catch (InterruptedException e) {
-                throw new RuntimeException(e);
+            if(goClaw == true) {
+                try {
+                    Thread.sleep(500);
+                } 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.subsystem.ClawSubsystem;
-import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem;
-import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.DualMotorSliderSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.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.subsystem.ArmSubsystem;
-import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem;
-import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem;
-import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.DualMotorSliderSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.MotorsSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem;
 
 public class CometBotTeleopCompetition {
 

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.002, ki = 0, kd = 0;
+    public static double kp = 0.0015, 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

@@ -4,8 +4,10 @@ import com.acmerobotics.dashboard.config.Config;
 
 @Config
 public class RobotConstants {
-    public final static double clawClose = 1;
-    public final static double clawOpen = 0.05;
+    public final static double clawClose = 0.95;
+    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 wristGrabBlueberrySkyhook = 0.1;
+    public final static double armGrabBlueberrySkyhook = 0.045;
+    public final static double wristGrabBlueberrySkyhook = 0.08;
     public final static double armHangBlueberrySkyhook = 0.18;
     public final static double wristHangBlueberrySkyhook = 0;
     public final static int slideHangBlueberrySkyhook = 500;
@@ -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; }
 
@@ -56,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.002, ki = 0, kd = 0;
+    public final static double kp = 0.0015, ki = 0, kd = 0;
 
     /*
     lastError/integralSum/timer - These 3 variables are placeholders in determining how much
@@ -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(){
@@ -55,12 +59,5 @@ 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

@@ -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;
-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")
+@TeleOp(name = "Hook Test", group = "Debug")
 public class HookTest extends OpMode {
 
     private HangMotorSubsystem skyhook;
@@ -81,12 +75,20 @@ public class HookTest extends OpMode {
         if (gamepad1.dpad_up && claw.getState() == ClawSubsystem.ClawState.OPEN) {
             skyhook.hangRobot();
         }
-        skyhook.setPower(0);
-        if (gamepad1.dpad_down) {
+        if (gamepad1.left_trigger > 0) {
             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) {