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 c6673db..ba86300 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 @@ -22,7 +22,7 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{ } private double getDrivePower() { - if (Math.abs(neededTicks) > 1) { + if (Math.abs(neededTicks) > 250) { drivePower = (drivePowerRaw * neededTicks) / 10; } return drivePower; @@ -48,6 +48,8 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{ lastMoveTime = System.currentTimeMillis(); readyToTurn = engine.blackboardGetInt("readyToTurn"); + robot.imu.resetYaw(); + } @Override 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 6b38986..bb63656 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 @@ -22,7 +22,7 @@ public class SodiPizzaAutoSecDriveState extends CyberarmState { } private double getDrivePower() { - if (Math.abs(neededTicks) > 1) { + if (Math.abs(neededTicks) > 250) { drivePower = (drivePowerRaw * neededTicks) / 10; } return drivePower; @@ -72,7 +72,7 @@ public class SodiPizzaAutoSecDriveState extends CyberarmState { robot.rightFront.setTargetPosition(targetTicks); robot.rightBack.setTargetPosition(targetTicks); - drivePowerRaw = 0.5; + drivePowerRaw = 0.3; getDrivePower(); robot.leftFront.setPower(drivePower); 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 540e8f5..9880e51 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 @@ -76,8 +76,6 @@ public class SodiPizzaAutoTurnState extends CyberarmState { robot.rightFront.setPower(turnSpeed); robot.rightBack.setPower(turnSpeed); - engine.blackboardSet("readyToTurn", 0); - } @SuppressLint("SuspiciousIndentation") @@ -91,25 +89,33 @@ public class SodiPizzaAutoTurnState extends CyberarmState { CalculateNeededRot(); - if (readyToTurn == 0) { - targetRot = 0; - - if (currentRot <= targetRot - 1) { - - turnSpeedRaw = 0.5; - getTurnSpeed(); - - robot.rightFront.setPower(robot.leftFront.getPower() + turnSpeed); - robot.rightBack.setPower(robot.leftBack.getPower() + turnSpeed); - - } else if (currentRot >= targetRot + 1) - - turnSpeedRaw = 0.5; - getTurnSpeed(); - - robot.leftFront.setPower(robot.rightFront.getPower() + turnSpeed); - - } +// if (readyToTurn == 0) { +// targetRot = 0; +// +// if (currentRot <= targetRot - 1) { +// +// turnSpeedRaw = 0.5; +// getTurnSpeed(); +// +// robot.rightFront.setPower(robot.leftFront.getPower() + turnSpeed); +// robot.rightBack.setPower(robot.leftBack.getPower() + turnSpeed); +// +// } else if (currentRot >= targetRot + 1) { +// +// turnSpeedRaw = 0.5; +// getTurnSpeed(); +// +// robot.leftFront.setPower(robot.rightFront.getPower() + turnSpeed); +// } else { +// +// robot.leftFront.setPower(robot.rightFront.getPower()); +// robot.leftBack.setPower(robot.rightBack.getPower()); +// robot.rightFront.setPower(robot.leftFront.getPower()); +// robot.rightBack.setPower(robot.leftBack.getPower()); +// +// } +// +// } if (readyToTurn == 1 && targetRot != -90) { @@ -149,6 +155,8 @@ public class SodiPizzaAutoTurnState extends CyberarmState { robot.rightBack.setPower(turnSpeed); engine.blackboardSet("readyToTurn", 2); + + setHasFinished(true); } }