fixed is motion magic flag

Team334 · Feb 8, 2025 · cea76c9 · cea76c9
1 parent 08728e5
commit cea76c9
Showing 1 changed file with 13 additions and 8 deletions.
diff --git a/src/main/java/frc/robot/subsystems/Wristevator.java b/src/main/java/frc/robot/subsystems/Wristevator.java
@@ -75,8 +75,8 @@ public class Wristevator extends AdvancedSubsystem {
   private final DynamicMotionMagicVoltage _angleSetter =
       new DynamicMotionMagicVoltage(HOME.getAngle().in(Rotations), 0, 0, 0);
 
-  private final StatusSignal<MotionMagicIsRunningValue> _isMotionMagic =
-      _leftMotor.getMotionMagicIsRunning();
+  @Logged(name = "Is Motion Magic")
+  private boolean _isMotionMagic = false;
 
   private final StatusSignal<Double> _elevatorReference = _leftMotor.getClosedLoopReference();
   private final StatusSignal<Double> _elevatorReferenceSlope =
@@ -329,7 +329,12 @@ private double distance(Setpoint b) {
 
   // refresh the references of the talonfx profiles
   private void refreshProfileReferences() {
-    if (_isMotionMagic.getValue() == MotionMagicIsRunningValue.Disabled) return;
+    _isMotionMagic =
+        _leftMotor.getMotionMagicIsRunning().getValue() == MotionMagicIsRunningValue.Enabled
+            && _wristMotor.getMotionMagicIsRunning().getValue()
+                == MotionMagicIsRunningValue.Enabled;
+
+    if (!_isMotionMagic) return;
 
     BaseStatusSignal.refreshAll(
         _elevatorReference, _elevatorReferenceSlope, _wristReference, _wristReferenceSlope);
@@ -458,7 +463,7 @@ public Command setGoal(Setpoint goal) {
 
           refreshProfileReferences();
 
-          if (_isManual) _isManual = false;
+          if (_isMotionMagic) _isManual = false;
 
           // once the next setpoint is reached, re-find the next one
           if (finishedProfiles(_nextSetpoint)) {
@@ -514,11 +519,11 @@ public Command setSpeeds(DoubleSupplier elevatorSpeed, DoubleSupplier wristSpeed
   public void periodic() {
     super.periodic();
 
-    _isMotionMagic.refresh();
+    _isMotionMagic =
+        _leftMotor.getMotionMagicIsRunning().getValue() == MotionMagicIsRunningValue.Enabled
+            && _wristMotor.getMotionMagicIsRunning().getValue()
+                == MotionMagicIsRunningValue.Enabled;
 
-    DogLog.log(
-        "Wristevator/Is Motion Magic",
-        _isMotionMagic.getValue() == MotionMagicIsRunningValue.Enabled);
     DogLog.log(
         "Wristevator/Elevator Reference",
         Units.rotationsToRadians(_elevatorReference.getValueAsDouble()));