Flip operands in calculation

lasarobotics · Oct 28, 2024 · a3b022a · a3b022a
1 parent 94acbb1
commit a3b022a
Showing 1 changed file with 5 additions and 4 deletions.
diff --git a/src/main/java/org/lasarobotics/drive/TractionControlController.java b/src/main/java/org/lasarobotics/drive/TractionControlController.java
@@ -129,12 +129,13 @@ public Measure<Velocity<Distance>> calculate(Measure<Velocity<Distance>> velocit
     inertialVelocity = Units.MetersPerSecond.of(Math.abs(inertialVelocity.in(Units.MetersPerSecond)));
 
     // See if user has been trying to accelerate for a while...
-    boolean slowWheel = wheelSpeed.minus(Units.MetersPerSecond.of(Math.abs(velocityRequest.in(Units.MetersPerSecond))))
+    boolean slowWheel = Units.MetersPerSecond.of(Math.abs(velocityRequest.in(Units.MetersPerSecond))).minus(wheelSpeed)
       .gt(VELOCITY_DIFFERENCE_THRESHOLD);
-    if (slowWheel) {
+    if (slowWheel) m_forceAccelerateTimer.start();
+    else {
       m_forceAccelerateTimer.reset();
-      m_forceAccelerateTimer.start();
-    } else m_forceAccelerateTimer.stop();
+      m_forceAccelerateTimer.stop();
+    }
     boolean forceAcceleration = m_staticForceAccelerationDebouncer.calculate(
       velocityRequest.gte(VELOCITY_REQUEST_THRESHOLD) && wheelSpeed.lte(WHEEL_SPEED_THRESHOLD)
     ) || m_dynamicForceAccelerationDebouncer.calculate(