[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.
    *