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");