From a7be704878b46c8aa542d70ce20ef2f8a919a964 Mon Sep 17 00:00:00 2001
From: Castor <robolionsgit@gmail.com>
Date: Sun, 2 Apr 2023 18:01:21 -0400
Subject: [PATCH] Arm changes from drive practice

---
 src/main/java/frc/robot/Arm.java   |  2 +-
 src/main/java/frc/robot/Auto.java  | 63 ++++++++++++++++++++++++++++++
 src/main/java/frc/robot/Robot.java |  2 +-
 3 files changed, 65 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/Arm.java b/src/main/java/frc/robot/Arm.java
index 1b4a36b..ef211bb 100644
--- a/src/main/java/frc/robot/Arm.java
+++ b/src/main/java/frc/robot/Arm.java
@@ -502,7 +502,7 @@ public boolean limitSwitchPressed() {
 			switchCount++;
 		}
 
-		if ((switchCount > 25) && (switchVal == true)) {
+		if ((switchCount > 75) && (switchVal == true)) {
 			switchCount = 0;
 			return true;
 		}
diff --git a/src/main/java/frc/robot/Auto.java b/src/main/java/frc/robot/Auto.java
index aa05585..e6f15f1 100644
--- a/src/main/java/frc/robot/Auto.java
+++ b/src/main/java/frc/robot/Auto.java
@@ -803,6 +803,69 @@ public int armToTopCone() {
         return Robot.CONT;
     }
 
+    /**
+     * 
+     * @return
+     */
+    public int armToTopConeTeleop() {    
+        double[] armAngles = Arm.TOP_CONE_ANGLES;
+
+		if (armFirstTime == true) {
+			armFirstTime = false;
+			armStep = 1;
+		}
+
+        switch(armStep) {
+            case 1:
+                // Middle out, base slightly
+                AngleStates middleStatus = arm.jointToAngle(2, Math.PI/6, 2.5);
+                arm.jointToAngle(1, 1.4, 3);
+                arm.jointToAngle(3, -2.4);
+
+                // If base and middle are close to or at target position, go to next step
+                if ((middleStatus == AngleStates.DONE || middleStatus == AngleStates.CLOSE)) {
+                    armStep++;
+                }
+                break;
+            case 2:
+                // Base and middle out
+                AngleStates baseStatus2   = arm.jointToAngle(1, armAngles[0], 1.5);
+                AngleStates middleStatus2 = arm.jointToAngle(2, armAngles[1], 2);
+                arm.hold(3);
+
+                // If base and middle are close to or at target position, go to next step
+                if ((baseStatus2   == AngleStates.DONE || baseStatus2   == AngleStates.CLOSE) &&
+                    (middleStatus2 == AngleStates.DONE || middleStatus2 == AngleStates.CLOSE)) {
+                    armStep++;
+                }
+                break;
+            case 3:
+                // Wrist out
+                AngleStates baseStatusEnd   = arm.jointToAngle(1, armAngles[0], 0.7);
+                AngleStates middleStatusEnd = arm.jointToAngle(2, armAngles[1]);
+                AngleStates endStatusEnd    = arm.jointToAngle(3, -0.1, 0.7);
+                if (baseStatusEnd   == AngleStates.DONE &&
+                    middleStatusEnd == AngleStates.DONE &&
+                    endStatusEnd    == AngleStates.DONE) {
+                        armStep++;
+                }
+                break;
+            case 4:
+                // Finished routine
+                arm.hold(1);
+                arm.hold(2);
+                arm.hold(3);
+                //arm.jointToAngle(1, armAngles[0]);
+                //arm.jointToAngle(2, armAngles[1]);
+                //arm.jointToAngle(3, armAngles[2]);
+                armFirstTime = true;
+                armStep = 1;
+                return Robot.DONE;
+        }
+        
+        return Robot.CONT;
+    }
+
     /**
      * 
      * @return
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index de103c7..3f4d513 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -449,7 +449,7 @@ else if (acceptedArmState == ArmStates.MID_CUBE) {
 					fromTop = false;
 				}
 				else if (acceptedArmState == ArmStates.TOP_CONE) {
-					armStatus = auto.armToTopCone();
+					armStatus = auto.armToTopConeTeleop();
 					fromTop = true;
 				}
 				else if (acceptedArmState == ArmStates.TOP_CUBE) {