From f70213af27c68a358364415d80cfaa2a33831a4a Mon Sep 17 00:00:00 2001
From: Xander Haemel <xhaemel@gmail.com>
Date: Tue, 28 Jan 2025 20:28:25 -0800
Subject: [PATCH] hang test

---
 .../ftc/teamcode/subsystem/ArmSubsystem.java  |  2 +-
 .../subsystem/HangMotorSubsystem.java         |  3 -
 .../ftc/teamcode/subsystem/HookTest.java      | 62 +++++++++++++++++++
 3 files changed, 63 insertions(+), 4 deletions(-)
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HookTest.java

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java
index 5c4137f..66a55f4 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java
+++ b/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);
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HangMotorSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HangMotorSubsystem.java
index b3e4792..414fdb3 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HangMotorSubsystem.java
+++ b/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(){
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HookTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HookTest.java
new file mode 100644
index 0000000..20114d6
--- /dev/null
+++ b/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
+        }
+
+    }
+
+}