From c12c813a14ea078d28cc2cfff2c327297a99b71c Mon Sep 17 00:00:00 2001
From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com>
Date: Sun, 11 Feb 2024 18:26:08 -0600
Subject: [PATCH] inverted logic fixed for the finish in drive to coordinates
 state

---
 .../CompetitionStates/DriveToCoordinatesState.java           | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java
index ea902ff..70a01f2 100644
--- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java
+++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java
@@ -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);
                 }
             }