Take out yaw displacement

Mechanical-Advantage · Feb 14, 2024 · 7d0763a · 7d0763a
1 parent 4f8e355
commit 7d0763a
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 12 deletions.
diff --git a/src/main/java/frc/robot/commands/TurnToSpeaker.java b/src/main/java/frc/robot/commands/TurnToSpeaker.java
@@ -1,7 +1,5 @@
 package frc.robot.commands;
 
-import static frc.robot.Constants.Mode.SIM;
-
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -58,15 +56,13 @@ public void initialize() {
     if (alliance == DriverStation.Alliance.Red) {
       controller.setSetpoint(
           Math.toDegrees(
-                  Math.atan2(
-                      Constants.SPEAKER_Y - drive.getPose().getY(),
-                      Constants.FIELD_LENGTH - drive.getPose().getX()))
-              - (Constants.currentMode == SIM ? Drive.YAW_DISPLACEMENT : 0));
+              Math.atan2(
+                  Constants.SPEAKER_Y - drive.getPose().getY(),
+                  Constants.FIELD_LENGTH - drive.getPose().getX())));
     } else {
       controller.setSetpoint(
           Math.toDegrees(
-              Math.atan2(Constants.SPEAKER_Y - drive.getPose().getY(), -drive.getPose().getX())
-                  - (Constants.currentMode == SIM ? Drive.YAW_DISPLACEMENT : 0)));
+              Math.atan2(Constants.SPEAKER_Y - drive.getPose().getY(), drive.getPose().getX())));
     }
   }
 

diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -39,7 +39,6 @@ public class Drive extends SubsystemBase {
   public static final double WHEEL_RADIUS = Units.inchesToMeters(3.0);
   public static final double TRACK_WIDTH = Units.inchesToMeters(26.0);
   public static final double MAX_SPEED_M_PER_S = Units.feetToMeters(10);
-  public static double YAW_DISPLACEMENT = 0;
   private static final double lKS = Constants.currentMode == SIM ? 0.0 : 0.98355;
   private static final double lKV = Constants.currentMode == SIM ? 0.227 : 30.42797;
 
@@ -147,9 +146,6 @@ public void setPose(Pose2d pose) {
         getRightPositionMeters(),
         pose);
     gyroIO.setGyro(gyroInputs, pose.getRotation().getDegrees());
-    if (Constants.currentMode == SIM) {
-      YAW_DISPLACEMENT = pose.getRotation().getDegrees();
-    }
   }
 
   /** Returns the position of the left wheels in meters. */