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 f1db6d6b32..0ac9cbf05e 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 @@ -20,9 +20,9 @@ public final class LinearSystemId { * Create a state-space model of an elevator system. The states of the system are [position, * velocity]ᵀ, inputs are [voltage], and outputs are [position]. * - * @param motor The motor (or gearbox) attached to the arm. + * @param motor The motor (or gearbox) attached to the carriage. * @param massKg The mass of the elevator carriage, in kilograms. - * @param radiusMeters The radius of thd driving drum of the elevator, in meters. + * @param radiusMeters The radius of the elevator's driving drum, in meters. * @param G The reduction between motor and drum, as a ratio of output to input. * @return A LinearSystem representing the given characterized constants. * @throws IllegalArgumentException if massKg <= 0, radiusMeters <= 0, or G <= 0. @@ -63,15 +63,15 @@ public final class LinearSystemId { * velocity], inputs are [voltage], and outputs are [angular velocity]. * * @param motor The motor (or gearbox) attached to the flywheel. - * @param jKgMetersSquared The moment of inertia J of the flywheel. + * @param JKgMetersSquared The moment of inertia J of the flywheel. * @param G The reduction between motor and drum, as a ratio of output to input. * @return A LinearSystem representing the given characterized constants. - * @throws IllegalArgumentException if jKgMetersSquared <= 0 or G <= 0. + * @throws IllegalArgumentException if JKgMetersSquared <= 0 or G <= 0. */ @SuppressWarnings("ParameterName") public static LinearSystem createFlywheelSystem( - DCMotor motor, double jKgMetersSquared, double G) { - if (jKgMetersSquared <= 0.0) { + DCMotor motor, double JKgMetersSquared, double G) { + if (JKgMetersSquared <= 0.0) { throw new IllegalArgumentException("J must be greater than zero."); } if (G <= 0.0) { @@ -83,8 +83,8 @@ public final class LinearSystemId { -G * G * motor.KtNMPerAmp - / (motor.KvRadPerSecPerVolt * motor.rOhms * jKgMetersSquared)), - VecBuilder.fill(G * motor.KtNMPerAmp / (motor.rOhms * jKgMetersSquared)), + / (motor.KvRadPerSecPerVolt * motor.rOhms * JKgMetersSquared)), + VecBuilder.fill(G * motor.KtNMPerAmp / (motor.rOhms * JKgMetersSquared)), Matrix.eye(Nat.N1()), new Matrix<>(Nat.N1(), Nat.N1())); } @@ -94,16 +94,16 @@ public final class LinearSystemId { * position, angular velocity], inputs are [voltage], and outputs are [angular position, angular * velocity]. * - * @param motor The motor (or gearbox) attached to the DC motor. - * @param jKgMetersSquared The moment of inertia J of the DC motor. + * @param motor The motor (or gearbox) attached to system. + * @param JKgMetersSquared The moment of inertia J of the DC motor. * @param G The reduction between motor and drum, as a ratio of output to input. * @return A LinearSystem representing the given characterized constants. - * @throws IllegalArgumentException if jKgMetersSquared <= 0 or G <= 0. + * @throws IllegalArgumentException if JKgMetersSquared <= 0 or G <= 0. */ @SuppressWarnings("ParameterName") public static LinearSystem createDCMotorSystem( - DCMotor motor, double jKgMetersSquared, double G) { - if (jKgMetersSquared <= 0.0) { + DCMotor motor, double JKgMetersSquared, double G) { + if (JKgMetersSquared <= 0.0) { throw new IllegalArgumentException("J must be greater than zero."); } if (G <= 0.0) { @@ -119,22 +119,23 @@ public final class LinearSystemId { -G * G * motor.KtNMPerAmp - / (motor.KvRadPerSecPerVolt * motor.rOhms * jKgMetersSquared)), - VecBuilder.fill(0, G * motor.KtNMPerAmp / (motor.rOhms * jKgMetersSquared)), + / (motor.KvRadPerSecPerVolt * motor.rOhms * JKgMetersSquared)), + VecBuilder.fill(0, G * motor.KtNMPerAmp / (motor.rOhms * JKgMetersSquared)), 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 - * [v_left, v_right]ᵀ, inputs are [V_left, V_right]ᵀ and outputs are [v_left, v_right]ᵀ. + * [left velocity, right velocity]ᵀ, inputs are [left voltage, right voltage]ᵀ, and outputs are + * [left velocity, right velocity]ᵀ. * - * @param motor the gearbox representing the motors driving the drivetrain. - * @param massKg the mass of the robot. - * @param rMeters the radius of the wheels in meters. - * @param rbMeters the radius of the base (half the track width) in meters. - * @param JKgMetersSquared the moment of inertia of the robot. - * @param G the gearing reduction as output over input. + * @param motor The motor (or gearbox) driving the drivetrain. + * @param massKg The mass of the robot in kilograms. + * @param rMeters The radius of the wheels in meters. + * @param rbMeters The radius of the base (half the track width) in meters. + * @param JKgMetersSquared The moment of inertia of the robot. + * @param G The gearing reduction as output over input. * @return A LinearSystem representing a differential drivetrain. * @throws IllegalArgumentException if m <= 0, r <= 0, rb <= 0, J <= 0, or G <= 0. */ @@ -181,16 +182,16 @@ public final class LinearSystemId { * angular velocity], inputs are [voltage], and outputs are [angle]. * * @param motor The motor (or gearbox) attached to the arm. - * @param jKgSquaredMeters The moment of inertia J of the arm. + * @param JKgSquaredMeters The moment of inertia J of the arm. * @param G The gearing between the motor and arm, in output over input. Most of the time this * will be greater than 1. * @return A LinearSystem representing the given characterized constants. */ @SuppressWarnings("ParameterName") public static LinearSystem createSingleJointedArmSystem( - DCMotor motor, double jKgSquaredMeters, double G) { - if (jKgSquaredMeters <= 0.0) { - throw new IllegalArgumentException("jKgSquaredMeters must be greater than zero."); + DCMotor motor, double JKgSquaredMeters, double G) { + if (JKgSquaredMeters <= 0.0) { + throw new IllegalArgumentException("JKgSquaredMeters must be greater than zero."); } if (G <= 0.0) { throw new IllegalArgumentException("G must be greater than zero."); @@ -204,22 +205,26 @@ public final class LinearSystemId { 0, -Math.pow(G, 2) * motor.KtNMPerAmp - / (motor.KvRadPerSecPerVolt * motor.rOhms * jKgSquaredMeters)), - VecBuilder.fill(0, G * motor.KtNMPerAmp / (motor.rOhms * jKgSquaredMeters)), + / (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())); } /** - * Identify a velocity system from it's kV (volts/(unit/sec)) and kA (volts/(unit/sec²). These - * constants cam be found using SysId. The states of the system are [velocity], inputs are - * [voltage], and outputs are [velocity]. + * Create a state-space model for a 1 DOF velocity system from its kV (volts/(unit/sec)) and kA + * (volts/(unit/sec²). These constants cam be found using SysId. The states of the system are + * [velocity], inputs are [voltage], and outputs are [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. * - * @param kV The velocity gain, in volts per (units per second) - * @param kA The acceleration gain, in volts per (units per second squared) + *

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 @@ -241,15 +246,19 @@ public final class LinearSystemId { } /** - * Identify a position system from it's kV (volts/(unit/sec)) and kA (volts/(unit/sec²). These - * constants cam be found using SysId. The states of the system are [position, velocity]ᵀ, inputs - * are [voltage], and outputs are [position]. + * Create a state-space model for a 1 DOF position system from its kV (volts/(unit/sec)) and kA + * (volts/(unit/sec²). These constants cam 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. * - * @param kV The velocity gain, in volts per (units per second) - * @param kA The acceleration gain, in volts per (units per second squared) + *

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 @@ -272,17 +281,17 @@ public final class LinearSystemId { /** * Identify a differential drive drivetrain given the drivetrain's kV and kA in both linear - * (volts/(meter/sec) and volts/(meter/sec²)) and angular (volts/(radian/sec) and - * volts/(radian/sec²)) cases. This can be found using SysId. + * {(volts/(meter/sec), (volts/(meter/sec²))} and angular {(volts/(radian/sec)), + * (volts/(radian/sec²))} cases. These constants can be found using SysId. * *

States: [[left velocity], [right velocity]]
* Inputs: [[left voltage], [right voltage]]
* Outputs: [[left velocity], [right velocity]] * - * @param kVLinear The linear velocity gain, volts per (meter per second). - * @param kALinear The linear acceleration gain, volts per (meter per second squared). - * @param kVAngular The angular velocity gain, volts per (meter per second). - * @param kAAngular The angular acceleration gain, volts per (meter per second squared). + * @param kVLinear The linear velocity gain in volts per (meters per second). + * @param kALinear The linear acceleration gain in volts per (meters per second squared). + * @param kVAngular The angular velocity gain in volts per (radians per second). + * @param kAAngular The angular acceleration gain in volts per (radians per second squared). * @return A LinearSystem representing the given characterized constants. * @throws IllegalArgumentException if kVLinear <= 0, kALinear <= 0, kVAngular <= 0, or * kAAngular <= 0. @@ -318,18 +327,18 @@ public final class LinearSystemId { /** * Identify a differential drive drivetrain given the drivetrain's kV and kA in both linear - * (volts/(meter/sec) and volts/(meter/sec²)) and angular (volts/(radian/sec) and - * volts/(radian/sec²)) cases. This can be found using SysId. + * {(volts/(meter/sec)), (volts/(meter/sec²))} and angular {(volts/(radian/sec)), + * (volts/(radian/sec²))} cases. This can be found using SysId. * *

States: [[left velocity], [right velocity]]
* Inputs: [[left voltage], [right voltage]]
* Outputs: [[left velocity], [right velocity]] * - * @param kVLinear The linear velocity gain, volts per (meter per second). - * @param kALinear The linear acceleration gain, volts per (meter per second squared). - * @param kVAngular The angular velocity gain, volts per (radians per second). - * @param kAAngular The angular acceleration gain, volts per (radians per second squared). - * @param trackwidth The distance between the differential drive's left and right wheels in + * @param kVLinear The linear velocity gain in volts per (meters per second). + * @param kALinear The linear acceleration gain in volts per (meters per second squared). + * @param kVAngular The angular velocity gain in volts per (radians per second). + * @param kAAngular The angular acceleration gain in volts per (radians per second squared). + * @param trackwidth The distance between the differential drive's left and right wheels, in * meters. * @return A LinearSystem representing the given characterized constants. * @throws IllegalArgumentException if kVLinear <= 0, kALinear <= 0, kVAngular <= 0, diff --git a/wpimath/src/main/native/cpp/system/plant/LinearSystemId.cpp b/wpimath/src/main/native/cpp/system/plant/LinearSystemId.cpp new file mode 100644 index 0000000000..3527092a97 --- /dev/null +++ b/wpimath/src/main/native/cpp/system/plant/LinearSystemId.cpp @@ -0,0 +1,193 @@ +// 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 "frc/system/plant/LinearSystemId.h" + +using namespace frc; + +LinearSystem<2, 1, 1> LinearSystemId::ElevatorSystem(DCMotor motor, + units::kilogram_t mass, + units::meter_t radius, + double G) { + if (mass <= 0_kg) { + throw std::domain_error("mass must be greater than zero."); + } + if (radius <= 0_m) { + throw std::domain_error("radius must be greater than zero."); + } + if (G <= 0.0) { + throw std::domain_error("G must be greater than zero."); + } + + Matrixd<2, 2> A{ + {0.0, 1.0}, + {0.0, (-std::pow(G, 2) * motor.Kt / + (motor.R * units::math::pow<2>(radius) * mass * motor.Kv)) + .value()}}; + Matrixd<2, 1> B{0.0, (G * motor.Kt / (motor.R * radius * mass)).value()}; + Matrixd<1, 2> C{1.0, 0.0}; + Matrixd<1, 1> D{0.0}; + + return LinearSystem<2, 1, 1>(A, B, C, D); +} + +LinearSystem<2, 1, 1> LinearSystemId::SingleJointedArmSystem( + DCMotor motor, units::kilogram_square_meter_t J, double G) { + if (J <= 0_kg_sq_m) { + throw std::domain_error("J must be greater than zero."); + } + if (G <= 0.0) { + throw std::domain_error("G must be greater than zero."); + } + + Matrixd<2, 2> A{ + {0.0, 1.0}, + {0.0, (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}}; + Matrixd<2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).value()}; + Matrixd<1, 2> C{1.0, 0.0}; + Matrixd<1, 1> D{0.0}; + + return LinearSystem<2, 1, 1>(A, B, C, D); +} + +LinearSystem<2, 2, 2> LinearSystemId::IdentifyDrivetrainSystem( + decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear, + decltype(1_V / 1_mps) kVAngular, decltype(1_V / 1_mps_sq) kAAngular) { + if (kVLinear <= decltype(kVLinear){0}) { + throw std::domain_error("Kv,linear must be greater than zero."); + } + if (kALinear <= decltype(kALinear){0}) { + throw std::domain_error("Ka,linear must be greater than zero."); + } + if (kVAngular <= decltype(kVAngular){0}) { + throw std::domain_error("Kv,angular must be greater than zero."); + } + if (kAAngular <= decltype(kAAngular){0}) { + throw std::domain_error("Ka,angular must be greater than zero."); + } + + double A1 = -(kVLinear.value() / kALinear.value() + + kVAngular.value() / kAAngular.value()); + double A2 = -(kVLinear.value() / kALinear.value() - + kVAngular.value() / kAAngular.value()); + double B1 = 1.0 / kALinear.value() + 1.0 / kAAngular.value(); + double B2 = 1.0 / kALinear.value() - 1.0 / kAAngular.value(); + + Matrixd<2, 2> A = 0.5 * Matrixd<2, 2>{{A1, A2}, {A2, A1}}; + Matrixd<2, 2> B = 0.5 * Matrixd<2, 2>{{B1, B2}, {B2, B1}}; + Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}}; + Matrixd<2, 2> D{{0.0, 0.0}, {0.0, 0.0}}; + + return LinearSystem<2, 2, 2>(A, B, C, D); +} + +LinearSystem<2, 2, 2> LinearSystemId::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, units::meter_t trackwidth) { + if (kVLinear <= decltype(kVLinear){0}) { + throw std::domain_error("Kv,linear must be greater than zero."); + } + if (kALinear <= decltype(kALinear){0}) { + throw std::domain_error("Ka,linear must be greater than zero."); + } + if (kVAngular <= decltype(kVAngular){0}) { + throw std::domain_error("Kv,angular must be greater than zero."); + } + if (kAAngular <= decltype(kAAngular){0}) { + throw std::domain_error("Ka,angular must be greater than zero."); + } + if (trackwidth <= 0_m) { + throw std::domain_error("r must be greater than zero."); + } + + // We want to find a factor to include in Kv,angular that will convert + // `u = Kv,angular omega` to `u = Kv,angular v`. + // + // v = omega r + // omega = v/r + // omega = 1/r v + // omega = 1/(trackwidth/2) v + // omega = 2/trackwidth v + // + // So multiplying by 2/trackwidth converts the angular gains from V/(rad/s) + // to V/(m/s). + return IdentifyDrivetrainSystem(kVLinear, kALinear, + kVAngular * 2.0 / trackwidth * 1_rad, + kAAngular * 2.0 / trackwidth * 1_rad); +} + +LinearSystem<1, 1, 1> LinearSystemId::FlywheelSystem( + DCMotor motor, units::kilogram_square_meter_t J, double G) { + if (J <= 0_kg_sq_m) { + throw std::domain_error("J must be greater than zero."); + } + if (G <= 0.0) { + throw std::domain_error("G must be greater than zero."); + } + + Matrixd<1, 1> A{ + (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}; + Matrixd<1, 1> B{(G * motor.Kt / (motor.R * J)).value()}; + Matrixd<1, 1> C{1.0}; + Matrixd<1, 1> D{0.0}; + + return LinearSystem<1, 1, 1>(A, B, C, D); +} + +LinearSystem<2, 1, 2> LinearSystemId::DCMotorSystem( + DCMotor motor, units::kilogram_square_meter_t J, double G) { + if (J <= 0_kg_sq_m) { + throw std::domain_error("J must be greater than zero."); + } + if (G <= 0.0) { + throw std::domain_error("G must be greater than zero."); + } + + Matrixd<2, 2> A{ + {0.0, 1.0}, + {0.0, (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}}; + Matrixd<2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).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); +} + +LinearSystem<2, 2, 2> LinearSystemId::DrivetrainVelocitySystem( + const DCMotor& motor, units::kilogram_t mass, units::meter_t r, + units::meter_t rb, units::kilogram_square_meter_t J, double G) { + if (mass <= 0_kg) { + throw std::domain_error("mass must be greater than zero."); + } + if (r <= 0_m) { + throw std::domain_error("r must be greater than zero."); + } + if (rb <= 0_m) { + throw std::domain_error("rb must be greater than zero."); + } + if (J <= 0_kg_sq_m) { + throw std::domain_error("J must be greater than zero."); + } + if (G <= 0.0) { + throw std::domain_error("G must be greater than zero."); + } + + auto C1 = -std::pow(G, 2) * motor.Kt / + (motor.Kv * motor.R * units::math::pow<2>(r)); + auto C2 = G * motor.Kt / (motor.R * r); + + Matrixd<2, 2> A{{((1 / mass + units::math::pow<2>(rb) / J) * C1).value(), + ((1 / mass - units::math::pow<2>(rb) / J) * C1).value()}, + {((1 / mass - units::math::pow<2>(rb) / J) * C1).value(), + ((1 / mass + units::math::pow<2>(rb) / J) * C1).value()}}; + Matrixd<2, 2> B{{((1 / mass + units::math::pow<2>(rb) / J) * C2).value(), + ((1 / mass - units::math::pow<2>(rb) / J) * C2).value()}, + {((1 / mass - units::math::pow<2>(rb) / J) * C2).value(), + ((1 / mass + units::math::pow<2>(rb) / J) * C2).value()}}; + Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}}; + Matrixd<2, 2> D{{0.0, 0.0}, {0.0, 0.0}}; + + return LinearSystem<2, 2, 2>(A, B, C, D); +} 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 0e1b5ee83d..d33f40bcfe 100644 --- a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h +++ b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h @@ -31,92 +31,49 @@ class WPILIB_DLLEXPORT LinearSystemId { units::inverse>>; /** - * Constructs the state-space model for an elevator. + * Create a state-space model of the elevator system. The states of the system + * are [position, velocity], inputs are [voltage], and outputs are [position]. * - * States: [[position], [velocity]] - * Inputs: [[voltage]] - * Outputs: [[position]] - * - * @param motor Instance of DCMotor. - * @param m Carriage mass. - * @param r Pulley radius. + * @param motor The motor (or gearbox) attached to the carriage. + * @param mass The mass of the elevator carriage, in kilograms. + * @param radius The radius of the elevator's driving drum, in meters. * @param G Gear ratio from motor to carriage. - * @throws std::domain_error if m <= 0, r <= 0, or G <= 0. + * @throws std::domain_error if mass <= 0, radius <= 0, or G <= 0. */ static LinearSystem<2, 1, 1> ElevatorSystem(DCMotor motor, - units::kilogram_t m, - units::meter_t r, double G) { - if (m <= 0_kg) { - throw std::domain_error("m must be greater than zero."); - } - if (r <= 0_m) { - throw std::domain_error("r must be greater than zero."); - } - if (G <= 0.0) { - throw std::domain_error("G must be greater than zero."); - } - - Matrixd<2, 2> A{{0.0, 1.0}, - {0.0, (-std::pow(G, 2) * motor.Kt / - (motor.R * units::math::pow<2>(r) * m * motor.Kv)) - .value()}}; - Matrixd<2, 1> B{0.0, (G * motor.Kt / (motor.R * r * m)).value()}; - Matrixd<1, 2> C{1.0, 0.0}; - Matrixd<1, 1> D{0.0}; - - return LinearSystem<2, 1, 1>(A, B, C, D); - } + units::kilogram_t mass, + units::meter_t radius, double G); /** - * Constructs the state-space model for a single-jointed arm. + * 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]. * - * States: [[angle], [angular velocity]] - * Inputs: [[voltage]] - * Outputs: [[angle]] - * - * @param motor Instance of DCMotor. - * @param J Moment of inertia. - * @param G Gear ratio from motor to carriage. + * @param motor The motor (or gearbox) attached to the arm. + * @param J The moment of inertia J of the arm. + * @param G Gear ratio from motor to arm. * @throws std::domain_error if J <= 0 or G <= 0. */ static LinearSystem<2, 1, 1> SingleJointedArmSystem( - DCMotor motor, units::kilogram_square_meter_t J, double G) { - if (J <= 0_kg_sq_m) { - throw std::domain_error("J must be greater than zero."); - } - if (G <= 0.0) { - throw std::domain_error("G must be greater than zero."); - } - - Matrixd<2, 2> A{ - {0.0, 1.0}, - {0.0, (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}}; - Matrixd<2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).value()}; - Matrixd<1, 2> C{1.0, 0.0}; - Matrixd<1, 1> D{0.0}; - - return LinearSystem<2, 1, 1>(A, B, C, D); - } + DCMotor motor, units::kilogram_square_meter_t J, double G); /** - * Constructs the state-space model for a 1 DOF velocity-only system from - * system identification data. + * Create a state-space model for a 1 DOF velocity 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 [velocity], inputs are + * [voltage], and outputs are [velocity]. * * 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]] - * * The parameters provided by the user are from this feedforward model: * * u = K_v v + K_a a * - * @param kV The velocity gain, in volt seconds per distance. - * @param kA The acceleration gain, in volt seconds² per distance. + * @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 * Inputs: [[left voltage], [right voltage]]
* Outputs: [[left velocity], [right velocity]] * - * @param kVlinear The linear velocity gain in volts per (meter per second). - * @param kAlinear The linear acceleration gain in volts per (meter per + * @param kVLinear The linear velocity gain in volts per (meters per second). + * @param kALinear The linear acceleration gain in volts per (meters per * second squared). - * @param kVangular The angular velocity gain in volts per (meter per second). - * @param kAangular The angular acceleration gain in volts per (meter per + * @param kVAngular The angular velocity gain in volts per (radians per + * second). + * @param kAAngular The angular acceleration gain in volts per (radians per * second squared). - * @throws domain_error if kVlinear <= 0, kAlinear <= 0, kVangular <= 0, - * or kAangular <= 0. + * @throws domain_error if kVLinear <= 0, kALinear <= 0, kVAngular <= 0, + * or kAAngular <= 0. */ static LinearSystem<2, 2, 2> IdentifyDrivetrainSystem( - decltype(1_V / 1_mps) kVlinear, decltype(1_V / 1_mps_sq) kAlinear, - decltype(1_V / 1_mps) kVangular, decltype(1_V / 1_mps_sq) kAangular) { - if (kVlinear <= decltype(kVlinear){0}) { - throw std::domain_error("Kv,linear must be greater than zero."); - } - if (kAlinear <= decltype(kAlinear){0}) { - throw std::domain_error("Ka,linear must be greater than zero."); - } - if (kVangular <= decltype(kVangular){0}) { - throw std::domain_error("Kv,angular must be greater than zero."); - } - if (kAangular <= decltype(kAangular){0}) { - throw std::domain_error("Ka,angular must be greater than zero."); - } - - double A1 = -(kVlinear.value() / kAlinear.value() + - kVangular.value() / kAangular.value()); - double A2 = -(kVlinear.value() / kAlinear.value() - - kVangular.value() / kAangular.value()); - double B1 = 1.0 / kAlinear.value() + 1.0 / kAangular.value(); - double B2 = 1.0 / kAlinear.value() - 1.0 / kAangular.value(); - - Matrixd<2, 2> A = 0.5 * Matrixd<2, 2>{{A1, A2}, {A2, A1}}; - Matrixd<2, 2> B = 0.5 * Matrixd<2, 2>{{B1, B2}, {B2, B1}}; - Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}}; - Matrixd<2, 2> D{{0.0, 0.0}, {0.0, 0.0}}; - - return LinearSystem<2, 2, 2>(A, B, C, D); - } + decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear, + decltype(1_V / 1_mps) kVAngular, decltype(1_V / 1_mps_sq) kAAngular); /** * Identify a differential drive drivetrain given the drivetrain's kV and kA - * in both linear (volts/(meter/sec) and volts/(meter/sec²)) and angular - * (volts/(radian/sec) and volts/(radian/sec²)) cases. This can be found using - * SysId. + * in both linear {(volts/(meter/sec)), (volts/(meter/sec²))} and angular + * {(volts/(radian/sec)), (volts/(radian/sec²))} cases. This can be found + * using SysId. * * States: [[left velocity], [right velocity]]
* Inputs: [[left voltage], [right voltage]]
* Outputs: [[left velocity], [right velocity]] * - * @param kVlinear The linear velocity gain in volts per (meter per second). - * @param kAlinear The linear acceleration gain in volts per (meter per + * @param kVLinear The linear velocity gain in volts per (meters per + * second). + * @param kALinear The linear acceleration gain in volts per (meters per * second squared). - * @param kVangular The angular velocity gain in volts per (radian per + * @param kVAngular The angular velocity gain in volts per (radians per * second). - * @param kAangular The angular acceleration gain in volts per (radian per + * @param kAAngular The angular acceleration gain in volts per (radians per * second squared). * @param trackwidth The distance between the differential drive's left and - * right wheels. - * @throws domain_error if kVlinear <= 0, kAlinear <= 0, kVangular <= 0, - * kAangular <= 0, or trackwidth <= 0. + * right wheels, in meters. + * @throws domain_error if kVLinear <= 0, kALinear <= 0, kVAngular <= 0, + * kAAngular <= 0, or trackwidth <= 0. */ 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, units::meter_t trackwidth) { - if (kVlinear <= decltype(kVlinear){0}) { - throw std::domain_error("Kv,linear must be greater than zero."); - } - if (kAlinear <= decltype(kAlinear){0}) { - throw std::domain_error("Ka,linear must be greater than zero."); - } - if (kVangular <= decltype(kVangular){0}) { - throw std::domain_error("Kv,angular must be greater than zero."); - } - if (kAangular <= decltype(kAangular){0}) { - throw std::domain_error("Ka,angular must be greater than zero."); - } - if (trackwidth <= 0_m) { - throw std::domain_error("r must be greater than zero."); - } - - // We want to find a factor to include in Kv,angular that will convert - // `u = Kv,angular omega` to `u = Kv,angular v`. - // - // v = omega r - // omega = v/r - // omega = 1/r v - // omega = 1/(trackwidth/2) v - // omega = 2/trackwidth v - // - // So multiplying by 2/trackwidth converts the angular gains from V/(rad/s) - // to V/(m/s). - return IdentifyDrivetrainSystem(kVlinear, kAlinear, - kVangular * 2.0 / trackwidth * 1_rad, - kAangular * 2.0 / trackwidth * 1_rad); - } + 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, units::meter_t trackwidth); /** - * Constructs the state-space model for a flywheel. + * Create a state-space model of a flywheel system, the states of the system + * are [angular velocity], inputs are [voltage], and outputs are [angular + * velocity]. * - * States: [[angular velocity]] - * Inputs: [[voltage]] - * Outputs: [[angular velocity]] - * - * @param motor Instance of DCMotor. - * @param J Moment of inertia. - * @param G Gear ratio from motor to carriage. + * @param motor The motor (or gearbox) attached to the flywheel. + * @param J The moment of inertia J of the flywheel. + * @param G Gear ratio from motor to flywheel. * @throws std::domain_error if J <= 0 or G <= 0. */ static LinearSystem<1, 1, 1> FlywheelSystem(DCMotor motor, units::kilogram_square_meter_t J, - double G) { - if (J <= 0_kg_sq_m) { - throw std::domain_error("J must be greater than zero."); - } - if (G <= 0.0) { - throw std::domain_error("G must be greater than zero."); - } - - Matrixd<1, 1> A{ - (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}; - Matrixd<1, 1> B{(G * motor.Kt / (motor.R * J)).value()}; - Matrixd<1, 1> C{1.0}; - Matrixd<1, 1> D{0.0}; - - return LinearSystem<1, 1, 1>(A, B, C, D); - } + double G); /** - * Constructs the state-space model for a DC motor motor. + * 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]. * - * States: [[angular position, angular velocity]] - * Inputs: [[voltage]] - * Outputs: [[angular position, angular velocity]] - * - * @param motor Instance of DCMotor. - * @param J Moment of inertia. - * @param G Gear ratio from motor to carriage. + * @param motor The motor (or gearbox) attached to the system. + * @param J the moment of inertia J of the DC motor. + * @param G Gear ratio from motor to output. * @throws std::domain_error if J <= 0 or G <= 0. */ static LinearSystem<2, 1, 2> DCMotorSystem(DCMotor motor, units::kilogram_square_meter_t J, - double G) { - if (J <= 0_kg_sq_m) { - throw std::domain_error("J must be greater than zero."); - } - if (G <= 0.0) { - throw std::domain_error("G must be greater than zero."); - } - - Matrixd<2, 2> A{ - {0.0, 1.0}, - {0.0, (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}}; - Matrixd<2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).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); - } - + double G); /** - * Constructs the state-space model for a drivetrain. + * Create a state-space model of differential drive drivetrain. In this model, + * the states are [left velocity, right velocity], the inputs are [left + * voltage, right voltage], and the outputs are [left velocity, right + * velocity] * - * States: [[left velocity], [right velocity]] - * Inputs: [[left voltage], [right voltage]] - * Outputs: [[left velocity], [right velocity]] - * - * @param motor Instance of DCMotor. - * @param m Drivetrain mass. - * @param r Wheel radius. - * @param rb Robot radius. - * @param J Moment of inertia. + * @param motor The motor (or gearbox) driving the drivetrain. + * @param mass The mass of the robot in kilograms. + * @param r The radius of the wheels in meters. + * @param rb The radius of the base (half of the track width), in meters. + * @param J The moment of inertia of the robot. * @param G Gear ratio from motor to wheel. - * @throws std::domain_error if m <= 0, r <= 0, rb <= 0, J <= 0, or + * @throws std::domain_error if mass <= 0, r <= 0, rb <= 0, J <= 0, or * G <= 0. */ static LinearSystem<2, 2, 2> DrivetrainVelocitySystem( - const DCMotor& motor, units::kilogram_t m, units::meter_t r, - units::meter_t rb, units::kilogram_square_meter_t J, double G) { - if (m <= 0_kg) { - throw std::domain_error("m must be greater than zero."); - } - if (r <= 0_m) { - throw std::domain_error("r must be greater than zero."); - } - if (rb <= 0_m) { - throw std::domain_error("rb must be greater than zero."); - } - if (J <= 0_kg_sq_m) { - throw std::domain_error("J must be greater than zero."); - } - if (G <= 0.0) { - throw std::domain_error("G must be greater than zero."); - } - - auto C1 = -std::pow(G, 2) * motor.Kt / - (motor.Kv * motor.R * units::math::pow<2>(r)); - auto C2 = G * motor.Kt / (motor.R * r); - - Matrixd<2, 2> A{{((1 / m + units::math::pow<2>(rb) / J) * C1).value(), - ((1 / m - units::math::pow<2>(rb) / J) * C1).value()}, - {((1 / m - units::math::pow<2>(rb) / J) * C1).value(), - ((1 / m + units::math::pow<2>(rb) / J) * C1).value()}}; - Matrixd<2, 2> B{{((1 / m + units::math::pow<2>(rb) / J) * C2).value(), - ((1 / m - units::math::pow<2>(rb) / J) * C2).value()}, - {((1 / m - units::math::pow<2>(rb) / J) * C2).value(), - ((1 / m + units::math::pow<2>(rb) / J) * C2).value()}}; - Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}}; - Matrixd<2, 2> D{{0.0, 0.0}, {0.0, 0.0}}; - - return LinearSystem<2, 2, 2>(A, B, C, D); - } + const DCMotor& motor, units::kilogram_t mass, units::meter_t r, + units::meter_t rb, units::kilogram_square_meter_t J, double G); }; } // namespace frc