Feat: reinitialize heading goal instead of constructor

TitaniumTitans · Mar 28, 2024 · a672ea5 · a672ea5
1 parent e444a7b
commit a672ea5
Showing 1 changed file with 5 additions and 4 deletions.
diff --git a/src/main/java/frc/robot/commands/JoystickDriveCommand.java b/src/main/java/frc/robot/commands/JoystickDriveCommand.java
@@ -46,6 +46,11 @@ public JoystickDriveCommand(DriveSubsystem driveSubsystem,
     addRequirements(this.driveSubsystem);
   }
 
+  @Override
+  public void initialize() {
+    m_headingGoal = driveSubsystem.getGyroRotation();
+  }
+
   @Override
   public void execute() {
     double xInput = setSensitivity(xSupplier.getAsDouble(), 0.25);
@@ -88,10 +93,6 @@ public void execute() {
       }
     }
 
-    if (!Constants.DriveConstants.HOLD_HEADING.getValue()) {
-      rotationOutput = omegaInput;
-    }
-
     Rotation2d heading;
 
     // if red change heading goal