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

Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com>

Author: OhmV-IR

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;
 }
 
 /**