[wpimath] Make Java DCMotor API consistent with C++ and fix motor calcs (#3046)

The stall torque, stall current, and free current are now multiplied by
the number of motors instead of just the stall torque. This produces the
same values for Kt and Kv regardless of the number of motors; the motor
resistance still affects the system response.

For an elevator model, the response should be the same as before since a
factor of "number of motors" shows up in the same place in the
acceleration calculation, but the current calculation will also be
correct now.

wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/DCMotor.java

@@ -8,20 +8,29 @@ import edu.wpi.first.wpilibj.util.Units;
 
 /** Holds the constants for a DC motor. */
 public class DCMotor {
-  public final double m_nominalVoltageVolts;
-  public final double m_stallTorqueNewtonMeters;
-  public final double m_stallCurrentAmps;
-  public final double m_freeCurrentAmps;
-  public final double m_freeSpeedRadPerSec;
+  @SuppressWarnings("MemberName")
+  public final double nominalVoltageVolts;
 
   @SuppressWarnings("MemberName")
-  public final double m_rOhms;
+  public final double stallTorqueNewtonMeters;
 
   @SuppressWarnings("MemberName")
-  public final double m_KvRadPerSecPerVolt;
+  public final double stallCurrentAmps;
 
   @SuppressWarnings("MemberName")
-  public final double m_KtNMPerAmp;
+  public final double freeCurrentAmps;
+
+  @SuppressWarnings("MemberName")
+  public final double freeSpeedRadPerSec;
+
+  @SuppressWarnings("MemberName")
+  public final double rOhms;
+
+  @SuppressWarnings("MemberName")
+  public final double KvRadPerSecPerVolt;
+
+  @SuppressWarnings("MemberName")
+  public final double KtNMPerAmp;
 
   /**
    * Constructs a DC motor.
@@ -31,23 +40,25 @@ public class DCMotor {
    * @param stallCurrentAmps Current draw when stalled.
    * @param freeCurrentAmps Current draw under no load.
    * @param freeSpeedRadPerSec Angular velocity under no load.
+   * @param numMotors Number of motors in a gearbox.
    */
   public DCMotor(
       double nominalVoltageVolts,
       double stallTorqueNewtonMeters,
       double stallCurrentAmps,
       double freeCurrentAmps,
-      double freeSpeedRadPerSec) {
-    this.m_nominalVoltageVolts = nominalVoltageVolts;
-    this.m_stallTorqueNewtonMeters = stallTorqueNewtonMeters;
-    this.m_stallCurrentAmps = stallCurrentAmps;
-    this.m_freeCurrentAmps = freeCurrentAmps;
-    this.m_freeSpeedRadPerSec = freeSpeedRadPerSec;
+      double freeSpeedRadPerSec,
+      int numMotors) {
+    this.nominalVoltageVolts = nominalVoltageVolts;
+    this.stallTorqueNewtonMeters = stallTorqueNewtonMeters * numMotors;
+    this.stallCurrentAmps = stallCurrentAmps * numMotors;
+    this.freeCurrentAmps = freeCurrentAmps * numMotors;
+    this.freeSpeedRadPerSec = freeSpeedRadPerSec;
 
-    this.m_rOhms = nominalVoltageVolts / stallCurrentAmps;
-    this.m_KvRadPerSecPerVolt =
-        freeSpeedRadPerSec / (nominalVoltageVolts - m_rOhms * freeCurrentAmps);
-    this.m_KtNMPerAmp = stallTorqueNewtonMeters / stallCurrentAmps;
+    this.rOhms = nominalVoltageVolts / this.stallCurrentAmps;
+    this.KvRadPerSecPerVolt =
+        freeSpeedRadPerSec / (nominalVoltageVolts - rOhms * this.freeCurrentAmps);
+    this.KtNMPerAmp = this.stallTorqueNewtonMeters / this.stallCurrentAmps;
   }
 
   /**
@@ -57,8 +68,7 @@ public class DCMotor {
    * @param voltageInputVolts The input voltage.
    */
   public double getCurrent(double speedRadiansPerSec, double voltageInputVolts) {
-    return -1.0 / m_KvRadPerSecPerVolt / m_rOhms * speedRadiansPerSec
-        + 1.0 / m_rOhms * voltageInputVolts;
+    return -1.0 / KvRadPerSecPerVolt / rOhms * speedRadiansPerSec + 1.0 / rOhms * voltageInputVolts;
   }
 
   /**
@@ -68,7 +78,7 @@ public class DCMotor {
    */
   public static DCMotor getCIM(int numMotors) {
     return new DCMotor(
-        12, 2.42 * numMotors, 133, 2.7, Units.rotationsPerMinuteToRadiansPerSecond(5310));
+        12, 2.42, 133, 2.7, Units.rotationsPerMinuteToRadiansPerSecond(5310), numMotors);
   }
 
   /**
@@ -77,9 +87,8 @@ public class DCMotor {
    * @param numMotors Number of motors in the gearbox.
    */
   public static DCMotor getVex775Pro(int numMotors) {
-    return gearbox(
-        new DCMotor(12, 0.71, 134, 0.7, Units.rotationsPerMinuteToRadiansPerSecond(18730)),
-        numMotors);
+    return new DCMotor(
+        12, 0.71, 134, 0.7, Units.rotationsPerMinuteToRadiansPerSecond(18730), numMotors);
   }
 
   /**
@@ -88,9 +97,8 @@ public class DCMotor {
    * @param numMotors Number of motors in the gearbox.
    */
   public static DCMotor getNEO(int numMotors) {
-    return gearbox(
-        new DCMotor(12, 2.6, 105, 1.8, Units.rotationsPerMinuteToRadiansPerSecond(5676)),
-        numMotors);
+    return new DCMotor(
+        12, 2.6, 105, 1.8, Units.rotationsPerMinuteToRadiansPerSecond(5676), numMotors);
   }
 
   /**
@@ -99,8 +107,8 @@ public class DCMotor {
    * @param numMotors Number of motors in the gearbox.
    */
   public static DCMotor getMiniCIM(int numMotors) {
-    return gearbox(
-        new DCMotor(12, 1.41, 89, 3, Units.rotationsPerMinuteToRadiansPerSecond(5840)), numMotors);
+    return new DCMotor(
+        12, 1.41, 89, 3, Units.rotationsPerMinuteToRadiansPerSecond(5840), numMotors);
   }
 
   /**
@@ -109,9 +117,8 @@ public class DCMotor {
    * @param numMotors Number of motors in the gearbox.
    */
   public static DCMotor getBag(int numMotors) {
-    return gearbox(
-        new DCMotor(12, 0.43, 53, 1.8, Units.rotationsPerMinuteToRadiansPerSecond(13180)),
-        numMotors);
+    return new DCMotor(
+        12, 0.43, 53, 1.8, Units.rotationsPerMinuteToRadiansPerSecond(13180), numMotors);
   }
 
   /**
@@ -120,9 +127,8 @@ public class DCMotor {
    * @param numMotors Number of motors in the gearbox.
    */
   public static DCMotor getAndymarkRs775_125(int numMotors) {
-    return gearbox(
-        new DCMotor(12, 0.28, 18, 1.6, Units.rotationsPerMinuteToRadiansPerSecond(5800.0)),
-        numMotors);
+    return new DCMotor(
+        12, 0.28, 18, 1.6, Units.rotationsPerMinuteToRadiansPerSecond(5800.0), numMotors);
   }
 
   /**
@@ -131,9 +137,8 @@ public class DCMotor {
    * @param numMotors Number of motors in the gearbox.
    */
   public static DCMotor getBanebotsRs775(int numMotors) {
-    return gearbox(
-        new DCMotor(12, 0.72, 97, 2.7, Units.rotationsPerMinuteToRadiansPerSecond(13050.0)),
-        numMotors);
+    return new DCMotor(
+        12, 0.72, 97, 2.7, Units.rotationsPerMinuteToRadiansPerSecond(13050.0), numMotors);
   }
 
   /**
@@ -142,9 +147,8 @@ public class DCMotor {
    * @param numMotors Number of motors in the gearbox.
    */
   public static DCMotor getAndymark9015(int numMotors) {
-    return gearbox(
-        new DCMotor(12, 0.36, 71, 3.7, Units.rotationsPerMinuteToRadiansPerSecond(14270.0)),
-        numMotors);
+    return new DCMotor(
+        12, 0.36, 71, 3.7, Units.rotationsPerMinuteToRadiansPerSecond(14270.0), numMotors);
   }
 
   /**
@@ -153,9 +157,8 @@ public class DCMotor {
    * @param numMotors Number of motors in the gearbox.
    */
   public static DCMotor getBanebotsRs550(int numMotors) {
-    return gearbox(
-        new DCMotor(12, 0.38, 84, 0.4, Units.rotationsPerMinuteToRadiansPerSecond(19000.0)),
-        numMotors);
+    return new DCMotor(
+        12, 0.38, 84, 0.4, Units.rotationsPerMinuteToRadiansPerSecond(19000.0), numMotors);
   }
 
   /**
@@ -164,9 +167,8 @@ public class DCMotor {
    * @param numMotors Number of motors in the gearbox.
    */
   public static DCMotor getNeo550(int numMotors) {
-    return gearbox(
-        new DCMotor(12, 0.97, 100, 1.4, Units.rotationsPerMinuteToRadiansPerSecond(11000.0)),
-        numMotors);
+    return new DCMotor(
+        12, 0.97, 100, 1.4, Units.rotationsPerMinuteToRadiansPerSecond(11000.0), numMotors);
   }
 
   /**
@@ -175,17 +177,7 @@ public class DCMotor {
    * @param numMotors Number of motors in the gearbox.
    */
   public static DCMotor getFalcon500(int numMotors) {
-    return gearbox(
-        new DCMotor(12, 4.69, 257, 1.5, Units.rotationsPerMinuteToRadiansPerSecond(6380.0)),
-        numMotors);
-  }
-
-  private static DCMotor gearbox(DCMotor motor, double numMotors) {
     return new DCMotor(
-        motor.m_nominalVoltageVolts,
-        motor.m_stallTorqueNewtonMeters * numMotors,
-        motor.m_stallCurrentAmps,
-        motor.m_freeCurrentAmps,
-        motor.m_freeSpeedRadPerSec);
+        12, 4.69, 257, 1.5, Units.rotationsPerMinuteToRadiansPerSecond(6380.0), numMotors);
   }
 }

wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/LinearSystemId.java

@@ -35,13 +35,13 @@ public final class LinearSystemId {
                 1,
                 0,
                 -Math.pow(G, 2)
-                    * motor.m_KtNMPerAmp
-                    / (motor.m_rOhms
+                    * motor.KtNMPerAmp
+                    / (motor.rOhms
                         * radiusMeters
                         * radiusMeters
                         * massKg
-                        * motor.m_KvRadPerSecPerVolt)),
-        VecBuilder.fill(0, G * motor.m_KtNMPerAmp / (motor.m_rOhms * radiusMeters * massKg)),
+                        * motor.KvRadPerSecPerVolt)),
+        VecBuilder.fill(0, G * motor.KtNMPerAmp / (motor.rOhms * radiusMeters * massKg)),
         Matrix.mat(Nat.N1(), Nat.N2()).fill(1, 0),
         new Matrix<>(Nat.N1(), Nat.N1()));
   }
@@ -61,9 +61,9 @@ public final class LinearSystemId {
         VecBuilder.fill(
             -G
                 * G
-                * motor.m_KtNMPerAmp
-                / (motor.m_KvRadPerSecPerVolt * motor.m_rOhms * jKgMetersSquared)),
-        VecBuilder.fill(G * motor.m_KtNMPerAmp / (motor.m_rOhms * jKgMetersSquared)),
+                * motor.KtNMPerAmp
+                / (motor.KvRadPerSecPerVolt * motor.rOhms * jKgMetersSquared)),
+        VecBuilder.fill(G * motor.KtNMPerAmp / (motor.rOhms * jKgMetersSquared)),
         Matrix.eye(Nat.N1()),
         new Matrix<>(Nat.N1(), Nat.N1()));
   }
