From 2d8ea6d43172f17cd04316511cb1445d0e63002e Mon Sep 17 00:00:00 2001
From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com>
Date: Thu, 9 Feb 2023 20:45:44 -0600
Subject: [PATCH] Autonomous work

---
 .../Engines/LeftSideAutonomousEngine.java     |  2 +-
 .../Engines/LeftStateAutoEngine.java          | 32 +++++++++++++++++++
 .../Engines/RightStateAutoEngine.java         |  2 +-
 .../States/DriverStateWithOdometer.java       | 16 +++++-----
 .../Autonomous/States/PathDecision.java       | 11 +++++++
 5 files changed, 53 insertions(+), 10 deletions(-)
 create mode 100644 TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftStateAutoEngine.java

diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java
index eee529c..83ca4fd 100644
--- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java
+++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java
@@ -16,7 +16,7 @@ import org.timecrafters.Autonomous.States.ServoCameraRotate;
 import org.timecrafters.Autonomous.States.TopArm;
 import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
 
-@Autonomous (name = "Left Side")
+//@Autonomous (name = "Left Side")
 @Disabled
 
 public class LeftSideAutonomousEngine extends CyberarmEngine {
diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftStateAutoEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftStateAutoEngine.java
new file mode 100644
index 0000000..5d70892
--- /dev/null
+++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftStateAutoEngine.java
@@ -0,0 +1,32 @@
+package org.timecrafters.Autonomous.Engines;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
+
+@Autonomous(name = "Left Side", group = "A Phoenix", preselectTeleOp = "APhoenixTeleOP")
+public class LeftStateAutoEngine extends CyberarmEngine {
+    PhoenixBot1 robot;
+
+    @Override
+    public void loop() {
+        super.loop();
+
+        telemetry.addData("BlackBoard Input", blackboardGetString("parkPlace"));
+    }
+
+    @Override
+    public void setup() {
+        robot = new PhoenixBot1(this);
+        robot.imu.resetYaw();
+
+        setupFromConfig(
+                robot.configuration,
+                "org.timecrafters.Autonomous.States",
+                robot,
+                PhoenixBot1.class,
+                "Left State Auto");
+
+    }
+}
diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightStateAutoEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightStateAutoEngine.java
index ee034d2..566872a 100644
--- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightStateAutoEngine.java
+++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightStateAutoEngine.java
@@ -10,7 +10,7 @@ import org.timecrafters.Autonomous.States.RotationState;
 import org.timecrafters.Autonomous.States.TopArmv2;
 import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
 
-@Autonomous(name = "Right Side")
+@Autonomous(name = "Right Side", group = "A Phoenix", preselectTeleOp = "APhoenixTeleOP")
 public class RightStateAutoEngine extends CyberarmEngine {
     PhoenixBot1 robot;
 
diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java
index 3c556de..8327a04 100644
--- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java
+++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java
@@ -231,18 +231,18 @@ public class DriverStateWithOdometer extends CyberarmState {
 
             if (currentAngle - direction > robot.ROTATION_TOLERANCE) {
 
-                robot.frontRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER  );
-                robot.frontLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER );
-                robot.backRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER  );
-                robot.backLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER  );
+                robot.frontRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER - 0.05 );
+                robot.frontLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER + 0.05);
+                robot.backRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER - 0.05  );
+                robot.backLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER + 0.05 );
 
             }
             else if (currentAngle - direction < -robot.ROTATION_TOLERANCE) {
 
-                robot.frontRightDrive.setPower(robot.ROTATION_MINIMUM_POWER);
-                robot.frontLeftDrive.setPower(-robot.ROTATION_MINIMUM_POWER);
-                robot.backRightDrive.setPower(robot.ROTATION_MINIMUM_POWER);
-                robot.backLeftDrive.setPower(-robot.ROTATION_MINIMUM_POWER);
+                robot.frontRightDrive.setPower(robot.ROTATION_MINIMUM_POWER + 0.05 );
+                robot.frontLeftDrive.setPower(-robot.ROTATION_MINIMUM_POWER - 0.05 );
+                robot.backRightDrive.setPower(robot.ROTATION_MINIMUM_POWER + 0.05 );
+                robot.backLeftDrive.setPower(-robot.ROTATION_MINIMUM_POWER - 0.05);
 
             } else {
                 robot.frontRightDrive.setPower(0);
diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/PathDecision.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/PathDecision.java
index 45bf43b..81ae1e3 100644
--- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/PathDecision.java
+++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/PathDecision.java
@@ -17,6 +17,17 @@ public class PathDecision extends CyberarmState {
     @Override
     public void exec() {
         String placement = engine.blackboardGetString("parkPlace");
+
+        if (placement.equals("1")) {
+
+            addState(new DriverStateWithOdometer(robot, "" + groupName + " Parking", "Park 1"));
+        }
+
+        else if (placement.equals("3")){
+            addState(new DriverStateWithOdometer(robot, "" + groupName + " Parking", "Park 3"));
+
+        }
+
         setHasFinished(true);
         }