From 2f310a748ccf1d2f3ce4b0c4f800f402fa5f3fae Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Sun, 5 Feb 2023 13:21:16 -0800
Subject: [PATCH] [wpimath] Fix DCMotor.getSpeed() (#5061)

This bug didn't occur in C++ because the units system caught it at
compile time.
---
 .../src/main/java/edu/wpi/first/math/system/plant/DCMotor.java | 3 ++-
 1 file changed, 2 insertions(+), 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
index cd431f7340..ee0b2c79f0 100644
--- 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 class DCMotor {
    * @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;
   }
 
   /**