@@ -89,10 +89,8 @@ public final class LinearSystemId {
       double JKgMetersSquared,
       double G) {
     var C1 =
-        -(G * G)
-            * motor.m_KtNMPerAmp
-            / (motor.m_KvRadPerSecPerVolt * motor.m_rOhms * rMeters * rMeters);
-    var C2 = G * motor.m_KtNMPerAmp / (motor.m_rOhms * rMeters);
+        -(G * G) * motor.KtNMPerAmp / (motor.KvRadPerSecPerVolt * motor.rOhms * rMeters * rMeters);
+    var C2 = G * motor.KtNMPerAmp / (motor.rOhms * rMeters);
 
     final double C3 = 1 / massKg + rbMeters * rbMeters / JKgMetersSquared;
     final double C4 = 1 / massKg - rbMeters * rbMeters / JKgMetersSquared;
@@ -123,9 +121,9 @@ public final class LinearSystemId {
                 1,
                 0,
                 -Math.pow(G, 2)
-                    * motor.m_KtNMPerAmp
-                    / (motor.m_KvRadPerSecPerVolt * motor.m_rOhms * jKgSquaredMeters)),
-        VecBuilder.fill(0, G * motor.m_KtNMPerAmp / (motor.m_rOhms * jKgSquaredMeters)),
+                    * motor.KtNMPerAmp
+                    / (motor.KvRadPerSecPerVolt * motor.rOhms * jKgSquaredMeters)),
+        VecBuilder.fill(0, G * motor.KtNMPerAmp / (motor.rOhms * jKgSquaredMeters)),
         Matrix.mat(Nat.N1(), Nat.N2()).fill(1, 0),
         new Matrix<>(Nat.N1(), Nat.N1()));
   }