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..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 @@ -9,18 +9,16 @@ 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()); + blackboardSet("readyToTurn", 0); addState(new SodiPizzaAutoTurnState()); + addParallelStateToLastState(new SodiPizzaAutoFirstDriveState()); -// addState(new SodiPizzaAutoSecDriveState()); - -// addState(new SodiPizzaAutoArmState()); -// addState(new SodiPizzaWheelTest()); } } 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 47da050..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,7 +15,14 @@ public class SodiPizzaAutoTurnState extends CyberarmState { private int startPos; private double targetRot; private double currentRot; - private double neededRot = targetRot - currentRot; + 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 **/ public SodiPizzaAutoTurnState() { @@ -23,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() { @@ -48,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; @@ -68,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 69bb832..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"); @@ -70,5 +70,7 @@ public class SodiPizzaMinibotObject extends Robot { gripper = engine.hardwareMap.servo.get("gripper"); //readyToTurn + + } }