diff --git a/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp index a9061b953c..6aa30ca108 100644 --- a/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp @@ -29,7 +29,8 @@ TEST(StateSpaceSimTest, TestFlywheelSim) { const frc::LinearSystem<1, 1, 1> plant = - frc::LinearSystemId::IdentifyVelocitySystem(0.02, 0.01); + frc::LinearSystemId::IdentifyVelocitySystem( + 0.02_V / 1_rad_per_s, 0.01_V / 1_rad_per_s_sq); frc::sim::FlywheelSim sim{plant, frc::DCMotor::NEO(2), 1.0}; frc2::PIDController controller{0.2, 0.0, 0.0}; frc::SimpleMotorFeedforward feedforward{ diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveSimulation/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveSimulation/include/Drivetrain.h index 6d29a27ea7..48efbc6df9 100644 --- a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveSimulation/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveSimulation/include/Drivetrain.h @@ -91,7 +91,9 @@ class Drivetrain { frc::sim::EncoderSim m_rightEncoderSim{m_rightEncoder}; frc::Field2d m_fieldSim{}; frc::LinearSystem<2, 2, 2> m_drivetrainSystem = - frc::LinearSystemId::IdentifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3); + frc::LinearSystemId::IdentifyDrivetrainSystem( + 1.98_V / 1_mps, 0.2_V / 1_mps_sq, 1.5_V / 1_rad_per_s, + 0.3_V / 1_rad_per_s_sq); frc::sim::DifferentialDrivetrainSim m_drivetrainSimulator{ m_drivetrainSystem, kTrackWidth, frc::DCMotor::CIM(2), 8, 2_in}; }; diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp index 5487a899e1..02883fb323 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp @@ -33,10 +33,10 @@ class Robot : public frc::TimedRobot { static constexpr units::radians_per_second_t kSpinup = 500_rpm; // Volts per (radian per second) - static constexpr double kFlywheelKv = 0.023; + static constexpr auto kFlywheelKv = 0.023_V / 1_rad_per_s; // Volts per (radian per second squared) - static constexpr double kFlywheelKa = 0.001; + static constexpr auto kFlywheelKa = 0.001_V / 1_rad_per_s_sq; // The plant holds a state-space model of our flywheel. This system has the // following properties: @@ -47,7 +47,8 @@ class Robot : public frc::TimedRobot { // // The Kv and Ka constants are found using the FRC Characterization toolsuite. frc::LinearSystem<1, 1, 1> m_flywheelPlant = - frc::LinearSystemId::IdentifyVelocitySystem(kFlywheelKv, kFlywheelKa); + frc::LinearSystemId::IdentifyVelocitySystem(kFlywheelKv, + kFlywheelKa); // The observer fuses our encoder data and voltage inputs to reject noise. frc::KalmanFilter<1, 1, 1> m_observer{ 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 eac6260ddb..25d11611cb 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 @@ -134,6 +134,9 @@ public final class LinearSystemId { * Identify a velocity system from it's kV (volts/(unit/sec)) and kA (volts/(unit/sec^2). * These constants cam be found using frc-characterization. * + *

The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use + * the {@link edu.wpi.first.wpilibj.util.Units} class for converting between unit types. + * * @param kV The velocity gain, in volts per (units per second) * @param kA The acceleration gain, in volts per (units per second squared) * @return A LinearSystem representing the given characterized constants. @@ -153,6 +156,9 @@ public final class LinearSystemId { * Identify a position system from it's kV (volts/(unit/sec)) and kA (volts/(unit/sec^2). * These constants cam be found using frc-characterization. * + *

The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use + * the {@link edu.wpi.first.wpilibj.util.Units} class for converting between unit types. + * * @param kV The velocity gain, in volts per (units per second) * @param kA The acceleration gain, in volts per (units per second squared) * @return A LinearSystem representing the given characterized constants. diff --git a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h index b206b88d94..b712460d67 100644 --- a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h +++ b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h @@ -10,12 +10,25 @@ #include "frc/StateSpaceUtil.h" #include "frc/system/LinearSystem.h" #include "frc/system/plant/DCMotor.h" +#include "units/acceleration.h" +#include "units/angular_acceleration.h" +#include "units/angular_velocity.h" #include "units/moment_of_inertia.h" +#include "units/velocity.h" +#include "units/voltage.h" namespace frc { - class LinearSystemId { public: + template + using Velocity_t = units::unit_t< + units::compound_unit>>; + + template + using Acceleration_t = units::unit_t>, + units::inverse>>; + /** * Constructs the state-space model for an elevator. * @@ -72,6 +85,11 @@ class LinearSystemId { * Constructs the state-space model for a 1 DOF velocity-only system from * system identification data. * + * You MUST use an SI unit (i.e. meters or radians) for the Distance template + * argument. You may still use non-SI units (such as feet or inches) for the + * actual method arguments; they will automatically be converted to SI + * internally. + * * States: [[velocity]] * Inputs: [[voltage]] * Outputs: [[velocity]] @@ -83,9 +101,15 @@ class LinearSystemId { * @param kV The velocity gain, in volt seconds per distance. * @param kA The acceleration gain, in volt seconds^2 per distance. */ - static LinearSystem<1, 1, 1> IdentifyVelocitySystem(double kV, double kA) { - auto A = frc::MakeMatrix<1, 1>(-kV / kA); - auto B = frc::MakeMatrix<1, 1>(1.0 / kA); + template || + std::is_same_v>> + static LinearSystem<1, 1, 1> IdentifyVelocitySystem( + decltype(1_V / Velocity_t(1)) kV, + decltype(1_V / Acceleration_t(1)) kA) { + auto A = frc::MakeMatrix<1, 1>(-kV.template to() / + kA.template to()); + auto B = frc::MakeMatrix<1, 1>(1.0 / kA.template to()); auto C = frc::MakeMatrix<1, 1>(1.0); auto D = frc::MakeMatrix<1, 1>(0.0); @@ -96,6 +120,11 @@ class LinearSystemId { * Constructs the state-space model for a 1 DOF position system from system * identification data. * + * You MUST use an SI unit (i.e. meters or radians) for the Distance template + * argument. You may still use non-SI units (such as feet or inches) for the + * actual method arguments; they will automatically be converted to SI + * internally. + * * States: [[position], [velocity]] * Inputs: [[voltage]] * Outputs: [[position]] @@ -107,9 +136,15 @@ class LinearSystemId { * @param kV The velocity gain, in volt seconds per distance. * @param kA The acceleration gain, in volt seconds^2 per distance. */ - static LinearSystem<2, 1, 1> IdentifyPositionSystem(double kV, double kA) { - auto A = frc::MakeMatrix<2, 2>(0.0, 1.0, 0.0, -kV / kA); - auto B = frc::MakeMatrix<2, 1>(0.0, 1.0 / kA); + template || + std::is_same_v>> + static LinearSystem<2, 1, 1> IdentifyPositionSystem( + decltype(1_V / Velocity_t(1)) kV, + decltype(1_V / Acceleration_t(1)) kA) { + auto A = frc::MakeMatrix<2, 2>( + 0.0, 1.0, 0.0, -kV.template to() / kA.template to()); + auto B = frc::MakeMatrix<2, 1>(0.0, 1.0 / kA.template to()); auto C = frc::MakeMatrix<1, 2>(1.0, 0.0); auto D = frc::MakeMatrix<1, 1>(0.0); @@ -131,15 +166,17 @@ class LinearSystemId { * @param kAangular The angular acceleration gain, in volt seconds^2 per * angle. */ - static LinearSystem<2, 2, 2> IdentifyDrivetrainSystem(double kVlinear, - double kAlinear, - double kVangular, - double kAangular) { - double c = 0.5 / (kAlinear * kAangular); - double A1 = c * (-kAlinear * kVangular - kVlinear * kAangular); - double A2 = c * (kAlinear * kVangular - kVlinear * kAangular); - double B1 = c * (kAlinear + kAangular); - double B2 = c * (kAangular - kAlinear); + static LinearSystem<2, 2, 2> IdentifyDrivetrainSystem( + decltype(1_V / 1_mps) kVlinear, decltype(1_V / 1_mps_sq) kAlinear, + decltype(1_V / 1_rad_per_s) kVangular, + decltype(1_V / 1_rad_per_s_sq) kAangular) { + double c = 0.5 / (kAlinear.to() * kAangular.to()); + double A1 = c * (-kAlinear.to() * kVangular.to() - + kVlinear.to() * kAangular.to()); + double A2 = c * (kAlinear.to() * kVangular.to() - + kVlinear.to() * kAangular.to()); + double B1 = c * (kAlinear.to() + kAangular.to()); + double B2 = c * (kAangular.to() - kAlinear.to()); auto A = frc::MakeMatrix<2, 2>(A1, A2, A2, A1); auto B = frc::MakeMatrix<2, 2>(B1, B2, B2, B1); diff --git a/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp b/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp index 4fa0601d4c..eedf8c076d 100644 --- a/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp +++ b/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp @@ -53,7 +53,8 @@ TEST(LinearSystemIDTest, IdentifyPositionSystem) { // x-dot = [0 1 | 0 -kv/ka] x = [0 | 1/ka] u double kv = 1.0; double ka = 0.5; - auto model = frc::LinearSystemId::IdentifyPositionSystem(kv, ka); + auto model = frc::LinearSystemId::IdentifyPositionSystem( + kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq); ASSERT_TRUE(model.A().isApprox(frc::MakeMatrix<2, 2>(0.0, 1.0, 0.0, -kv / ka), 0.001)); @@ -66,7 +67,8 @@ TEST(LinearSystemIDTest, IdentifyVelocitySystem) { // x-dot = -kv/ka * v + 1/ka \cdot V double kv = 1.0; double ka = 0.5; - auto model = frc::LinearSystemId::IdentifyVelocitySystem(kv, ka); + auto model = frc::LinearSystemId::IdentifyVelocitySystem( + kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq); ASSERT_TRUE(model.A().isApprox(frc::MakeMatrix<1, 1>(-kv / ka), 0.001)); ASSERT_TRUE(model.B().isApprox(frc::MakeMatrix<1, 1>(1.0 / ka), 0.001));