From d2632c854fcfa5bb39b006a54e60aad86455291f Mon Sep 17 00:00:00 2001
From: NerdyBirdy460 <111399933+NerdyBirdy460@users.noreply.github.com>
Date: Tue, 28 Nov 2023 20:30:11 -0600
Subject: [PATCH] =?UTF-8?q?Needs=20testing,=20feel=20good=20about=20this?=
 =?UTF-8?q?=20one=20=C2=AF\=5F(=E3=83=84)=5F/=C2=AF?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../Engines/SodiPizzaAutoRedRightEngine.java           | 10 +++++-----
 .../Autonomous/States/SodiPizzaAutoTurnState.java      |  6 ++++++
 .../CenterStage/Common/SodiPizzaMinibotObject.java     |  2 ++
 3 files changed, 13 insertions(+), 5 deletions(-)

diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/SodiPizzaAutoRedRightEngine.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/SodiPizzaAutoRedRightEngine.java
index b9d56ab..eae6bc2 100644
--- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/SodiPizzaAutoRedRightEngine.java
+++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/SodiPizzaAutoRedRightEngine.java
@@ -9,18 +9,18 @@ import org.timecrafters.CenterStage.Autonomous.States.SodiPizzaAutoTurnState;
 import org.timecrafters.CenterStage.Autonomous.States.SodiPizzaWheelTest;
 
 import dev.cyberarm.engine.V2.CyberarmEngine;
+import dev.cyberarm.engine.V2.CyberarmState;
 
 @Autonomous(name = "Sodi's Pizza Box Bot Auto", group = "")
 public class SodiPizzaAutoRedRightEngine extends CyberarmEngine {
+
     @Override
     public void setup() {
 
-        addState(new SodiPizzaAutoFirstDriveState());
         addState(new SodiPizzaAutoTurnState());
+        if (blackboardGetInt("readyToTurn") == 0) {
+            addTask(new SodiPizzaAutoFirstDriveState());
+        }
 
-//        addState(new SodiPizzaAutoSecDriveState());
-
-//        addState(new SodiPizzaAutoArmState());
-//        addState(new SodiPizzaWheelTest());
     }
 }
diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoTurnState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoTurnState.java
index 47da050..0d3f3f5 100644
--- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoTurnState.java
+++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoTurnState.java
@@ -15,6 +15,12 @@ public class SodiPizzaAutoTurnState extends CyberarmState {
     private double targetRot;
     private double currentRot;
     private double neededRot = targetRot - currentRot;
+
+    private double rightTurnCW = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) + 90;
+    private double rightTurnCCW = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) - 90;
+    private double backTurnCW = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) + 180;
+    private double backTurnCCW = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) - 180;
+
   /** Rot = rotation **/
 
     public SodiPizzaAutoTurnState() {
diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java
index 69bb832..58e6287 100644
--- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java
+++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java
@@ -70,5 +70,7 @@ public class SodiPizzaMinibotObject extends Robot {
         gripper = engine.hardwareMap.servo.get("gripper");
 
         //readyToTurn
+
+
     }
 }