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 3285058ca4..cd431f7340 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 @@ -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. * diff --git a/wpimath/src/main/native/include/frc/system/plant/DCMotor.h b/wpimath/src/main/native/include/frc/system/plant/DCMotor.h index a519b0e891..831f5322a7 100644 --- a/wpimath/src/main/native/include/frc/system/plant/DCMotor.h +++ b/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. */