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 8377022af3..4665aa976e 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 @@ -23,20 +23,20 @@ public final class LinearSystemId { * @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 the elevator's driving drum, in meters. - * @param G The reduction between motor and drum, as a ratio of output to input. + * @param gearing 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. + * @throws IllegalArgumentException if massKg <= 0, radiusMeters <= 0, or gearing <= 0. */ public static LinearSystem createElevatorSystem( - DCMotor motor, double massKg, double radiusMeters, double G) { + DCMotor motor, double massKg, double radiusMeters, double gearing) { if (massKg <= 0.0) { throw new IllegalArgumentException("massKg must be greater than zero."); } if (radiusMeters <= 0.0) { throw new IllegalArgumentException("radiusMeters must be greater than zero."); } - if (G <= 0) { - throw new IllegalArgumentException("G must be greater than zero."); + if (gearing <= 0) { + throw new IllegalArgumentException("gearing must be greater than zero."); } return new LinearSystem<>( @@ -45,14 +45,14 @@ public final class LinearSystemId { 0, 1, 0, - -Math.pow(G, 2) + -Math.pow(gearing, 2) * motor.KtNMPerAmp / (motor.rOhms * radiusMeters * radiusMeters * massKg * motor.KvRadPerSecPerVolt)), - VecBuilder.fill(0, G * motor.KtNMPerAmp / (motor.rOhms * radiusMeters * massKg)), + VecBuilder.fill(0, gearing * motor.KtNMPerAmp / (motor.rOhms * radiusMeters * massKg)), Matrix.mat(Nat.N1(), Nat.N2()).fill(1, 0), new Matrix<>(Nat.N1(), Nat.N1())); } @@ -63,26 +63,26 @@ public final class LinearSystemId { * * @param motor The motor (or gearbox) attached to 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. + * @param gearing 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 gearing <= 0. */ public static LinearSystem createFlywheelSystem( - DCMotor motor, double JKgMetersSquared, double G) { + DCMotor motor, double JKgMetersSquared, double gearing) { if (JKgMetersSquared <= 0.0) { throw new IllegalArgumentException("J must be greater than zero."); } - if (G <= 0.0) { - throw new IllegalArgumentException("G must be greater than zero."); + if (gearing <= 0.0) { + throw new IllegalArgumentException("gearing must be greater than zero."); } return new LinearSystem<>( VecBuilder.fill( - -G - * G + -gearing + * gearing * motor.KtNMPerAmp / (motor.KvRadPerSecPerVolt * motor.rOhms * JKgMetersSquared)), - VecBuilder.fill(G * motor.KtNMPerAmp / (motor.rOhms * JKgMetersSquared)), + VecBuilder.fill(gearing * motor.KtNMPerAmp / (motor.rOhms * JKgMetersSquared)), Matrix.eye(Nat.N1()), new Matrix<>(Nat.N1(), Nat.N1())); } @@ -94,17 +94,17 @@ public final class LinearSystemId { * * @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. + * @param gearing 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 gearing <= 0. */ public static LinearSystem createDCMotorSystem( - DCMotor motor, double JKgMetersSquared, double G) { + DCMotor motor, double JKgMetersSquared, double gearing) { if (JKgMetersSquared <= 0.0) { throw new IllegalArgumentException("J must be greater than zero."); } - if (G <= 0.0) { - throw new IllegalArgumentException("G must be greater than zero."); + if (gearing <= 0.0) { + throw new IllegalArgumentException("gearing must be greater than zero."); } return new LinearSystem<>( @@ -113,11 +113,11 @@ public final class LinearSystemId { 0, 1, 0, - -G - * G + -gearing + * gearing * motor.KtNMPerAmp / (motor.KvRadPerSecPerVolt * motor.rOhms * JKgMetersSquared)), - VecBuilder.fill(0, G * motor.KtNMPerAmp / (motor.rOhms * JKgMetersSquared)), + VecBuilder.fill(0, gearing * motor.KtNMPerAmp / (motor.rOhms * JKgMetersSquared)), Matrix.eye(Nat.N2()), new Matrix<>(Nat.N2(), Nat.N1())); } @@ -165,9 +165,10 @@ public final class LinearSystemId { * @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 gearing 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. + * @throws IllegalArgumentException if m <= 0, r <= 0, rb <= 0, J <= 0, or gearing + * <= 0. */ public static LinearSystem createDrivetrainVelocitySystem( DCMotor motor, @@ -175,7 +176,7 @@ public final class LinearSystemId { double rMeters, double rbMeters, double JKgMetersSquared, - double G) { + double gearing) { if (massKg <= 0.0) { throw new IllegalArgumentException("massKg must be greater than zero."); } @@ -188,13 +189,15 @@ public final class LinearSystemId { if (JKgMetersSquared <= 0.0) { throw new IllegalArgumentException("JKgMetersSquared must be greater than zero."); } - if (G <= 0.0) { - throw new IllegalArgumentException("G must be greater than zero."); + if (gearing <= 0.0) { + throw new IllegalArgumentException("gearing must be greater than zero."); } var C1 = - -(G * G) * motor.KtNMPerAmp / (motor.KvRadPerSecPerVolt * motor.rOhms * rMeters * rMeters); - var C2 = G * motor.KtNMPerAmp / (motor.rOhms * rMeters); + -(gearing * gearing) + * motor.KtNMPerAmp + / (motor.KvRadPerSecPerVolt * motor.rOhms * rMeters * rMeters); + var C2 = gearing * motor.KtNMPerAmp / (motor.rOhms * rMeters); final double C3 = 1 / massKg + rbMeters * rbMeters / JKgMetersSquared; final double C4 = 1 / massKg - rbMeters * rbMeters / JKgMetersSquared; @@ -212,17 +215,17 @@ public final class LinearSystemId { * * @param motor The motor (or gearbox) attached to 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. + * @param gearing 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. */ public static LinearSystem createSingleJointedArmSystem( - DCMotor motor, double JKgSquaredMeters, double G) { + DCMotor motor, double JKgSquaredMeters, double gearing) { 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."); + if (gearing <= 0.0) { + throw new IllegalArgumentException("gearing must be greater than zero."); } return new LinearSystem<>( @@ -231,10 +234,10 @@ public final class LinearSystemId { 0, 1, 0, - -Math.pow(G, 2) + -Math.pow(gearing, 2) * motor.KtNMPerAmp / (motor.KvRadPerSecPerVolt * motor.rOhms * JKgSquaredMeters)), - VecBuilder.fill(0, G * motor.KtNMPerAmp / (motor.rOhms * JKgSquaredMeters)), + VecBuilder.fill(0, gearing * motor.KtNMPerAmp / (motor.rOhms * JKgSquaredMeters)), Matrix.mat(Nat.N1(), Nat.N2()).fill(1, 0), new Matrix<>(Nat.N1(), Nat.N1())); } diff --git a/wpimath/src/main/native/cpp/system/plant/LinearSystemId.cpp b/wpimath/src/main/native/cpp/system/plant/LinearSystemId.cpp index 3527092a97..11c65eada3 100644 --- a/wpimath/src/main/native/cpp/system/plant/LinearSystemId.cpp +++ b/wpimath/src/main/native/cpp/system/plant/LinearSystemId.cpp @@ -9,23 +9,24 @@ using namespace frc; LinearSystem<2, 1, 1> LinearSystemId::ElevatorSystem(DCMotor motor, units::kilogram_t mass, units::meter_t radius, - double G) { + double gearing) { 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."); + if (gearing <= 0.0) { + throw std::domain_error("gearing must be greater than zero."); } Matrixd<2, 2> A{ {0.0, 1.0}, - {0.0, (-std::pow(G, 2) * motor.Kt / + {0.0, (-std::pow(gearing, 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<2, 1> B{0.0, + (gearing * motor.Kt / (motor.R * radius * mass)).value()}; Matrixd<1, 2> C{1.0, 0.0}; Matrixd<1, 1> D{0.0}; @@ -33,18 +34,19 @@ LinearSystem<2, 1, 1> LinearSystemId::ElevatorSystem(DCMotor motor, } LinearSystem<2, 1, 1> LinearSystemId::SingleJointedArmSystem( - DCMotor motor, units::kilogram_square_meter_t J, double G) { + DCMotor motor, units::kilogram_square_meter_t J, double gearing) { 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."); + if (gearing <= 0.0) { + throw std::domain_error("gearing 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()}; + {0.0, + (-std::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}}; + Matrixd<2, 1> B{0.0, (gearing * motor.Kt / (motor.R * J)).value()}; Matrixd<1, 2> C{1.0, 0.0}; Matrixd<1, 1> D{0.0}; @@ -119,17 +121,17 @@ LinearSystem<2, 2, 2> LinearSystemId::IdentifyDrivetrainSystem( } LinearSystem<1, 1, 1> LinearSystemId::FlywheelSystem( - DCMotor motor, units::kilogram_square_meter_t J, double G) { + DCMotor motor, units::kilogram_square_meter_t J, double gearing) { 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."); + if (gearing <= 0.0) { + throw std::domain_error("gearing 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()}; + (-std::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}; + Matrixd<1, 1> B{(gearing * motor.Kt / (motor.R * J)).value()}; Matrixd<1, 1> C{1.0}; Matrixd<1, 1> D{0.0}; @@ -137,18 +139,19 @@ LinearSystem<1, 1, 1> LinearSystemId::FlywheelSystem( } LinearSystem<2, 1, 2> LinearSystemId::DCMotorSystem( - DCMotor motor, units::kilogram_square_meter_t J, double G) { + DCMotor motor, units::kilogram_square_meter_t J, double gearing) { 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."); + if (gearing <= 0.0) { + throw std::domain_error("gearing 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()}; + {0.0, + (-std::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}}; + Matrixd<2, 1> B{0.0, (gearing * 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}; @@ -157,7 +160,7 @@ LinearSystem<2, 1, 2> LinearSystemId::DCMotorSystem( 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) { + units::meter_t rb, units::kilogram_square_meter_t J, double gearing) { if (mass <= 0_kg) { throw std::domain_error("mass must be greater than zero."); } @@ -170,13 +173,13 @@ LinearSystem<2, 2, 2> LinearSystemId::DrivetrainVelocitySystem( 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."); + if (gearing <= 0.0) { + throw std::domain_error("gearing must be greater than zero."); } - auto C1 = -std::pow(G, 2) * motor.Kt / + auto C1 = -std::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * units::math::pow<2>(r)); - auto C2 = G * motor.Kt / (motor.R * r); + auto C2 = gearing * 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()}, 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 64f34963a9..af9e799dea 100644 --- a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h +++ b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h @@ -38,12 +38,13 @@ class WPILIB_DLLEXPORT LinearSystemId { * @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 mass <= 0, radius <= 0, or G <= 0. + * @param gearing Gear ratio from motor to carriage. + * @throws std::domain_error if mass <= 0, radius <= 0, or gearing <= 0. */ static LinearSystem<2, 1, 1> ElevatorSystem(DCMotor motor, units::kilogram_t mass, - units::meter_t radius, double G); + units::meter_t radius, + double gearing); /** * Create a state-space model of a single-jointed arm system.The states of the @@ -52,11 +53,11 @@ class WPILIB_DLLEXPORT LinearSystemId { * * @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. + * @param gearing Gear ratio from motor to arm. + * @throws std::domain_error if J <= 0 or gearing <= 0. */ static LinearSystem<2, 1, 1> SingleJointedArmSystem( - DCMotor motor, units::kilogram_square_meter_t J, double G); + DCMotor motor, units::kilogram_square_meter_t J, double gearing); /** * Create a state-space model for a 1 DOF velocity system from its kV @@ -198,12 +199,12 @@ class WPILIB_DLLEXPORT LinearSystemId { * * @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. + * @param gearing Gear ratio from motor to flywheel. + * @throws std::domain_error if J <= 0 or gearing <= 0. */ static LinearSystem<1, 1, 1> FlywheelSystem(DCMotor motor, units::kilogram_square_meter_t J, - double G); + double gearing); /** * Create a state-space model of a DC motor system. The states of the system @@ -212,12 +213,12 @@ class WPILIB_DLLEXPORT LinearSystemId { * * @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. + * @param gearing Gear ratio from motor to output. + * @throws std::domain_error if J <= 0 or gearing <= 0. */ static LinearSystem<2, 1, 2> DCMotorSystem(DCMotor motor, units::kilogram_square_meter_t J, - double G); + double gearing); /** * Create a state-space model of a DC motor system from its kV @@ -271,13 +272,13 @@ class WPILIB_DLLEXPORT LinearSystemId { * @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. + * @param gearing Gear ratio from motor to wheel. * @throws std::domain_error if mass <= 0, r <= 0, rb <= 0, J <= 0, or - * G <= 0. + * gearing <= 0. */ static LinearSystem<2, 2, 2> DrivetrainVelocitySystem( const DCMotor& motor, units::kilogram_t mass, units::meter_t r, - units::meter_t rb, units::kilogram_square_meter_t J, double G); + units::meter_t rb, units::kilogram_square_meter_t J, double gearing); }; } // namespace frc