From edf0ec572ab239e4e0c6d44fca452265b0ee36ec Mon Sep 17 00:00:00 2001
From: Carlos <carlos@riv.as>
Date: Sun, 3 Nov 2024 21:36:07 -0800
Subject: [PATCH] Commit with states

---
 .../ftc/teamcode/runmodes/Auto.java           | 65 ++++++++++++++-----
 1 file changed, 48 insertions(+), 17 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Auto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Auto.java
index 257c3d8..6fd3fc2 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Auto.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Auto.java
@@ -33,6 +33,8 @@ public class Auto {
     public Follower follower;
     public Telemetry telemetry;
 
+    public int caseState = 1;
+
     public Auto(HardwareMap hardwareMap, Telemetry telemetry, Follower follower) {
         claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED);
         arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK);
@@ -61,23 +63,52 @@ public class Auto {
     }
 
     public void update() {
-        //follower.update();
-        if (clawTimer.getElapsedTimeSeconds() > 2) {
-            claw.openClaw();
-        }
-        if (armTimer.getElapsedTimeSeconds() > 4) {
-            arm.engageArm();
-        }
-        if (clawTimer.getElapsedTimeSeconds() > 6) {
-            claw.closeClaw();
-        }
-        if (armTimer.getElapsedTimeSeconds() > 8) {
-            arm.bucketArm();
-            wrist.bucketWrist();
-        }
-        if (clawTimer.getElapsedTimeSeconds() > 10) {
-            claw.openClaw();
+
+        this.telemetry.addData("Current State", caseState);
+        this.telemetry.addData("Claw Timer", clawTimer.getElapsedTimeSeconds());
+        this.telemetry.addData("Arm Timer", armTimer.getElapsedTimeSeconds());
+        this.telemetry.addData("Wrist Timer", wristTimer.getElapsedTimeSeconds());
+        this.telemetry.update();
+
+        switch(caseState) {
+            case 1:
+                claw.openClaw();
+                caseState = 2;
+                break;
+            case 2:
+                if (clawTimer.getElapsedTimeSeconds() > 2) {
+                    arm.engageArm();
+                    caseState = 3;
+                }
+                break;
+            case 3:
+                if (armTimer.getElapsedTimeSeconds() > 4) {
+                    wrist.floorWrist();
+                    caseState = 4;
+                }
+                break;
+            case 4:
+                if (clawTimer.getElapsedTimeSeconds() > 6) {
+                    claw.closeClaw();
+                    caseState = 5;
+                }
+                break;
+            case 5:
+                if (armTimer.getElapsedTimeSeconds() > 8) {
+                    arm.bucketArm();
+                    wrist.bucketWrist();
+                    caseState = 6;
+                }
+                break;
+            case 6:
+                if (clawTimer.getElapsedTimeSeconds() > 10) {
+                    claw.openClaw();
+                    caseState = 7;
+                }
+                break;
+            case 7:
+                this.init();
+                break;
         }
     }
-
 }