Working Arm Code

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);
+    }
+}