From c50b7d464bdb68c4044abe81175bc5f88a92d5b9 Mon Sep 17 00:00:00 2001 From: NerdyBirdy460 <111399933+NerdyBirdy460@users.noreply.github.com> Date: Thu, 7 Dec 2023 13:04:25 -0600 Subject: [PATCH] Started to format drive state similar to turn state, tests seem to work well for now --- .../Engines/SodiPizzaAutoRedRightEngine.java | 4 +- .../SodiPizzaAutoFirstDriveState.java | 50 +++++++++++++------ .../SodiPizzaAutoSecDriveState.java | 3 +- .../SodiPizzaAutoThirdDriveState.java | 11 ++++ .../SodiStates/SodiPizzaAutoTurnState.java | 16 ++++-- 5 files changed, 65 insertions(+), 19 deletions(-) create mode 100644 TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoThirdDriveState.java 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 6db291f..dec2d4d 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,7 +16,9 @@ public class SodiPizzaAutoRedRightEngine extends CyberarmEngine { blackboardSet("readyToTurn", 0); addState(new SodiPizzaAutoFirstDriveState()); addState(new SodiPizzaAutoTurnState()); - addState(new SodiPizzaAutoSecDriveState()); + blackboardGetInt("readyToTurn"); + addState(new SodiPizzaAutoFirstDriveState()); +// addState(new SodiPizzaAutoSecDriveState()); } } diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoFirstDriveState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoFirstDriveState.java index ba86300..ad9d6bb 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoFirstDriveState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoFirstDriveState.java @@ -2,6 +2,8 @@ package org.timecrafters.CenterStage.Autonomous.SodiStates; import android.annotation.SuppressLint; +import com.qualcomm.robotcore.hardware.DcMotor; + import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject; @@ -12,7 +14,7 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{ final private String groupName, actionName; private long lastMoveTime; private double drivePower, drivePowerRaw; - public int readyToTurn, neededTicks, currentTicks, targetTicks = 1500; + public int readyToTurn, neededTicks, currentTicks, targetTicks = 1000; public SodiPizzaAutoFirstDriveState() { groupName = " "; @@ -73,23 +75,43 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{ CalculateNeededTicks(); // Move forward from 0 to targetTicks - if (robot.leftFront.getCurrentPosition() <= 10 && robot.leftFront.getCurrentPosition() >= -10 && readyToTurn == 0) { + if (robot.leftFront.getCurrentPosition() <= 10 && robot.leftFront.getCurrentPosition() >= -10/* && robot.distSensor.getDistance(DistanceUnit.MM) >= 100*/) { + if (readyToTurn == 0) { + targetTicks = 1000; - robot.leftFront.setTargetPosition(targetTicks); - robot.leftBack.setTargetPosition(targetTicks); - robot.rightFront.setTargetPosition(targetTicks); - robot.rightBack.setTargetPosition(targetTicks); + robot.leftFront.setTargetPosition(targetTicks); + robot.leftBack.setTargetPosition(targetTicks); + robot.rightFront.setTargetPosition(targetTicks); + robot.rightBack.setTargetPosition(targetTicks); - drivePowerRaw = 0.5; - getDrivePower(); + drivePowerRaw = 0.5; + getDrivePower(); - robot.leftFront.setPower(drivePower); - robot.leftBack.setPower(drivePower); - robot.rightFront.setPower(drivePower); - robot.rightBack.setPower(drivePower); + robot.leftFront.setPower(drivePower); + robot.leftBack.setPower(drivePower); + robot.rightFront.setPower(drivePower); + robot.rightBack.setPower(drivePower); + } else if (readyToTurn == 2) { + + targetTicks = 500; + + robot.leftFront.setTargetPosition(targetTicks); + robot.leftBack.setTargetPosition(targetTicks); + robot.rightFront.setTargetPosition(targetTicks); + robot.rightBack.setTargetPosition(targetTicks); + + drivePowerRaw = 0.5; + getDrivePower(); + + robot.leftFront.setPower(drivePower); + robot.leftBack.setPower(drivePower); + robot.rightFront.setPower(drivePower); + robot.rightBack.setPower(drivePower); + } } - //Stop and finish set after return to 0 + + //Stop and finish set after reached targetTicks within tolerance of 2 else if (robot.leftFront.getCurrentPosition() >= targetTicks - 10 && robot.leftFront.getCurrentPosition() <= targetTicks + 10) { drivePowerRaw = 0; @@ -100,7 +122,7 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{ robot.rightFront.setPower(drivePower); robot.rightBack.setPower(drivePower); - engine.blackboardSet("readyToTurn", 1); + engine.blackboardSet("readyToTurn", readyToTurn + 1); setHasFinished(true); } diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoSecDriveState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoSecDriveState.java index bb63656..e4c6f36 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoSecDriveState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoSecDriveState.java @@ -2,6 +2,7 @@ package org.timecrafters.CenterStage.Autonomous.SodiStates; import com.qualcomm.robotcore.hardware.DcMotor; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject; import dev.cyberarm.engine.V2.CyberarmState; @@ -65,7 +66,7 @@ public class SodiPizzaAutoSecDriveState extends CyberarmState { CalculateNeededTicks(); // Move forward from 0 to targetPos - if (robot.leftFront.getCurrentPosition() <= 10 && readyToTurn == 2) { + if (robot.leftFront.getCurrentPosition() <= 10 && readyToTurn == 2 && robot.distSensor.getDistance(DistanceUnit.MM) >= 100) { robot.leftFront.setTargetPosition(targetTicks); robot.leftBack.setTargetPosition(targetTicks); diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoThirdDriveState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoThirdDriveState.java new file mode 100644 index 0000000..20152c1 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoThirdDriveState.java @@ -0,0 +1,11 @@ +package org.timecrafters.CenterStage.Autonomous.SodiStates; + +import dev.cyberarm.engine.V2.CyberarmState; + +public class SodiPizzaAutoThirdDriveState extends CyberarmState { + + @Override + public void exec() { + + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoTurnState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoTurnState.java index 9880e51..7ab79e6 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoTurnState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoTurnState.java @@ -2,6 +2,8 @@ package org.timecrafters.CenterStage.Autonomous.SodiStates; import android.annotation.SuppressLint; +import com.qualcomm.robotcore.hardware.DcMotor; + import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject; @@ -124,9 +126,10 @@ public class SodiPizzaAutoTurnState extends CyberarmState { } - if (currentRot >= -88) { + if (neededRot >= 2) { - turnSpeedRaw = 0.3; + turnSpeedRaw = 0.2; + CalculateNeededRot(); getTurnSpeed(); robot.leftFront.setPower(-turnSpeed); @@ -134,9 +137,10 @@ public class SodiPizzaAutoTurnState extends CyberarmState { robot.rightFront.setPower(turnSpeed); robot.rightBack.setPower(turnSpeed); - } else if (currentRot <= -92) { + } else if (neededRot <= -2) { turnSpeedRaw = 0.2; + CalculateNeededRot(); getTurnSpeed(); robot.leftFront.setPower(turnSpeed); @@ -147,6 +151,7 @@ public class SodiPizzaAutoTurnState extends CyberarmState { } else if (readyToTurn == 1 && Math.abs(neededRot) < 2) { turnSpeedRaw = 0; + CalculateNeededRot(); getTurnSpeed(); robot.leftFront.setPower(turnSpeed); @@ -154,6 +159,11 @@ public class SodiPizzaAutoTurnState extends CyberarmState { robot.rightFront.setPower(turnSpeed); robot.rightBack.setPower(turnSpeed); + robot.leftFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.leftBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.rightFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.rightBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + engine.blackboardSet("readyToTurn", 2); setHasFinished(true);