From 06ae8fa3c446d305b2a998648ce55a92be418cc4 Mon Sep 17 00:00:00 2001 From: Justin Tervay Date: Mon, 17 Nov 2025 16:35:16 -0500 Subject: [PATCH] Fix DCMotor equations to use freeCurrent --- .../wpi/first/math/system/plant/DCMotor.java | 10 +- .../native/include/frc/system/plant/DCMotor.h | 11 +- .../LinearQuadraticRegulatorTest.java | 10 +- .../first/math/system/LinearSystemIDTest.java | 16 +-- .../first/math/system/plant/DCMotorTest.java | 129 ++++++++++++++++++ .../LinearQuadraticRegulatorTest.cpp | 8 +- .../native/cpp/system/LinearSystemIDTest.cpp | 16 +-- .../native/cpp/system/plant/DCMotorTest.cpp | 108 +++++++++++++++ 8 files changed, 273 insertions(+), 35 deletions(-) create mode 100644 wpimath/src/test/java/edu/wpi/first/math/system/plant/DCMotorTest.java create mode 100644 wpimath/src/test/native/cpp/system/plant/DCMotorTest.cpp 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 dead6162f84..c289e6e42f4 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 @@ -68,7 +68,7 @@ public DCMotor( this.rOhms = nominalVoltageVolts / this.stallCurrentAmps; this.KvRadPerSecPerVolt = freeSpeedRadPerSec / (nominalVoltageVolts - rOhms * this.freeCurrentAmps); - this.KtNMPerAmp = this.stallTorqueNewtonMeters / this.stallCurrentAmps; + this.KtNMPerAmp = this.stallTorqueNewtonMeters / (this.stallCurrentAmps - this.freeCurrentAmps); } /** @@ -89,7 +89,7 @@ public double getCurrent(double speedRadiansPerSec, double voltageInputVolts) { * @return The current drawn by the motor. */ public double getCurrent(double torqueNm) { - return torqueNm / KtNMPerAmp; + return torqueNm / KtNMPerAmp + freeCurrentAmps; } /** @@ -99,7 +99,7 @@ public double getCurrent(double torqueNm) { * @return The torque output. */ public double getTorque(double currentAmpere) { - return currentAmpere * KtNMPerAmp; + return (currentAmpere - freeCurrentAmps) * KtNMPerAmp; } /** @@ -110,7 +110,7 @@ public double getTorque(double currentAmpere) { * @return The voltage of the motor. */ public double getVoltage(double torqueNm, double speedRadiansPerSec) { - return 1.0 / KvRadPerSecPerVolt * speedRadiansPerSec + 1.0 / KtNMPerAmp * rOhms * torqueNm; + return 1.0 / KvRadPerSecPerVolt * speedRadiansPerSec + 1.0 / KtNMPerAmp * rOhms * torqueNm + freeCurrentAmps * rOhms; } /** @@ -122,7 +122,7 @@ public double getVoltage(double torqueNm, double speedRadiansPerSec) { */ public double getSpeed(double torqueNm, double voltageInputVolts) { return voltageInputVolts * KvRadPerSecPerVolt - - 1.0 / KtNMPerAmp * torqueNm * rOhms * KvRadPerSecPerVolt; + - 1.0 / KtNMPerAmp * torqueNm * rOhms * KvRadPerSecPerVolt - KvRadPerSecPerVolt * freeCurrentAmps * rOhms; } /** 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 145cf02bfd3..1e84e332bb9 100644 --- a/wpimath/src/main/native/include/frc/system/plant/DCMotor.h +++ b/wpimath/src/main/native/include/frc/system/plant/DCMotor.h @@ -71,7 +71,7 @@ class WPILIB_DLLEXPORT DCMotor { freeSpeed(freeSpeed), R(nominalVoltage / this->stallCurrent), Kv(freeSpeed / (nominalVoltage - R * this->freeCurrent)), - Kt(this->stallTorque / this->stallCurrent) {} + Kt(this->stallTorque / (this->stallCurrent - this->freeCurrent)) {} /** * Returns current drawn by motor with given speed and input voltage. @@ -90,7 +90,7 @@ class WPILIB_DLLEXPORT DCMotor { * @param torque The torque produced by the motor. */ constexpr units::ampere_t Current(units::newton_meter_t torque) const { - return torque / Kt; + return torque / Kt + freeCurrent; } /** @@ -99,7 +99,7 @@ class WPILIB_DLLEXPORT DCMotor { * @param current The current drawn by the motor. */ constexpr units::newton_meter_t Torque(units::ampere_t current) const { - return current * Kt; + return (current - freeCurrent) * Kt; } /** @@ -111,7 +111,7 @@ class WPILIB_DLLEXPORT DCMotor { */ 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; + return 1.0 / Kv * speed + 1.0 / Kt * R * torque + freeCurrent * R; } /** @@ -123,7 +123,8 @@ class WPILIB_DLLEXPORT DCMotor { */ 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; + return inputVoltage * Kv - 1.0 / Kt * torque * R * Kv - + Kv * freeCurrent * R; } /** diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java index abb26dc8da3..c5dac24b678 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java @@ -33,8 +33,8 @@ void testLQROnElevator() { var K = new LinearQuadraticRegulator<>(plant, qElms, rElms, dt).getK(); - assertEquals(522.87006795347486, K.get(0, 0), 1e-6); - assertEquals(38.239878385020411, K.get(0, 1), 1e-6); + assertEquals(522.59, K.get(0, 0), 1e-2); + assertEquals(38.17, K.get(0, 1), 1e-2); } @Test @@ -49,7 +49,7 @@ void testFourMotorElevator() { new LinearQuadraticRegulator<>(plant, VecBuilder.fill(0.1, 0.2), VecBuilder.fill(12.0), dt) .getK(); - assertEquals(10.381, K.get(0, 0), 1e-2); + assertEquals(10.365, K.get(0, 0), 1e-2); assertEquals(0.6929, K.get(0, 1), 1e-2); } @@ -69,8 +69,8 @@ void testLQROnArm() { var K = new LinearQuadraticRegulator<>(plant, qElms, rElms, dt).getK(); - assertEquals(19.339349883583761, K.get(0, 0), 1e-6); - assertEquals(3.3542559517421582, K.get(0, 1), 1e-6); + assertEquals(19.247, K.get(0, 0), 1e-2); + assertEquals(3.335, K.get(0, 1), 1e-2); } /** diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java index 21ab6acf423..38605b3e8f2 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java @@ -24,14 +24,14 @@ void testDrivetrainVelocitySystem() { model .getA() .isEqual( - MatBuilder.fill(Nat.N2(), Nat.N2(), -10.14132, 3.06598, 3.06598, -10.14132), + MatBuilder.fill(Nat.N2(), Nat.N2(), -10.3182, 3.11946, 3.11946, -10.3182), 0.001)); assertTrue( model .getB() .isEqual( - MatBuilder.fill(Nat.N2(), Nat.N2(), 4.2590, -1.28762, -1.2876, 4.2590), 0.001)); + MatBuilder.fill(Nat.N2(), Nat.N2(), 4.33333, -1.31008, -1.31008, 4.33333), 0.001)); assertTrue( model.getC().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0), 0.001)); @@ -44,9 +44,9 @@ void testDrivetrainVelocitySystem() { void testElevatorSystem() { var model = LinearSystemId.createElevatorSystem(DCMotor.getNEO(2), 5, 0.05, 12); assertTrue( - model.getA().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -99.05473), 0.001)); + model.getA().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -100.782), 0.001)); - assertTrue(model.getB().isEqual(VecBuilder.fill(0, 20.8), 0.001)); + assertTrue(model.getB().isEqual(VecBuilder.fill(0, 21.1628), 0.001)); assertTrue(model.getC().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1), 0.001)); @@ -56,9 +56,9 @@ void testElevatorSystem() { @Test void testFlywheelSystem() { var model = LinearSystemId.createFlywheelSystem(DCMotor.getNEO(2), 0.00032, 1.0); - assertTrue(model.getA().isEqual(VecBuilder.fill(-26.87032), 0.001)); + assertTrue(model.getA().isEqual(VecBuilder.fill(-27.339), 0.001)); - assertTrue(model.getB().isEqual(VecBuilder.fill(1354.166667), 0.001)); + assertTrue(model.getB().isEqual(VecBuilder.fill(1377.785), 0.001)); assertTrue(model.getC().isEqual(VecBuilder.fill(1), 0.001)); @@ -69,9 +69,9 @@ void testFlywheelSystem() { void testDCMotorSystem() { var model = LinearSystemId.createDCMotorSystem(DCMotor.getNEO(2), 0.00032, 1.0); assertTrue( - model.getA().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -26.87032), 0.001)); + model.getA().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -27.339), 0.001)); - assertTrue(model.getB().isEqual(VecBuilder.fill(0, 1354.166667), 0.001)); + assertTrue(model.getB().isEqual(VecBuilder.fill(0, 1377.785), 0.001)); assertTrue(model.getC().isEqual(Matrix.eye(Nat.N2()), 0.001)); diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/plant/DCMotorTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/plant/DCMotorTest.java new file mode 100644 index 00000000000..39baed09c10 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/system/plant/DCMotorTest.java @@ -0,0 +1,129 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.system.plant; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import java.util.function.Supplier; +import java.util.stream.Stream; +import org.junit.jupiter.params.ParameterizedTest; +import org.junit.jupiter.params.provider.Arguments; +import org.junit.jupiter.params.provider.MethodSource; + +class DCMotorTest { + private static final double kTolerance = 1e-12; + + static Stream motors() { + return Stream.of( + Arguments.of("CIM", (Supplier) () -> DCMotor.getCIM(1)), + Arguments.of("MiniCIM", (Supplier) () -> DCMotor.getMiniCIM(1)), + Arguments.of("Bag", (Supplier) () -> DCMotor.getBag(1)), + Arguments.of("Vex775Pro", (Supplier) () -> DCMotor.getVex775Pro(1)), + Arguments.of("AndymarkRs775_125", (Supplier) () -> DCMotor.getAndymarkRs775_125(1)), + Arguments.of("BanebotsRs775", (Supplier) () -> DCMotor.getBanebotsRs775(1)), + Arguments.of("Andymark9015", (Supplier) () -> DCMotor.getAndymark9015(1)), + Arguments.of("BanebotsRs550", (Supplier) () -> DCMotor.getBanebotsRs550(1)), + Arguments.of("NEO", (Supplier) () -> DCMotor.getNEO(1)), + Arguments.of("Neo550", (Supplier) () -> DCMotor.getNeo550(1)), + Arguments.of("Falcon500", (Supplier) () -> DCMotor.getFalcon500(1)), + Arguments.of("Falcon500Foc", (Supplier) () -> DCMotor.getFalcon500Foc(1)), + Arguments.of("RomiBuiltIn", (Supplier) () -> DCMotor.getRomiBuiltIn(1)), + Arguments.of("KrakenX60", (Supplier) () -> DCMotor.getKrakenX60(1)), + Arguments.of("KrakenX60Foc", (Supplier) () -> DCMotor.getKrakenX60Foc(1)), + Arguments.of("KrakenX44", (Supplier) () -> DCMotor.getKrakenX44(1)), + Arguments.of("KrakenX44Foc", (Supplier) () -> DCMotor.getKrakenX44Foc(1)), + Arguments.of("Minion", (Supplier) () -> DCMotor.getMinion(1)), + Arguments.of("NeoVortex", (Supplier) () -> DCMotor.getNeoVortex(1))); + } + + @ParameterizedTest(name = "{0}") + @MethodSource("motors") + void testCurrentAtZeroSpeedAndNominalVoltageIsStallCurrent( + String name, Supplier motorFactory) { + var motor = motorFactory.get(); + assertEquals( + motor.stallCurrentAmps, + motor.getCurrent(0.0, motor.nominalVoltageVolts), + kTolerance); + } + + @ParameterizedTest(name = "{0}") + @MethodSource("motors") + void testCurrentAtFreeSpeedAndNominalVoltageIsFreeCurrent( + String name, Supplier motorFactory) { + var motor = motorFactory.get(); + assertEquals( + motor.freeCurrentAmps, + motor.getCurrent(motor.freeSpeedRadPerSec, motor.nominalVoltageVolts), + kTolerance); + } + + @ParameterizedTest(name = "{0}") + @MethodSource("motors") + void testCurrentAtStallTorqueIsStallCurrent(String name, Supplier motorFactory) { + var motor = motorFactory.get(); + assertEquals(motor.stallCurrentAmps, motor.getCurrent(motor.stallTorqueNewtonMeters), kTolerance); + } + + @ParameterizedTest(name = "{0}") + @MethodSource("motors") + void testCurrentAtZeroTorqueIsFreeCurrent(String name, Supplier motorFactory) { + var motor = motorFactory.get(); + assertEquals(motor.freeCurrentAmps, motor.getCurrent(0.0), kTolerance); + } + + @ParameterizedTest(name = "{0}") + @MethodSource("motors") + void testTorqueAtFreeCurrentIsZero(String name, Supplier motorFactory) { + var motor = motorFactory.get(); + assertEquals(0.0, motor.getTorque(motor.freeCurrentAmps), kTolerance); + } + + @ParameterizedTest(name = "{0}") + @MethodSource("motors") + void testTorqueAtStallCurrentIsStallTorque(String name, Supplier motorFactory) { + var motor = motorFactory.get(); + assertEquals( + motor.stallTorqueNewtonMeters, motor.getTorque(motor.stallCurrentAmps), kTolerance); + } + + @ParameterizedTest(name = "{0}") + @MethodSource("motors") + void testVoltageAtStallTorqueAndZeroSpeedIsNominalVoltage( + String name, Supplier motorFactory) { + var motor = motorFactory.get(); + assertEquals( + motor.nominalVoltageVolts, + motor.getVoltage(motor.stallTorqueNewtonMeters, 0.0), + kTolerance); + } + + @ParameterizedTest(name = "{0}") + @MethodSource("motors") + void testVoltageAtZeroTorqueAndFreeSpeedIsNominalVoltage( + String name, Supplier motorFactory) { + var motor = motorFactory.get(); + assertEquals( + motor.nominalVoltageVolts, motor.getVoltage(0.0, motor.freeSpeedRadPerSec), kTolerance); + } + + @ParameterizedTest(name = "{0}") + @MethodSource("motors") + void testSpeedAtStallTorqueAndNominalVoltageIsZero( + String name, Supplier motorFactory) { + var motor = motorFactory.get(); + assertEquals( + 0.0, motor.getSpeed(motor.stallTorqueNewtonMeters, motor.nominalVoltageVolts), kTolerance); + } + + @ParameterizedTest(name = "{0}") + @MethodSource("motors") + void testSpeedAtZeroTorqueAndNominalVoltageIsFreeSpeed( + String name, Supplier motorFactory) { + var motor = motorFactory.get(); + assertEquals( + motor.freeSpeedRadPerSec, motor.getSpeed(0.0, motor.nominalVoltageVolts), kTolerance); + } +} diff --git a/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp b/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp index 7f4a4e049ad..9f7a5cfcd09 100644 --- a/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp +++ b/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp @@ -33,8 +33,8 @@ TEST(LinearQuadraticRegulatorTest, ElevatorGains) { Matrixd<1, 2> K = LinearQuadraticRegulator<2, 1>{plant, {0.02, 0.4}, {12.0}, 5_ms}.K(); - EXPECT_NEAR(522.87006795347486, K(0, 0), 1e-6); - EXPECT_NEAR(38.239878385020411, K(0, 1), 1e-6); + EXPECT_NEAR(522.58976745337009, K(0, 0), 1e-6); + EXPECT_NEAR(38.170344262642963, K(0, 1), 1e-6); } TEST(LinearQuadraticRegulatorTest, ArmGains) { @@ -59,8 +59,8 @@ TEST(LinearQuadraticRegulatorTest, ArmGains) { LinearQuadraticRegulator<2, 1>{plant, {0.01745, 0.08726}, {12.0}, 5_ms} .K(); - EXPECT_NEAR(19.339349883583761, K(0, 0), 1e-6); - EXPECT_NEAR(3.3542559517421582, K(0, 1), 1e-6); + EXPECT_NEAR(19.24674874537445, K(0, 0), 1e-6); + EXPECT_NEAR(3.3352730789607676, K(0, 1), 1e-6); } TEST(LinearQuadraticRegulatorTest, FourMotorElevator) { diff --git a/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp b/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp index 6aec204fc74..b97f4f4dcdf 100644 --- a/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp +++ b/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp @@ -21,9 +21,9 @@ TEST(LinearSystemIDTest, IdentifyDrivetrainVelocitySystem) { #endif ASSERT_TRUE(model.A().isApprox( - frc::Matrixd<2, 2>{{-10.14132, 3.06598}, {3.06598, -10.14132}}, 0.001)); + frc::Matrixd<2, 2>{{-10.3182, 3.11946}, {3.11946, -10.3182}}, 0.001)); ASSERT_TRUE(model.B().isApprox( - frc::Matrixd<2, 2>{{4.2590, -1.28762}, {-1.2876, 4.2590}}, 0.001)); + frc::Matrixd<2, 2>{{4.33333, -1.31008}, {-1.31008, 4.33333}}, 0.001)); ASSERT_TRUE( model.C().isApprox(frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}}, 0.001)); ASSERT_TRUE( @@ -35,8 +35,8 @@ TEST(LinearSystemIDTest, ElevatorSystem) { 0.05_m, 12) .Slice(0); ASSERT_TRUE(model.A().isApprox( - frc::Matrixd<2, 2>{{0.0, 1.0}, {0.0, -99.05473}}, 0.001)); - ASSERT_TRUE(model.B().isApprox(frc::Matrixd<2, 1>{0.0, 20.8}, 0.001)); + frc::Matrixd<2, 2>{{0.0, 1.0}, {0.0, -100.782}}, 0.001)); + ASSERT_TRUE(model.B().isApprox(frc::Matrixd<2, 1>{0.0, 21.1628}, 0.001)); ASSERT_TRUE(model.C().isApprox(frc::Matrixd<1, 2>{1.0, 0.0}, 0.001)); ASSERT_TRUE(model.D().isApprox(frc::Matrixd<1, 1>{0.0}, 0.001)); } @@ -50,8 +50,8 @@ TEST(LinearSystemIDTest, FlywheelSystem) { frc::DCMotor::NEO(2), 0.00032_kg_sq_m, 1.0); #endif - ASSERT_TRUE(model.A().isApprox(frc::Matrixd<1, 1>{-26.87032}, 0.001)); - ASSERT_TRUE(model.B().isApprox(frc::Matrixd<1, 1>{1354.166667}, 0.001)); + ASSERT_TRUE(model.A().isApprox(frc::Matrixd<1, 1>{-27.339}, 0.001)); + ASSERT_TRUE(model.B().isApprox(frc::Matrixd<1, 1>{1377.79}, 0.001)); ASSERT_TRUE(model.C().isApprox(frc::Matrixd<1, 1>{1.0}, 0.001)); ASSERT_TRUE(model.D().isApprox(frc::Matrixd<1, 1>{0.0}, 0.001)); } @@ -66,8 +66,8 @@ TEST(LinearSystemIDTest, DCMotorSystem) { #endif ASSERT_TRUE( - model.A().isApprox(frc::Matrixd<2, 2>{{0, 1}, {0, -26.87032}}, 0.001)); - ASSERT_TRUE(model.B().isApprox(frc::Matrixd<2, 1>{0, 1354.166667}, 0.001)); + model.A().isApprox(frc::Matrixd<2, 2>{{0, 1}, {0, -27.339}}, 0.001)); + ASSERT_TRUE(model.B().isApprox(frc::Matrixd<2, 1>{0, 1377.79}, 0.001)); ASSERT_TRUE( model.C().isApprox(frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}}, 0.001)); ASSERT_TRUE(model.D().isApprox(frc::Matrixd<2, 1>{0.0, 0.0}, 0.001)); diff --git a/wpimath/src/test/native/cpp/system/plant/DCMotorTest.cpp b/wpimath/src/test/native/cpp/system/plant/DCMotorTest.cpp new file mode 100644 index 00000000000..bf261cea2d8 --- /dev/null +++ b/wpimath/src/test/native/cpp/system/plant/DCMotorTest.cpp @@ -0,0 +1,108 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include +#include +#include + +#include + +#include "frc/system/plant/DCMotor.h" + +using namespace frc; + +constexpr double kTolerance = 1e-12; + +struct MotorTestParam { + std::string name; + std::function factory; +}; + +class DCMotorModelTest : public ::testing::TestWithParam { + protected: + std::optional motor; + + void SetUp() override { motor = GetParam().factory(); } +}; + +TEST_P(DCMotorModelTest, CurrentAtZeroSpeedAndNominalVoltageIsStallCurrent) { + EXPECT_NEAR(motor->Current(0_rad_per_s, motor->nominalVoltage).value(), + motor->stallCurrent.value(), kTolerance); +} + +TEST_P(DCMotorModelTest, CurrentAtFreeSpeedAndNominalVoltageIsFreeCurrent) { + EXPECT_NEAR(motor->Current(motor->freeSpeed, motor->nominalVoltage).value(), + motor->freeCurrent.value(), kTolerance); +} + +TEST_P(DCMotorModelTest, CurrentAtStallTorqueIsStallCurrent) { + EXPECT_NEAR(motor->Current(motor->stallTorque).value(), + motor->stallCurrent.value(), kTolerance); +} + +TEST_P(DCMotorModelTest, CurrentAtZeroTorqueIsFreeCurrent) { + EXPECT_NEAR(motor->Current(0.0_Nm).value(), motor->freeCurrent.value(), + kTolerance); +} + +TEST_P(DCMotorModelTest, TorqueAtFreeCurrentIsZero) { + EXPECT_NEAR(motor->Torque(motor->freeCurrent).value(), 0.0, kTolerance); +} + +TEST_P(DCMotorModelTest, TorqueAtStallCurrentIsStallTorque) { + EXPECT_NEAR(motor->Torque(motor->stallCurrent).value(), + motor->stallTorque.value(), kTolerance); +} + +TEST_P(DCMotorModelTest, VoltageAtStallTorqueAndZeroSpeedIsNominalVoltage) { + EXPECT_NEAR(motor->Voltage(motor->stallTorque, 0_rad_per_s).value(), + motor->nominalVoltage.value(), kTolerance); +} + +TEST_P(DCMotorModelTest, VoltageAtZeroTorqueAndFreeSpeedIsNominalVoltage) { + EXPECT_NEAR(motor->Voltage(0.0_Nm, motor->freeSpeed).value(), + motor->nominalVoltage.value(), kTolerance); +} + +TEST_P(DCMotorModelTest, SpeedAtStallTorqueAndNominalVoltageIsZero) { + EXPECT_NEAR(motor->Speed(motor->stallTorque, motor->nominalVoltage).value(), + 0.0, kTolerance); +} + +TEST_P(DCMotorModelTest, SpeedAtZeroTorqueAndNominalVoltageIsFreeSpeed) { + EXPECT_NEAR(motor->Speed(0.0_Nm, motor->nominalVoltage).value(), + motor->freeSpeed.value(), kTolerance); +} + +INSTANTIATE_TEST_SUITE_P( + DCMotorTests, DCMotorModelTest, + testing::Values( + MotorTestParam{"CIM", []() { return DCMotor::CIM(); }}, + MotorTestParam{"MiniCIM", []() { return DCMotor::MiniCIM(); }}, + MotorTestParam{"Bag", []() { return DCMotor::Bag(); }}, + MotorTestParam{"Vex775Pro", []() { return DCMotor::Vex775Pro(); }}, + MotorTestParam{"RS775_125", []() { return DCMotor::RS775_125(); }}, + MotorTestParam{"BanebotsRS775", + []() { return DCMotor::BanebotsRS775(); }}, + MotorTestParam{"Andymark9015", + []() { return DCMotor::Andymark9015(); }}, + MotorTestParam{"BanebotsRS550", + []() { return DCMotor::BanebotsRS550(); }}, + MotorTestParam{"NEO", []() { return DCMotor::NEO(); }}, + MotorTestParam{"NEO550", []() { return DCMotor::NEO550(); }}, + MotorTestParam{"Falcon500", []() { return DCMotor::Falcon500(); }}, + MotorTestParam{"Falcon500FOC", + []() { return DCMotor::Falcon500FOC(); }}, + MotorTestParam{"RomiBuiltIn", []() { return DCMotor::RomiBuiltIn(); }}, + MotorTestParam{"KrakenX60", []() { return DCMotor::KrakenX60(); }}, + MotorTestParam{"KrakenX60FOC", + []() { return DCMotor::KrakenX60FOC(); }}, + MotorTestParam{"KrakenX44", []() { return DCMotor::KrakenX44(); }}, + MotorTestParam{"KrakenX44FOC", + []() { return DCMotor::KrakenX44FOC(); }}, + MotorTestParam{"Minion", []() { return DCMotor::Minion(); }}, + MotorTestParam{"NeoVortex", []() { return DCMotor::NeoVortex(); }}), + [](const testing::TestParamInfo& info) { + return info.param.name; + });