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 eae6bc2..2c05058 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 @@ -16,11 +16,9 @@ public class SodiPizzaAutoRedRightEngine extends CyberarmEngine { @Override public void setup() { - + blackboardSet("readyToTurn", 0); addState(new SodiPizzaAutoTurnState()); - if (blackboardGetInt("readyToTurn") == 0) { - addTask(new SodiPizzaAutoFirstDriveState()); - } + addParallelStateToLastState(new SodiPizzaAutoFirstDriveState()); } } diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoFirstDriveState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoFirstDriveState.java index a67ecf2..4c2905f 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoFirstDriveState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoFirstDriveState.java @@ -12,6 +12,7 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{ private long lastMoveTime; private int targetPos = 2500; private double drivePower; + public int readyToTurn; public SodiPizzaAutoFirstDriveState() { groupName = " "; @@ -26,7 +27,7 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{ public void start() { lastMoveTime = System.currentTimeMillis(); - robot.readyToTurn = 0; + readyToTurn = engine.blackboardGetInt("readyToTurn"); } @@ -36,12 +37,18 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{ engine.telemetry.addData("Current Ticks?", robot.leftFront.getCurrentPosition()); engine.telemetry.addData("Ticks Needed?", robot.leftFront.getTargetPosition() - robot.leftFront.getCurrentPosition()); + engine.telemetry.addLine(); + engine.telemetry.addData("Internal Ready To Turn Value", readyToTurn); + } @Override public void exec() { + + readyToTurn = engine.blackboardGet("readyToTurn"); + // Move forward from 0 to targetPos - if (robot.leftFront.getCurrentPosition() <= 10 && robot.leftFront.getCurrentPosition() >= -10) { + if (robot.leftFront.getCurrentPosition() <= 10 && robot.leftFront.getCurrentPosition() >= -10 && readyToTurn == 0) { robot.leftFront.setTargetPosition(targetPos); robot.leftBack.setTargetPosition(targetPos); @@ -66,7 +73,7 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{ robot.rightFront.setPower(drivePower); robot.rightBack.setPower(drivePower); - robot.readyToTurn = 1; + engine.blackboardSet("readyToTurn", 1); setHasFinished(true); } 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 0d3f3f5..b8b206e 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 @@ -3,6 +3,7 @@ package org.timecrafters.CenterStage.Autonomous.States; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject; +import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; import dev.cyberarm.engine.V2.CyberarmState; @@ -14,12 +15,13 @@ public class SodiPizzaAutoTurnState extends CyberarmState { private int startPos; 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; + private double neededRot; + public int readyToTurn; + +// 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 **/ @@ -29,20 +31,19 @@ public class SodiPizzaAutoTurnState extends CyberarmState { robot.setup(); } - public SodiPizzaAutoTurnState(int readyToTurnParm) { - groupName = " "; - actionName = " "; - robot.setup(); - robot.readyToTurn = readyToTurnParm; - } - private double getTurnSpeed() { if (Math.abs(neededRot) > 5) { - turnSpeed = turnSpeedRaw * neededRot / 10; + turnSpeed = (turnSpeedRaw * neededRot) / 10; } return turnSpeed; } + @Override + public void telemetry() { + engine.telemetry.addData("Target Pos", robot.leftFront.getTargetPosition()); + engine.telemetry.addData("Power", robot.leftFront.getPower()); + } + @Override public void start() { @@ -54,18 +55,21 @@ public class SodiPizzaAutoTurnState extends CyberarmState { robot.rightFront.setPower(turnSpeed); robot.rightBack.setPower(turnSpeed); - robot.imu.resetYaw(); + neededRot = (targetRot - robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)); +// robot.imu.resetYaw(); } @Override public void exec() { + readyToTurn = engine.blackboardGet("readyToTurn"); + currentRot = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); - if (robot.readyToTurn == 1 && robot.leftFront.getCurrentPosition() == startPos && Math.abs(neededRot) > 10) { + if (readyToTurn == 1 && robot.leftFront.getCurrentPosition() == startPos && Math.abs(neededRot) > 10) { - targetRot = currentRot + 90; + targetRot = -90; turnSpeedRaw = 0.3; @@ -74,14 +78,14 @@ public class SodiPizzaAutoTurnState extends CyberarmState { robot.rightFront.setPower(-turnSpeed); robot.rightBack.setPower(-turnSpeed); - } else if (robot.readyToTurn == 1 && Math.abs(neededRot) < 5) { + } else if (readyToTurn == 1 && Math.abs(neededRot) < 5) { turnSpeedRaw = 0; robot.leftFront.setPower(turnSpeed); robot.leftBack.setPower(turnSpeed); robot.rightFront.setPower(turnSpeed); robot.rightBack.setPower(turnSpeed); - robot.readyToTurn = 0; + engine.blackboardSet("readyToTurn", 0); } } 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 58e6287..1102635 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java @@ -6,6 +6,7 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.IMU; import com.qualcomm.robotcore.hardware.Servo; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; import org.timecrafters.Library.Robot; import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; @@ -19,8 +20,6 @@ public class SodiPizzaMinibotObject extends Robot { public IMU imu; private String string; - public int readyToTurn; - public static double GRIPPER_CLOSED = 0.333; // ~90 degrees public static double GRIPPER_OPEN = 0.75; // ~205 degrees @@ -43,6 +42,7 @@ public class SodiPizzaMinibotObject extends Robot { this.engine = CyberarmEngine.instance; this.hardwareMap = CyberarmEngine.instance.hardwareMap; + this.imu = CyberarmEngine.instance.hardwareMap.get(IMU.class, "imu"); //Motor defining leftFront = engine.hardwareMap.dcMotor.get("leftFront");