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 6ed5bec..0823dc9 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 @@ -85,17 +85,46 @@ public class SodiPizzaAutoTurnState extends CyberarmState { readyToTurn = engine.blackboardGet("readyToTurn"); - targetRot = -90; - currentRot = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); CalculateNeededRot(); - if (readyToTurn == 1 && Math.abs(neededRot) > 10) { + 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); + } + + } + + if (readyToTurn == 1 && targetRot != -90) { + + targetRot = -90; + CalculateNeededRot(); + + } + + if (currentRot >= -88) { turnSpeedRaw = 0.3; getTurnSpeed(); + robot.leftFront.setPower(-turnSpeed); + robot.leftBack.setPower(-turnSpeed); + robot.rightFront.setPower(turnSpeed); + robot.rightBack.setPower(turnSpeed); + + } else if (currentRot <= -92) { + + turnSpeedRaw = 0.2; + getTurnSpeed(); + robot.leftFront.setPower(turnSpeed); robot.leftBack.setPower(turnSpeed); robot.rightFront.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 8e4ca89..71ff2ff 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java @@ -54,10 +54,10 @@ public class SodiPizzaMinibotObject extends Robot { leftBack = engine.hardwareMap.dcMotor.get("leftBack"); rightBack = engine.hardwareMap.dcMotor.get("rightBack"); - leftFront.setDirection(DcMotorSimple.Direction.REVERSE); - leftBack.setDirection(DcMotorSimple.Direction.REVERSE); - rightFront.setDirection(DcMotorSimple.Direction.FORWARD); - rightBack.setDirection(DcMotorSimple.Direction.FORWARD); + leftFront.setDirection(DcMotorSimple.Direction.FORWARD); + leftBack.setDirection(DcMotorSimple.Direction.FORWARD); + rightFront.setDirection(DcMotorSimple.Direction.REVERSE); + rightBack.setDirection(DcMotorSimple.Direction.REVERSE); leftFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); rightFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);