From 89628c8fd95a273a6342d1beebf826732441b570 Mon Sep 17 00:00:00 2001
From: Sodi <ravenpaw5040@gmail.com>
Date: Fri, 11 Nov 2022 16:45:18 -0600
Subject: [PATCH] Configuring Autonomous

---
 .../Autonomous/Engines/LeftSideAutonomousEngine.java         | 5 ++++-
 .../Autonomous/Engines/RightSideAutonomousEngine.java        | 5 ++++-
 .../java/org/timecrafters/testing/states/PhoenixBot1.java    | 1 +
 3 files changed, 9 insertions(+), 2 deletions(-)

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 767dd3a..1798d83 100644
--- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java
+++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java
@@ -11,6 +11,7 @@ import org.timecrafters.Autonomous.States.DriverParkPlaceState;
 import org.timecrafters.Autonomous.States.DriverState;
 import org.timecrafters.Autonomous.States.PathDecision;
 import org.timecrafters.Autonomous.States.RotationState;
+import org.timecrafters.Autonomous.States.ServoCameraRotate;
 import org.timecrafters.Autonomous.States.TopArm;
 import org.timecrafters.testing.states.PhoenixBot1;
 
@@ -22,7 +23,9 @@ public class LeftSideAutonomousEngine extends CyberarmEngine {
     @Override
     public void setup() {
         robot = new PhoenixBot1(this);
-        addState(new ConeIdentification(robot, "LeftSideAutonomous", "00-0"));
+        addState (new ServoCameraRotate(robot, "LeftSideAutonomous", "00-0"));
+        addState(new ConeIdentification(robot, "LeftSideAutonomous", "00-1"));
+        addState (new ServoCameraRotate(robot, "LeftSideAutonomous", "00-2"));
         //drive to high pole
         addState(new DriverState(robot, "LeftSideAutonomous", "01-0"));
         //turn towards high pole
diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java
index 87de8c2..4b5b584 100644
--- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java
+++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java
@@ -11,6 +11,7 @@ import org.timecrafters.Autonomous.States.DriverState;
 import org.timecrafters.Autonomous.States.BottomArm;
 import org.timecrafters.Autonomous.States.PathDecision;
 import org.timecrafters.Autonomous.States.RotationState;
+import org.timecrafters.Autonomous.States.ServoCameraRotate;
 import org.timecrafters.Autonomous.States.TopArm;
 import org.timecrafters.testing.states.PhoenixBot1;
 
@@ -22,7 +23,9 @@ public class RightSideAutonomousEngine extends CyberarmEngine {
     @Override
     public void setup() {
         robot = new PhoenixBot1(this);
-        addState(new ConeIdentification(robot, "RightSideAutonomous", "00-0"));
+        addState(new ServoCameraRotate(robot, "RightSideAutonomous", "00-0"));
+        addState(new ConeIdentification(robot, "RightSideAutonomous", "00-1"));
+        addState(new ServoCameraRotate(robot, "RightSideAutonomous", "00-2"));
         //drive to high pole
         addState(new DriverState(robot, "RightSideAutonomous", "01-0"));
         //turn towards high pole
diff --git a/TeamCode/src/main/java/org/timecrafters/testing/states/PhoenixBot1.java b/TeamCode/src/main/java/org/timecrafters/testing/states/PhoenixBot1.java
index 30e2cba..fb39155 100644
--- a/TeamCode/src/main/java/org/timecrafters/testing/states/PhoenixBot1.java
+++ b/TeamCode/src/main/java/org/timecrafters/testing/states/PhoenixBot1.java
@@ -82,6 +82,7 @@ public class PhoenixBot1 {
 
             //Camera Servo
             CameraServo = engine.hardwareMap.servo.get("Camera Servo");
+            CameraServo.setDirection(Servo.Direction.REVERSE);
 
             // Collector
             collectorLeft = engine.hardwareMap.crservo.get("Collector Left");