From dc8a527a4e3d1776833ea2accc4dd1636fe58fbb Mon Sep 17 00:00:00 2001
From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com>
Date: Sat, 21 Oct 2023 11:50:09 -0500
Subject: [PATCH] worked on righting the sequence states for the servos

---
 .../Autonomous/States/AutoStateSample.java    |  2 -
 .../States/DepositorArmPosState.java          | 50 +++++++++++++++++++
 .../CenterStage/Common/PrototypeRobot.java    |  4 ++
 3 files changed, 54 insertions(+), 2 deletions(-)
 create mode 100644 TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/DepositorArmPosState.java

diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/AutoStateSample.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/AutoStateSample.java
index 0a60da1..1be2687 100644
--- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/AutoStateSample.java
+++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/AutoStateSample.java
@@ -7,8 +7,6 @@ import dev.cyberarm.engine.V2.CyberarmState;
 public class AutoStateSample extends CyberarmState {
 
     private final boolean stateDisabled;
-    private double newX;
-    private double newY;
     PrototypeRobot robot;
     public AutoStateSample(PrototypeRobot robot, String groupName, String actionName) {
         this.robot = robot;
diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/DepositorArmPosState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/DepositorArmPosState.java
new file mode 100644
index 0000000..2ebe289
--- /dev/null
+++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/DepositorArmPosState.java
@@ -0,0 +1,50 @@
+package org.timecrafters.CenterStage.Autonomous.States;
+
+import org.timecrafters.CenterStage.Common.PrototypeRobot;
+
+import dev.cyberarm.engine.V2.CyberarmState;
+
+public class DepositorArmPosState extends CyberarmState {
+
+    private final boolean stateDisabled;
+    PrototypeRobot robot;
+
+    public DepositorArmPosState(PrototypeRobot robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
+    }
+
+    @Override
+    public void start() {
+        //add variables that need to be reinitillized
+    }
+
+    @Override
+    public void exec() {
+        if (robot.armPosition == 0) { // transfer mode
+            if (robot.oldArmPosititon == 0){
+                setHasFinished(true);
+            } else if (robot.oldArmPosititon == 1){
+                robot.depositorElbow.setPosition(robot.ELBOW_COLLECT);
+            } else if (robot.oldArmPosititon == 2){
+
+            }
+//        } else if (robot.armPosition == 1) { // drive mode
+//            if (robot.oldArmPosititon == 1){
+//                setHasFinished(true);
+//            } else if (robot.oldArmPosititon == 0){
+//
+//            } else if (robot.oldArmPosititon == 2){
+//
+//            }
+//        } else if (robot.armPosition == 2) { // deposit mode
+//            if (robot.oldArmPosititon == 2){
+//                setHasFinished(true);
+//            } else if (robot.oldArmPosititon == 0){
+//
+//            } else if (robot.oldArmPosititon == 1){
+//
+//            }
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java
index a51ff16..adba0b3 100644
--- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java
+++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java
@@ -15,6 +15,10 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig
 import dev.cyberarm.engine.V2.CyberarmEngine;
 
 public class PrototypeRobot extends Robot {
+
+    public int armPosition = 0;
+
+    public int oldArmPosititon;
     public double servoWaitTime;
     public double servoSecPerDeg = 0.14/60;
     public float ELBOW_COLLECT;