From 8e99d1672ec7c651272084245a14f8ac66d5a383 Mon Sep 17 00:00:00 2001
From: Carlos Rivas <carlos@riv.as>
Date: Wed, 30 Oct 2024 21:31:17 -0700
Subject: [PATCH] Added arm positioning override and new state, BUCKET

---
 .../firstinspires/ftc/teamcode/ArmTest.java   |  9 ++++++++
 .../ftc/teamcode/subsystem/ArmSubsystem.java  | 23 +++++++++++++++++--
 2 files changed, 30 insertions(+), 2 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmTest.java
index 066e75a..273ea09 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmTest.java
@@ -126,9 +126,18 @@ public class ArmTest extends LinearOpMode {
                 arm.switchState();
             }
 
+            if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) {
+                arm.setPosition(arm.getPosition() - .05);
+            }
+
+            if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
+                arm.setPosition(arm.getPosition() + .05);
+            }
+
             // Show the elapsed game time and wheel power.
             telemetry.addData("Status", "Run Time: " + runtime.toString());
             telemetry.addData("Arm State", arm.getState());
+            telemetry.addData("Arm Position", arm.getPosition());
             telemetry.update();
         }
     }}
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 46bd677..4e828e9 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
@@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.subsystem;
 
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armEngage;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armPark;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armBucket;
 
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.ServoImplEx;
@@ -12,12 +13,12 @@ import org.firstinspires.ftc.teamcode.util.action.RunAction;
 public class ArmSubsystem {
 
     public enum ArmState {
-        PARK, ENGAGE
+        PARK, ENGAGE, BUCKET
     }
 
     public ServoImplEx arm;
     public ArmState state;
-    public RunAction engageArm, parkArm;
+    public RunAction engageArm, parkArm, bucketArm;
 
     public ArmSubsystem(HardwareMap hardwareMap, ArmState armState) {
         arm = hardwareMap.get(ServoImplEx.class, "arm-servo");
@@ -27,6 +28,7 @@ public class ArmSubsystem {
 
         parkArm = new RunAction(this::parkArm);
         engageArm = new RunAction(this::engageArm);
+        bucketArm = new RunAction(this::bucketArm);
     }
 
     public void setState(ArmState armState) {
@@ -36,6 +38,9 @@ public class ArmSubsystem {
         } else if (armState == ArmState.PARK) {
             arm.setPosition(armPark);
             this.state = ArmState.PARK;
+        } else if (armState == ArmState.BUCKET) {
+            arm.setPosition(armBucket);
+            this.state = ArmState.BUCKET;
         }
     }
 
@@ -47,10 +52,16 @@ public class ArmSubsystem {
         setState(ArmState.PARK);
     }
 
+    public void bucketArm() {
+        setState(ArmState.BUCKET);
+    }
+
     public void switchState() {
         if (state == ArmState.ENGAGE) {
             setState(ArmState.PARK);
         } else if (state == ArmState.PARK) {
+            setState(ArmState.BUCKET);
+        } else if (state == ArmState.BUCKET) {
             setState(ArmState.ENGAGE);
         }
     }
@@ -67,4 +78,12 @@ public class ArmSubsystem {
         Actions.runBlocking(parkArm);
     }
 
+    public double getPosition() {
+        return this.arm.getPosition();
+    }
+
+    public void setPosition(double position) {
+        this.arm.setPosition(position);
+    }
+
 }