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 2d796ca..a491415 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 @@ -4,6 +4,7 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import org.timecrafters.CenterStage.Autonomous.States.SodiPizzaAutoArmState; import org.timecrafters.CenterStage.Autonomous.States.SodiPizzaAutoFirstDriveState; +import org.timecrafters.CenterStage.Autonomous.States.SodiPizzaAutoTurnState; import org.timecrafters.CenterStage.Autonomous.States.SodiPizzaWheelTest; import dev.cyberarm.engine.V2.CyberarmEngine; @@ -12,8 +13,9 @@ import dev.cyberarm.engine.V2.CyberarmEngine; public class SodiPizzaAutoRedRightEngine extends CyberarmEngine { @Override public void setup() { -// addState(new SodiPizzaAutoFirstDriveState()); + addState(new SodiPizzaAutoFirstDriveState()); + addState(new SodiPizzaAutoTurnState()); // addState(new SodiPizzaAutoArmState()); - addState(new SodiPizzaWheelTest()); +// 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 aa6d0a8..b9a40bd 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 @@ -1,5 +1,7 @@ package org.timecrafters.CenterStage.Autonomous.States; +import android.annotation.SuppressLint; + import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject; import dev.cyberarm.engine.V2.CyberarmState; @@ -8,6 +10,9 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{ final private SodiPizzaMinibotObject robot; final private String groupName, actionName; private long lastMoveTime; + private int targetPos = 2500; + private double drivePower; + private boolean lastHalf; public SodiPizzaAutoFirstDriveState() { groupName = " "; @@ -15,10 +20,16 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{ robot = new SodiPizzaMinibotObject(); robot.setup(); } + + @SuppressLint("SuspiciousIndentation") @Override public void start() { + lastHalf = false; + lastMoveTime = System.currentTimeMillis(); + robot.readyToTurn = 0; + } @Override @@ -31,26 +42,81 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{ @Override public void exec() { + // Move forward from 0 to targetPos + if (robot.leftFront.getCurrentPosition() <= 10 && robot.leftFront.getCurrentPosition() >= -10 + && !lastHalf) { + robot.leftFront.setTargetPosition(targetPos); + robot.leftBack.setTargetPosition(targetPos); + robot.rightFront.setTargetPosition(targetPos); + robot.rightBack.setTargetPosition(targetPos); - if (robot.leftFront.getCurrentPosition() < 1000) { + drivePower = 0.5; - robot.leftFront.setTargetPosition(1000); - robot.leftBack.setTargetPosition(1000); - robot.rightFront.setTargetPosition(1000); - robot.rightBack.setTargetPosition(1000); + robot.leftFront.setPower(drivePower); + robot.leftBack.setPower(drivePower); + robot.rightFront.setPower(drivePower); + robot.rightBack.setPower(drivePower); - robot.leftFront.setPower(0.5); - robot.rightFront.setPower(0.5); - robot.leftBack.setPower(0.5); - robot.rightBack.setPower(0.5); + } + //Stop and finish set after return to 0 + else if (robot.leftFront.getCurrentPosition() >= targetPos - 10 && robot.leftFront.getCurrentPosition() <= targetPos + 10) { - } else { - robot.leftFront.setPower(0); - robot.leftBack.setPower(0); - robot.rightFront.setPower(0); - robot.rightBack.setPower(0); + drivePower = 0; + + robot.leftFront.setPower(drivePower); + robot.leftBack.setPower(drivePower); + robot.rightFront.setPower(drivePower); + robot.rightBack.setPower(drivePower); + + robot.readyToTurn = 1; setHasFinished(true); } + +/* //Stop moving and update lastMoveTime + if (robot.leftFront.getCurrentPosition() >= targetPos && drivePower > 0) { + + lastHalf = true; + + drivePower = 0; + + robot.leftFront.setPower(drivePower); + robot.leftBack.setPower(drivePower); + robot.rightFront.setPower(drivePower); + robot.rightBack.setPower(drivePower); + + lastMoveTime = System.currentTimeMillis(); + } + + //Move backwards from targetPos to 0 + if (robot.leftFront.getCurrentPosition() >= targetPos && drivePower == 0 + && System.currentTimeMillis() - lastMoveTime >= 500 && lastHalf) { + + robot.leftFront.setTargetPosition(0); + robot.leftBack.setTargetPosition(0); + robot.rightFront.setTargetPosition(0); + robot.rightBack.setTargetPosition(0); + + drivePower = -0.5; + + robot.leftFront.setPower(drivePower); + robot.leftBack.setPower(drivePower); + robot.rightFront.setPower(drivePower); + robot.rightBack.setPower(drivePower); + } */ + + /* if (robot.leftFront.getCurrentPosition() < 1250 && robot.leftFront.getCurrentPosition() >= targetPos && + robot.rightBack.getCurrentPosition() > 750) { + robot.leftFront.setPower(0.5); + robot.leftBack.setPower(0.5); + robot.rightFront.setPower(-0.5); + robot.rightBack.setPower(-0.5); + + robot.leftFront.setTargetPosition(1250); + robot.leftBack.setTargetPosition(1250); + robot.rightFront.setTargetPosition(750); + robot.rightBack.setTargetPosition(750); + } +*/ } } 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 new file mode 100644 index 0000000..3a31fe5 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoTurnState.java @@ -0,0 +1,71 @@ +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 dev.cyberarm.engine.V2.CyberarmState; + +public class SodiPizzaAutoTurnState extends CyberarmState { + final private SodiPizzaMinibotObject robot = new SodiPizzaMinibotObject(); + final private String groupName, actionName; + private long lastMoveTime; + private double turnSpeedRaw, turnSpeed; + private int startPos; + private double targetRot; + private double currentRot = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); + private double neededRot = targetRot - currentRot; + /** Rot = rotation **/ + + public SodiPizzaAutoTurnState() { + groupName = " "; + actionName = " "; + robot.setup(); + } + + private double getTurnSpeed() { + if (Math.abs(neededRot) > 5) { + turnSpeed = turnSpeedRaw * neededRot / 10; + } + return turnSpeed; + } + + @Override + public void start() { + + startPos = robot.leftFront.getCurrentPosition(); + turnSpeedRaw = 0; + + robot.leftFront.setPower(turnSpeed); + robot.leftBack.setPower(turnSpeed); + robot.rightFront.setPower(turnSpeed); + robot.rightBack.setPower(turnSpeed); + + robot.imu.resetYaw(); + } + + + @Override + public void exec() { + + if (robot.readyToTurn == 1 && robot.leftFront.getCurrentPosition() == startPos && Math.abs(neededRot) > 10) { + + targetRot = currentRot + 90; + + turnSpeedRaw = 0.3; + + robot.leftFront.setPower(turnSpeed); + robot.leftBack.setPower(turnSpeed); + robot.rightFront.setPower(-turnSpeed); + robot.rightBack.setPower(-turnSpeed); + + } else if (robot.readyToTurn == 1 && Math.abs(neededRot) < 10) { + turnSpeedRaw = 0; + robot.leftFront.setPower(turnSpeed); + robot.leftBack.setPower(turnSpeed); + robot.rightFront.setPower(turnSpeed); + robot.rightBack.setPower(turnSpeed); + } + + } +} 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 238812f..9421567 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java @@ -19,6 +19,8 @@ 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