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

OhmV-IR · KangarooKoala · web-flow · commit f8481ecf85cd · 2025-02-20T22:09:30.000-05:00
Co-authored-by: Joseph Eng &lt;91924258+KangarooKoala@users.noreply.github.com&gt;
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;
 }
 
 /**

Original file line number	Diff line number	Diff line change
`@@ -234,8 +234,11 @@ constexpr Translation2d SlewRateLimit(const Translation2d& current,`
`234`	`234`	`if (dist < 1e-9_m) {`
`235`	`235`	`return next;`
`236`	`236`	`}`
`237`		`- units::meters_per_second_t velocity = units::math::min(dist / dt, maxVelocity);`
`238`		`- return current + diff * (velocity / dist).value();`
	`237`	`+ if (dist > maxVelocity * dt) {`
	`238`	`+ // Move maximum allowed amount in direction of the difference`
	`239`	`+ return current + maxVelocity * dt * diff / dist;`
	`240`	`+ }`
	`241`	`+ return next;`
`239`	`242`	`}`
`240`	`243`
`241`	`244`	`/**`