From d8652cfd4f22843d64a51d87f5439305532fd5be Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Tue, 5 Jan 2021 18:28:57 -0800 Subject: [PATCH] [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. --- .../first/wpilibj/system/plant/DCMotor.java | 108 ++++++++---------- .../wpilibj/system/plant/LinearSystemId.java | 26 ++--- .../native/include/frc/system/plant/DCMotor.h | 10 +- .../estimator/ExtendedKalmanFilterTest.java | 6 +- .../estimator/UnscentedKalmanFilterTest.java | 6 +- 5 files changed, 72 insertions(+), 84 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/DCMotor.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/DCMotor.java index 1a681f2a82..49680b355a 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/DCMotor.java +++ b/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); } } diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/LinearSystemId.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/LinearSystemId.java index e7091c0449..f256728878 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/LinearSystemId.java +++ b/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())); } 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 a4ea1af441..24266430a9 100644 --- a/wpimath/src/main/native/include/frc/system/plant/DCMotor.h +++ b/wpimath/src/main/native/include/frc/system/plant/DCMotor.h @@ -55,12 +55,12 @@ class DCMotor { units::radians_per_second_t freeSpeed, int numMotors = 1) : nominalVoltage(nominalVoltage), stallTorque(stallTorque * numMotors), - stallCurrent(stallCurrent), - freeCurrent(freeCurrent), + stallCurrent(stallCurrent * numMotors), + freeCurrent(freeCurrent * numMotors), freeSpeed(freeSpeed), - R(nominalVoltage / stallCurrent), - Kv(freeSpeed / (nominalVoltage - R * freeCurrent)), - Kt(stallTorque * numMotors / stallCurrent) {} + R(nominalVoltage / this->stallCurrent), + Kv(freeSpeed / (nominalVoltage - R * this->freeCurrent)), + Kt(this->stallTorque / this->stallCurrent) {} /** * Returns current drawn by motor with given speed and input voltage. diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilterTest.java index 26b409c324..95b159cfff 100644 --- a/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilterTest.java +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilterTest.java @@ -39,10 +39,8 @@ public class ExtendedKalmanFilterTest { final var J = 5.6; // Robot moment of inertia final var C1 = - -Math.pow(gr, 2) - * motors.m_KtNMPerAmp - / (motors.m_KvRadPerSecPerVolt * motors.m_rOhms * r * r); - final var C2 = gr * motors.m_KtNMPerAmp / (motors.m_rOhms * r); + -Math.pow(gr, 2) * motors.KtNMPerAmp / (motors.KvRadPerSecPerVolt * motors.rOhms * r * r); + final var C2 = gr * motors.KtNMPerAmp / (motors.rOhms * r); final var k1 = 1.0 / m + rb * rb / J; final var k2 = 1.0 / m - rb * rb / J; diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilterTest.java index dbfd3907d7..135c85a26d 100644 --- a/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilterTest.java +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilterTest.java @@ -43,9 +43,9 @@ public class UnscentedKalmanFilterTest { var C1 = -Math.pow(gHigh, 2) - * motors.m_KtNMPerAmp - / (motors.m_KvRadPerSecPerVolt * motors.m_rOhms * r * r); - var C2 = gHigh * motors.m_KtNMPerAmp / (motors.m_rOhms * r); + * motors.KtNMPerAmp + / (motors.KvRadPerSecPerVolt * motors.rOhms * r * r); + var C2 = gHigh * motors.KtNMPerAmp / (motors.rOhms * r); var c = x.get(2, 0); var s = x.get(3, 0);