diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java index 332e690522..8377022af3 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java @@ -122,6 +122,39 @@ public final class LinearSystemId { new Matrix<>(Nat.N2(), Nat.N1())); } + /** + * Create a state-space model of a DC motor system. The states of the system are [angular + * position, angular velocity], inputs are [voltage], and outputs are [angular position, angular + * velocity]. + * + *

The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use the + * {@link edu.wpi.first.math.util.Units} class for converting between unit types. + * + *

The parameters provided by the user are from this feedforward model: + * + *

u = K_v v + K_a a + * + * @param kV The velocity gain, in volts/(unit/sec) + * @param kA The acceleration gain, in volts/(unit/sec^2) + * @return A LinearSystem representing the given characterized constants. + * @throws IllegalArgumentException if kV <= 0 or kA <= 0. + * @see https://github.com/wpilibsuite/sysid + */ + public static LinearSystem createDCMotorSystem(double kV, double kA) { + if (kV <= 0.0) { + throw new IllegalArgumentException("Kv must be greater than zero."); + } + if (kA <= 0.0) { + throw new IllegalArgumentException("Ka must be greater than zero."); + } + + return new LinearSystem<>( + Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -kV / kA), + VecBuilder.fill(0, 1 / kA), + Matrix.eye(Nat.N2()), + new Matrix<>(Nat.N2(), Nat.N1())); + } + /** * Create a state-space model of a differential drive drivetrain. In this model, the states are * [left velocity, right velocity]ᵀ, inputs are [left voltage, right voltage]ᵀ, and outputs are 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 dda0ceed33..64f34963a9 100644 --- a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h +++ b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h @@ -218,6 +218,48 @@ class WPILIB_DLLEXPORT LinearSystemId { static LinearSystem<2, 1, 2> DCMotorSystem(DCMotor motor, units::kilogram_square_meter_t J, double G); + + /** + * Create a state-space model of a DC motor system from its kV + * (volts/(unit/sec)) and kA (volts/(unit/sec²)). These constants can be + * found using SysId. the states of the system are [position, velocity], + * inputs are [voltage], and outputs are [position]. + * + * 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. + * + * The parameters provided by the user are from this feedforward model: + * + * u = K_v v + K_a a + * + * @param kV The velocity gain, in volts/(unit/sec). + * @param kA The acceleration gain, in volts/(unit/sec²). + * + * @throws std::domain_error if kV <= 0 or kA <= 0. + */ + template + requires std::same_as || + std::same_as + static LinearSystem<2, 1, 2> DCMotorSystem( + decltype(1_V / Velocity_t(1)) kV, + decltype(1_V / Acceleration_t(1)) kA) { + if (kV <= decltype(kV){0}) { + throw std::domain_error("Kv must be greater than zero."); + } + if (kA <= decltype(kA){0}) { + throw std::domain_error("Ka must be greater than zero."); + } + + Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -kV.value() / kA.value()}}; + Matrixd<2, 1> B{0.0, 1.0 / kA.value()}; + Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}}; + Matrixd<2, 1> D{{0.0}, {0.0}}; + + return LinearSystem<2, 1, 2>(A, B, C, D); + } + /** * Create a state-space model of differential drive drivetrain. In this model, * the states are [left velocity, right velocity], the inputs are [left