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);