From edea2abac2f5d52936e809ff2fbc05ba5e09ff49 Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Fri, 11 Oct 2024 17:59:37 -0400 Subject: [PATCH] DCMotorSim clasees review draft --- .../first/wpilibj/simulation/DCMotorSim.java | 30 +- .../wpilibj/simulation/DCMotorSimBase.java | 116 ++-- .../wpilibj/simulation/DCMotorSimTorque.java | 32 +- .../first/wpilibj/simulation/FlywheelSim.java | 3 +- .../wpilibj/simulation/FlywheelSimBase.java | 19 +- .../wpilibj/simulation/FlywheelSimTorque.java | 3 +- .../math/system/plant/LinearSystemId.java | 595 +++++++++--------- 7 files changed, 394 insertions(+), 404 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSim.java index 942c38c3ae2..92839d2c93e 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSim.java @@ -12,24 +12,30 @@ import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.system.LinearSystem; import edu.wpi.first.math.system.plant.Gearbox; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.units.measure.MomentOfInertia; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.RobotController; /** Represents a simulated DC motor mechanism controlled by voltage input. */ public class DCMotorSim extends DCMotorSimBase { /** - * Creates a simulated DC motor mechanism. + * Creates a simulated DC motor mechanism controlled by voltage input. * - * @param plant The linear system representing the DC motor. This system can be created with - * {@link edu.wpi.first.math.system.plant.LinearSystemId #createDCMotorSystem(DCMotor, - * double)} or {@link edu.wpi.first.math.system.plant.LinearSystemId - * #createDCMotorSystem(double, double)}. If {@link - * edu.wpi.first.math.system.plant.LinearSystemId #createDCMotorSystem(double, double)} is - * used, the distance unit must be radians. - * @param gearbox The type of and number of motors in the DC motor gearbox. + *

If using physical constants create the plant using either {@link + * LinearSystemId#createDCMotorSystem(Gearbox, double)} or {@link + * LinearSystemId#createDCMotorSystem(Gearbox, MomentOfInertia)}. + * + *

If using system characterization create the plant using either {@link + * LinearSystemId#identifyPositionSystem(double, double)} or the units class overload. The + * distance unit must be radians. The input unit must be volts. + * + * @param plant The linear system that represents the simulated DC motor mechanism. + * @param gearbox The gearbox of the simulated DC motor mechanism. * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no * noise is desired. If present must have 2 elements. The first element is for position. The - * second element is for velocity. + * second element is for velocity. The units should be radians for position and radians per + * second for velocity. */ public DCMotorSim(LinearSystem plant, Gearbox gearbox, double... measurementStdDevs) { // By theorem 6.10.1 of @@ -57,9 +63,9 @@ public DCMotorSim(LinearSystem plant, Gearbox gearbox, double... mea } /** - * Sets the input voltage for the DC motor. + * Sets the input voltage of the simulated DC motor mechanism. * - * @param volts The input voltage. + * @param volts The input voltage in volts. */ public void setInputVoltage(double volts) { setInput(volts); @@ -68,7 +74,7 @@ public void setInputVoltage(double volts) { } /** - * Sets the input voltage for the DC motor. + * Sets the input voltage of the simulated DC motor mechanism. * * @param voltage The input voltage. */ diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSimBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSimBase.java index 21346f29a09..5b93accfa71 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSimBase.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSimBase.java @@ -33,42 +33,43 @@ import edu.wpi.first.units.measure.Torque; import edu.wpi.first.units.measure.Voltage; -/** Represents a simulated DC motor mechanism. */ +/** The base clase for a simulated DC motor mechanism. */ public abstract class DCMotorSimBase extends LinearSystemSim { - /** Gearbox for the DC motor. */ + /** The gearbox of the simulated DC motor mechanism. */ protected final Gearbox m_gearbox; - /** The moment of inertia for the flywheel mechanism. */ + /** The moment of inertia of the simulated DC motor mechanism. */ private final MomentOfInertia m_j; - /** The angle of the system. */ + /** The angle of the simulated DC motor mechanism. */ private final MutAngle m_angle = Radians.mutable(0.0); - /** The angular velocity of the system. */ + /** The angular velocity of the simulated DC motor mechanism. */ protected final MutAngularVelocity m_angularVelocity = RadiansPerSecond.mutable(0.0); - /** The angular acceleration of the system. */ + /** The angular acceleration of the simulated DC motor mechanism. */ private final MutAngularAcceleration m_angularAcceleration = RadiansPerSecondPerSecond.mutable(0.0); - /** The current draw of flywheel. */ + /** The current draw of the simulated DC motor mechanism. */ protected final MutCurrent m_currentDraw = Amps.mutable(0.0); - /** The voltage of the flywheel. */ + /** The voltage of the simulated DC motor mechanism. */ protected final MutVoltage m_voltage = Volts.mutable(0.0); - /** The torque on the flywheel. */ + /** The torque of the simulated DC motor mechanism. */ protected final MutTorque m_torque = NewtonMeters.mutable(0.0); /** * Creates a simulated DC motor mechanism. * - * @param plant The linear system representing the DC motor. - * @param gearbox The type of and number of motors in the DC motor gearbox. - * @param J The moment of inertia for the flywheel mechanism. + * @param plant The linear system that represents the simulated DC motor mechanism. + * @param gearbox The gearbox of the simulated DC motor mechanism. + * @param J The moment of inertia of the simulated DC motor mechanism. * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no * noise is desired. If present must have 2 elements. The first element is for position. The - * second element is for velocity. + * second element is for velocity. The units should be radians for position and radians per + * second for velocity. */ public DCMotorSimBase( LinearSystem plant, @@ -81,7 +82,7 @@ public DCMotorSimBase( } /** - * Sets the state of the DC motor. + * Sets the state of the simulated DC motor mechanism. * * @param angularPositionRad The new angular position in radians. * @param angularVelocityRadPerSec The new angualr velocity in radians per second. @@ -91,7 +92,7 @@ public void setState(double angularPositionRad, double angularVelocityRadPerSec) } /** - * Sets the state of the DC motor. + * Sets the state of the simulated DC motor mechanism. * * @param angularPosition The new angular position. * @param angularVelocity The new angular velocity. @@ -101,16 +102,16 @@ public void setState(Angle angularPosition, AngularVelocity angularVelocity) { } /** - * Sets the DC motor's angular position. + * Sets the angular position of the simulated DC motor mechanism. * - * @param angularPositionRad The new position in radians. + * @param angularPositionRad The new angular position in radians. */ public void setAngle(double angularPositionRad) { setState(angularPositionRad, m_angularVelocity.in(RadiansPerSecond)); } /** - * Sets the DC motor's angular position. + * Sets the angular position of the simulated DC motor mechanism. * * @param angularPosition The new angular position. */ @@ -119,171 +120,172 @@ public void setAngle(Angle angularPosition) { } /** - * Sets the DC motor's angular velocity. + * Sets the angular velocity of the simulated DC motor mechanism. * - * @param angularVelocityRadPerSec The new velocity in radians per second. + * @param angularVelocityRadPerSec The new angular velocity in radians per second. */ public void setAngularVelocity(double angularVelocityRadPerSec) { setState(m_angle.in(Radians), angularVelocityRadPerSec); } /** - * Sets the DC motor's angular velocity. + * Sets the angular velocity of the simulated DC motor mechanism. * - * @param angularVelocity The new velocity. + * @param angularVelocity The new angular velocity. */ public void setAngularVelocity(AngularVelocity angularVelocity) { setAngularVelocity(angularVelocity.in(RadiansPerSecond)); } /** - * Returns the moment of inertia in kilograms meters squared. + * Returns the moment of inertia of the simulated DC motor mechanism. * - * @return The DC motor's moment of inertia in kilograms meters squared. + * @return The moment of inertia of the simulated DC motor mechanism in kilogram-square meters. */ public double getJKgMetersSquared() { return m_j.in(KilogramSquareMeters); } /** - * Returns the moment of inertia. + * Returns the moment of inertia of the simulated DC motor mechanism. * - * @return The DC motor's moment of inertia. + * @return The moment of inertia of the simulated DC motor mechanism. */ public MomentOfInertia getJ() { return m_j; } /** - * Returns the gearbox for the DC motor. + * Returns the gearbox of the simulated DC motor mechanism. * - * @return The DC motor's gearbox. + * @return The gearbox of the simulated DC motor mechanism. */ public Gearbox getGearbox() { return m_gearbox; } /** - * Returns the DC motor's position. + * Returns the angular position of the simulated DC motor mechanism. * - * @return The DC motor's position. + * @return The angular position of the simulated DC motor mechanism in radians. */ public double getAngularPositionRad() { return m_angle.in(Radians); } /** - * Returns the DC motor's position in rotations. + * Returns the angular position of the simulated DC motor mechanism. * - * @return The DC motor's position in rotations. + * @return The angular position of the simulated DC motor mechanism in rotations. */ public double getAngularPositionRotations() { return m_angle.in(Rotations); } /** - * Returns the DC motor's position. + * Returns the angular position of the simulated DC motor mechanism. * - * @return The DC motor's position + * @return The angular position of the simulated DC motor mechanism. */ public Angle getAngularPosition() { return m_angle; } /** - * Returns the DC motor's velocity. + * Returns the angular velocity of the simulated DC motor mechanism. * - * @return The DC motor's velocity. + * @return The angular velocity of the simulated DC motor mechanism in radians per second. */ public double getAngularVelocityRadPerSec() { return m_angularVelocity.in(RadiansPerSecond); } /** - * Returns the DC motor's velocity in RPM. + * Returns the angular velocity of the simulated DC motor mechanism. * - * @return The DC motor's velocity in RPM. + * @return The angular velocity of the simulated DC motor mechanism in RPM. */ public double getAngularVelocityRPM() { return m_angularVelocity.in(RPM); } /** - * Returns the DC motor's velocity. + * Returns the angular velocity of the simulated DC motor mechanism. * - * @return The DC motor's velocity + * @return The angular velocity of the simulated DC motor mechanism. */ public AngularVelocity getAngularVelocity() { return m_angularVelocity; } /** - * Returns the DC motor's acceleration in Radians Per Second Squared. + * Returns the angular acceleration of the simulated DC motor mechanism. * - * @return The DC motor's acceleration in Radians Per Second Squared. + * @return The angular acceleration of the simulated DC motor mechanism in radians per second + * squared. */ public double getAngularAccelerationRadPerSecSq() { return m_angularAcceleration.in(RadiansPerSecondPerSecond); } /** - * Returns the DC motor's acceleration. + * Returns the angular acceleration of the simulated DC motor mechanism. * - * @return The DC motor's acceleration. + * @return The angular acceleration of the simulated DC motor mechanism. */ public AngularAcceleration getAngularAcceleration() { return m_angularAcceleration; } /** - * Returns the DC motor's current draw in Amps. + * Returns the current draw of the simulated DC motor mechanism. * - * @return The DC motor's current draw in Amps. + * @return The current draw of the simulated DC motor mechanism in amps. */ public double getCurrentDrawAmps() { return m_currentDraw.in(Amps); } /** - * Returns the DC motor's current draw. + * Returns the current draw of the simulated DC motor mechanism. * - * @return The DC motor's current draw. + * @return The current draw of the simulated DC motor mechanism. */ public Current getCurrentDraw() { return m_currentDraw; } /** - * Returns the DC motor's torque in Newton-Meters. + * Returns the torque of the simulated DC motor mechanism. * - * @return The DC motor's torque in Newton-Meters. + * @return The torque of the simulated DC motor mechanism in newton-meters. */ public double getTorqueNewtonMeters() { return m_torque.in(NewtonMeters); } /** - * Returns the DC motor's torque. + * Returns the torque of the simulated DC motor mechanism. * - * @return The DC motor's torque. + * @return The torque of the simulated DC motor mechanism. */ public Torque getTorque() { return m_torque; } /** - * Returns the voltage of the DC motor in volts. + * Returns the voltage of the simulated DC motor mechanism. * - * @return The DC motor's voltage in volts. + * @return The voltage of the simulated DC motor mechanism in volts. */ - public double getVoltageVolts() { + public double getVolts() { return m_voltage.in(Volts); } /** - * Returns the voltage of the DC motor. + * Returns the voltage of the simulated DC motor mechanism. * - * @return The DC motor's voltage. + * @return The voltage of the simulated DC motor mechanism. */ public Voltage getVoltage() { return m_voltage; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSimTorque.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSimTorque.java index 2e814334c31..75d5cc4b3bc 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSimTorque.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSimTorque.java @@ -13,22 +13,30 @@ import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.system.LinearSystem; import edu.wpi.first.math.system.plant.Gearbox; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.units.measure.MomentOfInertia; import edu.wpi.first.units.measure.Torque; /** Represents a simulated DC motor mechanism controlled by torque input. */ public class DCMotorSimTorque extends DCMotorSimBase { /** - * Creates a simulated DC motor mechanism. + * Creates a simulated DC motor mechanism controlled by torque input. * - * @param plant The linear system representing the DC motor. This system can be created with - * {@link edu.wpi.first.math.system.plant.LinearSystemId#createDCMotorTorqueSystem(double)} or - * {@link edu.wpi.first.math.system.plant.LinearSystemId#createDCMotorSystem(double, double)}. - * If {@link edu.wpi.first.math.system.plant.LinearSystemId#createDCMotorSystem(double, - * double)} is used, the distance unit must be radians. + *

If using physical constants create the plant using either {@link + * LinearSystemId#createDCMotorSystem(double)} or {@link + * LinearSystemId#createDCMotorSystem(MomentOfInertia)}. + * + *

If using system characterization create the plant using either {@link + * LinearSystemId#identifyPositionSystem(double, double)} or the units class overload. The + * distance unit must be radians. The input unit must be newton-meters. + * + * @param plant The linear system that represents the simulated DC motor mechanism controlled by + * torque input. * @param gearbox The type of and number of motors in the DC motor gearbox. * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no * noise is desired. If present must have 2 elements. The first element is for position. The - * second element is for velocity. + * second element is for velocity. The units should be radians for position and radians per + * second for velocity. */ public DCMotorSimTorque( LinearSystem plant, Gearbox gearbox, double... measurementStdDevs) { @@ -50,18 +58,18 @@ public DCMotorSimTorque( } /** - * Sets the input torque for the DC motor. + * Sets the input torque of the simulated DC motor mechanism. * - * @param torqueNM The input torque. + * @param torqueNewtonMeters The input torque in newton-meters. */ - public void setInputTorque(double torqueNM) { - setInput(torqueNM); + public void setInputTorque(double torqueNewtonMeters) { + setInput(torqueNewtonMeters); // TODO: Need some guidance on clamping. m_torque.mut_replace(m_u.get(0, 0), NewtonMeters); } /** - * Sets the input torque for the DC motor. + * Sets the input torque of the simulated DC motor mechanism. * * @param torque The input torque. */ diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java index b93c279c73e..95124cfe1bb 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java @@ -26,7 +26,8 @@ public class FlywheelSim extends FlywheelSimBase { * LinearSystemId#createFlywheelSystem(Gearbox, MomentOfInertia)}. * *

If using system characterization create the plant using either {@link - * LinearSystemId#identifyVelocitySystem(double, double)} or the units class overload. + * LinearSystemId#identifyVelocitySystem(double, double)} or the units class overload. The + * distance unit must be radians. The input unit must be volts. * * @param plant The linear system that represents the simulated flywheel mechanism controlled by * voltage input. diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSimBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSimBase.java index 530263e2601..69c1c8d5166 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSimBase.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSimBase.java @@ -33,7 +33,7 @@ public abstract class FlywheelSimBase extends LinearSystemSim { /** The gearbox of the simulated flywheel mechanism. */ protected final Gearbox m_gearbox; - /** The moment of inertia of simulated the flywheel mechanism. */ + /** The moment of inertia of the simulated flywheel mechanism. */ private final MomentOfInertia m_j; /** The angular velocity of the simulated flywheel mechanism. */ @@ -75,19 +75,19 @@ public FlywheelSimBase( /** * Sets the angular velocity of the simulated flywheel mechanism. * - * @param velocityRadPerSec The new angular velocity in radians per second. + * @param angularVelocityRadPerSec The new angular velocity in radians per second. */ - public void setAngularVelocity(double velocityRadPerSec) { - setState(VecBuilder.fill(velocityRadPerSec)); + public void setAngularVelocity(double angularVelocityRadPerSec) { + setState(VecBuilder.fill(angularVelocityRadPerSec)); } /** * Sets the angular velocity of the simulated flywheel mechanism. * - * @param velocity The new angular velocity. + * @param angularVelocity The new angular velocity. */ - public void setAngularVelocity(AngularVelocity velocity) { - setAngularVelocity(velocity.in(RadiansPerSecond)); + public void setAngularVelocity(AngularVelocity angularVelocity) { + setAngularVelocity(angularVelocity.in(RadiansPerSecond)); } /** @@ -147,7 +147,8 @@ public AngularVelocity getAngularVelocity() { /** * Returns the angular acceleration of the simulated flywheel mechanism. * - * @return The angular acceleration of the simulated flywheel mechanism in radians per squared. + * @return The angular acceleration of the simulated flywheel mechanism in radians per second + * squared. */ public double getAngularAccelerationRadPerSecSq() { return m_angularAcceleration.in(RadiansPerSecondPerSecond); @@ -203,7 +204,7 @@ public Torque getTorque() { * * @return The voltage of the simulated flywheel mechanism in volts. */ - public double getVoltageVolts() { + public double getVolts() { return m_voltage.in(Volts); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSimTorque.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSimTorque.java index 1bc62d99ff4..370619b1a59 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSimTorque.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSimTorque.java @@ -26,7 +26,8 @@ public class FlywheelSimTorque extends FlywheelSimBase { * LinearSystemId#createFlywheelSystem(MomentOfInertia)}. * *

If using system characterization create the plant using either {@link - * LinearSystemId#identifyVelocitySystem(double, double)} or the units class overload. + * LinearSystemId#identifyVelocitySystem(double, double)} or the units class overload. The + * distance unit must be radians. The input unit must be newton-meters. * * @param plant The linear system that represents the simulated flywheel mechanism controlled by * torque input. 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 4e1908bce53..68f0873c0a3 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 @@ -29,7 +29,144 @@ private LinearSystemId() { } /** - * Create a state-space model of an elevator system. The states of the system are [position, + * Creates 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]. + * + * @param gearbox The gearbox of the DC motor mechanism. + * @param JKgMetersSquared The moment of inertia of the DC motor mechanism in kilograms-square + * meters. + * @return A LinearSystem representing the given characterized constants. + * @throws IllegalArgumentException if JKgMetersSquared ≤ 0. + */ + public static LinearSystem createDCMotorSystem( + Gearbox gearbox, double JKgMetersSquared) { + if (JKgMetersSquared <= 0.0) { + throw new IllegalArgumentException("JKgMetersSquared must be greater than zero."); + } + + return new LinearSystem<>( + MatBuilder.fill( + Nat.N2(), + Nat.N2(), + 0, + 1, + 0, + -Math.pow(gearbox.reduction, 2) + * gearbox.numMotors + * gearbox.motorType.KtNMPerAmp + / (gearbox.motorType.KvRadPerSecPerVolt + * gearbox.motorType.rOhms + * JKgMetersSquared)), + VecBuilder.fill( + 0, + gearbox.numMotors + * gearbox.reduction + * gearbox.motorType.KtNMPerAmp + / (gearbox.motorType.rOhms * JKgMetersSquared)), + Matrix.eye(Nat.N2()), + new Matrix<>(Nat.N2(), Nat.N1())); + } + + /** + * Creates 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]. + * + * @param gearbox The gearbox of the DC motor mechanism. + * @param J The moment of inertia of the DC motor mechanism. + * @return A LinearSystem representing the given characterized constants. + * @throws IllegalArgumentException if J ≤ 0. + */ + public static LinearSystem createDCMotorSystem(Gearbox gearbox, MomentOfInertia J) { + return createDCMotorSystem(gearbox, J.in(KilogramSquareMeters)); + } + + /** + * Creates a state-space model of a DC motor system. The states of the system are [angular + * position, angular velocity], inputs are [torque], and outputs are [angular position, angular + * velocity]. + * + * @param JKgMetersSquared The moment of inertia of the DC motor mechanism in kilograms-square + * meters. + * @return A LinearSystem representing the given characterized constants. + * @throws IllegalArgumentException if JKgMetersSquared ≤ 0. + */ + public static LinearSystem createDCMotorSystem(double JKgMetersSquared) { + if (JKgMetersSquared <= 0.0) { + throw new IllegalArgumentException("J must be greater than zero."); + } + + return new LinearSystem<>( + Matrix.eye(Nat.N2()), + VecBuilder.fill(0, 1.0 / JKgMetersSquared), + Matrix.eye(Nat.N2()), + new Matrix<>(Nat.N2(), Nat.N1())); + } + + /** + * Creates a state-space model of a DC motor system. The states of the system are [angular + * position, angular velocity], inputs are [torque], and outputs are [angular position, angular + * velocity]. + * + * @param J The moment of inertia of the DC motor mechanism. + * @return A LinearSystem representing the given characterized constants. + * @throws IllegalArgumentException if J ≤ 0. + */ + public static LinearSystem createDCMotorSystem(MomentOfInertia J) { + return createDCMotorSystem(J.in(KilogramSquareMeters)); + } + + /** + * Creates 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 + * [left velocity, right velocity]ᵀ. + * + * @param driveWheel A {@link Wheel} representing one of the drivetrain's wheels. + * @param massKg The mass of the robot in kilograms. + * @param rbMeters The radius of the base (half the track width) in meters. + * @param JKgMetersSquared The moment of inertia of the robot. + * @return A LinearSystem representing a differential drivetrain. + * @throws IllegalArgumentException if m <= 0, r <= 0, rb <= 0, J <= 0, or gearing + * <= 0. + */ + public static LinearSystem createDrivetrainVelocitySystem( + Wheel driveWheel, double massKg, double rbMeters, double JKgMetersSquared) { + if (massKg <= 0.0) { + throw new IllegalArgumentException("massKg must be greater than zero."); + } + if (rbMeters <= 0.0) { + throw new IllegalArgumentException("rbMeters must be greater than zero."); + } + if (JKgMetersSquared <= 0.0) { + throw new IllegalArgumentException("JKgMetersSquared must be greater than zero."); + } + + var C1 = + -driveWheel.gearbox.numMotors + * Math.pow(driveWheel.gearbox.reduction, 2) + * driveWheel.gearbox.motorType.KtNMPerAmp + / (driveWheel.gearbox.motorType.KvRadPerSecPerVolt + * driveWheel.gearbox.motorType.rOhms + * Math.pow(driveWheel.radiusMeters, 2)); + var C2 = + driveWheel.gearbox.numMotors + * driveWheel.gearbox.reduction + * driveWheel.gearbox.motorType.KtNMPerAmp + / (driveWheel.gearbox.motorType.rOhms * driveWheel.radiusMeters); + + final double C3 = 1 / massKg + rbMeters * rbMeters / JKgMetersSquared; + final double C4 = 1 / massKg - rbMeters * rbMeters / JKgMetersSquared; + var A = MatBuilder.fill(Nat.N2(), Nat.N2(), C3 * C1, C4 * C1, C4 * C1, C3 * C1); + var B = MatBuilder.fill(Nat.N2(), Nat.N2(), C3 * C2, C4 * C2, C4 * C2, C3 * C2); + var C = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0); + var D = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 0.0, 0.0, 0.0); + + return new LinearSystem<>(A, B, C, D); + } + + /** + * Creates a state-space model of an elevator system. The states of the system are [position, * velocity]ᵀ, inputs are [voltage], and outputs are [position]. * * @param drum The elevator's drum. @@ -67,7 +204,7 @@ public static LinearSystem createElevatorSystem(Wheel drum, double m } /** - * Create a state-space model of an elevator system. The states of the system are [position, + * Creates a state-space model of an elevator system. The states of the system are [position, * velocity]ᵀ, inputs are [voltage], and outputs are [position]. * * @param drum The elevator's drum. @@ -80,7 +217,7 @@ public static LinearSystem createElevatorSystem(Wheel drum, Mass mas } /** - * Create a state-space model of an elevator system. The states of the system are [position, + * Creates a state-space model of an elevator system. The states of the system are [position, * velocity]ᵀ, inputs are [torque], and outputs are [position]. * * @param massKg The mass of the elevator carriage, in kilograms. @@ -105,7 +242,7 @@ public static LinearSystem createElevatorTorqueSystem( } /** - * Create a state-space model of an elevator system. The states of the system are [position, + * Creates a state-space model of an elevator system. The states of the system are [position, * velocity]ᵀ, inputs are [torque], and outputs are [position]. * * @param mass The mass of the elevator carriage. @@ -119,7 +256,7 @@ public static LinearSystem createElevatorTorqueSystem( } /** - * Create a state-space model of a flywheel system. The states of the system are [angular + * Creates a state-space model of a flywheel system. The states of the system are [angular * velocity], inputs are [voltage], and outputs are [angular velocity]. * * @param gearbox The gearbox of the flywheel mechanism. @@ -151,7 +288,7 @@ public static LinearSystem createFlywheelSystem( } /** - * Create a state-space model of a flywheel system. The states of the system are [angular + * Creates a state-space model of a flywheel system. The states of the system are [angular * velocity], inputs are [voltage], and outputs are [angular velocity]. * * @param gearbox The gearbox attached to the flywheel. @@ -164,7 +301,7 @@ public static LinearSystem createFlywheelSystem(Gearbox gearbox, Mom } /** - * Create a state-space model of a flywheel system. The states of the system are [angular + * Creates a state-space model of a flywheel system. The states of the system are [angular * velocity], inputs are [torque], and outputs are [angular velocity]. * * @param JKgMetersSquared The moment of inertia of the flywheel in kilograms-square meters. @@ -184,7 +321,7 @@ public static LinearSystem createFlywheelSystem(double JKgMetersSqua } /** - * Create a state-space model of a flywheel system. The states of the system are [angular + * Creates a state-space model of a flywheel system. The states of the system are [angular * velocity], inputs are [torque], and outputs are [angular velocity]. * * @param J The moment of inertia of the flywheel. @@ -196,178 +333,8 @@ public static LinearSystem createFlywheelSystem(MomentOfInertia J) { } /** - * 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]. - * - * @param gearbox The gearbox attached to system. - * @param JKgMetersSquared The moment of inertia J of the DC motor. - * @return A LinearSystem representing the given characterized constants. - * @throws IllegalArgumentException if JKgMetersSquared ≤ 0. - */ - public static LinearSystem createDCMotorSystem( - Gearbox gearbox, double JKgMetersSquared) { - if (JKgMetersSquared <= 0.0) { - throw new IllegalArgumentException("J must be greater than zero."); - } - - return new LinearSystem<>( - MatBuilder.fill( - Nat.N2(), - Nat.N2(), - 0, - 1, - 0, - -Math.pow(gearbox.reduction, 2) - * gearbox.numMotors - * gearbox.motorType.KtNMPerAmp - / (gearbox.motorType.KvRadPerSecPerVolt - * gearbox.motorType.rOhms - * JKgMetersSquared)), - VecBuilder.fill( - 0, - gearbox.numMotors - * gearbox.reduction - * gearbox.motorType.KtNMPerAmp - / (gearbox.motorType.rOhms * JKgMetersSquared)), - Matrix.eye(Nat.N2()), - 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]. - * - * @param gearbox The gearbox attached to system. - * @param J The moment of inertia J of the DC motor. - * @return A LinearSystem representing the given characterized constants. - * @throws IllegalArgumentException if J ≤ 0. - */ - public static LinearSystem createDCMotorSystem(Gearbox gearbox, MomentOfInertia J) { - return createDCMotorSystem(gearbox, J.in(KilogramSquareMeters)); - } - - /** - * 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]. - * - *

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²) - * @return A LinearSystem representing the given characterized constants. - * @throws IllegalArgumentException if kV < 0 or kA <= 0. - * @deprecated Use identifyPositionSystem instead. - * @see https://github.com/wpilibsuite/sysid - */ - @Deprecated - public static LinearSystem createDCMotorSystem(double kV, double kA) { - if (kV < 0.0) { - throw new IllegalArgumentException("Kv must be greater than or equal to zero."); - } - if (kA <= 0.0) { - throw new IllegalArgumentException("Ka must be greater than zero."); - } - - return new LinearSystem<>( - MatBuilder.fill(Nat.N2(), Nat.N2(), 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 DC motor system. The states of the system are [angular - * position, angular velocity], inputs are [torque], and outputs are [angular position, angular - * velocity]. - * - * @param JKgMetersSquared The moment of inertia J of the DC motor. - * @return A LinearSystem representing the given characterized constants. - * @throws IllegalArgumentException if JKgMetersSquared <= 0. - */ - public static LinearSystem createDCMotorTorqueSystem(double JKgMetersSquared) { - if (JKgMetersSquared <= 0.0) { - throw new IllegalArgumentException("J must be greater than zero."); - } - - return new LinearSystem<>( - Matrix.eye(Nat.N2()), - VecBuilder.fill(0, 1.0 / JKgMetersSquared), - Matrix.eye(Nat.N2()), - 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 [torque], and outputs are [angular position, angular - * velocity]. - * - * @param J The moment of inertia J of the DC motor. - * @return A LinearSystem representing the given characterized constants. - * @throws IllegalArgumentException if J <= 0. - */ - public static LinearSystem createDCMotorTorqueSystem(MomentOfInertia J) { - return createDCMotorTorqueSystem(J.in(KilogramSquareMeters)); - } - - /** - * 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 - * [left velocity, right velocity]ᵀ. - * - * @param driveWheel A {@link Wheel} representing one of the drivetrain's wheels. - * @param massKg The mass of the robot in kilograms. - * @param rbMeters The radius of the base (half the track width) in meters. - * @param JKgMetersSquared The moment of inertia of the robot. - * @return A LinearSystem representing a differential drivetrain. - * @throws IllegalArgumentException if m <= 0, r <= 0, rb <= 0, J <= 0, or gearing - * <= 0. - */ - public static LinearSystem createDrivetrainVelocitySystem( - Wheel driveWheel, double massKg, double rbMeters, double JKgMetersSquared) { - if (massKg <= 0.0) { - throw new IllegalArgumentException("massKg must be greater than zero."); - } - if (rbMeters <= 0.0) { - throw new IllegalArgumentException("rbMeters must be greater than zero."); - } - if (JKgMetersSquared <= 0.0) { - throw new IllegalArgumentException("JKgMetersSquared must be greater than zero."); - } - - var C1 = - -driveWheel.gearbox.numMotors - * Math.pow(driveWheel.gearbox.reduction, 2) - * driveWheel.gearbox.motorType.KtNMPerAmp - / (driveWheel.gearbox.motorType.KvRadPerSecPerVolt - * driveWheel.gearbox.motorType.rOhms - * Math.pow(driveWheel.radiusMeters, 2)); - var C2 = - driveWheel.gearbox.numMotors - * driveWheel.gearbox.reduction - * driveWheel.gearbox.motorType.KtNMPerAmp - / (driveWheel.gearbox.motorType.rOhms * driveWheel.radiusMeters); - - final double C3 = 1 / massKg + rbMeters * rbMeters / JKgMetersSquared; - final double C4 = 1 / massKg - rbMeters * rbMeters / JKgMetersSquared; - var A = MatBuilder.fill(Nat.N2(), Nat.N2(), C3 * C1, C4 * C1, C4 * C1, C3 * C1); - var B = MatBuilder.fill(Nat.N2(), Nat.N2(), C3 * C2, C4 * C2, C4 * C2, C3 * C2); - var C = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0); - var D = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 0.0, 0.0, 0.0); - - return new LinearSystem<>(A, B, C, D); - } - - /** - * Create a state-space model of a single jointed arm system. The states of the system are [angle, - * angular velocity], inputs are [voltage], and outputs are [angle]. + * Creates a state-space model of a single jointed arm system. The states of the system are + * [angle, angular velocity], inputs are [voltage], and outputs are [angle]. * * @param gearbox The gearbox attached to the arm. * @param massKg The mass of the arm. @@ -421,8 +388,8 @@ public static LinearSystem createSingleJointedArmSystem( } /** - * Create a state-space model of a single jointed arm system. The states of the system are [angle, - * angular velocity], inputs are [voltage], and outputs are [angle]. + * Creates a state-space model of a single jointed arm system. The states of the system are + * [angle, angular velocity], inputs are [voltage], and outputs are [angle]. * * @param gearbox The gearbox attached to the arm. * @param mass The mass of the arm. @@ -440,8 +407,8 @@ public static LinearSystem createSingleJointedArmSystem( } /** - * Create a state-space model of a single jointed arm system. The states of the system are [angle, - * angular velocity], inputs are [torque], and outputs are [angle]. + * Creates a state-space model of a single jointed arm system. The states of the system are + * [angle, angular velocity], inputs are [torque], and outputs are [angle]. * * @param massKg The mass of the arm. * @param armLengthMeters The length of the arm. @@ -478,8 +445,8 @@ public static LinearSystem createSingleJointedArmTorqueSystem( } /** - * Create a state-space model of a single jointed arm system. The states of the system are [angle, - * angular velocity], inputs are [torque], and outputs are [angle]. + * Creates a state-space model of a single jointed arm system. The states of the system are + * [angle, angular velocity], inputs are [torque], and outputs are [angle]. * * @param mass The mass of the arm. * @param armLength The length of the arm. @@ -495,7 +462,7 @@ public static LinearSystem createSingleJointedArmTorqueSystem( } /** - * Create a state-space model of a wheel system. The states of the system are [linear position, + * Creates a state-space model of a wheel system. The states of the system are [linear position, * linear velocity], inputs are [voltage], and outputs are [linear position, linear velocity]. * * @param wheel The wheel object containing the gearbox and wheel radius. @@ -540,7 +507,7 @@ public static LinearSystem createWheelSystem( } /** - * Create a state-space model of a wheel system. The states of the system are [linear position, + * Creates a state-space model of a wheel system. The states of the system are [linear position, * linear velocity], inputs are [voltage], and outputs are [linear position, linear velocity]. * * @param wheel The wheel object containing the gearbox and wheel radius. @@ -554,7 +521,7 @@ public static LinearSystem createWheelSystem(Wheel wheel, Mass mass, } /** - * Create a state-space model of a wheel system. The states of the system are [linear position, + * Creates a state-space model of a wheel system. The states of the system are [linear position, * linear velocity], inputs are [torque], and outputs are [linear position, linear velocity]. * * @param massKg The mass of the system driven by the wheel, in kilograms. @@ -583,7 +550,7 @@ public static LinearSystem createWheelTorqueSystem( } /** - * Create a state-space model of a wheel system. The states of the system are [linear position, + * Creates a state-space model of a wheel system. The states of the system are [linear position, * linear velocity], inputs are [torque], and outputs are [linear position, linear velocity]. * * @param mass The mass of the system driven by the wheel. @@ -597,128 +564,6 @@ public static LinearSystem createWheelTorqueSystem( return createWheelTorqueSystem(mass.in(Kilograms), radius.in(Meters), numWheels); } - /** - * Create a state-space model for a 1 DOF velocity system from its kV (input-units/(unit/sec)) and - * kA (input-units/(unit/sec²). These constants can be found using SysId. The states of the system - * are [velocity], inputs are [voltage or torque], and outputs are [velocity]. - * - *

The distance-units you choose MUST be an SI unit (i.e. meters or radians). - * - *

The input-units you choose MUST be an SI unit (i.e. volts or newton-meters). - * - *

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 input-units/(distance-units/sec) - * @param kA The acceleration gain, in input-units/(distance-units/sec²) - * @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 identifyVelocitySystem(double kV, double kA) { - if (kV < 0.0) { - throw new IllegalArgumentException("Kv must be greater than or equal to zero."); - } - if (kA <= 0.0) { - throw new IllegalArgumentException("Ka must be greater than zero."); - } - - return new LinearSystem<>( - VecBuilder.fill(-kV / kA), - VecBuilder.fill(1.0 / kA), - VecBuilder.fill(1.0), - VecBuilder.fill(0.0)); - } - - /** - * Create a state-space model for a 1 DOF velocity system from its kV - * (input-units/(distance-units/sec)) and kA (input-units/(distance-units/sec²). These constants - * can be found using SysId. The states of the system are [velocity], inputs are [voltage or - * torque], and outputs are [velocity]. - * - *

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

u = K_v v + K_a a - * - * @param The input parameter either as voltage or torque. - * @param The velocity gain parameter either as LinearVelocity or AngularVelocity. - * @param The acceleration gain parameter either as LinearAcceleration or AngularAcceleration. - * @param kV The velocity gain, in input units/(unit/sec) - * @param kA The acceleration gain, in input units/(unit/sec²) - * @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 identifyVelocitySystem( - Measure> kV, Measure> kA) { - return identifyVelocitySystem(kV.baseUnitMagnitude(), kA.baseUnitMagnitude()); - } - - /** - * Create a state-space model for a 1 DOF position system from its kV (input units/(unit/sec)) and - * kA (input units/(unit/sec²). These constants can be found using SysId. The states of the system - * are [position, velocity]ᵀ, inputs are [voltage or torque], and outputs are [position]. - * - *

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 The input parameter either as voltage or torque. - * @param The velocity gain parameter either as LinearVelocity or AngularVelocity. - * @param The acceleration gain parameter either as LinearAcceleration or AngularAcceleration. - * @param kV The velocity gain, in volts/(unit/sec) - * @param kA The acceleration gain, in volts/(unit/sec²) - * @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 identifyPositionSystem( - Measure> kV, Measure> kA) { - return identifyPositionSystem(kV.baseUnitMagnitude(), kA.baseUnitMagnitude()); - } - - /** - * Create a state-space model for a 1 DOF position system from its kV (input units/(unit/sec)) and - * kA (input units/(unit/sec²). These constants can be found using SysId. The states of the system - * are [position, velocity]ᵀ, inputs are [voltage or torque], and outputs are [position]. - * - *

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²) - * @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 identifyPositionSystem(double kV, double kA) { - if (kV < 0.0) { - throw new IllegalArgumentException("Kv must be greater than or equal to zero."); - } - if (kA <= 0.0) { - throw new IllegalArgumentException("Ka must be greater than zero."); - } - - return new LinearSystem<>( - MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 0.0, -kV / kA), - VecBuilder.fill(0.0, 1.0 / kA), - Matrix.eye(Nat.N2()), - new Matrix<>(Nat.N2(), Nat.N1())); - } - /** * Identify a differential drive drivetrain given the drivetrain's kV and kA in both linear * {(volts/(meter/sec), (volts/(meter/sec²))} and angular {(volts/(radian/sec)), @@ -816,4 +661,130 @@ public static LinearSystem identifyDrivetrainSystem( return identifyDrivetrainSystem( kVLinear, kALinear, kVAngular * 2.0 / trackwidth, kAAngular * 2.0 / trackwidth); } + + /** + * Creates a state-space model for a 1 DOF position system from its kV + * (input-units/(distance-units/sec)) and kA (input-units/(distance-units/sec²). These constants + * can be found using SysId. The states of the system are [position, velocity], inputs are + * [voltage or torque], and outputs are [position, velocity]. + * + *

The distance-units you choose MUST be an SI unit (i.e. meters or radians). + * + *

The input-units you choose MUST be an SI unit (i.e. volts or newton-meters). + * + *

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 + kₐ a + * + * @param kV The velocity gain, in input-units/(distance-units/sec) + * @param kA The acceleration gain, in input-units/(distance-units/sec²) + * @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 identifyPositionSystem(double kV, double kA) { + if (kV < 0.0) { + throw new IllegalArgumentException("kV must be greater than or equal to zero."); + } + if (kA <= 0.0) { + throw new IllegalArgumentException("kA must be greater than zero."); + } + + return new LinearSystem<>( + MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 0.0, -kV / kA), + VecBuilder.fill(0.0, 1.0 / kA), + Matrix.eye(Nat.N2()), + new Matrix<>(Nat.N2(), Nat.N1())); + } + + /** + * Creates a state-space model for a 1 DOF velocity system from its kV + * (input-units/(distance-units/sec)) and kA (input-units/(distance-units/sec²). These constants + * can be found using SysId. The states of the system are [velocity], inputs are [voltage or + * torque], and outputs are [velocity]. + * + *

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

u = kᵥ v + kₐ a + * + * @param The input parameter either as voltage or torque. + * @param The velocity gain parameter either as LinearVelocity or AngularVelocity. + * @param The acceleration gain parameter either as LinearAcceleration or AngularAcceleration. + * @param kV The velocity gain, in input-units/(distance-units/sec) + * @param kA The acceleration gain, in input-units/(distance-units/sec²) + * @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 identifyPositionSystem( + Measure> kV, Measure> kA) { + return identifyPositionSystem(kV.baseUnitMagnitude(), kA.baseUnitMagnitude()); + } + + /** + * Creates a state-space model for a 1 DOF velocity system from its kV + * (input-units/(distance-units/sec)) and kA (input-units/(distance-units/sec²). These constants + * can be found using SysId. The states of the system are [velocity], inputs are [voltage or + * torque], and outputs are [velocity]. + * + *

The distance-units you choose MUST be an SI unit (i.e. meters or radians). + * + *

The input-units you choose MUST be an SI unit (i.e. volts or newton-meters). + * + *

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 + kₐ a + * + * @param kV The velocity gain, in input-units/(distance-units/sec) + * @param kA The acceleration gain, in input-units/(distance-units/sec²) + * @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 identifyVelocitySystem(double kV, double kA) { + if (kV < 0.0) { + throw new IllegalArgumentException("kV must be greater than or equal to zero."); + } + if (kA <= 0.0) { + throw new IllegalArgumentException("kA must be greater than zero."); + } + + return new LinearSystem<>( + VecBuilder.fill(-kV / kA), + VecBuilder.fill(1.0 / kA), + VecBuilder.fill(1.0), + VecBuilder.fill(0.0)); + } + + /** + * Creates a state-space model for a 1 DOF velocity system from its kV + * (input-units/(distance-units/sec)) and kA (input-units/(distance-units/sec²). These constants + * can be found using SysId. The states of the system are [velocity], inputs are [voltage or + * torque], and outputs are [velocity]. + * + *

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

u = kᵥ v + kₐ a + * + * @param The input parameter either as voltage or torque. + * @param The velocity gain parameter either as LinearVelocity or AngularVelocity. + * @param The acceleration gain parameter either as LinearAcceleration or AngularAcceleration. + * @param kV The velocity gain, in input-units/(distance-units/sec) + * @param kA The acceleration gain, in input-units/(distance-units/sec²) + * @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 identifyVelocitySystem( + Measure> kV, Measure> kA) { + return identifyVelocitySystem(kV.baseUnitMagnitude(), kA.baseUnitMagnitude()); + } }