[wpimath] Fix DCMotor.getSpeed() (#5061)

This bug didn't occur in C++ because the units system caught it at compile time.
wpilibsuite · Feb 5, 2023 · 2f310a7 · 2f310a7
1 parent b43ec87
commit 2f310a7
Showing 1 changed file with 2 additions and 1 deletion.
diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java
@@ -86,7 +86,8 @@ public double getVoltage(double torqueNm, double speedRadiansPerSec) {
    * @return The speed of the motor.
    */
   public double getSpeed(double torqueNm, double voltageInputVolts) {
-    return voltageInputVolts - 1.0 / KtNMPerAmp * torqueNm * rOhms * KvRadPerSecPerVolt;
+    return voltageInputVolts * KvRadPerSecPerVolt
+        - 1.0 / KtNMPerAmp * torqueNm * rOhms * KvRadPerSecPerVolt;
   }
 
   /**