[wpimath] Add new DCMotor functions for alternative calculations and reduction calculation (#4749)

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

@@ -49,7 +49,7 @@ public class DCMotor {
   /**
    * Estimate the current being drawn by this motor.
    *
-   * @param speedRadiansPerSec The speed of the rotor.
+   * @param speedRadiansPerSec The speed of the motor.
    * @param voltageInputVolts The input voltage.
    * @return The estimated current.
    */
@@ -57,6 +57,54 @@ public class DCMotor {
     return -1.0 / KvRadPerSecPerVolt / rOhms * speedRadiansPerSec + 1.0 / rOhms * voltageInputVolts;
   }
 
+  /**
+   * Calculate the torque produced by the motor for a given current.
+   *
+   * @param currentAmpere The current drawn by the motor.
+   * @return The torque produced.
+   */
+  public double getTorque(double currentAmpere) {
+    return currentAmpere * KtNMPerAmp;
+  }
+
+  /**
+   * Calculate the voltage provided to the motor at a given torque and angular velocity.
+   *
+   * @param torqueNm The torque produced by the motor.
+   * @param speedRadiansPerSec The speed of the motor.
+   * @return The voltage of the motor.
+   */
+  public double getVoltage(double torqueNm, double speedRadiansPerSec) {
+    return 1.0 / KvRadPerSecPerVolt * speedRadiansPerSec + 1.0 / KtNMPerAmp * rOhms * torqueNm;
+  }
+
+  /**
+   * Calculate the speed of the motor at a given torque and input voltage.
+   *
+   * @param torqueNm The torque produced by the motor.
+   * @param voltageInputVolts The voltage applied to the motor.
+   * @return The speed of the motor.
+   */
+  public double getSpeed(double torqueNm, double voltageInputVolts) {
+    return voltageInputVolts - 1.0 / KtNMPerAmp * torqueNm * rOhms * KvRadPerSecPerVolt;
+  }
+
+  /**
+   * Returns a copy of this motor with the given gearbox reduction applied.
+   *
+   * @param gearboxReduction The gearbox reduction.
+   * @return A motor with the gearbox reduction applied.
+   */
+  public DCMotor withReduction(double gearboxReduction) {
+    return new DCMotor(
+        nominalVoltageVolts,
+        stallTorqueNewtonMeters * gearboxReduction,
+        stallCurrentAmps,
+        freeCurrentAmps,
+        freeSpeedRadPerSec / gearboxReduction,
+        1);
+  }
+
   /**
    * Return a gearbox of CIM motors.
    *

wpimath/src/main/native/include/frc/system/plant/DCMotor.h

@@ -75,6 +75,49 @@ class WPILIB_DLLEXPORT DCMotor {
     return -1.0 / Kv / R * speed + 1.0 / R * inputVoltage;
   }
 
+  /**
+   * Returns torque produced by the motor with a given current.
+   *
+   * @param current     The current drawn by the motor.
+   */
+  constexpr units::newton_meter_t Torque(units::ampere_t current) const {
+    return current * Kt;
+  }
+
+  /**
+   * Returns the voltage provided to the motor for a given torque and
+   * angular velocity.
+   *
+   * @param torque      The torque produced by the motor.
+   * @param speed       The current angular velocity of the motor.
+   */
+  constexpr units::volt_t Voltage(units::newton_meter_t torque,
+                                  units::radians_per_second_t speed) const {
+    return 1.0 / Kv * speed + 1.0 / Kt * R * torque;
+  }
+
+  /**
+   * Returns the speed produced by the motor at a given torque and input
+   * voltage.
+   *
+   * @param torque        The torque produced by the motor.
+   * @param inputVoltage  The input voltage provided to the motor.
+   */
+  constexpr units::radians_per_second_t Speed(
+      units::newton_meter_t torque, units::volt_t inputVoltage) const {
+    return inputVoltage * Kv - 1.0 / Kt * torque * R * Kv;
+  }
+
+  /**
+   * Returns a copy of this motor with the given gearbox reduction applied.
+   *
+   * @param gearboxReduction  The gearbox reduction.
+   */
+  constexpr DCMotor WithReduction(double gearboxReduction) {
+    return DCMotor(nominalVoltage, stallTorque * gearboxReduction, stallCurrent,
+                   freeCurrent, freeSpeed / gearboxReduction);
+  }
+
   /**
    * Returns instance of CIM.
    */