TimeCrafters/CenterStage
1
0
Fork 0
mirror of https://github.com/TimeCrafters/CenterStage synced 2025-12-15 16:52:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

Turnstate Turns!!!

Browse Source
This commit is contained in:
NerdyBirdy460
2023-12-01 21:00:47 -06:00
parent f82dd3a16b
commit 83e88d6f75
1 changed files with 23 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoTurnState.java
View File

@@ -39,9 +39,22 @@ public class SodiPizzaAutoTurnState extends CyberarmState {
return turnSpeed;
}
public void CalculateNeededRot() {
if (targetRot >= 0 && currentRot >= 0) {
neededRot = Math.abs(targetRot - currentRot);
} else if (targetRot <= 0 && currentRot <= 0) {
neededRot = Math.abs(targetRot - currentRot);
} else if (targetRot >= 0 && currentRot <= 0) {
neededRot = (targetRot + Math.abs(currentRot));
} else if (targetRot <= 0 && currentRot >= 0) {
neededRot = (currentRot + Math.abs(targetRot));
}
}
@Override
public void telemetry() {
engine.telemetry.addData("Target Rotation", targetRot);
engine.telemetry.addData("Current Rotation", currentRot);
engine.telemetry.addData("Power", robot.leftFront.getPower());
engine.telemetry.addData("Distance Sensor Reading", robot.distSensor.getDistance(DistanceUnit.MM));
engine.telemetry.addLine();
@@ -54,15 +67,14 @@ public class SodiPizzaAutoTurnState extends CyberarmState {
@Override
public void start() {
startPos = robot.leftFront.getCurrentPosition();
turnSpeedRaw = 0;
getTurnSpeed();
robot.leftFront.setPower(turnSpeed);
robot.leftBack.setPower(turnSpeed);
robot.rightFront.setPower(turnSpeed);
robot.rightBack.setPower(turnSpeed);
neededRot = (targetRot - robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
engine.blackboardSet("readyToTurn", 0);
}
@@ -73,21 +85,27 @@ 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) {
targetRot = -90;
turnSpeedRaw = 0.3;
getTurnSpeed();
robot.leftFront.setPower(turnSpeed);
robot.leftBack.setPower(turnSpeed);
robot.rightFront.setPower(-turnSpeed);
robot.rightBack.setPower(-turnSpeed);
} else if (readyToTurn == 1 && Math.abs(neededRot) < 5) {
} else if (readyToTurn == 1 && Math.abs(neededRot) < 3) {
turnSpeedRaw = 0;
getTurnSpeed();
robot.leftFront.setPower(turnSpeed);
robot.leftBack.setPower(turnSpeed);
robot.rightFront.setPower(turnSpeed);