Update wpimath/src/main/native/include/frc/MathUtil.h

Co-authored-by: Joseph Eng <[email protected]>
wpilibsuite · Feb 21, 2025 · f8481ec · f8481ec
1 parent 5852c95
commit f8481ec
Showing 1 changed file with 5 additions and 2 deletions.
diff --git a/wpimath/src/main/native/include/frc/MathUtil.h b/wpimath/src/main/native/include/frc/MathUtil.h
@@ -234,8 +234,11 @@ constexpr Translation2d SlewRateLimit(const Translation2d& current,
   if (dist < 1e-9_m) {
     return next;
   }
-  units::meters_per_second_t velocity = units::math::min(dist / dt, maxVelocity);
-  return current + diff * (velocity / dist).value();
+  if (dist > maxVelocity * dt) {
+    // Move maximum allowed amount in direction of the difference
+    return current + maxVelocity * dt * diff / dist;
+  }
+  return next;
 }
 
 /**