From 362ab0791de9d92304e14117de242d750811a28b Mon Sep 17 00:00:00 2001
From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com>
Date: Sat, 30 Dec 2023 11:25:12 -0600
Subject: [PATCH] fixed arm control, attempted to start autonomous

---
 .../Competition2BlueBackStage.java            |  6 ++++
 .../CompetitionStates/ClawArmState.java       | 35 ++----------------
 .../DriveToCoordinatesState.java              | 15 +++++---
 .../Common/CompetitionRobotV1.java            | 36 +++++++++++++++++++
 4 files changed, 55 insertions(+), 37 deletions(-)

diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/Competition2BlueBackStage.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/Competition2BlueBackStage.java
index ee80472..24ab34a 100644
--- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/Competition2BlueBackStage.java
+++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/Competition2BlueBackStage.java
@@ -39,6 +39,12 @@ public class Competition2BlueBackStage extends CyberarmEngine {
 
         addState(new DriveToCoordinatesState(robot,"burnsville backdrop blue", "3-03-0"));
 
+        addState(new ClawFingerState(robot,"burnsville backdrop blue", "0-04-0"));
+
+        addState(new DriveToCoordinatesState(robot,"burnsville backdrop blue", "3-05-0"));
+        addState(new DriveToCoordinatesState(robot,"burnsville backdrop blue", "3-06-0"));
+
+
 
 
 
diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/ClawArmState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/ClawArmState.java
index ee09469..19be294 100644
--- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/ClawArmState.java
+++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/ClawArmState.java
@@ -25,41 +25,10 @@ public class ClawArmState extends CyberarmState {
         robot.OdometryLocalizer();
 
         // driving arm to pos
-        if (armPos == "collect") {
-            if (robot.lift.getCurrentPosition() >= 20) {
-                robot.lift.setPower(-0.6);
-            } else {
-                robot.lift.setPower(0);
-                robot.shoulder.setPosition(robot.shoulderCollect);
-                robot.elbow.setPosition(robot.elbowCollect);
-                robot.target = 30;
-
-            }
-        }
-        if (armPos == "deposit") {
-            robot.shoulder.setPosition(robot.shoulderDeposit);
-            robot.elbow.setPosition(robot.elbowDeposit);
-            robot.target = 400;
-
-
-        }
-        if (armPos == "hover") {
-            if (robot.lift.getCurrentPosition() >= 20) {
-                robot.lift.setPower(-0.6);
-            } else {
-                robot.shoulder.setPosition(0.38);
-                robot.target = 200;
-            }
-
-        }
-        if (armPos.equals("search")) {
-            robot.shoulder.setPosition(0.15);
-            robot.target = 570;
-
-        }
+        robot.armPos = armPos;
         robot.clawArmControl();
 
-        if (robot.power < 0.1) {
+        if (robot.clawArm.getCurrentPosition() > robot.target - 20 || robot.clawArm.getCurrentPosition() < robot.target + 20) {
             setHasFinished(true);
         }
 
diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java
index d01490f..8ea1294 100644
--- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java
+++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java
@@ -21,6 +21,7 @@ public class DriveToCoordinatesState extends CyberarmState {
     public boolean posSpecific;
     public double maxXPower;
     public double maxYPower;
+    public long initTime;
 
     public DriveToCoordinatesState(CompetitionRobotV1 robot, String groupName, String actionName) {
         this.robot = robot;
@@ -34,6 +35,12 @@ public class DriveToCoordinatesState extends CyberarmState {
         this.posSpecific = robot.configuration.variable(groupName, actionName, "posSpecific").value();
     }
 
+    @Override
+    public void start() {
+        super.start();
+        initTime = System.currentTimeMillis();
+    }
+
     @Override
     public void exec() {
         if (!posSpecific) {
@@ -63,10 +70,10 @@ public class DriveToCoordinatesState extends CyberarmState {
         robot.OdometryLocalizer();
 
 
-        if (Math.abs(robot.backLeftPower) < 0.15 &&
-                Math.abs(robot.backRightPower) < 0.15 &&
-                Math.abs(robot.frontLeftPower) < 0.15 &&
-                Math.abs(robot.frontRightPower) < 0.15) {
+        if ((Math.abs(robot.backLeftPower) < 0.18 &&
+                Math.abs(robot.backRightPower) < 0.18 &&
+                Math.abs(robot.frontLeftPower) < 0.18 &&
+                Math.abs(robot.frontRightPower) < 0.18) || (System.currentTimeMillis() - initTime > 5000)) {
             posAchieved = true;
         }
     }
diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java
index 1bba8fb..43a8871 100644
--- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java
+++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java
@@ -22,6 +22,8 @@ import org.openftc.easyopencv.OpenCvWebcam;
 import org.timecrafters.Library.Robot;
 import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
 
+import java.util.Objects;
+
 import dev.cyberarm.engine.V2.CyberarmEngine;
 
 @Config
@@ -94,6 +96,9 @@ public class CompetitionRobotV1 extends Robot {
 
     //-------------------------------------------------------------------------------------------------------------- arm sequence variables:
     PIDController pidController;
+
+    public String armPos;
+
     public int target;
     public  double p = 0.007, i = 0,  d = 0.0001, f = 0;
     public  double shoulderCollect = 0.38;
@@ -324,6 +329,37 @@ public class CompetitionRobotV1 extends Robot {
     }
 
     public void clawArmControl(){
+        if (armPos.equals("collect")) {
+            if (lift.getCurrentPosition() >= 20) {
+                lift.setPower(-0.6);
+            } else {
+                lift.setPower(0);
+                shoulder.setPosition(shoulderCollect);
+                elbow.setPosition(elbowCollect);
+                target = 30;
+            }
+        }
+        if (Objects.equals(armPos, "deposit")) {
+            shoulder.setPosition(shoulderDeposit);
+            elbow.setPosition(elbowDeposit);
+            target = 400;
+
+
+        }
+        if (Objects.equals(armPos, "hover")) {
+            if (lift.getCurrentPosition() >= 20) {
+                lift.setPower(-0.6);
+            } else {
+                shoulder.setPosition(0.38);
+                target = 100;
+            }
+
+        }
+        if (armPos.equals("search")) {
+            shoulder.setPosition(0.15);
+            target = 570;
+
+        }
 
         pidController.setPID(p, i, d);
         int armPos = clawArm.getCurrentPosition();