Add FOC-enabled Falcon constants to the DCMotor class

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

@@ -21,10 +21,10 @@ public class DCMotor {
    * Constructs a DC motor.
    *
    * @param nominalVoltageVolts Voltage at which the motor constants were measured.
-   * @param stallTorqueNewtonMeters Torque when stalled in Newton-meters.
-   * @param stallCurrentAmps Current draw when stalled in amps.
-   * @param freeCurrentAmps Current draw under no load in amps.
-   * @param freeSpeedRadPerSec Angular velocity under no load in radians per second.
+   * @param stallTorqueNewtonMeters Torque when stalled.
+   * @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(
@@ -47,43 +47,43 @@ public DCMotor(
   }
 
   /**
-   * Estimate the current being drawn by this motor.
+   * Calculate current drawn by motor with given speed and input voltage.
    *
-   * @param speedRadiansPerSec The speed of the motor.
-   * @param voltageInputVolts The input voltage.
+   * @param speedRadiansPerSec The current angular velocity of the motor.
+   * @param voltageInputVolts The voltage being applied to the motor.
    * @return The estimated current.
    */
   public double getCurrent(double speedRadiansPerSec, double voltageInputVolts) {
     return -1.0 / KvRadPerSecPerVolt / rOhms * speedRadiansPerSec + 1.0 / rOhms * voltageInputVolts;
   }
 
   /**
-   * Calculate the torque produced by the motor for a given current.
+   * Calculate torque produced by the motor with a given current.
    *
    * @param currentAmpere The current drawn by the motor.
-   * @return The torque produced.
+   * @return The torque output.
    */
   public double getTorque(double currentAmpere) {
     return currentAmpere * KtNMPerAmp;
   }
 
   /**
-   * Calculate the voltage provided to the motor at a given torque and angular velocity.
+   * Calculate the voltage provided to the motor for a given torque and angular velocity.
    *
    * @param torqueNm The torque produced by the motor.
-   * @param speedRadiansPerSec The speed of the motor.
+   * @param speedRadiansPerSec The current angular velocity 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.
+   * Calculates the angular speed produced by 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.
+   * @return The angular speed of the motor.
    */
   public double getSpeed(double torqueNm, double voltageInputVolts) {
     return voltageInputVolts * KvRadPerSecPerVolt
@@ -227,6 +227,17 @@ public static DCMotor getFalcon500(int numMotors) {
         12, 4.69, 257, 1.5, Units.rotationsPerMinuteToRadiansPerSecond(6380.0), numMotors);
   }
 
+  /**
+   * Return a gearbox of Falcon 500 motors with FOC (Field Orientated Control) enabled.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return A gearbox of Falcon 500 FOC enabled motors.
+   */
+  public static DCMotor getFalcon500Foc(int numMotors) {
+    return new DCMotor(
+        12, 5.84, 304, 1.5, Units.rotationsPerMinuteToRadiansPerSecond(6080.0), numMotors);
+  }
+
   /**
    * Return a gearbox of Romi/TI_RSLK MAX motors.
    *

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

@@ -97,7 +97,7 @@ class WPILIB_DLLEXPORT DCMotor {
   }
 
   /**
-   * Returns the speed produced by the motor at a given torque and input
+   * Returns the angular speed produced by the motor at a given torque and input
    * voltage.
    *
    * @param torque        The torque produced by the motor.
@@ -119,82 +119,90 @@ class WPILIB_DLLEXPORT DCMotor {
   }
 
   /**
-   * Returns instance of CIM.
+   * Returns a gearbox of CIM motors.
    */
   static constexpr DCMotor CIM(int numMotors = 1) {
     return DCMotor(12_V, 2.42_Nm, 133_A, 2.7_A, 5310_rpm, numMotors);
   }
 
   /**
-   * Returns instance of MiniCIM.
+   * Returns a gearbox of MiniCIM motors.
    */
   static constexpr DCMotor MiniCIM(int numMotors = 1) {
     return DCMotor(12_V, 1.41_Nm, 89_A, 3_A, 5840_rpm, numMotors);
   }
 
   /**
-   * Returns instance of Bag motor.
+   * Returns a gearbox of Bag motor motors.
    */
   static constexpr DCMotor Bag(int numMotors = 1) {
     return DCMotor(12_V, 0.43_Nm, 53_A, 1.8_A, 13180_rpm, numMotors);
   }
 
   /**
-   * Returns instance of Vex 775 Pro.
+   * Returns a gearbox of Vex 775 Pro motors.
    */
   static constexpr DCMotor Vex775Pro(int numMotors = 1) {
     return DCMotor(12_V, 0.71_Nm, 134_A, 0.7_A, 18730_rpm, numMotors);
   }
 
   /**
-   * Returns instance of Andymark RS 775-125.
+   * Returns a gearbox of Andymark RS 775-125 motors.
    */
   static constexpr DCMotor RS775_125(int numMotors = 1) {
     return DCMotor(12_V, 0.28_Nm, 18_A, 1.6_A, 5800_rpm, numMotors);
   }
 
   /**
-   * Returns instance of Banebots RS 775.
+   * Returns a gearbox of Banebots RS 775 motors.
    */
   static constexpr DCMotor BanebotsRS775(int numMotors = 1) {
     return DCMotor(12_V, 0.72_Nm, 97_A, 2.7_A, 13050_rpm, numMotors);
   }
 
   /**
-   * Returns instance of Andymark 9015.
+   * Returns a gearbox of Andymark 9015 motors.
    */
   static constexpr DCMotor Andymark9015(int numMotors = 1) {
     return DCMotor(12_V, 0.36_Nm, 71_A, 3.7_A, 14270_rpm, numMotors);
   }
 
   /**
-   * Returns instance of Banebots RS 550.
+   * Returns a gearbox of Banebots RS 550 motors.
    */
   static constexpr DCMotor BanebotsRS550(int numMotors = 1) {
     return DCMotor(12_V, 0.38_Nm, 84_A, 0.4_A, 19000_rpm, numMotors);
   }
 
   /**
-   * Returns instance of NEO brushless motor.
+   * Returns a gearbox of NEO brushless motors.
    */
   static constexpr DCMotor NEO(int numMotors = 1) {
     return DCMotor(12_V, 2.6_Nm, 105_A, 1.8_A, 5676_rpm, numMotors);
   }
 
   /**
-   * Returns instance of NEO 550 brushless motor.
+   * Returns a gearbox of NEO 550 brushless motors.
    */
   static constexpr DCMotor NEO550(int numMotors = 1) {
     return DCMotor(12_V, 0.97_Nm, 100_A, 1.4_A, 11000_rpm, numMotors);
   }
 
   /**
-   * Returns instance of Falcon 500 brushless motor.
+   * Returns a gearbox of Falcon 500 brushless motors.
    */
   static constexpr DCMotor Falcon500(int numMotors = 1) {
     return DCMotor(12_V, 4.69_Nm, 257_A, 1.5_A, 6380_rpm, numMotors);
   }
 
+  /**
+   * Return a gearbox of Falcon 500 motors with FOC (Field-Oriented Control)
+   * enabled.
+   */
+  static constexpr DCMotor Falcon500FOC(int numMotors = 1) {
+    return DCMotor(12_V, 5.84_Nm, 304_A, 1.5_A, 6080_rpm, numMotors);
+  }
+
   /**
    * Return a gearbox of Romi/TI_RSLK MAX motors.
    */