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 + + } }