Merge branch 'main' of https://github.com/Team334/R2024

Team334 · Jan 26, 2024 · f5798f2 · f5798f2
2 parents a0e7b26 + b6d3371
commit f5798f2
Showing 1 changed file with 8 additions and 3 deletions.
diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java
@@ -405,10 +405,15 @@ public double[] anglesToSpeaker() {
 
 
     Rotation2d currentRotation = getHeading();
-    double rotateDifference = Math.atan((getPose().getY() - Constants.FIELD_CONSTANTS.SPEAKER_POSE.getY()) / (getPose().getX() - Constants.FIELD_CONSTANTS.SPEAKER_POSE.getX()));
-
-    double angleX = currentRotation.getDegrees() - rotateDifference;
+    double angleX = 0;
 
+    if (getPose().getY() > Constants.FIELD_CONSTANTS.SPEAKER_POSE.getY()){
+      angleX = -currentRotation.getDegrees() + Math.atan((getPose().getY() - Constants.FIELD_CONSTANTS.SPEAKER_POSE.getY()) / (getPose().getX() - Constants.FIELD_CONSTANTS.SPEAKER_POSE.getX()));
+    }
+    else{
+      angleX = -currentRotation.getDegrees() - Math.atan((Constants.FIELD_CONSTANTS.SPEAKER_POSE.getY() - getPose().getY()) / (getPose().getX() - Constants.FIELD_CONSTANTS.SPEAKER_POSE.getX()));
+    }
+
     double[] angles = {angleX, angleY};
 
     return angles;