From a3b022a9e94def950738fc248297b8213b754077 Mon Sep 17 00:00:00 2001 From: Vignesh Balasubramaniam Date: Mon, 28 Oct 2024 08:06:57 -0500 Subject: [PATCH] Flip operands in calculation --- .../lasarobotics/drive/TractionControlController.java | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/lasarobotics/drive/TractionControlController.java b/src/main/java/org/lasarobotics/drive/TractionControlController.java index 99b856d..30c9db7 100644 --- a/src/main/java/org/lasarobotics/drive/TractionControlController.java +++ b/src/main/java/org/lasarobotics/drive/TractionControlController.java @@ -129,12 +129,13 @@ public Measure> calculate(Measure> 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(