From e8eff6367dceb906b0443293e8411377d2a02772 Mon Sep 17 00:00:00 2001
From: robotics1 <troy@scds.org>
Date: Sun, 3 Nov 2024 10:24:10 -0800
Subject: [PATCH] Working Arm Code

---
 .../firstinspires/ftc/teamcode/DevTeleop.java | 67 +++++++++++++++++++
 1 file changed, 67 insertions(+)
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java
new file mode 100644
index 0000000..341b24a
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java
@@ -0,0 +1,67 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.Gamepad;
+
+import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
+
+@TeleOp(name = "ArmTelop", group = "Debug")
+public class DevTeleop extends OpMode {
+
+    public ClawSubsystem claw;
+    public ArmSubsystem arm;
+    public WristSubsystem wrist;
+    public Gamepad currentGamepad1;
+    public Gamepad previousGamepad1;
+
+    @Override
+    public void init() {
+        claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED);
+        arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK);
+        wrist = new WristSubsystem(hardwareMap, WristSubsystem.WristState.FLOOR);
+        claw.init();
+        arm.init();
+        wrist.init();
+      
+        currentGamepad1 = new Gamepad();
+        previousGamepad1 = new Gamepad();
+    }
+
+    public void theDrop(ClawSubsystem claw, ArmSubsystem arm, WristSubsystem wrist) {
+
+            if (currentGamepad1.a && !previousGamepad1.a) {
+                claw.openClaw();
+                wrist.floorWrist();
+                arm.engageArm();
+            }
+
+    }
+    public void thePickup(ClawSubsystem claw, ArmSubsystem arm, WristSubsystem wrist) {
+
+        if (currentGamepad1.x && !previousGamepad1.x) {
+            claw.switchState();
+
+        }
+
+    }
+    public void theLift(ClawSubsystem claw, ArmSubsystem arm, WristSubsystem wrist) {
+
+        if (currentGamepad1.b && !previousGamepad1.b) {
+            arm.parkArm();
+            wrist.bucketWrist();
+        }
+
+    }
+    @Override
+    public void loop() {
+        previousGamepad1.copy(currentGamepad1);
+        currentGamepad1.copy(gamepad1);
+
+        theDrop(claw, arm, wrist);
+        thePickup(claw, arm, wrist);
+        theLift(claw, arm, wrist);
+    }
+}