hang test

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

@@ -26,7 +26,7 @@ public class ArmSubsystem {
     }
     public void grabBlueberrySkyhook(){
         setState(ArmState.GRAB_BLUEBERRY_SKYHOOK);
-        arm.setPosition(armGrabBlueberrySkyhook);
+        arm.setPosition(armGrabBlueberrySkyhook+ 0.3);
     }
     public void hangBlueberrySkyhook(){
         setState(ArmState.HANG_BLUEBERRY_SKYHOOK);

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

@@ -15,10 +15,7 @@ public class HangMotorSubsystem {
     private DcMotorEx hang;
 
     public HangMotorSubsystem(HardwareMap hardwareMap) {
-
         hang = hardwareMap.get(DcMotorEx.class, HOOK);
-
-
     }
 
     public int getCurrentPosition(){

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

@@ -0,0 +1,62 @@
+package org.firstinspires.ftc.teamcode.subsystem;
+
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+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;
+
+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();
+    }
+
+    @Override
+    public void loop() {
+        telemetry.addData("arm", arm.getState());
+        telemetry.addData("wrist", wrist.getState());
+        telemetry.update();
+        if(gamepad1.a){
+            arm.grabBlueberrySkyhook();
+            telemetry.update();
+            try {
+                Thread.sleep(500);
+            } catch (InterruptedException e) {
+                throw new RuntimeException(e);
+            }
+            //claw Open small amount
+            wrist.grabBlueberrySkyhook();
+            //wrist grab in strange way
+            telemetry.update();
+        }
+        if(gamepad1.b){
+            //after confirmed grab, close claw
+            claw.closeClaw();
+        }
+        if(gamepad1.x){
+            //now slap on bar, first wrist, then arm, then claw then driver must drive away
+        }
+        if(gamepad1.y){
+            //go up
+        }
+
+    }
+
+}