TimeCrafters/CenterStage
1
0
Fork 0
mirror of https://github.com/TimeCrafters/CenterStage synced 2025-12-12 23:32:36 +00:00
Code Issues Packages Projects Releases Wiki Activity

inverted logic fixed for the finish in drive to coordinates state

Browse Source
This commit is contained in:
SpencerPiha
2024-02-11 18:26:08 -06:00
parent b57b4c54f1
commit c12c813a14
1 changed files with 3 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java
View File

@@ -69,7 +69,7 @@ public class DriveToCoordinatesState extends CyberarmState {
if (Math.abs(robot.positionX - robot.xTarget) < 5
&& Math.abs(robot.positionY - robot.yTarget) < 5
&& Math.abs(robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) - Math.toDegrees(robot.hTarget)) > 2) {
&& Math.abs(robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) - Math.toDegrees(robot.hTarget)) < 2) {
setHasFinished(true);
}
}
@@ -79,7 +79,8 @@ public class DriveToCoordinatesState extends CyberarmState {
}
if (Math.abs(robot.positionX - robot.xTarget) < 5
&& Math.abs(robot.positionY - robot.yTarget) < 5) {
&& Math.abs(robot.positionY - robot.yTarget) < 5
&& Math.abs(robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) - Math.toDegrees(robot.hTarget)) < 2) {
setHasFinished(true);
}
}