From db56d094818b250cf63b77fe1cbd3994895eec3d Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Mon, 25 Nov 2024 19:09:36 -0500 Subject: [PATCH 1/7] initial commit --- .../simulation/DifferentialDrivetrainSim.java | 71 ++-- .../first/wpilibj/simulation/ElevatorSim.java | 27 +- .../first/wpilibj/simulation/FlywheelSim.java | 51 +-- .../{DCMotorSim.java => GearboxSim.java} | 137 +++---- .../simulation/SingleJointedArmSim.java | 40 +- .../DifferentialDrivetrainSimTest.java | 23 +- .../wpilibj/simulation/ElevatorSimTest.java | 19 +- ...CMotorSimTest.java => GearboxSimTest.java} | 19 +- .../simulation/SingleJointedArmSimTest.java | 6 +- .../armsimulation/subsystems/Arm.java | 6 +- .../Drivetrain.java | 5 +- .../subsystems/Elevator.java | 6 +- .../subsystems/Elevator.java | 5 +- .../flywheelbangbangcontroller/Robot.java | 7 +- .../Drivetrain.java | 5 +- .../wpilibj/examples/statespacearm/Robot.java | 5 +- .../examples/statespaceelevator/Robot.java | 5 +- .../examples/statespaceflywheel/Robot.java | 5 +- .../PotentiometerPIDTest.java | 6 +- .../ultrasonicpid/UltrasonicPIDTest.java | 6 +- .../wpi/first/math/system/plant/DCMotor.java | 357 +++++------------- .../wpi/first/math/system/plant/Gearbox.java | 222 +++++++++++ .../first/math/system/plant/KnownDCMotor.java | 87 +++++ .../math/system/plant/LinearSystemId.java | 330 ++++++++-------- .../math/system/plant/proto/DCMotorProto.java | 18 +- .../system/plant/struct/DCMotorStruct.java | 18 +- .../LinearQuadraticRegulatorTest.java | 66 ++-- .../math/controller/LinearSystemLoopTest.java | 67 ++-- .../estimator/ExtendedKalmanFilterTest.java | 79 ++-- .../math/estimator/KalmanFilterTest.java | 11 +- .../estimator/UnscentedKalmanFilterTest.java | 16 +- .../first/math/system/LinearSystemIDTest.java | 23 +- .../system/plant/proto/DCMotorProtoTest.java | 18 +- .../plant/struct/DCMotorStructTest.java | 18 +- 34 files changed, 932 insertions(+), 852 deletions(-) rename wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/{DCMotorSim.java => GearboxSim.java} (52%) rename wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/{DCMotorSimTest.java => GearboxSimTest.java} (82%) create mode 100644 wpimath/src/main/java/edu/wpi/first/math/system/plant/Gearbox.java create mode 100644 wpimath/src/main/java/edu/wpi/first/math/system/plant/KnownDCMotor.java diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java index 6e25b344393..0257886d5e3 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java @@ -15,7 +15,8 @@ import edu.wpi.first.math.numbers.N7; import edu.wpi.first.math.system.LinearSystem; import edu.wpi.first.math.system.NumericalIntegration; -import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.Gearbox; +import edu.wpi.first.math.system.plant.KnownDCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.RobotController; @@ -38,7 +39,7 @@ *

y = x */ public class DifferentialDrivetrainSim { - private final DCMotor m_motor; + private final Gearbox m_gearbox; private final double m_originalGearing; private final Matrix m_measurementStdDevs; private double m_currentGearing; @@ -54,9 +55,7 @@ public class DifferentialDrivetrainSim { /** * Creates a simulated differential drivetrain. * - * @param driveMotor A {@link DCMotor} representing the left side of the drivetrain. - * @param gearing The gearing ratio between motor and wheel, as output over input. This must be - * the same ratio as the ratio used to identify or create the drivetrainPlant. + * @param gearbox A {@link Gearbox} representing the left side of the drivetrain. * @param jKgMetersSquared The moment of inertia of the drivetrain about its center. * @param massKg The mass of the drivebase. * @param wheelRadiusMeters The radius of the wheels on the drivetrain. @@ -68,8 +67,7 @@ public class DifferentialDrivetrainSim { * point. */ public DifferentialDrivetrainSim( - DCMotor driveMotor, - double gearing, + Gearbox gearbox, double jKgMetersSquared, double massKg, double wheelRadiusMeters, @@ -77,14 +75,8 @@ public DifferentialDrivetrainSim( Matrix measurementStdDevs) { this( LinearSystemId.createDrivetrainVelocitySystem( - driveMotor, - massKg, - wheelRadiusMeters, - trackWidthMeters / 2.0, - jKgMetersSquared, - gearing), - driveMotor, - gearing, + gearbox, massKg, wheelRadiusMeters, trackWidthMeters / 2.0, jKgMetersSquared), + gearbox, trackWidthMeters, wheelRadiusMeters, measurementStdDevs); @@ -95,13 +87,11 @@ public DifferentialDrivetrainSim( * * @param plant The {@link LinearSystem} representing the robot's drivetrain. This system can be * created with {@link - * edu.wpi.first.math.system.plant.LinearSystemId#createDrivetrainVelocitySystem(DCMotor, - * double, double, double, double, double)} or {@link + * edu.wpi.first.math.system.plant.LinearSystemId#createDrivetrainVelocitySystem(Gearbox, + * double, double, double, double)} or {@link * edu.wpi.first.math.system.plant.LinearSystemId#identifyDrivetrainSystem(double, double, * double, double)}. - * @param driveMotor A {@link DCMotor} representing the drivetrain. - * @param gearing The gearingRatio ratio of the robot, as output over input. This must be the same - * ratio as the ratio used to identify or create the drivetrainPlant. + * @param gearbox A {@link Gearbox} representing the drivetrain. * @param trackWidthMeters The distance between the two sides of the drivetrain. Can be found with * SysId. * @param wheelRadiusMeters The radius of the wheels on the drivetrain, in meters. @@ -113,15 +103,14 @@ public DifferentialDrivetrainSim( */ public DifferentialDrivetrainSim( LinearSystem plant, - DCMotor driveMotor, - double gearing, + Gearbox gearbox, double trackWidthMeters, double wheelRadiusMeters, Matrix measurementStdDevs) { this.m_plant = plant; this.m_rb = trackWidthMeters / 2.0; - this.m_motor = driveMotor; - this.m_originalGearing = gearing; + this.m_gearbox = gearbox; + this.m_originalGearing = gearbox.getGearboxReduction(); this.m_measurementStdDevs = measurementStdDevs; m_wheelRadiusMeters = wheelRadiusMeters; m_currentGearing = m_originalGearing; @@ -236,8 +225,9 @@ public double getLeftVelocityMetersPerSecond() { * @return the drivetrain's left side current draw, in amps */ public double getLeftCurrentDrawAmps() { - return m_motor.getCurrent( + return m_gearbox.getTorqueNewtonMeters( getState(State.kLeftVelocity) * m_currentGearing / m_wheelRadiusMeters, m_u.get(0, 0)) + / m_gearbox.dcMotor.kt.baseUnitMagnitude() * Math.signum(m_u.get(0, 0)); } @@ -247,8 +237,9 @@ public double getLeftCurrentDrawAmps() { * @return the drivetrain's right side current draw, in amps */ public double getRightCurrentDrawAmps() { - return m_motor.getCurrent( + return m_gearbox.getTorqueNewtonMeters( getState(State.kRightVelocity) * m_currentGearing / m_wheelRadiusMeters, m_u.get(1, 0)) + / m_gearbox.dcMotor.kt.baseUnitMagnitude() * Math.signum(m_u.get(1, 0)); } @@ -309,12 +300,14 @@ public void setPose(Pose2d pose) { * @return The state derivative with respect to time. */ protected Matrix getDynamics(Matrix x, Matrix u) { - // Because G can be factored out of B, we can divide by the old ratio and multiply + // Because G can be factored out of B, we can divide by the old ratio and + // multiply // by the new ratio to get a new drivetrain model. var B = new Matrix<>(Nat.N4(), Nat.N2()); B.assignBlock(0, 0, m_plant.getB().times(this.m_currentGearing / this.m_originalGearing)); - // Because G² can be factored out of A, we can divide by the old ratio squared and multiply + // Because G² can be factored out of A, we can divide by the old ratio squared + // and multiply // by the new ratio squared to get a new drivetrain model. var A = new Matrix<>(Nat.N4(), Nat.N4()); A.assignBlock( @@ -398,26 +391,26 @@ public enum KitbotGearing { /** Represents common motor layouts of the kit drivetrain. */ public enum KitbotMotor { /** One CIM motor per drive side. */ - kSingleCIMPerSide(DCMotor.getCIM(1)), + kSingleCIMPerSide(new Gearbox(KnownDCMotor.CIM.dcMotor, 1)), /** Two CIM motors per drive side. */ - kDualCIMPerSide(DCMotor.getCIM(2)), + kDualCIMPerSide(new Gearbox(KnownDCMotor.CIM.dcMotor, 2)), /** One Mini CIM motor per drive side. */ - kSingleMiniCIMPerSide(DCMotor.getMiniCIM(1)), + kSingleMiniCIMPerSide(new Gearbox(KnownDCMotor.MiniCIM.dcMotor, 1)), /** Two Mini CIM motors per drive side. */ - kDualMiniCIMPerSide(DCMotor.getMiniCIM(2)), + kDualMiniCIMPerSide(new Gearbox(KnownDCMotor.MiniCIM.dcMotor, 2)), /** One Falcon 500 motor per drive side. */ - kSingleFalcon500PerSide(DCMotor.getFalcon500(1)), + kSingleFalcon500PerSide(new Gearbox(KnownDCMotor.Falcon500.dcMotor, 1)), /** Two Falcon 500 motors per drive side. */ - kDoubleFalcon500PerSide(DCMotor.getFalcon500(2)), + kDoubleFalcon500PerSide(new Gearbox(KnownDCMotor.Falcon500.dcMotor, 2)), /** One NEO motor per drive side. */ - kSingleNEOPerSide(DCMotor.getNEO(1)), + kSingleNEOPerSide(new Gearbox(KnownDCMotor.NEO.dcMotor, 1)), /** Two NEO motors per drive side. */ - kDoubleNEOPerSide(DCMotor.getNEO(2)); + kDoubleNEOPerSide(new Gearbox(KnownDCMotor.NEO.dcMotor, 2)); /** KitbotMotor value. */ - public final DCMotor value; + public final Gearbox value; - KitbotMotor(DCMotor i) { + KitbotMotor(Gearbox i) { this.value = i; } } @@ -487,9 +480,9 @@ public static DifferentialDrivetrainSim createKitbotSim( KitbotWheelSize wheelSize, double jKgMetersSquared, Matrix measurementStdDevs) { + motor.value.withReduction(gearing.value); return new DifferentialDrivetrainSim( motor.value, - gearing.value, jKgMetersSquared, Units.lbsToKilograms(60), wheelSize.value / 2.0, diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java index 2f7ce3e768d..2d5234a7b80 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java @@ -11,14 +11,14 @@ import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.system.LinearSystem; import edu.wpi.first.math.system.NumericalIntegration; -import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.Gearbox; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.wpilibj.RobotController; /** Represents a simulated elevator mechanism. */ public class ElevatorSim extends LinearSystemSim { // Gearbox for the elevator. - private final DCMotor m_gearbox; + private final Gearbox m_gearbox; // The min allowable height for the elevator. private final double m_minHeight; @@ -33,8 +33,8 @@ public class ElevatorSim extends LinearSystemSim { * Creates a simulated elevator mechanism. * * @param plant The linear system that represents the elevator. This system can be created with - * {@link edu.wpi.first.math.system.plant.LinearSystemId#createElevatorSystem(DCMotor, double, - * double, double)}. + * {@link edu.wpi.first.math.system.plant.LinearSystemId#createElevatorSystem(Gearbox, double, + * double)}. * @param gearbox The type of and number of motors in the elevator gearbox. * @param minHeightMeters The min allowable height of the elevator. * @param maxHeightMeters The max allowable height of the elevator. @@ -46,7 +46,7 @@ public class ElevatorSim extends LinearSystemSim { @SuppressWarnings("this-escape") public ElevatorSim( LinearSystem plant, - DCMotor gearbox, + Gearbox gearbox, double minHeightMeters, double maxHeightMeters, boolean simulateGravity, @@ -77,7 +77,7 @@ public ElevatorSim( public ElevatorSim( double kV, double kA, - DCMotor gearbox, + Gearbox gearbox, double minHeightMeters, double maxHeightMeters, boolean simulateGravity, @@ -97,7 +97,6 @@ public ElevatorSim( * Creates a simulated elevator mechanism. * * @param gearbox The type of and number of motors in the elevator gearbox. - * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions). * @param carriageMassKg The mass of the elevator carriage. * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around. * @param minHeightMeters The min allowable height of the elevator. @@ -108,8 +107,7 @@ public ElevatorSim( * noise is desired. If present must have 1 element for position. */ public ElevatorSim( - DCMotor gearbox, - double gearing, + Gearbox gearbox, double carriageMassKg, double drumRadiusMeters, double minHeightMeters, @@ -118,7 +116,7 @@ public ElevatorSim( double startingHeightMeters, double... measurementStdDevs) { this( - LinearSystemId.createElevatorSystem(gearbox, carriageMassKg, drumRadiusMeters, gearing), + LinearSystemId.createElevatorSystem(gearbox, carriageMassKg, drumRadiusMeters), gearbox, minHeightMeters, maxHeightMeters, @@ -202,15 +200,12 @@ public double getVelocityMetersPerSecond() { * @return The elevator current draw. */ public double getCurrentDrawAmps() { - // I = V / R - omega / (Kv * R) - // Reductions are greater than 1, so a reduction of 10:1 would mean the motor is - // spinning 10x faster than the output - // v = r w, so w = v/r double kA = 1 / m_plant.getB().get(1, 0); double kV = -m_plant.getA().get(1, 1) * kA; - double motorVelocityRadPerSec = m_x.get(1, 0) * kV * m_gearbox.KvRadPerSecPerVolt; + double motorVelocityRadPerSec = m_x.get(1, 0) * kV * m_gearbox.dcMotor.kv.baseUnitMagnitude(); var appliedVoltage = m_u.get(0, 0); - return m_gearbox.getCurrent(motorVelocityRadPerSec, appliedVoltage) + return m_gearbox.getCurrentAmps( + m_gearbox.getTorqueNewtonMeters(motorVelocityRadPerSec, appliedVoltage)) * Math.signum(appliedVoltage); } 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 95d01e16217..db81be2a9df 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 @@ -10,7 +10,7 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.system.LinearSystem; -import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.Gearbox; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.AngularAcceleration; @@ -20,10 +20,7 @@ /** Represents a simulated flywheel mechanism. */ public class FlywheelSim extends LinearSystemSim { // Gearbox for the flywheel. - private final DCMotor m_gearbox; - - // The gearing from the motors to the output. - private final double m_gearing; + private final Gearbox m_gearbox; // The moment of inertia for the flywheel mechanism. private final double m_jKgMetersSquared; @@ -32,15 +29,14 @@ public class FlywheelSim extends LinearSystemSim { * Creates a simulated flywheel mechanism. * * @param plant The linear system that represents the flywheel. Use either {@link - * LinearSystemId#createFlywheelSystem(DCMotor, double, double)} if using physical constants - * or {@link LinearSystemId#identifyVelocitySystem(double, double)} if using system - * characterization. + * LinearSystemId#createFlywheelSystem(Gearbox, double)} if using physical constants or {@link + * LinearSystemId#identifyVelocitySystem(double, double)} if using system characterization. * @param gearbox The type of and number of motors in the flywheel gearbox. * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no * noise is desired. If present must have 1 element for velocity. */ public FlywheelSim( - LinearSystem plant, DCMotor gearbox, double... measurementStdDevs) { + LinearSystem plant, Gearbox gearbox, double... measurementStdDevs) { super(plant, measurementStdDevs); m_gearbox = gearbox; @@ -51,17 +47,15 @@ public FlywheelSim( // A = -G²Kₜ/(KᵥRJ) // B = GKₜ/(RJ) // - // Solve for G. - // - // A/B = -G/Kᵥ - // G = -KᵥA/B - // // Solve for J. // - // B = GKₜ/(RJ) - // J = GKₜ/(RB) - m_gearing = -gearbox.KvRadPerSecPerVolt * plant.getA(0, 0) / plant.getB(0, 0); - m_jKgMetersSquared = m_gearing * gearbox.KtNMPerAmp / (gearbox.rOhms * plant.getB(0, 0)); + // B = nGKₜ/(RJ) + // J = nGKₜ/(RB) + m_jKgMetersSquared = + m_gearbox.numMotors + * m_gearbox.getGearboxReduction() + * gearbox.dcMotor.kt.baseUnitMagnitude() + / (gearbox.dcMotor.internalResistance.baseUnitMagnitude() * plant.getB(1, 0)); } /** @@ -73,15 +67,6 @@ public void setAngularVelocity(double velocityRadPerSec) { setState(VecBuilder.fill(velocityRadPerSec)); } - /** - * Returns the gear ratio of the flywheel. - * - * @return the flywheel's gear ratio. - */ - public double getGearing() { - return m_gearing; - } - /** * Returns the moment of inertia in kilograms meters squared. * @@ -96,7 +81,7 @@ public double getJKgMetersSquared() { * * @return The flywheel's gearbox. */ - public DCMotor getGearbox() { + public Gearbox getGearbox() { return m_gearbox; } @@ -156,16 +141,12 @@ public double getTorqueNewtonMeters() { } /** - * Returns the flywheel's current draw. + * Returns the flywheel's net current draw. * - * @return The flywheel's current draw. + * @return The flywheel's net current draw. */ public double getCurrentDrawAmps() { - // I = V / R - omega / (Kv * R) - // Reductions are output over input, so a reduction of 2:1 means the motor is spinning - // 2x faster than the flywheel - return m_gearbox.getCurrent(m_x.get(0, 0) * m_gearing, m_u.get(0, 0)) - * Math.signum(m_u.get(0, 0)); + return m_gearbox.getCurrentAmps(getTorqueNewtonMeters()) * Math.signum(m_u.get(0, 0)); } /** diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/GearboxSim.java similarity index 52% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSim.java rename to wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/GearboxSim.java index 99b546923b9..aba0e593bf2 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/GearboxSim.java @@ -12,64 +12,60 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.system.LinearSystem; -import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.Gearbox; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularAcceleration; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.RobotController; -/** Represents a simulated DC motor mechanism. */ -public class DCMotorSim extends LinearSystemSim { - // Gearbox for the DC motor. - private final DCMotor m_gearbox; +/** Represents a simulated Gearbox mechanism. */ +public class GearboxSim extends LinearSystemSim { + // Gearbox for the system. + private final Gearbox m_gearbox; - // The gearing from the motors to the output. - private final double m_gearing; - - // The moment of inertia for the DC motor mechanism. + // The moment of inertia for the Gearbox mechanism. private final double m_jKgMetersSquared; /** - * Creates a simulated DC motor mechanism. + * Creates a simulated Gearbox mechanism. * - * @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, - * 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. + * @param plant The linear system representing the Gearbox. This system can be created with {@link + * edu.wpi.first.math.system.plant.LinearSystemId#createGearboxSystem(Gearbox, double)} or + * {@link edu.wpi.first.math.system.plant.LinearSystemId#identifyPositionSystem(double, + * double)}. If {@link + * edu.wpi.first.math.system.plant.LinearSystemId#identifyPositionSystem(double, double)} is + * used, the distance unit must be radians. + * @param gearbox The gearbox of the system. * @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. */ - public DCMotorSim(LinearSystem plant, DCMotor gearbox, double... measurementStdDevs) { + public GearboxSim(LinearSystem plant, Gearbox gearbox, double... measurementStdDevs) { super(plant, measurementStdDevs); m_gearbox = gearbox; - // By theorem 6.10.1 of https://file.tavsys.net/control/controls-engineering-in-frc.pdf, + // By theorem 6.10.1 of + // https://file.tavsys.net/control/controls-engineering-in-frc.pdf, // the DC motor state-space model is: // - // dx/dt = -G²Kₜ/(KᵥRJ)x + (GKₜ)/(RJ)u - // A = -G²Kₜ/(KᵥRJ) - // B = GKₜ/(RJ) - // - // Solve for G. - // - // A/B = -G/Kᵥ - // G = -KᵥA/B + // dx/dt = -nG²Kₜ/(KᵥRJ)x + nGKₜ/(RJ)u + // A = -nG²Kₜ/(KᵥRJ) + // B = nGKₜ/(RJ) // // Solve for J. // - // B = GKₜ/(RJ) - // J = GKₜ/(RB) - m_gearing = -gearbox.KvRadPerSecPerVolt * plant.getA(1, 1) / plant.getB(1, 0); - m_jKgMetersSquared = m_gearing * gearbox.KtNMPerAmp / (gearbox.rOhms * plant.getB(1, 0)); + // B = nGKₜ/(RJ) + // J = nGKₜ/(RB) + m_jKgMetersSquared = + m_gearbox.numMotors + * m_gearbox.getGearboxReduction() + * gearbox.dcMotor.kt.baseUnitMagnitude() + / (gearbox.dcMotor.internalResistance.baseUnitMagnitude() * plant.getB(1, 0)); } /** - * Sets the state of the DC motor. + * Sets the state of the Gearbox. * * @param angularPositionRad The new position in radians. * @param angularVelocityRadPerSec The new velocity in radians per second. @@ -79,7 +75,7 @@ public void setState(double angularPositionRad, double angularVelocityRadPerSec) } /** - * Sets the DC motor's angular position. + * Sets the Gearbox's angular position. * * @param angularPositionRad The new position in radians. */ @@ -88,7 +84,7 @@ public void setAngle(double angularPositionRad) { } /** - * Sets the DC motor's angular velocity. + * Sets the Gearbox's angular velocity. * * @param angularVelocityRadPerSec The new velocity in radians per second. */ @@ -97,90 +93,81 @@ public void setAngularVelocity(double angularVelocityRadPerSec) { } /** - * Returns the gear ratio of the DC motor. - * - * @return the DC motor's gear ratio. - */ - public double getGearing() { - return m_gearing; - } - - /** - * Returns the moment of inertia of the DC motor. + * Returns the moment of inertia of the Gearbox. * - * @return The DC motor's moment of inertia. + * @return The Gearbox's moment of inertia. */ public double getJKgMetersSquared() { return m_jKgMetersSquared; } /** - * Returns the gearbox for the DC motor. + * Returns the gearbox for the system. * - * @return The DC motor's gearbox. + * @return The gearbox for the system. */ - public DCMotor getGearbox() { + public Gearbox getGearbox() { return m_gearbox; } /** - * Returns the DC motor's position. + * Returns the Gearbox's position. * - * @return The DC motor's position. + * @return The Gearbox's position. */ public double getAngularPositionRad() { return getOutput(0); } /** - * Returns the DC motor's position in rotations. + * Returns the Gearbox's position in rotations. * - * @return The DC motor's position in rotations. + * @return The Gearbox's position in rotations. */ public double getAngularPositionRotations() { return Units.radiansToRotations(getAngularPositionRad()); } /** - * Returns the DC motor's position. + * Returns the Gearbox's position. * - * @return The DC motor's position + * @return The Gearbox's position */ public Angle getAngularPosition() { return Radians.of(getAngularPositionRad()); } /** - * Returns the DC motor's velocity. + * Returns the Gearbox's velocity. * - * @return The DC motor's velocity. + * @return The Gearbox's velocity. */ public double getAngularVelocityRadPerSec() { return getOutput(1); } /** - * Returns the DC motor's velocity in RPM. + * Returns the Gearbox's velocity in RPM. * - * @return The DC motor's velocity in RPM. + * @return The Gearbox's velocity in RPM. */ public double getAngularVelocityRPM() { return Units.radiansPerSecondToRotationsPerMinute(getAngularVelocityRadPerSec()); } /** - * Returns the DC motor's velocity. + * Returns the Gearbox's velocity. * - * @return The DC motor's velocity + * @return The Gearbox's velocity */ public AngularVelocity getAngularVelocity() { return RadiansPerSecond.of(getAngularVelocityRadPerSec()); } /** - * Returns the DC motor's acceleration in Radians Per Second Squared. + * Returns the Gearbox's acceleration in Radians Per Second Squared. * - * @return The DC motor's acceleration in Radians Per Second Squared. + * @return The Gearbox's acceleration in Radians Per Second Squared. */ public double getAngularAccelerationRadPerSecSq() { var acceleration = (m_plant.getA().times(m_x)).plus(m_plant.getB().times(m_u)); @@ -188,49 +175,45 @@ public double getAngularAccelerationRadPerSecSq() { } /** - * Returns the DC motor's acceleration. + * Returns the Gearbox's acceleration. * - * @return The DC motor's acceleration. + * @return The Gearbox's acceleration. */ public AngularAcceleration getAngularAcceleration() { return RadiansPerSecondPerSecond.of(getAngularAccelerationRadPerSecSq()); } /** - * Returns the DC motor's torque in Newton-Meters. + * Returns the Gearbox's torque in Newton-Meters. * - * @return The DC motor's torque in Newton-Meters. + * @return The Gearbox's torque in Newton-Meters. */ public double getTorqueNewtonMeters() { return getAngularAccelerationRadPerSecSq() * m_jKgMetersSquared; } /** - * Returns the DC motor's current draw. + * Returns the Gearbox's net current draw. * - * @return The DC motor's current draw. + * @return The Gearbox's net current draw. */ public double getCurrentDrawAmps() { - // I = V / R - omega / (Kv * R) - // Reductions are output over input, so a reduction of 2:1 means the motor is spinning - // 2x faster than the output - return m_gearbox.getCurrent(m_x.get(1, 0) * m_gearing, m_u.get(0, 0)) - * Math.signum(m_u.get(0, 0)); + return m_gearbox.getCurrentAmps(getTorqueNewtonMeters()) * Math.signum(m_u.get(0, 0)); } /** - * Gets the input voltage for the DC motor. + * Gets the input voltage for each of the Gearbox's motors. * - * @return The DC motor's input voltage. + * @return The input voltage for each of the Gearbox's motors. */ public double getInputVoltage() { return getInput(0); } /** - * Sets the input voltage for the DC motor. + * Sets the input voltage for each of the Gearbox's motors. * - * @param volts The input voltage. + * @param volts The input voltage for each of the Gearbox's motors. */ public void setInputVoltage(double volts) { setInput(volts); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java index 7d7ae78284d..e34f24f242f 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java @@ -11,17 +11,14 @@ import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.system.LinearSystem; import edu.wpi.first.math.system.NumericalIntegration; -import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.Gearbox; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.wpilibj.RobotController; /** Represents a simulated single jointed arm mechanism. */ public class SingleJointedArmSim extends LinearSystemSim { // The gearbox for the arm. - private final DCMotor m_gearbox; - - // The gearing between the motors and the output. - private final double m_gearing; + private final Gearbox m_gearbox; // The length of the arm. private final double m_armLenMeters; @@ -39,10 +36,9 @@ public class SingleJointedArmSim extends LinearSystemSim { * Creates a simulated arm mechanism. * * @param plant The linear system that represents the arm. This system can be created with {@link - * edu.wpi.first.math.system.plant.LinearSystemId#createSingleJointedArmSystem(DCMotor, - * double, double)}. + * edu.wpi.first.math.system.plant.LinearSystemId#createSingleJointedArmSystem(Gearbox, + * double)}. * @param gearbox The type of and number of motors in the arm gearbox. - * @param gearing The gearing of the arm (numbers greater than 1 represent reductions). * @param armLengthMeters The length of the arm. * @param minAngleRads The minimum angle that the arm is capable of. * @param maxAngleRads The maximum angle that the arm is capable of. @@ -54,8 +50,7 @@ public class SingleJointedArmSim extends LinearSystemSim { @SuppressWarnings("this-escape") public SingleJointedArmSim( LinearSystem plant, - DCMotor gearbox, - double gearing, + Gearbox gearbox, double armLengthMeters, double minAngleRads, double maxAngleRads, @@ -64,7 +59,6 @@ public SingleJointedArmSim( double... measurementStdDevs) { super(plant, measurementStdDevs); m_gearbox = gearbox; - m_gearing = gearing; m_armLenMeters = armLengthMeters; m_minAngle = minAngleRads; m_maxAngle = maxAngleRads; @@ -77,7 +71,6 @@ public SingleJointedArmSim( * Creates a simulated arm mechanism. * * @param gearbox The type of and number of motors in the arm gearbox. - * @param gearing The gearing of the arm (numbers greater than 1 represent reductions). * @param jKgMetersSquared The moment of inertia of the arm; can be calculated from CAD software. * @param armLengthMeters The length of the arm. * @param minAngleRads The minimum angle that the arm is capable of. @@ -88,8 +81,7 @@ public SingleJointedArmSim( * noise is desired. If present must have 1 element for position. */ public SingleJointedArmSim( - DCMotor gearbox, - double gearing, + Gearbox gearbox, double jKgMetersSquared, double armLengthMeters, double minAngleRads, @@ -98,9 +90,8 @@ public SingleJointedArmSim( double startingAngleRads, double... measurementStdDevs) { this( - LinearSystemId.createSingleJointedArmSystem(gearbox, jKgMetersSquared, gearing), + LinearSystemId.createSingleJointedArmSystem(gearbox, jKgMetersSquared), gearbox, - gearing, armLengthMeters, minAngleRads, maxAngleRads, @@ -185,8 +176,9 @@ public double getVelocityRadPerSec() { public double getCurrentDrawAmps() { // Reductions are greater than 1, so a reduction of 10:1 would mean the motor is // spinning 10x faster than the output - var motorVelocity = m_x.get(1, 0) * m_gearing; - return m_gearbox.getCurrent(motorVelocity, m_u.get(0, 0)) * Math.signum(m_u.get(0, 0)); + var motorVelocity = m_x.get(1, 0) * m_gearbox.getGearboxReduction(); + return m_gearbox.getCurrentAmps( + m_gearbox.getTorqueNewtonMeters(motorVelocity, m_u.get(0, 0)) * Math.signum(m_u.get(0, 0))); } /** @@ -227,21 +219,21 @@ protected Matrix updateX(Matrix currentXhat, Matrix u, d // // We substitute in F = m⋅g⋅cos(θ), where θ is the angle from horizontal: // - // α = (m⋅g⋅cos(θ))⋅r/J + // α = (m⋅g⋅cos(θ))⋅r/J // // Multiply RHS by cos(θ) to account for the arm angle. Further, we know the // arm mass-moment of inertia J of our arm is given by J=1/3 mL², modeled as a // rod rotating about it's end, where L is the overall rod length. The mass // distribution is assumed to be uniform. Substitute r=L/2 to find: // - // α = (m⋅g⋅cos(θ))⋅r/(1/3 mL²) - // α = (m⋅g⋅cos(θ))⋅(L/2)/(1/3 mL²) - // α = 3/2⋅g⋅cos(θ)/L + // α = (m⋅g⋅cos(θ))⋅r/(1/3 mL²) + // α = (m⋅g⋅cos(θ))⋅(L/2)/(1/3 mL²) + // α = 3/2⋅g⋅cos(θ)/L // // This acceleration is next added to the linear system dynamics ẋ=Ax+Bu // - // f(x, u) = Ax + Bu + [0 α]ᵀ - // f(x, u) = Ax + Bu + [0 3/2⋅g⋅cos(θ)/L]ᵀ + // f(x, u) = Ax + Bu + [0 α]ᵀ + // f(x, u) = Ax + Bu + [0 3/2⋅g⋅cos(θ)/L]ᵀ Matrix updatedXhat = NumericalIntegration.rkdp( diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java index 25bbdb59834..6a47a0ac3f1 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java @@ -19,7 +19,8 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N7; import edu.wpi.first.math.system.NumericalIntegration; -import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.Gearbox; +import edu.wpi.first.math.system.plant.KnownDCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; @@ -31,17 +32,16 @@ class DifferentialDrivetrainSimTest { @Test void testConvergence() { - var motor = DCMotor.getNEO(2); + var gearbox = new Gearbox(KnownDCMotor.NEO.dcMotor, 2); var plant = LinearSystemId.createDrivetrainVelocitySystem( - motor, 50, Units.inchesToMeters(2), Units.inchesToMeters(12), 0.5, 1.0); + gearbox, 50, Units.inchesToMeters(2), Units.inchesToMeters(12), 0.5); var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(24)); var sim = new DifferentialDrivetrainSim( plant, - motor, - 1, + gearbox, kinematics.trackWidthMeters, Units.inchesToMeters(2), VecBuilder.fill(0, 0, 0.0001, 0.1, 0.1, 0.005, 0.005)); @@ -97,14 +97,14 @@ void testConvergence() { @Test void testCurrent() { - var motor = DCMotor.getNEO(2); + var gearbox = new Gearbox(KnownDCMotor.NEO.dcMotor, 2); var plant = LinearSystemId.createDrivetrainVelocitySystem( - motor, 50, Units.inchesToMeters(2), Units.inchesToMeters(12), 0.5, 1.0); + gearbox, 50, Units.inchesToMeters(2), Units.inchesToMeters(12), 0.5); var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(24)); var sim = new DifferentialDrivetrainSim( - plant, motor, 1, kinematics.trackWidthMeters, Units.inchesToMeters(2), null); + plant, gearbox, kinematics.trackWidthMeters, Units.inchesToMeters(2), null); sim.setInputs(-12, -12); for (int i = 0; i < 10; i++) { @@ -127,17 +127,16 @@ void testCurrent() { @Test void testModelStability() { - var motor = DCMotor.getNEO(2); + var gearbox = new Gearbox(KnownDCMotor.NEO.dcMotor, 2, 5.0); var plant = LinearSystemId.createDrivetrainVelocitySystem( - motor, 50, Units.inchesToMeters(2), Units.inchesToMeters(12), 2.0, 5.0); + gearbox, 50, Units.inchesToMeters(2), Units.inchesToMeters(12), 2.0); var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(24)); var sim = new DifferentialDrivetrainSim( plant, - motor, - 5, + gearbox, kinematics.trackWidthMeters, Units.inchesToMeters(2), VecBuilder.fill(0, 0, 0.0001, 0.1, 0.1, 0.005, 0.005)); diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java index deade245fa1..705fda4889f 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java @@ -9,7 +9,8 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.Gearbox; +import edu.wpi.first.math.system.plant.KnownDCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Encoder; @@ -27,8 +28,7 @@ void testStateSpaceSimWithElevator() { var sim = new ElevatorSim( - DCMotor.getVex775Pro(4), - 14.67, + new Gearbox(KnownDCMotor.Vex775Pro.dcMotor, 4, 14.67), 8, 0.75 * 25.4 / 1000.0, 0.0, @@ -67,8 +67,7 @@ void testStateSpaceSimWithElevator() { void testMinMax() { var sim = new ElevatorSim( - DCMotor.getVex775Pro(4), - 14.67, + new Gearbox(KnownDCMotor.Vex775Pro.dcMotor, 4, 14.67), 8.0, 0.75 * 25.4 / 1000.0, 0.0, @@ -97,7 +96,13 @@ void testMinMax() { void testStability() { var sim = new ElevatorSim( - DCMotor.getVex775Pro(4), 100, 4, Units.inchesToMeters(0.5), 0, 10, false, 0.0); + new Gearbox(KnownDCMotor.Vex775Pro.dcMotor, 4, 100), + 4, + Units.inchesToMeters(0.5), + 0, + 10, + false, + 0.0); sim.setState(VecBuilder.fill(0, 0)); sim.setInput(12); @@ -107,7 +112,7 @@ void testStability() { var system = LinearSystemId.createElevatorSystem( - DCMotor.getVex775Pro(4), 4, Units.inchesToMeters(0.5), 100); + new Gearbox(KnownDCMotor.Vex775Pro.dcMotor, 4, 100), 4, Units.inchesToMeters(0.5)); assertEquals( system.calculateX(VecBuilder.fill(0, 0), VecBuilder.fill(12), 0.02 * 50.0).get(0, 0), sim.getPositionMeters(), diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DCMotorSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/GearboxSimTest.java similarity index 82% rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DCMotorSimTest.java rename to wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/GearboxSimTest.java index b7a4a6dd875..a33ec5a864a 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DCMotorSimTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/GearboxSimTest.java @@ -10,21 +10,22 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.system.LinearSystem; -import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.Gearbox; +import edu.wpi.first.math.system.plant.KnownDCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.motorcontrol.PWMVictorSPX; import org.junit.jupiter.api.Test; -class DCMotorSimTest { +class GearboxSimTest { @Test void testVoltageSteadyState() { RoboRioSim.resetData(); - DCMotor gearbox = DCMotor.getNEO(1); - LinearSystem plant = LinearSystemId.createDCMotorSystem(gearbox, 0.0005, 1); - DCMotorSim sim = new DCMotorSim(plant, gearbox); + Gearbox gearbox = new Gearbox(KnownDCMotor.NEO.dcMotor); + LinearSystem plant = LinearSystemId.createGearboxSystem(gearbox, 0.0005); + GearboxSim sim = new GearboxSim(plant, gearbox); try (var motor = new PWMVictorSPX(0); var encoder = new Encoder(0, 1)) { @@ -42,7 +43,7 @@ void testVoltageSteadyState() { encoderSim.setRate(sim.getAngularVelocityRadPerSec()); } - assertEquals(gearbox.KvRadPerSecPerVolt * 12, encoder.getRate(), 0.1); + assertEquals(gearbox.dcMotor.kv.baseUnitMagnitude() * 12, encoder.getRate(), 0.1); for (int i = 0; i < 100; i++) { motor.setVoltage(0); @@ -63,9 +64,9 @@ void testVoltageSteadyState() { void testPositionFeedbackControl() { RoboRioSim.resetData(); - DCMotor gearbox = DCMotor.getNEO(1); - LinearSystem plant = LinearSystemId.createDCMotorSystem(gearbox, 0.0005, 1); - DCMotorSim sim = new DCMotorSim(plant, gearbox); + Gearbox gearbox = new Gearbox(KnownDCMotor.NEO.dcMotor); + LinearSystem plant = LinearSystemId.createGearboxSystem(gearbox, 0.0005); + GearboxSim sim = new GearboxSim(plant, gearbox); try (var motor = new PWMVictorSPX(0); var encoder = new Encoder(0, 1); diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSimTest.java index 928fd3edc89..2e7e5491ce7 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSimTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSimTest.java @@ -7,15 +7,15 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.Gearbox; +import edu.wpi.first.math.system.plant.KnownDCMotor; import edu.wpi.first.math.util.Units; import org.junit.jupiter.api.Test; class SingleJointedArmSimTest { SingleJointedArmSim m_sim = new SingleJointedArmSim( - DCMotor.getVex775Pro(2), - 300, + new Gearbox(KnownDCMotor.Vex775Pro.dcMotor, 2, 300), 3.0, Units.inchesToMeters(30.0), -Math.PI, diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/subsystems/Arm.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/subsystems/Arm.java index 3c92370019e..830199b78a8 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/subsystems/Arm.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/subsystems/Arm.java @@ -5,7 +5,8 @@ package edu.wpi.first.wpilibj.examples.armsimulation.subsystems; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.Gearbox; +import edu.wpi.first.math.system.plant.KnownDCMotor; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.Preferences; @@ -29,7 +30,7 @@ public class Arm implements AutoCloseable { private double m_armSetpointDegrees = Constants.kDefaultArmSetpointDegrees; // The arm gearbox represents a gearbox containing two Vex 775pro motors. - private final DCMotor m_armGearbox = DCMotor.getVex775Pro(2); + private final Gearbox m_armGearbox = new Gearbox(KnownDCMotor.Vex775Pro.dcMotor, 2, Constants.kArmReduction); // Standard classes for controlling our arm private final PIDController m_controller = new PIDController(m_armKp, 0, 0); @@ -43,7 +44,6 @@ public class Arm implements AutoCloseable { private final SingleJointedArmSim m_armSim = new SingleJointedArmSim( m_armGearbox, - Constants.kArmReduction, SingleJointedArmSim.estimateMOI(Constants.kArmLength, Constants.kArmMass), Constants.kArmLength, Constants.kMinAngleRads, diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java index 7df9f3b4496..5d8e84f6721 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java @@ -25,7 +25,8 @@ import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.system.LinearSystem; -import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.Gearbox; +import edu.wpi.first.math.system.plant.KnownDCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.DoubleArrayEntry; @@ -101,7 +102,7 @@ public class Drivetrain { LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3); private final DifferentialDrivetrainSim m_drivetrainSimulator = new DifferentialDrivetrainSim( - m_drivetrainSystem, DCMotor.getCIM(2), 8, kTrackWidth, kWheelRadius, null); + m_drivetrainSystem, new Gearbox(KnownDCMotor.CIM.dcMotor, 2, 8), kTrackWidth, kWheelRadius, null); /** * Constructs a differential drive object. Sets the encoder distance per pulse and resets the diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java index 3b2765a9907..fb9c47b3480 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java @@ -9,7 +9,8 @@ import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.Gearbox; +import edu.wpi.first.math.system.plant.KnownDCMotor; import edu.wpi.first.math.trajectory.ExponentialProfile; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Encoder; @@ -28,7 +29,7 @@ public class Elevator implements AutoCloseable { // This gearbox represents a gearbox containing 4 Vex 775pro motors. - private final DCMotor m_elevatorGearbox = DCMotor.getNEO(2); + private final Gearbox m_elevatorGearbox = new Gearbox(KnownDCMotor.NEO.dcMotor, 2, Constants.kElevatorGearing); private final ExponentialProfile m_profile = new ExponentialProfile( @@ -55,7 +56,6 @@ public class Elevator implements AutoCloseable { private final ElevatorSim m_elevatorSim = new ElevatorSim( m_elevatorGearbox, - Constants.kElevatorGearing, Constants.kCarriageMass, Constants.kElevatorDrumRadius, Constants.kMinElevatorHeightMeters, diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java index 90378bd119a..9b70b2c0e11 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java @@ -10,6 +10,8 @@ import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.Gearbox; +import edu.wpi.first.math.system.plant.KnownDCMotor; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.RobotController; @@ -27,7 +29,7 @@ public class Elevator implements AutoCloseable { // This gearbox represents a gearbox containing 4 Vex 775pro motors. - private final DCMotor m_elevatorGearbox = DCMotor.getVex775Pro(4); + private final Gearbox m_elevatorGearbox = new Gearbox(KnownDCMotor.Vex775Pro.dcMotor, 4, Constants.kElevatorGearing); // Standard classes for controlling our elevator private final ProfiledPIDController m_controller = @@ -50,7 +52,6 @@ public class Elevator implements AutoCloseable { private final ElevatorSim m_elevatorSim = new ElevatorSim( m_elevatorGearbox, - Constants.kElevatorGearing, Constants.kCarriageMass, Constants.kElevatorDrumRadius, Constants.kMinElevatorHeightMeters, diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/flywheelbangbangcontroller/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/flywheelbangbangcontroller/Robot.java index abba8a7365c..cbf79f13295 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/flywheelbangbangcontroller/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/flywheelbangbangcontroller/Robot.java @@ -11,7 +11,8 @@ import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.system.LinearSystem; -import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.Gearbox; +import edu.wpi.first.math.system.plant.KnownDCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Encoder; @@ -60,10 +61,10 @@ public class Robot extends TimedRobot { private static final double kFlywheelMomentOfInertia = 0.5 * Units.lbsToKilograms(1.5) * Math.pow(Units.inchesToMeters(4), 2); - private final DCMotor m_gearbox = DCMotor.getNEO(1); + private final Gearbox m_gearbox = new Gearbox(KnownDCMotor.NEO.dcMotor, 1, kFlywheelGearing); private final LinearSystem m_plant = - LinearSystemId.createFlywheelSystem(m_gearbox, kFlywheelGearing, kFlywheelMomentOfInertia); + LinearSystemId.createFlywheelSystem(m_gearbox, kFlywheelMomentOfInertia); private final FlywheelSim m_flywheelSim = new FlywheelSim(m_plant, m_gearbox); private final EncoderSim m_encoderSim = new EncoderSim(m_encoder); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java index eb911054a58..49d01565196 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java @@ -16,7 +16,8 @@ import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.system.LinearSystem; -import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.Gearbox; +import edu.wpi.first.math.system.plant.KnownDCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.wpilibj.AnalogGyro; import edu.wpi.first.wpilibj.Encoder; @@ -70,7 +71,7 @@ public class Drivetrain { LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3); private final DifferentialDrivetrainSim m_drivetrainSimulator = new DifferentialDrivetrainSim( - m_drivetrainSystem, DCMotor.getCIM(2), 8, kTrackWidth, kWheelRadius, null); + m_drivetrainSystem, new Gearbox(KnownDCMotor.CIM.dcMotor, 2, 8), kTrackWidth, kWheelRadius, null); /** Subsystem constructor. */ public Drivetrain() { diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java index 77251079354..27b32ef09e5 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java @@ -12,7 +12,8 @@ import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.system.LinearSystem; import edu.wpi.first.math.system.LinearSystemLoop; -import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.Gearbox; +import edu.wpi.first.math.system.plant.KnownDCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; @@ -53,7 +54,7 @@ public class Robot extends TimedRobot { // Inputs (what we can "put in"): [voltage], in volts. // Outputs (what we can measure): [position], in radians. private final LinearSystem m_armPlant = - LinearSystemId.createSingleJointedArmSystem(DCMotor.getNEO(2), kArmMOI, kArmGearing); + LinearSystemId.createSingleJointedArmSystem(new Gearbox(KnownDCMotor.NEO.dcMotor, 2, kArmGearing), kArmMOI); // The observer fuses our encoder data and voltage inputs to reject noise. @SuppressWarnings("unchecked") diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java index f9bd7bf3603..19a7c94a3dd 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java @@ -12,7 +12,8 @@ import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.system.LinearSystem; import edu.wpi.first.math.system.LinearSystemLoop; -import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.Gearbox; +import edu.wpi.first.math.system.plant.KnownDCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; @@ -59,7 +60,7 @@ public class Robot extends TimedRobot { */ private final LinearSystem m_elevatorPlant = LinearSystemId.createElevatorSystem( - DCMotor.getNEO(2), kCarriageMass, kDrumRadius, kElevatorGearing); + new Gearbox(KnownDCMotor.NEO.dcMotor, 2, kElevatorGearing), kCarriageMass, kDrumRadius); // The observer fuses our encoder data and voltage inputs to reject noise. @SuppressWarnings("unchecked") diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Robot.java index 73226a6af01..829ee08140c 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Robot.java @@ -11,7 +11,8 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.system.LinearSystem; import edu.wpi.first.math.system.LinearSystemLoop; -import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.Gearbox; +import edu.wpi.first.math.system.plant.KnownDCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Encoder; @@ -43,7 +44,7 @@ public class Robot extends TimedRobot { // Outputs (what we can measure): [velocity], in radians per second. private final LinearSystem m_flywheelPlant = LinearSystemId.createFlywheelSystem( - DCMotor.getNEO(2), kFlywheelMomentOfInertia, kFlywheelGearing); + new Gearbox(KnownDCMotor.NEO.dcMotor, 2, kFlywheelGearing), kFlywheelMomentOfInertia); // The observer fuses our encoder data and voltage inputs to reject noise. private final KalmanFilter m_observer = diff --git a/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/potentiometerpid/PotentiometerPIDTest.java b/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/potentiometerpid/PotentiometerPIDTest.java index 8a3d92b8e66..fc41951102d 100644 --- a/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/potentiometerpid/PotentiometerPIDTest.java +++ b/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/potentiometerpid/PotentiometerPIDTest.java @@ -9,7 +9,8 @@ import edu.wpi.first.hal.HAL; import edu.wpi.first.hal.HAL.SimPeriodicBeforeCallback; -import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.Gearbox; +import edu.wpi.first.math.system.plant.KnownDCMotor; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.simulation.AnalogInputSim; @@ -25,7 +26,7 @@ @ResourceLock("timing") class PotentiometerPIDTest { - private final DCMotor m_elevatorGearbox = DCMotor.getVex775Pro(4); + private final Gearbox m_elevatorGearbox = new Gearbox(KnownDCMotor.Vex775Pro.dcMotor, 4, kElevatorGearing); private static final double kElevatorGearing = 10.0; private static final double kElevatorDrumRadius = Units.inchesToMeters(2.0); private static final double kCarriageMassKg = 4.0; // kg @@ -49,7 +50,6 @@ void startThread() { m_elevatorSim = new ElevatorSim( m_elevatorGearbox, - kElevatorGearing, kCarriageMassKg, kElevatorDrumRadius, 0.0, diff --git a/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/UltrasonicPIDTest.java b/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/UltrasonicPIDTest.java index f0a27b8f9c0..8f953827f68 100644 --- a/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/UltrasonicPIDTest.java +++ b/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/UltrasonicPIDTest.java @@ -9,7 +9,8 @@ import edu.wpi.first.hal.HAL; import edu.wpi.first.hal.HAL.SimPeriodicBeforeCallback; -import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.Gearbox; +import edu.wpi.first.math.system.plant.KnownDCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim; @@ -25,7 +26,7 @@ @ResourceLock("timing") class UltrasonicPIDTest { - private final DCMotor m_gearbox = DCMotor.getFalcon500(2); + private final Gearbox m_gearbox = new Gearbox(KnownDCMotor.Falcon500.dcMotor, 2, kGearing); private static final double kGearing = KitbotGearing.k10p71.value; public static final double kvVoltSecondsPerMeter = 1.98; public static final double kaVoltSecondsSquaredPerMeter = 0.2; @@ -63,7 +64,6 @@ private void startThread() { kvVoltSecondsPerRadian, kaVoltSecondsSquaredPerRadian), m_gearbox, - kGearing, kTrackwidthMeters, kWheelDiameterMeters / 2.0, null); diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java index de8346d7ddb..fc7522b0895 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java @@ -4,37 +4,44 @@ package edu.wpi.first.math.system.plant; +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.NewtonMeters; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.Volts; + import edu.wpi.first.math.system.plant.proto.DCMotorProto; import edu.wpi.first.math.system.plant.struct.DCMotorStruct; -import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.AngularVelocityUnit; +import edu.wpi.first.units.CurrentUnit; +import edu.wpi.first.units.TorqueUnit; +import edu.wpi.first.units.VoltageUnit; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Per; +import edu.wpi.first.units.measure.Resistance; +import edu.wpi.first.units.measure.Torque; +import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.util.protobuf.ProtobufSerializable; import edu.wpi.first.util.struct.StructSerializable; -/** Holds the constants for a DC motor. */ +/** Represents the properties and models the behavior of a DC motor. */ public class DCMotor implements ProtobufSerializable, StructSerializable { /** Voltage at which the motor constants were measured. */ - public final double nominalVoltageVolts; - + public final Voltage nominalVoltage; /** Torque when stalled. */ - public final double stallTorqueNewtonMeters; - + public final Torque stallTorque; /** Current draw when stalled. */ - public final double stallCurrentAmps; - + public final Current stallCurrent; /** Current draw under no load. */ - public final double freeCurrentAmps; - + public final Current freeCurrent; /** Angular velocity under no load. */ - public final double freeSpeedRadPerSec; - + public final AngularVelocity freeSpeed; /** Motor internal resistance. */ - public final double rOhms; - + public final Resistance internalResistance; /** Motor velocity constant. */ - public final double KvRadPerSecPerVolt; - + public final Per kv; /** Motor torque constant. */ - public final double KtNMPerAmp; + public final Per kt; /** DCMotor protobuf for serialization. */ public static final DCMotorProto proto = new DCMotorProto(); @@ -45,280 +52,102 @@ public class DCMotor implements ProtobufSerializable, StructSerializable { /** * Constructs a DC motor. * - * @param nominalVoltageVolts Voltage at which the motor constants were measured. - * @param stallTorqueNewtonMeters Torque when stalled. - * @param stallCurrentAmps Current draw when stalled. - * @param freeCurrentAmps Current draw under no load. - * @param freeSpeedRadPerSec Angular velocity under no load. - * @param numMotors Number of motors in a gearbox. + * @param nominalVoltageVolts Voltage at which the motor constants were + * measured in Volts. + * @param stallTorqueNewtonMeters Torque when stalled in Newton-Meters. + * @param stallCurrentAmps Current draw when stalled in Amps. + * @param freeCurrentAmps Current draw under no load in Amps. + * @param freeSpeedRadPerSec Angular velocity under no load in Radians per + * Second. */ public DCMotor( double nominalVoltageVolts, double stallTorqueNewtonMeters, double stallCurrentAmps, double freeCurrentAmps, - double freeSpeedRadPerSec, - int numMotors) { - this.nominalVoltageVolts = nominalVoltageVolts; - this.stallTorqueNewtonMeters = stallTorqueNewtonMeters * numMotors; - this.stallCurrentAmps = stallCurrentAmps * numMotors; - this.freeCurrentAmps = freeCurrentAmps * numMotors; - this.freeSpeedRadPerSec = freeSpeedRadPerSec; - - this.rOhms = nominalVoltageVolts / this.stallCurrentAmps; - this.KvRadPerSecPerVolt = - freeSpeedRadPerSec / (nominalVoltageVolts - rOhms * this.freeCurrentAmps); - this.KtNMPerAmp = this.stallTorqueNewtonMeters / this.stallCurrentAmps; - } - - /** - * Calculate current drawn by motor with given speed and input voltage. - * - * @param speedRadiansPerSec The current angular velocity of the motor. - * @param voltageInputVolts The voltage being applied to the motor. - * @return The estimated current. - */ - public double getCurrent(double speedRadiansPerSec, double voltageInputVolts) { - return -1.0 / KvRadPerSecPerVolt / rOhms * speedRadiansPerSec + 1.0 / rOhms * voltageInputVolts; - } - - /** - * Calculate current drawn by motor for a given torque. - * - * @param torqueNm The torque produced by the motor. - * @return The current drawn by the motor. - */ - public double getCurrent(double torqueNm) { - return torqueNm / KtNMPerAmp; + double freeSpeedRadPerSec) { + this( + Volts.of(nominalVoltageVolts), + NewtonMeters.of(stallTorqueNewtonMeters), + Amps.of(stallCurrentAmps), + Amps.of(freeCurrentAmps), + RadiansPerSecond.of(freeSpeedRadPerSec)); } /** - * Calculate torque produced by the motor with a given current. + * Constructs a DC motor. * - * @param currentAmpere The current drawn by the motor. - * @return The torque output. + * @param nominalVoltage Voltage at which the motor constants were + * measured. + * @param stallTorque Torque when stalled. + * @param stallCurrent Current draw when stalled. + * @param freeCurrent Current draw under no load. + * @param freeSpeed Angular velocity under no load. */ - public double getTorque(double currentAmpere) { - return currentAmpere * KtNMPerAmp; - } - - /** - * Calculate the voltage provided to the motor for a given torque and angular velocity. - * - * @param torqueNm The torque produced by the motor. - * @param speedRadiansPerSec The current angular velocity of the motor. + public DCMotor( + Voltage nominalVoltage, + Torque stallTorque, + Current stallCurrent, + Current freeCurrent, + AngularVelocity freeSpeed) { + this.nominalVoltage = nominalVoltage; + this.stallTorque = stallTorque; + this.stallCurrent = stallCurrent; + this.freeCurrent = freeCurrent; + this.freeSpeed = freeSpeed; + internalResistance = nominalVoltage.div(stallCurrent); + kv = freeSpeed.div(nominalVoltage.minus(internalResistance.times(freeCurrent))); + kt = stallTorque.div(stallCurrent); + } + + /** + * Calculate the input voltage of the motor for a given torque and + * angular velocity. + * + * @param torqueNewtonMeters The torque produced by the motor. + * @param angularVelocityRadiansPerSec The current angular velocity of the + * motor. * @return The voltage of the motor. */ - public double getVoltage(double torqueNm, double speedRadiansPerSec) { - return 1.0 / KvRadPerSecPerVolt * speedRadiansPerSec + 1.0 / KtNMPerAmp * rOhms * torqueNm; - } - - /** - * Calculates the angular speed produced by the motor at a given torque and input voltage. - * - * @param torqueNm The torque produced by the motor. - * @param voltageInputVolts The voltage applied to the motor. - * @return The angular speed of the motor. - */ - public double getSpeed(double torqueNm, double voltageInputVolts) { - return voltageInputVolts * KvRadPerSecPerVolt - - 1.0 / KtNMPerAmp * torqueNm * rOhms * KvRadPerSecPerVolt; - } - - /** - * Returns a copy of this motor with the given gearbox reduction applied. - * - * @param gearboxReduction The gearbox reduction. - * @return A motor with the gearbox reduction applied. - */ - public DCMotor withReduction(double gearboxReduction) { - return new DCMotor( - nominalVoltageVolts, - stallTorqueNewtonMeters * gearboxReduction, - stallCurrentAmps, - freeCurrentAmps, - freeSpeedRadPerSec / gearboxReduction, - 1); - } - - /** - * Return a gearbox of CIM motors. - * - * @param numMotors Number of motors in the gearbox. - * @return A gearbox of CIM motors. - */ - public static DCMotor getCIM(int numMotors) { - return new DCMotor( - 12, 2.42, 133, 2.7, Units.rotationsPerMinuteToRadiansPerSecond(5310), numMotors); - } - - /** - * Return a gearbox of 775Pro motors. - * - * @param numMotors Number of motors in the gearbox. - * @return A gearbox of 775Pro motors. - */ - public static DCMotor getVex775Pro(int numMotors) { - return new DCMotor( - 12, 0.71, 134, 0.7, Units.rotationsPerMinuteToRadiansPerSecond(18730), numMotors); - } - - /** - * Return a gearbox of NEO motors. - * - * @param numMotors Number of motors in the gearbox. - * @return A gearbox of NEO motors. - */ - public static DCMotor getNEO(int numMotors) { - return new DCMotor( - 12, 2.6, 105, 1.8, Units.rotationsPerMinuteToRadiansPerSecond(5676), numMotors); + public double getVoltageInputVolts(double torqueNewtonMeters, double angularVelocityRadiansPerSec) { + return 1.0 / kv.baseUnitMagnitude() * angularVelocityRadiansPerSec + + 1.0 / kt.baseUnitMagnitude() * internalResistance.baseUnitMagnitude() * torqueNewtonMeters; } /** - * Return a gearbox of MiniCIM motors. + * Calculates the angular velocity in Radians per Second produced by the motor + * at a given torque and input voltage. * - * @param numMotors Number of motors in the gearbox. - * @return A gearbox of MiniCIM motors. + * @param torqueNewtonMeters The torque produced by the motor. + * @param voltageInputVolts The voltage applied to the motor. + * @return The angular velocity of the motor. */ - public static DCMotor getMiniCIM(int numMotors) { - return new DCMotor( - 12, 1.41, 89, 3, Units.rotationsPerMinuteToRadiansPerSecond(5840), numMotors); + public double getAngularVelocityRadiansPerSecond(double torqueNewtonMeters, double voltageInputVolts) { + return voltageInputVolts * kv.baseUnitMagnitude() + - 1.0 / kt.baseUnitMagnitude() * torqueNewtonMeters * internalResistance.baseUnitMagnitude() + * kv.baseUnitMagnitude(); } /** - * Return a gearbox of Bag motors. + * Calculate torque in Newton-Meters produced by the motor at a given angular + * velocity and input voltage. * - * @param numMotors Number of motors in the gearbox. - * @return A gearbox of Bag motors. - */ - public static DCMotor getBag(int numMotors) { - return new DCMotor( - 12, 0.43, 53, 1.8, Units.rotationsPerMinuteToRadiansPerSecond(13180), numMotors); - } - - /** - * Return a gearbox of Andymark RS775-125 motors. - * - * @param numMotors Number of motors in the gearbox. - * @return A gearbox of Andymark RS775-125 motors. - */ - public static DCMotor getAndymarkRs775_125(int numMotors) { - return new DCMotor( - 12, 0.28, 18, 1.6, Units.rotationsPerMinuteToRadiansPerSecond(5800.0), numMotors); - } - - /** - * Return a gearbox of Banebots RS775 motors. - * - * @param numMotors Number of motors in the gearbox. - * @return A gearbox of Banebots RS775 motors. - */ - public static DCMotor getBanebotsRs775(int numMotors) { - return new DCMotor( - 12, 0.72, 97, 2.7, Units.rotationsPerMinuteToRadiansPerSecond(13050.0), numMotors); - } - - /** - * Return a gearbox of Andymark 9015 motors. - * - * @param numMotors Number of motors in the gearbox. - * @return A gearbox of Andymark 9015 motors. - */ - public static DCMotor getAndymark9015(int numMotors) { - return new DCMotor( - 12, 0.36, 71, 3.7, Units.rotationsPerMinuteToRadiansPerSecond(14270.0), numMotors); - } - - /** - * Return a gearbox of Banebots RS 550 motors. - * - * @param numMotors Number of motors in the gearbox. - * @return A gearbox of Banebots RS 550 motors. - */ - public static DCMotor getBanebotsRs550(int numMotors) { - return new DCMotor( - 12, 0.38, 84, 0.4, Units.rotationsPerMinuteToRadiansPerSecond(19000.0), numMotors); - } - - /** - * Return a gearbox of NEO 550 motors. - * - * @param numMotors Number of motors in the gearbox. - * @return A gearbox of NEO 550 motors. - */ - public static DCMotor getNeo550(int numMotors) { - return new DCMotor( - 12, 0.97, 100, 1.4, Units.rotationsPerMinuteToRadiansPerSecond(11000.0), numMotors); - } - - /** - * Return a gearbox of Falcon 500 motors. - * - * @param numMotors Number of motors in the gearbox. - * @return A gearbox of Falcon 500 motors. - */ - public static DCMotor getFalcon500(int numMotors) { - return new DCMotor( - 12, 4.69, 257, 1.5, Units.rotationsPerMinuteToRadiansPerSecond(6380.0), numMotors); - } - - /** - * Return a gearbox of Falcon 500 motors with FOC (Field-Oriented Control) enabled. - * - * @param numMotors Number of motors in the gearbox. - * @return A gearbox of Falcon 500 FOC enabled motors. - */ - public static DCMotor getFalcon500Foc(int numMotors) { - // https://store.ctr-electronics.com/falcon-500-powered-by-talon-fx/ - return new DCMotor( - 12, 5.84, 304, 1.5, Units.rotationsPerMinuteToRadiansPerSecond(6080.0), numMotors); - } - - /** - * Return a gearbox of Romi/TI_RSLK MAX motors. - * - * @param numMotors Number of motors in the gearbox. - * @return A gearbox of Romi/TI_RSLK MAX motors. - */ - public static DCMotor getRomiBuiltIn(int numMotors) { - // From https://www.pololu.com/product/1520/specs - return new DCMotor( - 4.5, 0.1765, 1.25, 0.13, Units.rotationsPerMinuteToRadiansPerSecond(150.0), numMotors); - } - - /** - * Return a gearbox of Kraken X60 brushless motors. - * - * @param numMotors Number of motors in the gearbox. - * @return a gearbox of Kraken X60 motors. - */ - public static DCMotor getKrakenX60(int numMotors) { - // From https://store.ctr-electronics.com/announcing-kraken-x60/ - return new DCMotor( - 12, 7.09, 366, 2, Units.rotationsPerMinuteToRadiansPerSecond(6000), numMotors); - } - - /** - * Return a gearbox of Kraken X60 brushless motors with FOC (Field-Oriented Control) enabled. - * - * @param numMotors Number of motors in the gearbox. - * @return A gearbox of Kraken X60 FOC enabled motors. + * @param angularVelocityRadiansPerSec The current angular velocity of the + * motor. + * @param voltageInputVolts The voltage applied to the motor. + * @return The torque output. */ - public static DCMotor getKrakenX60Foc(int numMotors) { - // From https://store.ctr-electronics.com/announcing-kraken-x60/ - return new DCMotor( - 12, 9.37, 483, 2, Units.rotationsPerMinuteToRadiansPerSecond(5800), numMotors); + public double getTorqueNewtonMeters(double angularVelocityRadiansPerSec, double voltageInputVolts) { + return kt.baseUnitMagnitude() * (voltageInputVolts - (angularVelocityRadiansPerSec / kv.baseUnitMagnitude())); } /** - * Return a gearbox of Neo Vortex brushless motors. + * Calculate current drawn in Amps by motor for a given torque. * - * @param numMotors Number of motors in the gearbox. - * @return a gearbox of Neo Vortex motors. + * @param torqueNewtonMeters The torque produced by the motor. + * @return The current drawn by the motor. */ - public static DCMotor getNeoVortex(int numMotors) { - // From https://www.revrobotics.com/next-generation-spark-neo/ - return new DCMotor( - 12, 3.60, 211, 3.6, Units.rotationsPerMinuteToRadiansPerSecond(6784), numMotors); + public double getCurrentAmps(double torqueNewtonMeters) { + return torqueNewtonMeters / kt.baseUnitMagnitude(); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/Gearbox.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/Gearbox.java new file mode 100644 index 00000000000..e36ad37c9f3 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/Gearbox.java @@ -0,0 +1,222 @@ +// 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. + +package edu.wpi.first.math.system.plant; + +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.NewtonMeters; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.Volts; + +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.MutAngularVelocity; +import edu.wpi.first.units.measure.MutCurrent; +import edu.wpi.first.units.measure.MutTorque; +import edu.wpi.first.units.measure.MutVoltage; +import edu.wpi.first.units.measure.Torque; +import edu.wpi.first.units.measure.Voltage; + +/** Models the behavior of a gearbox. */ +public class Gearbox { + public final DCMotor dcMotor; + public final int numMotors; + private double gearboxReduction; + + /** The returned measure of angular velocity. Exists to minimize calls to GC. */ + private final MutAngularVelocity angularVelocity = RadiansPerSecond.mutable(0.0); + /** The returned measure of input voltage. Exists to minimize calls to GC. */ + private final MutVoltage voltageInput = Volts.mutable(0.0); + /** The returned measure of torque. Exists to minimize calls to GC. */ + private final MutTorque torque = NewtonMeters.mutable(0.0); + /** The returned measure of current. Exists to minimize calls to GC. */ + private final MutCurrent current = Amps.mutable(0.0); + + /** + * Constructs a Gearbox. + * + * @param dcMotor The DC motor used in this gearbox. + * @param numMotors Number of identical motors in the gearbox. + */ + public Gearbox( + DCMotor dcMotor) { + this(dcMotor, 1, 1.0); + } + + /** + * Constructs a Gearbox. + * + * @param dcMotor The DC motor used in this gearbox. + * @param numMotors Number of identical motors in the gearbox. + */ + public Gearbox( + DCMotor dcMotor, + int numMotors) { + this(dcMotor, numMotors, 1.0); + } + + /** + * Constructs a Gearbox. + * + * @param dcMotor The DC motor used in this gearbox. + * @param gearboxReduction The gearbox reduction. + */ + public Gearbox( + DCMotor dcMotor, + double gearboxReduction) { + this(dcMotor, 1, gearboxReduction); + } + + /** + * Constructs a Gearbox. + * + * @param dcMotor The DC motor used in this gearbox. + * @param numMotors Number of identical motors in the gearbox. + * @param gearboxReduction The gearbox reduction. + * @throws IllegalArgumentException if numMotors < 1, gearboxReduction ≤ 0. + */ + public Gearbox( + DCMotor dcMotor, + int numMotors, + double gearboxReduction) { + if(numMotors < 1){ + throw new IllegalArgumentException("numMotors must be greater than or equal to 1."); + } + if (gearboxReduction <= 0) { + throw new IllegalArgumentException("gearboxReduction must be greater than zero."); + } + this.dcMotor = dcMotor; + this.numMotors = numMotors; + this.gearboxReduction = gearboxReduction; + } + + /** + * Returns the gearbox reduction as a ratio of output rotations to input rotations. + * @return The gearbox reduction. + */ + public double getGearboxReduction(){ + return gearboxReduction; + } + + /** + * Returns this gearbox with the given gearbox reduction applied. + * + * @param gearboxReduction The gearbox reduction. + * @return A motor with the gearbox reduction applied. + */ + public Gearbox withReduction(double gearboxReduction) { + this.gearboxReduction = gearboxReduction; + return this; + } + + /** + * Calculate the input voltage in Volts of each motor for a given torque and + * angular velocity. + * + * @param torqueNewtonMeters The torque produced by the gearbox. + * @param angularVelocityRadiansPerSec The current angular velocity of the + * gearbox's output. + * @return The voltage of each motor. + */ + public double getVoltageInputVolts(double torqueNewtonMeters, double angularVelocityRadiansPerSec) { + return dcMotor.getAngularVelocityRadiansPerSecond( + torqueNewtonMeters / gearboxReduction / numMotors, + angularVelocityRadiansPerSec * gearboxReduction); + } + + /** + * Calculate the input voltage of each motor for a given torque and + * angular velocity. + * + * @param torque The torque produced by the gearbox. + * @param angularVelocity The current angular velocity of the gearbox's output. + * @return The voltage of each motor. + */ + public Voltage getVoltageInput(Torque torque, AngularVelocity angularVelocity) { + return voltageInput + .mut_setMagnitude(getVoltageInputVolts(torque.baseUnitMagnitude(), angularVelocity.baseUnitMagnitude())); + } + + /** + * Calculates the angular velocity in Radians per Second produced by the gearbox + * at a given torque and input voltage. + * + * @param torqueNewtonMeters The torque produced by the gearbox. + * @param voltageInputVolts The voltage applied to each motor. + * @return The angular velocity of the gearbox's output. + */ + public double getAngularVelocityRadiansPerSecond(double torqueNewtonMeters, double voltageInputVolts) { + return dcMotor.getAngularVelocityRadiansPerSecond( + torqueNewtonMeters / gearboxReduction / numMotors, + voltageInputVolts) + / gearboxReduction; + } + + /** + * Calculates the angular velocity produced by the gearbox + * at a given torque and input voltage. + * + * @param torque The torque produced by the gearbox. + * @param voltageInput The voltage applied to each motor. + * @return The angular velocity of the gearbox's output. + */ + public AngularVelocity getAngularVelocity(Torque torque, Voltage voltageInput) { + return angularVelocity.mut_setMagnitude( + getAngularVelocityRadiansPerSecond( + torque.baseUnitMagnitude(), + voltageInput.baseUnitMagnitude())); + } + + /** + * Calculate torque in Newton-Meters produced by the gearbox at a given angular + * velocity and input voltage. + * + * @param angularVelocityRadiansPerSec The current angular velocity of the + * gearbox's output. + * @param voltageInputVolts The voltage applied to each motor. + * @return The torque output of the gearbox. + */ + public double getTorqueNewtonMeters(double angularVelocityRadiansPerSec, double voltageInputVolts) { + return numMotors * gearboxReduction * dcMotor.getTorqueNewtonMeters( + angularVelocityRadiansPerSec * gearboxReduction, + voltageInputVolts); + } + + /** + * Calculate torque in produced by the gearbox at a given angular + * velocity and input voltage. + * + * @param angularVelocity The current angular velocity of the + * gearbox's output. + * @param voltageInput The voltage applied to each motor. + * @return The torque output of the gearbox. + */ + public Torque getTorque(AngularVelocity angularVelocity, Voltage voltageInput) { + return torque + .mut_setMagnitude( + getTorqueNewtonMeters( + angularVelocity.baseUnitMagnitude(), + voltageInput.baseUnitMagnitude())); + } + + /** + * Calculate net current drawn in Amps by all of the motors for a given torque. + * + * @param torqueNewtonMeters The torque produced by the gearbox. + * @return The net current drawn by the all of the motors. + */ + public double getCurrentAmps(double torqueNewtonMeters) { + return numMotors * dcMotor.getCurrentAmps(torqueNewtonMeters / numMotors / gearboxReduction); + } + + /** + * Calculate net current drawn by all of the motors for a given torque. + * + * @param torqueNewtonMeters The torque produced by the gearbox. + * @return The net current drawn by the all of the motors. + */ + public Current getCurrent(Torque torque) { + return current.mut_setBaseUnitMagnitude(getCurrentAmps(torque.baseUnitMagnitude())); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/KnownDCMotor.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/KnownDCMotor.java new file mode 100644 index 00000000000..9a034e1a89a --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/KnownDCMotor.java @@ -0,0 +1,87 @@ +package edu.wpi.first.math.system.plant; + +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.NewtonMeters; +import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.Volts; + +/** Holds the constants for known DC motors. */ +public enum KnownDCMotor { + /** TODO: CIM DOCUMENTATION */ + CIM(12, 2.42, 133, 2.7, 5310), + /** TODO: 775Pro DOCUMENTATION */ + Vex775Pro(12, 0.71, 134, 0.7, 18730), + /** TODO: NEO DOCUMENTATION */ + NEO(12, 2.6, 105, 1.8, 5676), + /** TODO: MiniCIM DOCUMENTATION */ + MiniCIM(12, 1.41, 89, 3, 5840), + /** TODO: Bag DOCUMENTATION */ + Bag(12, 0.43, 53, 1.8, 13180), + /** TODO: Andymark RS775-125 DOCUMENTATION */ + AndymarkRs775_125(12, 0.28, 18, 1.6, 5800), + /** TODO: Banebots RS775 DOOCUMENTATION */ + BanebotsRs775(12, 0.72, 97, 2.7, 13050), + /** TODO: Andymark 9015 DOCUMENTATION */ + Andymark9015(12, 0.36, 71, 3.7, 14270), + /** TODO: Banebots RS 550 DOCUMENTATION */ + BanebotsRS550(12, 0.38, 84, 0.4, 19000), + /** TODO: Neo 550 DOCUMENTATION */ + Neo550(12, 0.97, 100, 1.4, 11000), + + /** + * @see Falcon 500 Documentation + */ + Falcon500(12, 4.69, 257, 1.5, 6380), + + /** + * @see Falcon + * 500 FOC (Field-Oriented Control) Documentation + */ + Falcon500Foc(12, 5.84, 304, 1.5, 6080), + + /** + * @see Romi Built-In + * Documentation + */ + RomiBuiltIn(4.5, 0.1765, 1.25, 0.13, 150), + + /** + * @see Kraken X60 + * Documentation + */ + KrakenX60(12, 7.09, 366, 2, 6000), + + /** + * @see Kraken X60 + * FOC (Field-Oriented Control) Documentation + */ + KrakenX60Foc(12, 9.37, 483, 2, 5800), + + /** + * @see NEO + * Vortex Documentation + */ + NeoVortex(12, 3.60, 211, 3.6, 6784); + + /** DCMotor object for this known type. */ + public final DCMotor dcMotor; + + private KnownDCMotor( + double nominalVoltageVolts, + double stallTorqueNewtonMeters, + double stallCurrentAmps, + double freeCurrentAmps, + double freeSpeedRPM) { + dcMotor = new DCMotor( + Volts.of(nominalVoltageVolts), + NewtonMeters.of(stallTorqueNewtonMeters), + Amps.of(stallCurrentAmps), + Amps.of(freeCurrentAmps), + RPM.of(freeSpeedRPM)); + + } +} 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 ac48939ef04..487ca7a3c3e 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 @@ -19,164 +19,117 @@ private LinearSystemId() { } /** - * Create a state-space model of an elevator system. The states of the system are [position, + * 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 carriage. - * @param massKg The mass of the elevator carriage, in kilograms. + * @param gearbox The gearbox attached to the elevator's driving drum. + * @param massKg The mass of the elevator carriage, in kilograms. * @param radiusMeters The radius of the elevator's driving drum, in meters. - * @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 gearing <= 0. + * @throws IllegalArgumentException if massKg ≤ 0, radiusMeters ≤ 0. */ public static LinearSystem createElevatorSystem( - DCMotor motor, double massKg, double radiusMeters, double gearing) { + Gearbox gearbox, double massKg, double radiusMeters) { 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 (gearing <= 0) { - throw new IllegalArgumentException("gearing must be greater than zero."); - } - return new LinearSystem<>( - MatBuilder.fill( - Nat.N2(), - Nat.N2(), - 0, - 1, - 0, - -Math.pow(gearing, 2) - * motor.KtNMPerAmp - / (motor.rOhms * radiusMeters * radiusMeters * massKg * motor.KvRadPerSecPerVolt)), - VecBuilder.fill(0, gearing * motor.KtNMPerAmp / (motor.rOhms * radiusMeters * massKg)), - Matrix.eye(Nat.N2()), - new Matrix<>(Nat.N2(), Nat.N1())); + double n = gearbox.numMotors; + double G = gearbox.getGearboxReduction(); + double ktNMPerAmp = gearbox.dcMotor.kt.baseUnitMagnitude(); + double kvRadPerSecPerVolt = gearbox.dcMotor.kv.baseUnitMagnitude(); + double rOhms = gearbox.dcMotor.internalResistance.baseUnitMagnitude(); + + double kvVoltsPerMetersPerSecond = G / (radiusMeters * kvRadPerSecPerVolt); + double kaVoltsPerMetersPerSecondSquared = (rOhms * radiusMeters * massKg) / (n * G * ktNMPerAmp); + + return identifyPositionSystem(kvVoltsPerMetersPerSecond, kaVoltsPerMetersPerSecondSquared); } /** - * Create a state-space model of a flywheel system. The states of the system are [angular + * 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]. * - * @param motor The motor (or gearbox) attached to the flywheel. + * @param gearbox The gearbox attached to the flywheel. * @param JKgMetersSquared The moment of inertia J of the flywheel. - * @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 gearing <= 0. + * @throws IllegalArgumentException if JKgMetersSquared ≤ 0. */ public static LinearSystem createFlywheelSystem( - DCMotor motor, double JKgMetersSquared, double gearing) { + Gearbox gearbox, double JKgMetersSquared) { if (JKgMetersSquared <= 0.0) { throw new IllegalArgumentException("J must be greater than zero."); } - if (gearing <= 0.0) { - throw new IllegalArgumentException("gearing must be greater than zero."); - } - return new LinearSystem<>( - VecBuilder.fill( - -gearing - * gearing - * motor.KtNMPerAmp - / (motor.KvRadPerSecPerVolt * motor.rOhms * JKgMetersSquared)), - VecBuilder.fill(gearing * motor.KtNMPerAmp / (motor.rOhms * JKgMetersSquared)), - Matrix.eye(Nat.N1()), - new Matrix<>(Nat.N1(), Nat.N1())); + double n = gearbox.numMotors; + double G = gearbox.getGearboxReduction(); + double ktNMPerAmp = gearbox.dcMotor.kt.baseUnitMagnitude(); + double kvRadPerSecPerVolt = gearbox.dcMotor.kv.baseUnitMagnitude(); + double rOhms = gearbox.dcMotor.internalResistance.baseUnitMagnitude(); + + double kvVoltsPerRadiansPerSecond = G / (kvRadPerSecPerVolt); + double kaVoltsPerRadiansPerSecondSquared = (rOhms * JKgMetersSquared) / (n * G * ktNMPerAmp); + + return identifyVelocitySystem(kvVoltsPerRadiansPerSecond, kaVoltsPerRadiansPerSecondSquared); } - /** - * 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]. + /** + * Create a state-space model of a gearbox system. The states of the system are + * [angular + * velocity], inputs are [voltage], and outputs are [angular velocity]. * - * @param motor The motor (or gearbox) attached to system. - * @param JKgMetersSquared The moment of inertia J of the DC motor. - * @param gearing The reduction between motor and drum, as a ratio of output to input. + * @param gearbox The gearbox of the system. + * @param JKgMetersSquared The moment of inertia J of the gearbox. * @return A LinearSystem representing the given characterized constants. - * @throws IllegalArgumentException if JKgMetersSquared <= 0 or gearing <= 0. + * @throws IllegalArgumentException if JKgMetersSquared ≤ 0. */ - public static LinearSystem createDCMotorSystem( - DCMotor motor, double JKgMetersSquared, double gearing) { + public static LinearSystem createGearboxSystem( + Gearbox gearbox, double JKgMetersSquared) { if (JKgMetersSquared <= 0.0) { throw new IllegalArgumentException("J must be greater than zero."); } - if (gearing <= 0.0) { - throw new IllegalArgumentException("gearing must be greater than zero."); - } - return new LinearSystem<>( - MatBuilder.fill( - Nat.N2(), - Nat.N2(), - 0, - 1, - 0, - -gearing - * gearing - * motor.KtNMPerAmp - / (motor.KvRadPerSecPerVolt * motor.rOhms * JKgMetersSquared)), - VecBuilder.fill(0, gearing * motor.KtNMPerAmp / (motor.rOhms * JKgMetersSquared)), - Matrix.eye(Nat.N2()), - new Matrix<>(Nat.N2(), Nat.N1())); - } + double n = gearbox.numMotors; + double G = gearbox.getGearboxReduction(); + double ktNMPerAmp = gearbox.dcMotor.kt.baseUnitMagnitude(); + double kvRadPerSecPerVolt = gearbox.dcMotor.kv.baseUnitMagnitude(); + double rOhms = gearbox.dcMotor.internalResistance.baseUnitMagnitude(); - /** - * 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. - * @see https://github.com/wpilibsuite/sysid - */ - public static LinearSystem createDCMotorSystem(double kV, double kA) { - if (kV < 0.0) { - throw new IllegalArgumentException("Kv must be greater than or equal to zero."); - } - if (kA <= 0.0) { - throw new IllegalArgumentException("Ka must be greater than zero."); - } + double kvVoltsPerRadiansPerSecond = G / (kvRadPerSecPerVolt); + double kaVoltsPerRadiansPerSecondSquared = (rOhms * JKgMetersSquared) / (n * G * ktNMPerAmp); - 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())); + return identifyPositionSystem(kvVoltsPerRadiansPerSecond, kaVoltsPerRadiansPerSecondSquared); } /** - * 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 + * 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 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 gearbox The 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 gearing 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 gearing - * <= 0. + * @throws IllegalArgumentException if m ≤ 0, r ≤ 0, rb ≤ 0, or J ≤ + * 0. */ public static LinearSystem createDrivetrainVelocitySystem( - DCMotor motor, + Gearbox gearbox, double massKg, double rMeters, double rbMeters, - double JKgMetersSquared, - double gearing) { + double JKgMetersSquared) { if (massKg <= 0.0) { throw new IllegalArgumentException("massKg must be greater than zero."); } @@ -189,15 +142,15 @@ public static LinearSystem createDrivetrainVelocitySystem( if (JKgMetersSquared <= 0.0) { throw new IllegalArgumentException("JKgMetersSquared must be greater than zero."); } - if (gearing <= 0.0) { - throw new IllegalArgumentException("gearing must be greater than zero."); - } - var C1 = - -(gearing * gearing) - * motor.KtNMPerAmp - / (motor.KvRadPerSecPerVolt * motor.rOhms * rMeters * rMeters); - var C2 = gearing * motor.KtNMPerAmp / (motor.rOhms * rMeters); + double n = gearbox.numMotors; + double G = gearbox.getGearboxReduction(); + double ktNMPerAmp = gearbox.dcMotor.kt.baseUnitMagnitude(); + double kvRadPerSecPerVolt = gearbox.dcMotor.kv.baseUnitMagnitude(); + double rOhms = gearbox.dcMotor.internalResistance.baseUnitMagnitude(); + + var C1 = -n * Math.pow(G, 2) * ktNMPerAmp / (kvRadPerSecPerVolt * rOhms * Math.pow(rMeters, 2)); + var C2 = G * ktNMPerAmp / (rOhms * rMeters); final double C3 = 1 / massKg + rbMeters * rbMeters / JKgMetersSquared; final double C4 = 1 / massKg - rbMeters * rbMeters / JKgMetersSquared; @@ -210,56 +163,58 @@ public static LinearSystem createDrivetrainVelocitySystem( } /** - * Create a state-space model of a single jointed arm system. The states of the system are [angle, + * 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]. * - * @param motor The motor (or gearbox) attached to the arm. - * @param JKgSquaredMeters The moment of inertia J of the arm. - * @param gearing The gearing between the motor and arm, in output over input. Most of the time - * this will be greater than 1. + * @param gearbox The gearbox attached to the arm. + * @param JKgMetersSquared The moment of inertia J of the arm. * @return A LinearSystem representing the given characterized constants. + * @throws IllegalArgumentException if JKgMetersSquared ≤ 0. */ public static LinearSystem createSingleJointedArmSystem( - DCMotor motor, double JKgSquaredMeters, double gearing) { - if (JKgSquaredMeters <= 0.0) { + Gearbox gearbox, double JKgMetersSquared) { + if (JKgMetersSquared <= 0.0) { throw new IllegalArgumentException("JKgSquaredMeters must be greater than zero."); } - if (gearing <= 0.0) { - throw new IllegalArgumentException("gearing must be greater than zero."); - } - return new LinearSystem<>( - MatBuilder.fill( - Nat.N2(), - Nat.N2(), - 0, - 1, - 0, - -Math.pow(gearing, 2) - * motor.KtNMPerAmp - / (motor.KvRadPerSecPerVolt * motor.rOhms * JKgSquaredMeters)), - VecBuilder.fill(0, gearing * motor.KtNMPerAmp / (motor.rOhms * JKgSquaredMeters)), - Matrix.eye(Nat.N2()), - new Matrix<>(Nat.N2(), Nat.N1())); + double n = gearbox.numMotors; + double G = gearbox.getGearboxReduction(); + double ktNMPerAmp = gearbox.dcMotor.kt.baseUnitMagnitude(); + double kvRadPerSecPerVolt = gearbox.dcMotor.kv.baseUnitMagnitude(); + double rOhms = gearbox.dcMotor.internalResistance.baseUnitMagnitude(); + + double kvVoltsPerRadiansPerSecond = G / (kvRadPerSecPerVolt); + double kaVoltsPerRadiansPerSecondSquared = (rOhms * JKgMetersSquared) / (n * G * ktNMPerAmp); + + return identifyPositionSystem(kvVoltsPerRadiansPerSecond, kaVoltsPerRadiansPerSecondSquared); } /** - * 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 + * 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. + *

+ * 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: + *

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

u = K_v v + K_a a + *

+ * 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 + * @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) { @@ -277,22 +232,30 @@ public static LinearSystem identifyVelocitySystem(double kV, double } /** - * 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 + * 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. + *

+ * 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: + *

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

u = K_v v + K_a a + *

+ * 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 + * @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) { @@ -310,22 +273,28 @@ public static LinearSystem identifyPositionSystem(double kV, double } /** - * Identify a differential drive drivetrain given the drivetrain's kV and kA in both linear + * 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)), * (volts/(radian/sec²))} cases. These constants can be found using SysId. * - *

States: [[left velocity], [right velocity]]
+ *

+ * States: [[left velocity], [right velocity]]
* Inputs: [[left voltage], [right voltage]]
* Outputs: [[left velocity], [right velocity]] * - * @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 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 (meters per second). - * @param kAAngular The angular acceleration gain in volts per (meters per second squared). + * @param kAAngular The angular acceleration gain in volts per (meters per + * second squared). * @return A LinearSystem representing the given characterized constants. - * @throws IllegalArgumentException if kVLinear <= 0, kALinear <= 0, kVAngular <= 0, or - * kAAngular <= 0. - * @see https://github.com/wpilibsuite/sysid + * @throws IllegalArgumentException if kVLinear <= 0, kALinear <= 0, + * kVAngular <= 0, or + * kAAngular <= 0. + * @see https://github.com/wpilibsuite/sysid */ public static LinearSystem identifyDrivetrainSystem( double kVLinear, double kALinear, double kVAngular, double kAAngular) { @@ -355,24 +324,33 @@ public static LinearSystem identifyDrivetrainSystem( } /** - * 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)), + * 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)), * (volts/(radian/sec²))} cases. This can be found using SysId. * - *

States: [[left velocity], [right velocity]]
+ *

+ * States: [[left velocity], [right velocity]]
* Inputs: [[left voltage], [right voltage]]
* Outputs: [[left velocity], [right velocity]] * - * @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. + * @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, - * kAAngular <= 0, or trackwidth <= 0. - * @see https://github.com/wpilibsuite/sysid + * @throws IllegalArgumentException if kVLinear <= 0, kALinear <= 0, + * kVAngular <= 0, + * kAAngular <= 0, or trackwidth <= 0. + * @see https://github.com/wpilibsuite/sysid */ public static LinearSystem identifyDrivetrainSystem( double kVLinear, double kALinear, double kVAngular, double kAAngular, double trackwidth) { diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/proto/DCMotorProto.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/proto/DCMotorProto.java index f9e4aa39576..6993a188412 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/plant/proto/DCMotorProto.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/proto/DCMotorProto.java @@ -4,6 +4,11 @@ package edu.wpi.first.math.system.plant.proto; +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.NewtonMeters; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.Volts; + import edu.wpi.first.math.proto.Plant.ProtobufDCMotor; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.util.protobuf.Protobuf; @@ -32,16 +37,15 @@ public DCMotor unpack(ProtobufDCMotor msg) { msg.getStallTorque(), msg.getStallCurrent(), msg.getFreeCurrent(), - msg.getFreeSpeed(), - 1); + msg.getFreeSpeed()); } @Override public void pack(ProtobufDCMotor msg, DCMotor value) { - msg.setNominalVoltage(value.nominalVoltageVolts); - msg.setStallTorque(value.stallTorqueNewtonMeters); - msg.setStallCurrent(value.stallCurrentAmps); - msg.setFreeCurrent(value.freeCurrentAmps); - msg.setFreeSpeed(value.freeSpeedRadPerSec); + msg.setNominalVoltage(value.nominalVoltage.in(Volts)); + msg.setStallTorque(value.stallTorque.in(NewtonMeters)); + msg.setStallCurrent(value.stallCurrent.in(Amps)); + msg.setFreeCurrent(value.freeCurrent.in(Amps)); + msg.setFreeSpeed(value.freeSpeed.in(RadiansPerSecond)); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/struct/DCMotorStruct.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/struct/DCMotorStruct.java index a55264b966a..f743dacbce6 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/plant/struct/DCMotorStruct.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/struct/DCMotorStruct.java @@ -6,6 +6,12 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.util.struct.Struct; + +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.NewtonMeters; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.Volts; + import java.nio.ByteBuffer; public class DCMotorStruct implements Struct { @@ -37,15 +43,15 @@ public DCMotor unpack(ByteBuffer bb) { double stallCurrent = bb.getDouble(); double freeCurrent = bb.getDouble(); double freeSpeed = bb.getDouble(); - return new DCMotor(nominalVoltage, stallTorque, stallCurrent, freeCurrent, freeSpeed, 1); + return new DCMotor(nominalVoltage, stallTorque, stallCurrent, freeCurrent, freeSpeed); } @Override public void pack(ByteBuffer bb, DCMotor value) { - bb.putDouble(value.nominalVoltageVolts); - bb.putDouble(value.stallTorqueNewtonMeters); - bb.putDouble(value.stallCurrentAmps); - bb.putDouble(value.freeCurrentAmps); - bb.putDouble(value.freeSpeedRadPerSec); + bb.putDouble(value.nominalVoltage.in(Volts)); + bb.putDouble(value.stallTorque.in(NewtonMeters)); + bb.putDouble(value.stallCurrent.in(Amps)); + bb.putDouble(value.freeCurrent.in(Amps)); + bb.putDouble(value.freeSpeed.in(RadiansPerSecond)); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java index 48278ced6e3..c60f6e2b4d9 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java @@ -12,20 +12,22 @@ import edu.wpi.first.math.Num; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.system.Discretization; -import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.Gearbox; +import edu.wpi.first.math.system.plant.KnownDCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import org.junit.jupiter.api.Test; class LinearQuadraticRegulatorTest { @Test void testLQROnElevator() { - var motors = DCMotor.getVex775Pro(2); + var motor = KnownDCMotor.Vex775Pro.dcMotor; + var n = 2; + var gearbox = new Gearbox(motor, n); var m = 5.0; var r = 0.0181864; - var G = 1.0; - var plant = LinearSystemId.createElevatorSystem(motors, m, r, G); + var plant = LinearSystemId.createElevatorSystem(gearbox, m, r); var qElms = VecBuilder.fill(0.02, 0.4); var rElms = VecBuilder.fill(12.0); @@ -41,13 +43,16 @@ void testLQROnElevator() { void testFourMotorElevator() { var dt = 0.020; - var plant = - LinearSystemId.createElevatorSystem( - DCMotor.getVex775Pro(4), 8.0, 0.75 * 25.4 / 1000.0, 14.67); + var motor = KnownDCMotor.Vex775Pro.dcMotor; + var n = 4; + var G = 14.67; + var gearbox = new Gearbox(motor, n, G); - var K = - new LinearQuadraticRegulator<>(plant, VecBuilder.fill(0.1, 0.2), VecBuilder.fill(12.0), dt) - .getK(); + var plant = LinearSystemId.createElevatorSystem( + gearbox, 8.0, 0.75 * 25.4 / 1000.0); + + var K = new LinearQuadraticRegulator<>(plant, VecBuilder.fill(0.1, 0.2), VecBuilder.fill(12.0), dt) + .getK(); assertEquals(10.381, K.get(0, 0), 1e-2); assertEquals(0.6929, K.get(0, 1), 1e-2); @@ -55,13 +60,15 @@ void testFourMotorElevator() { @Test void testLQROnArm() { - var motors = DCMotor.getVex775Pro(2); + var motor = KnownDCMotor.Vex775Pro.dcMotor; + var n = 2; + var G = 100.0; + var gearbox = new Gearbox(motor, n, G); var m = 4.0; var r = 0.4; - var G = 100.0; - var plant = LinearSystemId.createSingleJointedArmSystem(motors, 1d / 3d * m * r * r, G); + var plant = LinearSystemId.createSingleJointedArmSystem(gearbox, 1d / 3d * m * r * r); var qElms = VecBuilder.fill(0.01745, 0.08726); var rElms = VecBuilder.fill(12.0); @@ -76,15 +83,16 @@ void testLQROnArm() { /** * Returns feedback control gain for implicit model following. * - *

This is used to test the QRN overload of LQR. + *

+ * This is used to test the QRN overload of LQR. * - * @param Number of states. - * @param Number of inputs. - * @param A State matrix. - * @param B Input matrix. - * @param Q State cost matrix. - * @param R Input cost matrix. - * @param Aref Desired state matrix. + * @param Number of states. + * @param Number of inputs. + * @param A State matrix. + * @param B Input matrix. + * @param Q State cost matrix. + * @param R Input cost matrix. + * @param Aref Desired state matrix. * @param dtSeconds Discretization timestep in seconds. */ Matrix getImplicitModelFollowingK( @@ -102,8 +110,7 @@ Matrix getImplicitModel // Discretize desired dynamics var discAref = Discretization.discretizeA(Aref, dtSeconds); - Matrix Qimf = - (discA.minus(discAref)).transpose().times(Q).times(discA.minus(discAref)); + Matrix Qimf = (discA.minus(discAref)).transpose().times(Q).times(discA.minus(discAref)); Matrix Rimf = discB.transpose().times(Q).times(discB).plus(R); Matrix Nimf = (discA.minus(discAref)).transpose().times(Q).times(discB); @@ -159,12 +166,15 @@ void testMatrixOverloadsWithDoubleIntegrator() { void testLatencyCompensate() { var dt = 0.02; - var plant = - LinearSystemId.createElevatorSystem( - DCMotor.getVex775Pro(4), 8.0, 0.75 * 25.4 / 1000.0, 14.67); + var motor = KnownDCMotor.Vex775Pro.dcMotor; + var n = 4; + var G = 14.67; + var gearbox = new Gearbox(motor, n, G); + + var plant = LinearSystemId.createElevatorSystem( + gearbox, 8.0, 0.75 * 25.4 / 1000.0); - var regulator = - new LinearQuadraticRegulator<>(plant, VecBuilder.fill(0.1, 0.2), VecBuilder.fill(12.0), dt); + var regulator = new LinearQuadraticRegulator<>(plant, VecBuilder.fill(0.1, 0.2), VecBuilder.fill(12.0), dt); regulator.latencyCompensate(plant, dt, 0.01); diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java index a34b8b68a88..8057cc4801f 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java @@ -16,6 +16,8 @@ import edu.wpi.first.math.system.LinearSystem; import edu.wpi.first.math.system.LinearSystemLoop; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.Gearbox; +import edu.wpi.first.math.system.plant.KnownDCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.math.trajectory.TrapezoidProfile; import java.util.Random; @@ -26,31 +28,31 @@ class LinearSystemLoopTest { private static final double kPositionStddev = 0.0001; private static final Random random = new Random(); - LinearSystem m_plant = - LinearSystemId.createElevatorSystem(DCMotor.getVex775Pro(2), 5, 0.0181864, 1.0); + DCMotor motor = KnownDCMotor.Vex775Pro.dcMotor; + int numMotors = 2; + Gearbox gearbox = new Gearbox(motor, numMotors); + + LinearSystem m_plant = LinearSystemId.createElevatorSystem(gearbox, 5, 0.0181864); @SuppressWarnings("unchecked") - KalmanFilter m_observer = - new KalmanFilter<>( - Nat.N2(), - Nat.N1(), - (LinearSystem) m_plant.slice(0), - VecBuilder.fill(0.05, 1.0), - VecBuilder.fill(0.0001), - kDt); + KalmanFilter m_observer = new KalmanFilter<>( + Nat.N2(), + Nat.N1(), + (LinearSystem) m_plant.slice(0), + VecBuilder.fill(0.05, 1.0), + VecBuilder.fill(0.0001), + kDt); @SuppressWarnings("unchecked") - LinearQuadraticRegulator m_controller = - new LinearQuadraticRegulator<>( - (LinearSystem) m_plant.slice(0), - VecBuilder.fill(0.02, 0.4), - VecBuilder.fill(12.0), - 0.00505); + LinearQuadraticRegulator m_controller = new LinearQuadraticRegulator<>( + (LinearSystem) m_plant.slice(0), + VecBuilder.fill(0.02, 0.4), + VecBuilder.fill(12.0), + 0.00505); @SuppressWarnings("unchecked") - private final LinearSystemLoop m_loop = - new LinearSystemLoop<>( - (LinearSystem) m_plant.slice(0), m_controller, m_observer, 12, 0.00505); + private final LinearSystemLoop m_loop = new LinearSystemLoop<>( + (LinearSystem) m_plant.slice(0), m_controller, m_observer, 12, 0.00505); private static void updateTwoState( LinearSystem plant, LinearSystemLoop loop, double noise) { @@ -72,11 +74,10 @@ void testStateSpaceEnabled() { TrapezoidProfile.State state; for (int i = 0; i < 1000; i++) { profile = new TrapezoidProfile(constraints); - state = - profile.calculate( - kDt, - new TrapezoidProfile.State(m_loop.getXHat(0), m_loop.getXHat(1)), - new TrapezoidProfile.State(references.get(0, 0), references.get(1, 0))); + state = profile.calculate( + kDt, + new TrapezoidProfile.State(m_loop.getXHat(0), m_loop.getXHat(1)), + new TrapezoidProfile.State(references.get(0, 0), references.get(1, 0))); m_loop.setNextR(VecBuilder.fill(state.position, state.velocity)); updateTwoState( @@ -94,11 +95,12 @@ void testStateSpaceEnabled() { @Test void testFlywheelEnabled() { - LinearSystem plant = - LinearSystemId.createFlywheelSystem(DCMotor.getNEO(2), 0.00289, 1.0); - KalmanFilter observer = - new KalmanFilter<>( - Nat.N1(), Nat.N1(), plant, VecBuilder.fill(1.0), VecBuilder.fill(kPositionStddev), kDt); + DCMotor motor = KnownDCMotor.NEO.dcMotor; + int numMotors = 2; + Gearbox gearbox = new Gearbox(motor, numMotors); + LinearSystem plant = LinearSystemId.createFlywheelSystem(gearbox, 0.00289); + KalmanFilter observer = new KalmanFilter<>( + Nat.N1(), Nat.N1(), plant, VecBuilder.fill(1.0), VecBuilder.fill(kPositionStddev), kDt); var qElms = VecBuilder.fill(9.0); var rElms = VecBuilder.fill(12.0); @@ -122,10 +124,9 @@ void testFlywheelEnabled() { loop.setNextR(references); - Matrix y = - plant - .calculateY(loop.getXHat(), loop.getU()) - .plus(VecBuilder.fill(random.nextGaussian() * kPositionStddev)); + Matrix y = plant + .calculateY(loop.getXHat(), loop.getU()) + .plus(VecBuilder.fill(random.nextGaussian() * kPositionStddev)); loop.correct(y); loop.predict(kDt); diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java index a1b5b3a1cf0..2e1f5822a9c 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java @@ -20,6 +20,7 @@ import edu.wpi.first.math.system.NumericalIntegration; import edu.wpi.first.math.system.NumericalJacobian; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.KnownDCMotor; import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; import java.util.List; @@ -27,17 +28,17 @@ class ExtendedKalmanFilterTest { private static Matrix getDynamics(final Matrix x, final Matrix u) { - final var motors = DCMotor.getCIM(2); - + final DCMotor motor = KnownDCMotor.CIM.dcMotor; + final int n = 2; final var gr = 7.08; // Gear ratio + final var rb = 0.8382 / 2.0; // Wheelbase radius (track width) final var r = 0.0746125; // Wheel radius final var m = 63.503; // Robot mass final var J = 5.6; // Robot moment of inertia - final var C1 = - -Math.pow(gr, 2) * motors.KtNMPerAmp / (motors.KvRadPerSecPerVolt * motors.rOhms * r * r); - final var C2 = gr * motors.KtNMPerAmp / (motors.rOhms * r); + final var C1 = -n * Math.pow(gr, 2) * motor.kt.baseUnitMagnitude() / (motor.kv.baseUnitMagnitude() * motor.internalResistance.baseUnitMagnitude() * r * r); + final var C2 = n * gr * motor.kt.baseUnitMagnitude() / (motor.internalResistance.baseUnitMagnitude() * r); final var k1 = 1.0 / m + rb * rb / J; final var k2 = 1.0 / m - rb * rb / J; @@ -72,16 +73,15 @@ void testInit() { assertDoesNotThrow( () -> { - ExtendedKalmanFilter observer = - new ExtendedKalmanFilter<>( - Nat.N5(), - Nat.N2(), - Nat.N3(), - ExtendedKalmanFilterTest::getDynamics, - ExtendedKalmanFilterTest::getLocalMeasurementModel, - VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0), - VecBuilder.fill(0.0001, 0.01, 0.01), - dtSeconds); + ExtendedKalmanFilter observer = new ExtendedKalmanFilter<>( + Nat.N5(), + Nat.N2(), + Nat.N3(), + ExtendedKalmanFilterTest::getDynamics, + ExtendedKalmanFilterTest::getLocalMeasurementModel, + VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0), + VecBuilder.fill(0.0001, 0.01, 0.01), + dtSeconds); Matrix u = VecBuilder.fill(12.0, 12.0); observer.predict(u, dtSeconds); @@ -101,36 +101,32 @@ void testConvergence() { double dtSeconds = 0.00505; double rbMeters = 0.8382 / 2.0; // Robot radius - ExtendedKalmanFilter observer = - new ExtendedKalmanFilter<>( - Nat.N5(), - Nat.N2(), - Nat.N3(), - ExtendedKalmanFilterTest::getDynamics, - ExtendedKalmanFilterTest::getLocalMeasurementModel, - VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0), - VecBuilder.fill(0.001, 0.01, 0.01), - dtSeconds); - - List waypoints = - List.of( - new Pose2d(2.75, 22.521, Rotation2d.kZero), - new Pose2d(24.73, 19.68, Rotation2d.fromDegrees(5.846))); - var trajectory = - TrajectoryGenerator.generateTrajectory(waypoints, new TrajectoryConfig(8.8, 0.1)); + ExtendedKalmanFilter observer = new ExtendedKalmanFilter<>( + Nat.N5(), + Nat.N2(), + Nat.N3(), + ExtendedKalmanFilterTest::getDynamics, + ExtendedKalmanFilterTest::getLocalMeasurementModel, + VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0), + VecBuilder.fill(0.001, 0.01, 0.01), + dtSeconds); + + List waypoints = List.of( + new Pose2d(2.75, 22.521, Rotation2d.kZero), + new Pose2d(24.73, 19.68, Rotation2d.fromDegrees(5.846))); + var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, new TrajectoryConfig(8.8, 0.1)); Matrix r = new Matrix<>(Nat.N5(), Nat.N1()); Matrix nextR = new Matrix<>(Nat.N5(), Nat.N1()); Matrix u = new Matrix<>(Nat.N2(), Nat.N1()); - var B = - NumericalJacobian.numericalJacobianU( - Nat.N5(), - Nat.N2(), - ExtendedKalmanFilterTest::getDynamics, - new Matrix<>(Nat.N5(), Nat.N1()), - u); + var B = NumericalJacobian.numericalJacobianU( + Nat.N5(), + Nat.N2(), + ExtendedKalmanFilterTest::getDynamics, + new Matrix<>(Nat.N5(), Nat.N1()), + u); observer.setXhat( VecBuilder.fill( @@ -163,9 +159,8 @@ void testConvergence() { observer.predict(u, dtSeconds); - groundTruthX = - NumericalIntegration.rk4( - ExtendedKalmanFilterTest::getDynamics, groundTruthX, u, dtSeconds); + groundTruthX = NumericalIntegration.rk4( + ExtendedKalmanFilterTest::getDynamics, groundTruthX, u, dtSeconds); r = nextR; } diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java index 42fbd92db0a..ba8a6471a34 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java @@ -18,7 +18,8 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.numbers.N6; import edu.wpi.first.math.system.LinearSystem; -import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.Gearbox; +import edu.wpi.first.math.system.plant.KnownDCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; @@ -36,12 +37,14 @@ class KalmanFilterTest { } private static void createElevator() { - var motors = DCMotor.getVex775Pro(2); + var motor = KnownDCMotor.Vex775Pro.dcMotor; + var n = 2; + + var gearbox = new Gearbox(motor, n); var m = 5.0; var r = 0.0181864; - var G = 1.0; - elevatorPlant = LinearSystemId.createElevatorSystem(motors, m, r, G); + elevatorPlant = LinearSystemId.createElevatorSystem(gearbox, m, r); } // A swerve drive system where the states are [x, y, theta, vx, vy, vTheta]ᵀ, diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java index c5a6cd11a5b..b6a4f1d4eb1 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java @@ -22,7 +22,8 @@ import edu.wpi.first.math.system.Discretization; import edu.wpi.first.math.system.NumericalIntegration; import edu.wpi.first.math.system.NumericalJacobian; -import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.Gearbox; +import edu.wpi.first.math.system.plant.KnownDCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; @@ -31,20 +32,21 @@ class UnscentedKalmanFilterTest { private static Matrix getDynamics(Matrix x, Matrix u) { - var motors = DCMotor.getCIM(2); - + var motor = KnownDCMotor.CIM.dcMotor; + var n = 2; // var gLow = 15.32; // Low gear ratio var gHigh = 7.08; // High gear ratio + var rb = 0.8382 / 2.0; // Robot radius var r = 0.0746125; // Wheel radius var m = 63.503; // Robot mass var J = 5.6; // Robot moment of inertia var C1 = - -Math.pow(gHigh, 2) - * motors.KtNMPerAmp - / (motors.KvRadPerSecPerVolt * motors.rOhms * r * r); - var C2 = gHigh * motors.KtNMPerAmp / (motors.rOhms * r); + -n * Math.pow(gHigh, 2) + * motor.kt.baseUnitMagnitude() + / (motor.kv.baseUnitMagnitude() * motor.internalResistance.baseUnitMagnitude() * r * r); + var C2 = n * gHigh * motor.kt.baseUnitMagnitude() / (motor.internalResistance.baseUnitMagnitude() * r); var k1 = 1.0 / m + Math.pow(rb, 2) / J; var k2 = 1.0 / m - Math.pow(rb, 2) / J; diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java index 21ab6acf423..24d12dc81a1 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java @@ -8,10 +8,10 @@ import static org.junit.jupiter.api.Assertions.assertTrue; import edu.wpi.first.math.MatBuilder; -import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.Gearbox; +import edu.wpi.first.math.system.plant.KnownDCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import org.junit.jupiter.api.Test; @@ -19,7 +19,7 @@ class LinearSystemIDTest { @Test void testDrivetrainVelocitySystem() { var model = - LinearSystemId.createDrivetrainVelocitySystem(DCMotor.getNEO(4), 70, 0.05, 0.4, 6.0, 6); + LinearSystemId.createDrivetrainVelocitySystem(new Gearbox(KnownDCMotor.NEO.dcMotor, 4, 6), 70, 0.05, 0.4, 6.0); assertTrue( model .getA() @@ -42,7 +42,7 @@ void testDrivetrainVelocitySystem() { @Test void testElevatorSystem() { - var model = LinearSystemId.createElevatorSystem(DCMotor.getNEO(2), 5, 0.05, 12); + var model = LinearSystemId.createElevatorSystem(new Gearbox(KnownDCMotor.NEO.dcMotor, 2, 12), 5, 0.05); assertTrue( model.getA().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -99.05473), 0.001)); @@ -55,7 +55,7 @@ void testElevatorSystem() { @Test void testFlywheelSystem() { - var model = LinearSystemId.createFlywheelSystem(DCMotor.getNEO(2), 0.00032, 1.0); + var model = LinearSystemId.createFlywheelSystem(new Gearbox(KnownDCMotor.NEO.dcMotor, 2), 0.00032); assertTrue(model.getA().isEqual(VecBuilder.fill(-26.87032), 0.001)); assertTrue(model.getB().isEqual(VecBuilder.fill(1354.166667), 0.001)); @@ -65,19 +65,6 @@ void testFlywheelSystem() { assertTrue(model.getD().isEqual(VecBuilder.fill(0), 0.001)); } - @Test - void testDCMotorSystem() { - var model = LinearSystemId.createDCMotorSystem(DCMotor.getNEO(2), 0.00032, 1.0); - assertTrue( - model.getA().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -26.87032), 0.001)); - - assertTrue(model.getB().isEqual(VecBuilder.fill(0, 1354.166667), 0.001)); - - assertTrue(model.getC().isEqual(Matrix.eye(Nat.N2()), 0.001)); - - assertTrue(model.getD().isEqual(new Matrix<>(Nat.N2(), Nat.N1()), 0.001)); - } - @Test void testIdentifyPositionSystem() { // By controls engineering in frc, diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/plant/proto/DCMotorProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/plant/proto/DCMotorProtoTest.java index 3a6e64a0db8..bba79138909 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/system/plant/proto/DCMotorProtoTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/system/plant/proto/DCMotorProtoTest.java @@ -11,7 +11,7 @@ import org.junit.jupiter.api.Test; class DCMotorProtoTest { - private static final DCMotor DATA = new DCMotor(1.91, 19.1, 1.74, 1.74, 22.9, 3); + private static final DCMotor DATA = new DCMotor(1.91, 19.1, 1.74, 1.74, 22.9); @Test void testRoundtrip() { @@ -19,13 +19,13 @@ void testRoundtrip() { DCMotor.proto.pack(proto, DATA); DCMotor data = DCMotor.proto.unpack(proto); - assertEquals(DATA.nominalVoltageVolts, data.nominalVoltageVolts); - assertEquals(DATA.stallTorqueNewtonMeters, data.stallTorqueNewtonMeters); - assertEquals(DATA.stallCurrentAmps, data.stallCurrentAmps); - assertEquals(DATA.freeCurrentAmps, data.freeCurrentAmps); - assertEquals(DATA.freeSpeedRadPerSec, data.freeSpeedRadPerSec); - assertEquals(DATA.rOhms, data.rOhms); - assertEquals(DATA.KvRadPerSecPerVolt, data.KvRadPerSecPerVolt); - assertEquals(DATA.KtNMPerAmp, data.KtNMPerAmp); + assertEquals(DATA.nominalVoltage.baseUnitMagnitude(), data.nominalVoltage.baseUnitMagnitude()); + assertEquals(DATA.stallTorque.baseUnitMagnitude(), data.stallTorque.baseUnitMagnitude()); + assertEquals(DATA.stallCurrent.baseUnitMagnitude(), data.stallCurrent.baseUnitMagnitude()); + assertEquals(DATA.freeCurrent.baseUnitMagnitude(), data.freeCurrent.baseUnitMagnitude()); + assertEquals(DATA.freeSpeed.baseUnitMagnitude(), data.freeSpeed.baseUnitMagnitude()); + assertEquals(DATA.internalResistance.baseUnitMagnitude(), data.internalResistance.baseUnitMagnitude()); + assertEquals(DATA.kv.baseUnitMagnitude(), data.kv.baseUnitMagnitude()); + assertEquals(DATA.kt.baseUnitMagnitude(), data.kt.baseUnitMagnitude()); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/plant/struct/DCMotorStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/plant/struct/DCMotorStructTest.java index 806254dc23c..a770c6a0a25 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/system/plant/struct/DCMotorStructTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/system/plant/struct/DCMotorStructTest.java @@ -12,7 +12,7 @@ import org.junit.jupiter.api.Test; class DCMotorStructTest { - private static final DCMotor DATA = new DCMotor(1.91, 19.1, 1.74, 1.74, 22.9, 3); + private static final DCMotor DATA = new DCMotor(1.91, 19.1, 1.74, 1.74, 22.9); @Test void testRoundtrip() { @@ -22,13 +22,13 @@ void testRoundtrip() { buffer.rewind(); DCMotor data = DCMotor.struct.unpack(buffer); - assertEquals(DATA.nominalVoltageVolts, data.nominalVoltageVolts); - assertEquals(DATA.stallTorqueNewtonMeters, data.stallTorqueNewtonMeters); - assertEquals(DATA.stallCurrentAmps, data.stallCurrentAmps); - assertEquals(DATA.freeCurrentAmps, data.freeCurrentAmps); - assertEquals(DATA.freeSpeedRadPerSec, data.freeSpeedRadPerSec); - assertEquals(DATA.rOhms, data.rOhms); - assertEquals(DATA.KvRadPerSecPerVolt, data.KvRadPerSecPerVolt); - assertEquals(DATA.KtNMPerAmp, data.KtNMPerAmp); + assertEquals(DATA.nominalVoltage.baseUnitMagnitude(), data.nominalVoltage.baseUnitMagnitude()); + assertEquals(DATA.stallTorque.baseUnitMagnitude(), data.stallTorque.baseUnitMagnitude()); + assertEquals(DATA.stallCurrent.baseUnitMagnitude(), data.stallCurrent.baseUnitMagnitude()); + assertEquals(DATA.freeCurrent.baseUnitMagnitude(), data.freeCurrent.baseUnitMagnitude()); + assertEquals(DATA.freeSpeed.baseUnitMagnitude(), data.freeSpeed.baseUnitMagnitude()); + assertEquals(DATA.internalResistance.baseUnitMagnitude(), data.internalResistance.baseUnitMagnitude()); + assertEquals(DATA.kv.baseUnitMagnitude(), data.kv.baseUnitMagnitude()); + assertEquals(DATA.kt.baseUnitMagnitude(), data.kt.baseUnitMagnitude()); } } From 844135eccefb7d18013f9a4f658a48cce28b9b8d Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Mon, 25 Nov 2024 21:57:58 -0500 Subject: [PATCH 2/7] java first draft complete --- .../first/wpilibj/simulation/GearboxSim.java | 2 +- .../armsimulation/subsystems/Arm.java | 3 +- .../Drivetrain.java | 6 +- .../subsystems/Elevator.java | 3 +- .../subsystems/Elevator.java | 4 +- .../Drivetrain.java | 6 +- .../wpilibj/examples/statespacearm/Robot.java | 3 +- .../PotentiometerPIDTest.java | 3 +- .../wpi/first/math/system/plant/DCMotor.java | 73 ++++---- .../wpi/first/math/system/plant/Gearbox.java | 112 ++++++------ .../first/math/system/plant/KnownDCMotor.java | 146 ++++++++-------- .../math/system/plant/LinearSystemId.java | 159 +++++++----------- .../system/plant/struct/DCMotorStruct.java | 5 +- .../LinearQuadraticRegulatorTest.java | 34 ++-- .../math/controller/LinearSystemLoopTest.java | 52 +++--- .../estimator/ExtendedKalmanFilterTest.java | 77 +++++---- .../estimator/UnscentedKalmanFilterTest.java | 12 +- .../first/math/system/LinearSystemIDTest.java | 9 +- .../system/plant/proto/DCMotorProtoTest.java | 3 +- .../plant/struct/DCMotorStructTest.java | 3 +- 20 files changed, 358 insertions(+), 357 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/GearboxSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/GearboxSim.java index aba0e593bf2..42fdd019a05 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/GearboxSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/GearboxSim.java @@ -189,7 +189,7 @@ public AngularAcceleration getAngularAcceleration() { * @return The Gearbox's torque in Newton-Meters. */ public double getTorqueNewtonMeters() { - return getAngularAccelerationRadPerSecSq() * m_jKgMetersSquared; + return m_gearbox.getTorqueNewtonMeters(m_x.get(1, 0), m_u.get(0, 0)); } /** diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/subsystems/Arm.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/subsystems/Arm.java index 830199b78a8..e553b57a5a4 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/subsystems/Arm.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/subsystems/Arm.java @@ -30,7 +30,8 @@ public class Arm implements AutoCloseable { private double m_armSetpointDegrees = Constants.kDefaultArmSetpointDegrees; // The arm gearbox represents a gearbox containing two Vex 775pro motors. - private final Gearbox m_armGearbox = new Gearbox(KnownDCMotor.Vex775Pro.dcMotor, 2, Constants.kArmReduction); + private final Gearbox m_armGearbox = + new Gearbox(KnownDCMotor.Vex775Pro.dcMotor, 2, Constants.kArmReduction); // Standard classes for controlling our arm private final PIDController m_controller = new PIDController(m_armKp, 0, 0); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java index 5d8e84f6721..2b3d078ea69 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java @@ -102,7 +102,11 @@ public class Drivetrain { LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3); private final DifferentialDrivetrainSim m_drivetrainSimulator = new DifferentialDrivetrainSim( - m_drivetrainSystem, new Gearbox(KnownDCMotor.CIM.dcMotor, 2, 8), kTrackWidth, kWheelRadius, null); + m_drivetrainSystem, + new Gearbox(KnownDCMotor.CIM.dcMotor, 2, 8), + kTrackWidth, + kWheelRadius, + null); /** * Constructs a differential drive object. Sets the encoder distance per pulse and resets the diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java index fb9c47b3480..9ad19211c0b 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java @@ -29,7 +29,8 @@ public class Elevator implements AutoCloseable { // This gearbox represents a gearbox containing 4 Vex 775pro motors. - private final Gearbox m_elevatorGearbox = new Gearbox(KnownDCMotor.NEO.dcMotor, 2, Constants.kElevatorGearing); + private final Gearbox m_elevatorGearbox = + new Gearbox(KnownDCMotor.NEO.dcMotor, 2, Constants.kElevatorGearing); private final ExponentialProfile m_profile = new ExponentialProfile( diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java index 9b70b2c0e11..03cabe70e14 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java @@ -9,7 +9,6 @@ import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.Gearbox; import edu.wpi.first.math.system.plant.KnownDCMotor; import edu.wpi.first.math.trajectory.TrapezoidProfile; @@ -29,7 +28,8 @@ public class Elevator implements AutoCloseable { // This gearbox represents a gearbox containing 4 Vex 775pro motors. - private final Gearbox m_elevatorGearbox = new Gearbox(KnownDCMotor.Vex775Pro.dcMotor, 4, Constants.kElevatorGearing); + private final Gearbox m_elevatorGearbox = + new Gearbox(KnownDCMotor.Vex775Pro.dcMotor, 4, Constants.kElevatorGearing); // Standard classes for controlling our elevator private final ProfiledPIDController m_controller = diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java index 49d01565196..beb884da72c 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java @@ -71,7 +71,11 @@ public class Drivetrain { LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3); private final DifferentialDrivetrainSim m_drivetrainSimulator = new DifferentialDrivetrainSim( - m_drivetrainSystem, new Gearbox(KnownDCMotor.CIM.dcMotor, 2, 8), kTrackWidth, kWheelRadius, null); + m_drivetrainSystem, + new Gearbox(KnownDCMotor.CIM.dcMotor, 2, 8), + kTrackWidth, + kWheelRadius, + null); /** Subsystem constructor. */ public Drivetrain() { diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java index 27b32ef09e5..2eff7002c17 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java @@ -54,7 +54,8 @@ public class Robot extends TimedRobot { // Inputs (what we can "put in"): [voltage], in volts. // Outputs (what we can measure): [position], in radians. private final LinearSystem m_armPlant = - LinearSystemId.createSingleJointedArmSystem(new Gearbox(KnownDCMotor.NEO.dcMotor, 2, kArmGearing), kArmMOI); + LinearSystemId.createSingleJointedArmSystem( + new Gearbox(KnownDCMotor.NEO.dcMotor, 2, kArmGearing), kArmMOI); // The observer fuses our encoder data and voltage inputs to reject noise. @SuppressWarnings("unchecked") diff --git a/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/potentiometerpid/PotentiometerPIDTest.java b/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/potentiometerpid/PotentiometerPIDTest.java index fc41951102d..5ebb1336624 100644 --- a/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/potentiometerpid/PotentiometerPIDTest.java +++ b/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/potentiometerpid/PotentiometerPIDTest.java @@ -26,7 +26,8 @@ @ResourceLock("timing") class PotentiometerPIDTest { - private final Gearbox m_elevatorGearbox = new Gearbox(KnownDCMotor.Vex775Pro.dcMotor, 4, kElevatorGearing); + private final Gearbox m_elevatorGearbox = + new Gearbox(KnownDCMotor.Vex775Pro.dcMotor, 4, kElevatorGearing); private static final double kElevatorGearing = 10.0; private static final double kElevatorDrumRadius = Units.inchesToMeters(2.0); private static final double kCarriageMassKg = 4.0; // kg diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java index fc7522b0895..89c209bf53c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java @@ -28,18 +28,25 @@ public class DCMotor implements ProtobufSerializable, StructSerializable { /** Voltage at which the motor constants were measured. */ public final Voltage nominalVoltage; + /** Torque when stalled. */ public final Torque stallTorque; + /** Current draw when stalled. */ public final Current stallCurrent; + /** Current draw under no load. */ public final Current freeCurrent; + /** Angular velocity under no load. */ public final AngularVelocity freeSpeed; + /** Motor internal resistance. */ public final Resistance internalResistance; + /** Motor velocity constant. */ public final Per kv; + /** Motor torque constant. */ public final Per kt; @@ -52,13 +59,11 @@ public class DCMotor implements ProtobufSerializable, StructSerializable { /** * Constructs a DC motor. * - * @param nominalVoltageVolts Voltage at which the motor constants were - * measured in Volts. + * @param nominalVoltageVolts Voltage at which the motor constants were measured in Volts. * @param stallTorqueNewtonMeters Torque when stalled in Newton-Meters. - * @param stallCurrentAmps Current draw when stalled in Amps. - * @param freeCurrentAmps Current draw under no load in Amps. - * @param freeSpeedRadPerSec Angular velocity under no load in Radians per - * Second. + * @param stallCurrentAmps Current draw when stalled in Amps. + * @param freeCurrentAmps Current draw under no load in Amps. + * @param freeSpeedRadPerSec Angular velocity under no load in Radians per Second. */ public DCMotor( double nominalVoltageVolts, @@ -77,12 +82,11 @@ public DCMotor( /** * Constructs a DC motor. * - * @param nominalVoltage Voltage at which the motor constants were - * measured. - * @param stallTorque Torque when stalled. - * @param stallCurrent Current draw when stalled. - * @param freeCurrent Current draw under no load. - * @param freeSpeed Angular velocity under no load. + * @param nominalVoltage Voltage at which the motor constants were measured. + * @param stallTorque Torque when stalled. + * @param stallCurrent Current draw when stalled. + * @param freeCurrent Current draw under no load. + * @param freeSpeed Angular velocity under no load. */ public DCMotor( Voltage nominalVoltage, @@ -101,44 +105,51 @@ public DCMotor( } /** - * Calculate the input voltage of the motor for a given torque and - * angular velocity. + * Calculate the input voltage of the motor for a given torque and angular velocity. * - * @param torqueNewtonMeters The torque produced by the motor. - * @param angularVelocityRadiansPerSec The current angular velocity of the - * motor. + * @param torqueNewtonMeters The torque produced by the motor. + * @param angularVelocityRadiansPerSec The current angular velocity of the motor. * @return The voltage of the motor. */ - public double getVoltageInputVolts(double torqueNewtonMeters, double angularVelocityRadiansPerSec) { + public double getVoltageInputVolts( + double torqueNewtonMeters, double angularVelocityRadiansPerSec) { return 1.0 / kv.baseUnitMagnitude() * angularVelocityRadiansPerSec - + 1.0 / kt.baseUnitMagnitude() * internalResistance.baseUnitMagnitude() * torqueNewtonMeters; + + 1.0 + / kt.baseUnitMagnitude() + * internalResistance.baseUnitMagnitude() + * torqueNewtonMeters; } /** - * Calculates the angular velocity in Radians per Second produced by the motor - * at a given torque and input voltage. + * Calculates the angular velocity in Radians per Second produced by the motor at a given torque + * and input voltage. * * @param torqueNewtonMeters The torque produced by the motor. - * @param voltageInputVolts The voltage applied to the motor. + * @param voltageInputVolts The voltage applied to the motor. * @return The angular velocity of the motor. */ - public double getAngularVelocityRadiansPerSecond(double torqueNewtonMeters, double voltageInputVolts) { + public double getAngularVelocityRadiansPerSecond( + double torqueNewtonMeters, double voltageInputVolts) { return voltageInputVolts * kv.baseUnitMagnitude() - - 1.0 / kt.baseUnitMagnitude() * torqueNewtonMeters * internalResistance.baseUnitMagnitude() + - 1.0 + / kt.baseUnitMagnitude() + * torqueNewtonMeters + * internalResistance.baseUnitMagnitude() * kv.baseUnitMagnitude(); } /** - * Calculate torque in Newton-Meters produced by the motor at a given angular - * velocity and input voltage. + * Calculate torque in Newton-Meters produced by the motor at a given angular velocity and input + * voltage. * - * @param angularVelocityRadiansPerSec The current angular velocity of the - * motor. - * @param voltageInputVolts The voltage applied to the motor. + * @param angularVelocityRadiansPerSec The current angular velocity of the motor. + * @param voltageInputVolts The voltage applied to the motor. * @return The torque output. */ - public double getTorqueNewtonMeters(double angularVelocityRadiansPerSec, double voltageInputVolts) { - return kt.baseUnitMagnitude() * (voltageInputVolts - (angularVelocityRadiansPerSec / kv.baseUnitMagnitude())); + public double getTorqueNewtonMeters( + double angularVelocityRadiansPerSec, double voltageInputVolts) { + return kt.baseUnitMagnitude() + * (voltageInputVolts - (angularVelocityRadiansPerSec / kv.baseUnitMagnitude())); } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/Gearbox.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/Gearbox.java index e36ad37c9f3..8ec7c4d4d98 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/plant/Gearbox.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/Gearbox.java @@ -26,61 +26,55 @@ public class Gearbox { /** The returned measure of angular velocity. Exists to minimize calls to GC. */ private final MutAngularVelocity angularVelocity = RadiansPerSecond.mutable(0.0); + /** The returned measure of input voltage. Exists to minimize calls to GC. */ private final MutVoltage voltageInput = Volts.mutable(0.0); + /** The returned measure of torque. Exists to minimize calls to GC. */ private final MutTorque torque = NewtonMeters.mutable(0.0); + /** The returned measure of current. Exists to minimize calls to GC. */ private final MutCurrent current = Amps.mutable(0.0); /** * Constructs a Gearbox. * - * @param dcMotor The DC motor used in this gearbox. - * @param numMotors Number of identical motors in the gearbox. + * @param dcMotor The DC motor used in this gearbox. */ - public Gearbox( - DCMotor dcMotor) { + public Gearbox(DCMotor dcMotor) { this(dcMotor, 1, 1.0); } /** * Constructs a Gearbox. * - * @param dcMotor The DC motor used in this gearbox. + * @param dcMotor The DC motor used in this gearbox. * @param numMotors Number of identical motors in the gearbox. */ - public Gearbox( - DCMotor dcMotor, - int numMotors) { + public Gearbox(DCMotor dcMotor, int numMotors) { this(dcMotor, numMotors, 1.0); } /** * Constructs a Gearbox. * - * @param dcMotor The DC motor used in this gearbox. + * @param dcMotor The DC motor used in this gearbox. * @param gearboxReduction The gearbox reduction. */ - public Gearbox( - DCMotor dcMotor, - double gearboxReduction) { + public Gearbox(DCMotor dcMotor, double gearboxReduction) { this(dcMotor, 1, gearboxReduction); } /** * Constructs a Gearbox. * - * @param dcMotor The DC motor used in this gearbox. - * @param numMotors Number of identical motors in the gearbox. + * @param dcMotor The DC motor used in this gearbox. + * @param numMotors Number of identical motors in the gearbox. * @param gearboxReduction The gearbox reduction. * @throws IllegalArgumentException if numMotors < 1, gearboxReduction ≤ 0. */ - public Gearbox( - DCMotor dcMotor, - int numMotors, - double gearboxReduction) { - if(numMotors < 1){ + public Gearbox(DCMotor dcMotor, int numMotors, double gearboxReduction) { + if (numMotors < 1) { throw new IllegalArgumentException("numMotors must be greater than or equal to 1."); } if (gearboxReduction <= 0) { @@ -93,9 +87,10 @@ public Gearbox( /** * Returns the gearbox reduction as a ratio of output rotations to input rotations. + * * @return The gearbox reduction. */ - public double getGearboxReduction(){ + public double getGearboxReduction() { return gearboxReduction; } @@ -111,93 +106,86 @@ public Gearbox withReduction(double gearboxReduction) { } /** - * Calculate the input voltage in Volts of each motor for a given torque and - * angular velocity. + * Calculate the input voltage in Volts of each motor for a given torque and angular velocity. * - * @param torqueNewtonMeters The torque produced by the gearbox. - * @param angularVelocityRadiansPerSec The current angular velocity of the - * gearbox's output. + * @param torqueNewtonMeters The torque produced by the gearbox. + * @param angularVelocityRadiansPerSec The current angular velocity of the gearbox's output. * @return The voltage of each motor. */ - public double getVoltageInputVolts(double torqueNewtonMeters, double angularVelocityRadiansPerSec) { + public double getVoltageInputVolts( + double torqueNewtonMeters, double angularVelocityRadiansPerSec) { return dcMotor.getAngularVelocityRadiansPerSecond( torqueNewtonMeters / gearboxReduction / numMotors, angularVelocityRadiansPerSec * gearboxReduction); } /** - * Calculate the input voltage of each motor for a given torque and - * angular velocity. + * Calculate the input voltage of each motor for a given torque and angular velocity. * - * @param torque The torque produced by the gearbox. + * @param torque The torque produced by the gearbox. * @param angularVelocity The current angular velocity of the gearbox's output. * @return The voltage of each motor. */ public Voltage getVoltageInput(Torque torque, AngularVelocity angularVelocity) { - return voltageInput - .mut_setMagnitude(getVoltageInputVolts(torque.baseUnitMagnitude(), angularVelocity.baseUnitMagnitude())); + return voltageInput.mut_setMagnitude( + getVoltageInputVolts(torque.baseUnitMagnitude(), angularVelocity.baseUnitMagnitude())); } /** - * Calculates the angular velocity in Radians per Second produced by the gearbox - * at a given torque and input voltage. + * Calculates the angular velocity in Radians per Second produced by the gearbox at a given torque + * and input voltage. * * @param torqueNewtonMeters The torque produced by the gearbox. - * @param voltageInputVolts The voltage applied to each motor. + * @param voltageInputVolts The voltage applied to each motor. * @return The angular velocity of the gearbox's output. */ - public double getAngularVelocityRadiansPerSecond(double torqueNewtonMeters, double voltageInputVolts) { + public double getAngularVelocityRadiansPerSecond( + double torqueNewtonMeters, double voltageInputVolts) { return dcMotor.getAngularVelocityRadiansPerSecond( - torqueNewtonMeters / gearboxReduction / numMotors, - voltageInputVolts) + torqueNewtonMeters / gearboxReduction / numMotors, voltageInputVolts) / gearboxReduction; } /** - * Calculates the angular velocity produced by the gearbox - * at a given torque and input voltage. + * Calculates the angular velocity produced by the gearbox at a given torque and input voltage. * - * @param torque The torque produced by the gearbox. + * @param torque The torque produced by the gearbox. * @param voltageInput The voltage applied to each motor. * @return The angular velocity of the gearbox's output. */ public AngularVelocity getAngularVelocity(Torque torque, Voltage voltageInput) { return angularVelocity.mut_setMagnitude( getAngularVelocityRadiansPerSecond( - torque.baseUnitMagnitude(), - voltageInput.baseUnitMagnitude())); + torque.baseUnitMagnitude(), voltageInput.baseUnitMagnitude())); } /** - * Calculate torque in Newton-Meters produced by the gearbox at a given angular - * velocity and input voltage. + * Calculate torque in Newton-Meters produced by the gearbox at a given angular velocity and input + * voltage. * - * @param angularVelocityRadiansPerSec The current angular velocity of the - * gearbox's output. - * @param voltageInputVolts The voltage applied to each motor. + * @param angularVelocityRadiansPerSec The current angular velocity of the gearbox's output. + * @param voltageInputVolts The voltage applied to each motor. * @return The torque output of the gearbox. */ - public double getTorqueNewtonMeters(double angularVelocityRadiansPerSec, double voltageInputVolts) { - return numMotors * gearboxReduction * dcMotor.getTorqueNewtonMeters( - angularVelocityRadiansPerSec * gearboxReduction, - voltageInputVolts); + public double getTorqueNewtonMeters( + double angularVelocityRadiansPerSec, double voltageInputVolts) { + return numMotors + * gearboxReduction + * dcMotor.getTorqueNewtonMeters( + angularVelocityRadiansPerSec * gearboxReduction, voltageInputVolts); } /** - * Calculate torque in produced by the gearbox at a given angular - * velocity and input voltage. + * Calculate torque in produced by the gearbox at a given angular velocity and input voltage. * - * @param angularVelocity The current angular velocity of the - * gearbox's output. - * @param voltageInput The voltage applied to each motor. + * @param angularVelocity The current angular velocity of the gearbox's output. + * @param voltageInput The voltage applied to each motor. * @return The torque output of the gearbox. */ public Torque getTorque(AngularVelocity angularVelocity, Voltage voltageInput) { - return torque - .mut_setMagnitude( - getTorqueNewtonMeters( - angularVelocity.baseUnitMagnitude(), - voltageInput.baseUnitMagnitude())); + return torque.mut_setMagnitude( + getTorqueNewtonMeters( + angularVelocity.baseUnitMagnitude(), voltageInput.baseUnitMagnitude())); } /** @@ -213,7 +201,7 @@ public double getCurrentAmps(double torqueNewtonMeters) { /** * Calculate net current drawn by all of the motors for a given torque. * - * @param torqueNewtonMeters The torque produced by the gearbox. + * @param torque The torque produced by the gearbox. * @return The net current drawn by the all of the motors. */ public Current getCurrent(Torque torque) { diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/KnownDCMotor.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/KnownDCMotor.java index 9a034e1a89a..278dc43bc93 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/plant/KnownDCMotor.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/KnownDCMotor.java @@ -7,81 +7,89 @@ /** Holds the constants for known DC motors. */ public enum KnownDCMotor { - /** TODO: CIM DOCUMENTATION */ - CIM(12, 2.42, 133, 2.7, 5310), - /** TODO: 775Pro DOCUMENTATION */ - Vex775Pro(12, 0.71, 134, 0.7, 18730), - /** TODO: NEO DOCUMENTATION */ - NEO(12, 2.6, 105, 1.8, 5676), - /** TODO: MiniCIM DOCUMENTATION */ - MiniCIM(12, 1.41, 89, 3, 5840), - /** TODO: Bag DOCUMENTATION */ - Bag(12, 0.43, 53, 1.8, 13180), - /** TODO: Andymark RS775-125 DOCUMENTATION */ - AndymarkRs775_125(12, 0.28, 18, 1.6, 5800), - /** TODO: Banebots RS775 DOOCUMENTATION */ - BanebotsRs775(12, 0.72, 97, 2.7, 13050), - /** TODO: Andymark 9015 DOCUMENTATION */ - Andymark9015(12, 0.36, 71, 3.7, 14270), - /** TODO: Banebots RS 550 DOCUMENTATION */ - BanebotsRS550(12, 0.38, 84, 0.4, 19000), - /** TODO: Neo 550 DOCUMENTATION */ - Neo550(12, 0.97, 100, 1.4, 11000), + /** CIM motor constants. */ + CIM(12, 2.42, 133, 2.7, 5310), + /** VEX 775 Pro motor constants. */ + Vex775Pro(12, 0.71, 134, 0.7, 18730), + /** NEO motor constants. */ + NEO(12, 2.6, 105, 1.8, 5676), + /** MiniCIM motor constants. */ + MiniCIM(12, 1.41, 89, 3, 5840), + /** Bag motor constants. */ + Bag(12, 0.43, 53, 1.8, 13180), + /** Andymark RS775_125 motor constants. */ + AndymarkRs775_125(12, 0.28, 18, 1.6, 5800), + /** Banebots RS775 motor constants. */ + BanebotsRs775(12, 0.72, 97, 2.7, 13050), + /** Andymark9015 motor constants. */ + Andymark9015(12, 0.36, 71, 3.7, 14270), + /** Banebots RS550 motor constants. */ + BanebotsRS550(12, 0.38, 84, 0.4, 19000), + /** Neo 550 motor constants. */ + Neo550(12, 0.97, 100, 1.4, 11000), - /** - * @see Falcon 500 Documentation - */ - Falcon500(12, 4.69, 257, 1.5, 6380), + /** + * Falcon500 constants. + * + * @see Falcon 500 + * Documentation + */ + Falcon500(12, 4.69, 257, 1.5, 6380), - /** - * @see Falcon - * 500 FOC (Field-Oriented Control) Documentation - */ - Falcon500Foc(12, 5.84, 304, 1.5, 6080), + /** + * Falcon 500 FOC constants. + * + * @see Falcon 500 + * FOC (Field-Oriented Control) Documentation + */ + Falcon500Foc(12, 5.84, 304, 1.5, 6080), - /** - * @see Romi Built-In - * Documentation - */ - RomiBuiltIn(4.5, 0.1765, 1.25, 0.13, 150), + /** + * Romi BuiltIn constants. + * + * @see Romi Built-In Documentation + */ + RomiBuiltIn(4.5, 0.1765, 1.25, 0.13, 150), - /** - * @see Kraken X60 - * Documentation - */ - KrakenX60(12, 7.09, 366, 2, 6000), + /** + * Kraken X60 constants. + * + * @see Kraken X60 + * Documentation + */ + KrakenX60(12, 7.09, 366, 2, 6000), - /** - * @see Kraken X60 - * FOC (Field-Oriented Control) Documentation - */ - KrakenX60Foc(12, 9.37, 483, 2, 5800), + /** + * Kraken X60 FOC constants. + * + * @see Kraken X60 FOC + * (Field-Oriented Control) Documentation + */ + KrakenX60Foc(12, 9.37, 483, 2, 5800), - /** - * @see NEO - * Vortex Documentation - */ - NeoVortex(12, 3.60, 211, 3.6, 6784); + /** + * NEO Vortex constants. + * + * @see NEO Vortex + * Documentation + */ + NeoVortex(12, 3.60, 211, 3.6, 6784); - /** DCMotor object for this known type. */ - public final DCMotor dcMotor; + /** DCMotor object for this known type. */ + public final DCMotor dcMotor; - private KnownDCMotor( - double nominalVoltageVolts, - double stallTorqueNewtonMeters, - double stallCurrentAmps, - double freeCurrentAmps, - double freeSpeedRPM) { - dcMotor = new DCMotor( - Volts.of(nominalVoltageVolts), - NewtonMeters.of(stallTorqueNewtonMeters), - Amps.of(stallCurrentAmps), - Amps.of(freeCurrentAmps), - RPM.of(freeSpeedRPM)); - - } + KnownDCMotor( + double nominalVoltageVolts, + double stallTorqueNewtonMeters, + double stallCurrentAmps, + double freeCurrentAmps, + double freeSpeedRPM) { + dcMotor = + new DCMotor( + Volts.of(nominalVoltageVolts), + NewtonMeters.of(stallTorqueNewtonMeters), + Amps.of(stallCurrentAmps), + Amps.of(freeCurrentAmps), + RPM.of(freeSpeedRPM)); + } } 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 487ca7a3c3e..d468ce1fc19 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 @@ -19,12 +19,11 @@ private LinearSystemId() { } /** - * Create a state-space model of an elevator system. The states of the system - * are [position, + * 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 gearbox The gearbox attached to the elevator's driving drum. - * @param massKg The mass of the elevator carriage, in kilograms. + * @param gearbox The gearbox attached to the elevator's driving drum. + * @param massKg The mass of the elevator carriage, in kilograms. * @param radiusMeters The radius of the elevator's driving drum, in meters. * @return A LinearSystem representing the given characterized constants. * @throws IllegalArgumentException if massKg ≤ 0, radiusMeters ≤ 0. @@ -45,17 +44,17 @@ public static LinearSystem createElevatorSystem( double rOhms = gearbox.dcMotor.internalResistance.baseUnitMagnitude(); double kvVoltsPerMetersPerSecond = G / (radiusMeters * kvRadPerSecPerVolt); - double kaVoltsPerMetersPerSecondSquared = (rOhms * radiusMeters * massKg) / (n * G * ktNMPerAmp); + double kaVoltsPerMetersPerSecondSquared = + (rOhms * radiusMeters * massKg) / (n * G * ktNMPerAmp); return identifyPositionSystem(kvVoltsPerMetersPerSecond, kaVoltsPerMetersPerSecondSquared); } /** - * Create a state-space model of a flywheel system. The states of the system are - * [angular + * 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]. * - * @param gearbox The gearbox attached to the flywheel. + * @param gearbox The gearbox attached to the flywheel. * @param JKgMetersSquared The moment of inertia J of the flywheel. * @return A LinearSystem representing the given characterized constants. * @throws IllegalArgumentException if JKgMetersSquared ≤ 0. @@ -72,18 +71,17 @@ public static LinearSystem createFlywheelSystem( double kvRadPerSecPerVolt = gearbox.dcMotor.kv.baseUnitMagnitude(); double rOhms = gearbox.dcMotor.internalResistance.baseUnitMagnitude(); - double kvVoltsPerRadiansPerSecond = G / (kvRadPerSecPerVolt); + double kvVoltsPerRadiansPerSecond = G / kvRadPerSecPerVolt; double kaVoltsPerRadiansPerSecondSquared = (rOhms * JKgMetersSquared) / (n * G * ktNMPerAmp); return identifyVelocitySystem(kvVoltsPerRadiansPerSecond, kaVoltsPerRadiansPerSecondSquared); } - /** - * Create a state-space model of a gearbox system. The states of the system are - * [angular + /** + * Create a state-space model of a gearbox system. The states of the system are [angular * velocity], inputs are [voltage], and outputs are [angular velocity]. * - * @param gearbox The gearbox of the system. + * @param gearbox The gearbox of the system. * @param JKgMetersSquared The moment of inertia J of the gearbox. * @return A LinearSystem representing the given characterized constants. * @throws IllegalArgumentException if JKgMetersSquared ≤ 0. @@ -100,36 +98,27 @@ public static LinearSystem createGearboxSystem( double kvRadPerSecPerVolt = gearbox.dcMotor.kv.baseUnitMagnitude(); double rOhms = gearbox.dcMotor.internalResistance.baseUnitMagnitude(); - double kvVoltsPerRadiansPerSecond = G / (kvRadPerSecPerVolt); + double kvVoltsPerRadiansPerSecond = G / kvRadPerSecPerVolt; double kaVoltsPerRadiansPerSecondSquared = (rOhms * JKgMetersSquared) / (n * G * ktNMPerAmp); return identifyPositionSystem(kvVoltsPerRadiansPerSecond, kaVoltsPerRadiansPerSecondSquared); } /** - * 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 + * 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 gearbox The 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 gearbox The 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 gearing The gearing reduction as output over input. * @return A LinearSystem representing a differential drivetrain. - * @throws IllegalArgumentException if m ≤ 0, r ≤ 0, rb ≤ 0, or J ≤ - * 0. + * @throws IllegalArgumentException if m ≤ 0, r ≤ 0, rb ≤ 0, or J ≤ 0. */ public static LinearSystem createDrivetrainVelocitySystem( - Gearbox gearbox, - double massKg, - double rMeters, - double rbMeters, - double JKgMetersSquared) { + Gearbox gearbox, double massKg, double rMeters, double rbMeters, double JKgMetersSquared) { if (massKg <= 0.0) { throw new IllegalArgumentException("massKg must be greater than zero."); } @@ -150,7 +139,7 @@ public static LinearSystem createDrivetrainVelocitySystem( double rOhms = gearbox.dcMotor.internalResistance.baseUnitMagnitude(); var C1 = -n * Math.pow(G, 2) * ktNMPerAmp / (kvRadPerSecPerVolt * rOhms * Math.pow(rMeters, 2)); - var C2 = G * ktNMPerAmp / (rOhms * rMeters); + var C2 = n * G * ktNMPerAmp / (rOhms * rMeters); final double C3 = 1 / massKg + rbMeters * rbMeters / JKgMetersSquared; final double C4 = 1 / massKg - rbMeters * rbMeters / JKgMetersSquared; @@ -163,11 +152,10 @@ public static LinearSystem createDrivetrainVelocitySystem( } /** - * Create a state-space model of a single jointed arm system. The states of the - * system are [angle, + * 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]. * - * @param gearbox The gearbox attached to the arm. + * @param gearbox The gearbox attached to the arm. * @param JKgMetersSquared The moment of inertia J of the arm. * @return A LinearSystem representing the given characterized constants. * @throws IllegalArgumentException if JKgMetersSquared ≤ 0. @@ -184,37 +172,29 @@ public static LinearSystem createSingleJointedArmSystem( double kvRadPerSecPerVolt = gearbox.dcMotor.kv.baseUnitMagnitude(); double rOhms = gearbox.dcMotor.internalResistance.baseUnitMagnitude(); - double kvVoltsPerRadiansPerSecond = G / (kvRadPerSecPerVolt); + double kvVoltsPerRadiansPerSecond = G / kvRadPerSecPerVolt; double kaVoltsPerRadiansPerSecondSquared = (rOhms * JKgMetersSquared) / (n * G * ktNMPerAmp); return identifyPositionSystem(kvVoltsPerRadiansPerSecond, kaVoltsPerRadiansPerSecondSquared); } /** - * 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 + * 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. + *

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: + *

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

- * u = K_v v + K_a a + *

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 + * @see https://github.com/wpilibsuite/sysid */ public static LinearSystem identifyVelocitySystem(double kV, double kA) { if (kV < 0.0) { @@ -232,30 +212,22 @@ public static LinearSystem identifyVelocitySystem(double kV, double } /** - * 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 + * 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. + *

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: + *

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

- * u = K_v v + K_a a + *

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 + * @see https://github.com/wpilibsuite/sysid */ public static LinearSystem identifyPositionSystem(double kV, double kA) { if (kV < 0.0) { @@ -273,28 +245,22 @@ public static LinearSystem identifyPositionSystem(double kV, double } /** - * Identify a differential drive drivetrain given the drivetrain's kV and kA in - * both linear + * 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)), * (volts/(radian/sec²))} cases. These constants can be found using SysId. * - *

- * States: [[left velocity], [right velocity]]
+ *

States: [[left velocity], [right velocity]]
* Inputs: [[left voltage], [right voltage]]
* Outputs: [[left velocity], [right velocity]] * - * @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 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 (meters per second). - * @param kAAngular The angular acceleration gain in volts per (meters per - * second squared). + * @param kAAngular The angular acceleration gain in volts per (meters per second squared). * @return A LinearSystem representing the given characterized constants. - * @throws IllegalArgumentException if kVLinear <= 0, kALinear <= 0, - * kVAngular <= 0, or - * kAAngular <= 0. - * @see https://github.com/wpilibsuite/sysid + * @throws IllegalArgumentException if kVLinear <= 0, kALinear <= 0, kVAngular <= 0, or + * kAAngular <= 0. + * @see https://github.com/wpilibsuite/sysid */ public static LinearSystem identifyDrivetrainSystem( double kVLinear, double kALinear, double kVAngular, double kAAngular) { @@ -324,33 +290,24 @@ public static LinearSystem identifyDrivetrainSystem( } /** - * 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)), + * 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)), * (volts/(radian/sec²))} cases. This can be found using SysId. * - *

- * States: [[left velocity], [right velocity]]
+ *

States: [[left velocity], [right velocity]]
* Inputs: [[left voltage], [right voltage]]
* Outputs: [[left velocity], [right velocity]] * - * @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. + * @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, - * kAAngular <= 0, or trackwidth <= 0. - * @see https://github.com/wpilibsuite/sysid + * @throws IllegalArgumentException if kVLinear <= 0, kALinear <= 0, kVAngular <= 0, + * kAAngular <= 0, or trackwidth <= 0. + * @see https://github.com/wpilibsuite/sysid */ public static LinearSystem identifyDrivetrainSystem( double kVLinear, double kALinear, double kVAngular, double kAAngular, double trackwidth) { diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/struct/DCMotorStruct.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/struct/DCMotorStruct.java index f743dacbce6..e8ce9425998 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/plant/struct/DCMotorStruct.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/struct/DCMotorStruct.java @@ -4,14 +4,13 @@ package edu.wpi.first.math.system.plant.struct; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.util.struct.Struct; - import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.NewtonMeters; import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.Volts; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.util.struct.Struct; import java.nio.ByteBuffer; public class DCMotorStruct implements Struct { diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java index c60f6e2b4d9..7a5044d4583 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java @@ -48,11 +48,11 @@ void testFourMotorElevator() { var G = 14.67; var gearbox = new Gearbox(motor, n, G); - var plant = LinearSystemId.createElevatorSystem( - gearbox, 8.0, 0.75 * 25.4 / 1000.0); + var plant = LinearSystemId.createElevatorSystem(gearbox, 8.0, 0.75 * 25.4 / 1000.0); - var K = new LinearQuadraticRegulator<>(plant, VecBuilder.fill(0.1, 0.2), VecBuilder.fill(12.0), dt) - .getK(); + var K = + new LinearQuadraticRegulator<>(plant, VecBuilder.fill(0.1, 0.2), VecBuilder.fill(12.0), dt) + .getK(); assertEquals(10.381, K.get(0, 0), 1e-2); assertEquals(0.6929, K.get(0, 1), 1e-2); @@ -83,16 +83,15 @@ void testLQROnArm() { /** * Returns feedback control gain for implicit model following. * - *

- * This is used to test the QRN overload of LQR. + *

This is used to test the QRN overload of LQR. * - * @param Number of states. - * @param Number of inputs. - * @param A State matrix. - * @param B Input matrix. - * @param Q State cost matrix. - * @param R Input cost matrix. - * @param Aref Desired state matrix. + * @param Number of states. + * @param Number of inputs. + * @param A State matrix. + * @param B Input matrix. + * @param Q State cost matrix. + * @param R Input cost matrix. + * @param Aref Desired state matrix. * @param dtSeconds Discretization timestep in seconds. */ Matrix getImplicitModelFollowingK( @@ -110,7 +109,8 @@ Matrix getImplicitModel // Discretize desired dynamics var discAref = Discretization.discretizeA(Aref, dtSeconds); - Matrix Qimf = (discA.minus(discAref)).transpose().times(Q).times(discA.minus(discAref)); + Matrix Qimf = + (discA.minus(discAref)).transpose().times(Q).times(discA.minus(discAref)); Matrix Rimf = discB.transpose().times(Q).times(discB).plus(R); Matrix Nimf = (discA.minus(discAref)).transpose().times(Q).times(discB); @@ -171,10 +171,10 @@ void testLatencyCompensate() { var G = 14.67; var gearbox = new Gearbox(motor, n, G); - var plant = LinearSystemId.createElevatorSystem( - gearbox, 8.0, 0.75 * 25.4 / 1000.0); + var plant = LinearSystemId.createElevatorSystem(gearbox, 8.0, 0.75 * 25.4 / 1000.0); - var regulator = new LinearQuadraticRegulator<>(plant, VecBuilder.fill(0.1, 0.2), VecBuilder.fill(12.0), dt); + var regulator = + new LinearQuadraticRegulator<>(plant, VecBuilder.fill(0.1, 0.2), VecBuilder.fill(12.0), dt); regulator.latencyCompensate(plant, dt, 0.01); diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java index 8057cc4801f..80b6e902936 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java @@ -35,24 +35,27 @@ class LinearSystemLoopTest { LinearSystem m_plant = LinearSystemId.createElevatorSystem(gearbox, 5, 0.0181864); @SuppressWarnings("unchecked") - KalmanFilter m_observer = new KalmanFilter<>( - Nat.N2(), - Nat.N1(), - (LinearSystem) m_plant.slice(0), - VecBuilder.fill(0.05, 1.0), - VecBuilder.fill(0.0001), - kDt); + KalmanFilter m_observer = + new KalmanFilter<>( + Nat.N2(), + Nat.N1(), + (LinearSystem) m_plant.slice(0), + VecBuilder.fill(0.05, 1.0), + VecBuilder.fill(0.0001), + kDt); @SuppressWarnings("unchecked") - LinearQuadraticRegulator m_controller = new LinearQuadraticRegulator<>( - (LinearSystem) m_plant.slice(0), - VecBuilder.fill(0.02, 0.4), - VecBuilder.fill(12.0), - 0.00505); + LinearQuadraticRegulator m_controller = + new LinearQuadraticRegulator<>( + (LinearSystem) m_plant.slice(0), + VecBuilder.fill(0.02, 0.4), + VecBuilder.fill(12.0), + 0.00505); @SuppressWarnings("unchecked") - private final LinearSystemLoop m_loop = new LinearSystemLoop<>( - (LinearSystem) m_plant.slice(0), m_controller, m_observer, 12, 0.00505); + private final LinearSystemLoop m_loop = + new LinearSystemLoop<>( + (LinearSystem) m_plant.slice(0), m_controller, m_observer, 12, 0.00505); private static void updateTwoState( LinearSystem plant, LinearSystemLoop loop, double noise) { @@ -74,10 +77,11 @@ void testStateSpaceEnabled() { TrapezoidProfile.State state; for (int i = 0; i < 1000; i++) { profile = new TrapezoidProfile(constraints); - state = profile.calculate( - kDt, - new TrapezoidProfile.State(m_loop.getXHat(0), m_loop.getXHat(1)), - new TrapezoidProfile.State(references.get(0, 0), references.get(1, 0))); + state = + profile.calculate( + kDt, + new TrapezoidProfile.State(m_loop.getXHat(0), m_loop.getXHat(1)), + new TrapezoidProfile.State(references.get(0, 0), references.get(1, 0))); m_loop.setNextR(VecBuilder.fill(state.position, state.velocity)); updateTwoState( @@ -99,8 +103,9 @@ void testFlywheelEnabled() { int numMotors = 2; Gearbox gearbox = new Gearbox(motor, numMotors); LinearSystem plant = LinearSystemId.createFlywheelSystem(gearbox, 0.00289); - KalmanFilter observer = new KalmanFilter<>( - Nat.N1(), Nat.N1(), plant, VecBuilder.fill(1.0), VecBuilder.fill(kPositionStddev), kDt); + KalmanFilter observer = + new KalmanFilter<>( + Nat.N1(), Nat.N1(), plant, VecBuilder.fill(1.0), VecBuilder.fill(kPositionStddev), kDt); var qElms = VecBuilder.fill(9.0); var rElms = VecBuilder.fill(12.0); @@ -124,9 +129,10 @@ void testFlywheelEnabled() { loop.setNextR(references); - Matrix y = plant - .calculateY(loop.getXHat(), loop.getU()) - .plus(VecBuilder.fill(random.nextGaussian() * kPositionStddev)); + Matrix y = + plant + .calculateY(loop.getXHat(), loop.getU()) + .plus(VecBuilder.fill(random.nextGaussian() * kPositionStddev)); loop.correct(y); loop.predict(kDt); diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java index 2e1f5822a9c..2d4ff74dbea 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java @@ -37,8 +37,13 @@ private static Matrix getDynamics(final Matrix x, final Matrix { - ExtendedKalmanFilter observer = new ExtendedKalmanFilter<>( - Nat.N5(), - Nat.N2(), - Nat.N3(), - ExtendedKalmanFilterTest::getDynamics, - ExtendedKalmanFilterTest::getLocalMeasurementModel, - VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0), - VecBuilder.fill(0.0001, 0.01, 0.01), - dtSeconds); + ExtendedKalmanFilter observer = + new ExtendedKalmanFilter<>( + Nat.N5(), + Nat.N2(), + Nat.N3(), + ExtendedKalmanFilterTest::getDynamics, + ExtendedKalmanFilterTest::getLocalMeasurementModel, + VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0), + VecBuilder.fill(0.0001, 0.01, 0.01), + dtSeconds); Matrix u = VecBuilder.fill(12.0, 12.0); observer.predict(u, dtSeconds); @@ -101,32 +107,36 @@ void testConvergence() { double dtSeconds = 0.00505; double rbMeters = 0.8382 / 2.0; // Robot radius - ExtendedKalmanFilter observer = new ExtendedKalmanFilter<>( - Nat.N5(), - Nat.N2(), - Nat.N3(), - ExtendedKalmanFilterTest::getDynamics, - ExtendedKalmanFilterTest::getLocalMeasurementModel, - VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0), - VecBuilder.fill(0.001, 0.01, 0.01), - dtSeconds); - - List waypoints = List.of( - new Pose2d(2.75, 22.521, Rotation2d.kZero), - new Pose2d(24.73, 19.68, Rotation2d.fromDegrees(5.846))); - var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, new TrajectoryConfig(8.8, 0.1)); + ExtendedKalmanFilter observer = + new ExtendedKalmanFilter<>( + Nat.N5(), + Nat.N2(), + Nat.N3(), + ExtendedKalmanFilterTest::getDynamics, + ExtendedKalmanFilterTest::getLocalMeasurementModel, + VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0), + VecBuilder.fill(0.001, 0.01, 0.01), + dtSeconds); + + List waypoints = + List.of( + new Pose2d(2.75, 22.521, Rotation2d.kZero), + new Pose2d(24.73, 19.68, Rotation2d.fromDegrees(5.846))); + var trajectory = + TrajectoryGenerator.generateTrajectory(waypoints, new TrajectoryConfig(8.8, 0.1)); Matrix r = new Matrix<>(Nat.N5(), Nat.N1()); Matrix nextR = new Matrix<>(Nat.N5(), Nat.N1()); Matrix u = new Matrix<>(Nat.N2(), Nat.N1()); - var B = NumericalJacobian.numericalJacobianU( - Nat.N5(), - Nat.N2(), - ExtendedKalmanFilterTest::getDynamics, - new Matrix<>(Nat.N5(), Nat.N1()), - u); + var B = + NumericalJacobian.numericalJacobianU( + Nat.N5(), + Nat.N2(), + ExtendedKalmanFilterTest::getDynamics, + new Matrix<>(Nat.N5(), Nat.N1()), + u); observer.setXhat( VecBuilder.fill( @@ -159,8 +169,9 @@ void testConvergence() { observer.predict(u, dtSeconds); - groundTruthX = NumericalIntegration.rk4( - ExtendedKalmanFilterTest::getDynamics, groundTruthX, u, dtSeconds); + groundTruthX = + NumericalIntegration.rk4( + ExtendedKalmanFilterTest::getDynamics, groundTruthX, u, dtSeconds); r = nextR; } diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java index b6a4f1d4eb1..13d17836568 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java @@ -22,7 +22,6 @@ import edu.wpi.first.math.system.Discretization; import edu.wpi.first.math.system.NumericalIntegration; import edu.wpi.first.math.system.NumericalJacobian; -import edu.wpi.first.math.system.plant.Gearbox; import edu.wpi.first.math.system.plant.KnownDCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.math.trajectory.TrajectoryConfig; @@ -36,17 +35,22 @@ private static Matrix getDynamics(Matrix x, Matrix u) { var n = 2; // var gLow = 15.32; // Low gear ratio var gHigh = 7.08; // High gear ratio - + var rb = 0.8382 / 2.0; // Robot radius var r = 0.0746125; // Wheel radius var m = 63.503; // Robot mass var J = 5.6; // Robot moment of inertia var C1 = - -n * Math.pow(gHigh, 2) + -n + * Math.pow(gHigh, 2) * motor.kt.baseUnitMagnitude() / (motor.kv.baseUnitMagnitude() * motor.internalResistance.baseUnitMagnitude() * r * r); - var C2 = n * gHigh * motor.kt.baseUnitMagnitude() / (motor.internalResistance.baseUnitMagnitude() * r); + var C2 = + n + * gHigh + * motor.kt.baseUnitMagnitude() + / (motor.internalResistance.baseUnitMagnitude() * r); var k1 = 1.0 / m + Math.pow(rb, 2) / J; var k2 = 1.0 / m - Math.pow(rb, 2) / J; diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java index 24d12dc81a1..9d5c48179ab 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java @@ -19,7 +19,8 @@ class LinearSystemIDTest { @Test void testDrivetrainVelocitySystem() { var model = - LinearSystemId.createDrivetrainVelocitySystem(new Gearbox(KnownDCMotor.NEO.dcMotor, 4, 6), 70, 0.05, 0.4, 6.0); + LinearSystemId.createDrivetrainVelocitySystem( + new Gearbox(KnownDCMotor.NEO.dcMotor, 4, 6.0), 70.0, 0.05, 0.4, 6.0); assertTrue( model .getA() @@ -42,7 +43,8 @@ void testDrivetrainVelocitySystem() { @Test void testElevatorSystem() { - var model = LinearSystemId.createElevatorSystem(new Gearbox(KnownDCMotor.NEO.dcMotor, 2, 12), 5, 0.05); + var model = + LinearSystemId.createElevatorSystem(new Gearbox(KnownDCMotor.NEO.dcMotor, 2, 12), 5, 0.05); assertTrue( model.getA().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -99.05473), 0.001)); @@ -55,7 +57,8 @@ void testElevatorSystem() { @Test void testFlywheelSystem() { - var model = LinearSystemId.createFlywheelSystem(new Gearbox(KnownDCMotor.NEO.dcMotor, 2), 0.00032); + var model = + LinearSystemId.createFlywheelSystem(new Gearbox(KnownDCMotor.NEO.dcMotor, 2), 0.00032); assertTrue(model.getA().isEqual(VecBuilder.fill(-26.87032), 0.001)); assertTrue(model.getB().isEqual(VecBuilder.fill(1354.166667), 0.001)); diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/plant/proto/DCMotorProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/plant/proto/DCMotorProtoTest.java index bba79138909..21e057c9c17 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/system/plant/proto/DCMotorProtoTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/system/plant/proto/DCMotorProtoTest.java @@ -24,7 +24,8 @@ void testRoundtrip() { assertEquals(DATA.stallCurrent.baseUnitMagnitude(), data.stallCurrent.baseUnitMagnitude()); assertEquals(DATA.freeCurrent.baseUnitMagnitude(), data.freeCurrent.baseUnitMagnitude()); assertEquals(DATA.freeSpeed.baseUnitMagnitude(), data.freeSpeed.baseUnitMagnitude()); - assertEquals(DATA.internalResistance.baseUnitMagnitude(), data.internalResistance.baseUnitMagnitude()); + assertEquals( + DATA.internalResistance.baseUnitMagnitude(), data.internalResistance.baseUnitMagnitude()); assertEquals(DATA.kv.baseUnitMagnitude(), data.kv.baseUnitMagnitude()); assertEquals(DATA.kt.baseUnitMagnitude(), data.kt.baseUnitMagnitude()); } diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/plant/struct/DCMotorStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/plant/struct/DCMotorStructTest.java index a770c6a0a25..38dadeaa17f 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/system/plant/struct/DCMotorStructTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/system/plant/struct/DCMotorStructTest.java @@ -27,7 +27,8 @@ void testRoundtrip() { assertEquals(DATA.stallCurrent.baseUnitMagnitude(), data.stallCurrent.baseUnitMagnitude()); assertEquals(DATA.freeCurrent.baseUnitMagnitude(), data.freeCurrent.baseUnitMagnitude()); assertEquals(DATA.freeSpeed.baseUnitMagnitude(), data.freeSpeed.baseUnitMagnitude()); - assertEquals(DATA.internalResistance.baseUnitMagnitude(), data.internalResistance.baseUnitMagnitude()); + assertEquals( + DATA.internalResistance.baseUnitMagnitude(), data.internalResistance.baseUnitMagnitude()); assertEquals(DATA.kv.baseUnitMagnitude(), data.kv.baseUnitMagnitude()); assertEquals(DATA.kt.baseUnitMagnitude(), data.kt.baseUnitMagnitude()); } From 0d048de2b2967bfe0942b7d15e00ad7c4eb9348b Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Mon, 25 Nov 2024 22:11:38 -0500 Subject: [PATCH 3/7] slight name change to getCurrent methods in Gearbox --- .../java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java | 2 +- .../java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java | 2 +- .../java/edu/wpi/first/wpilibj/simulation/GearboxSim.java | 2 +- .../wpi/first/wpilibj/simulation/SingleJointedArmSim.java | 2 +- .../main/java/edu/wpi/first/math/system/plant/Gearbox.java | 6 +++--- 5 files changed, 7 insertions(+), 7 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java index 2d5234a7b80..29d362dc77a 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java @@ -204,7 +204,7 @@ public double getCurrentDrawAmps() { double kV = -m_plant.getA().get(1, 1) * kA; double motorVelocityRadPerSec = m_x.get(1, 0) * kV * m_gearbox.dcMotor.kv.baseUnitMagnitude(); var appliedVoltage = m_u.get(0, 0); - return m_gearbox.getCurrentAmps( + return m_gearbox.getNetCurrentAmps( m_gearbox.getTorqueNewtonMeters(motorVelocityRadPerSec, appliedVoltage)) * Math.signum(appliedVoltage); } 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 db81be2a9df..960968d031f 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 @@ -146,7 +146,7 @@ public double getTorqueNewtonMeters() { * @return The flywheel's net current draw. */ public double getCurrentDrawAmps() { - return m_gearbox.getCurrentAmps(getTorqueNewtonMeters()) * Math.signum(m_u.get(0, 0)); + return m_gearbox.getNetCurrentAmps(getTorqueNewtonMeters()) * Math.signum(m_u.get(0, 0)); } /** diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/GearboxSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/GearboxSim.java index 42fdd019a05..e2421950657 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/GearboxSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/GearboxSim.java @@ -198,7 +198,7 @@ public double getTorqueNewtonMeters() { * @return The Gearbox's net current draw. */ public double getCurrentDrawAmps() { - return m_gearbox.getCurrentAmps(getTorqueNewtonMeters()) * Math.signum(m_u.get(0, 0)); + return m_gearbox.getNetCurrentAmps(getTorqueNewtonMeters()) * Math.signum(m_u.get(0, 0)); } /** diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java index e34f24f242f..ff57de73899 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java @@ -177,7 +177,7 @@ public double getCurrentDrawAmps() { // Reductions are greater than 1, so a reduction of 10:1 would mean the motor is // spinning 10x faster than the output var motorVelocity = m_x.get(1, 0) * m_gearbox.getGearboxReduction(); - return m_gearbox.getCurrentAmps( + return m_gearbox.getNetCurrentAmps( m_gearbox.getTorqueNewtonMeters(motorVelocity, m_u.get(0, 0)) * Math.signum(m_u.get(0, 0))); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/Gearbox.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/Gearbox.java index 8ec7c4d4d98..5f8e9998f65 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/plant/Gearbox.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/Gearbox.java @@ -194,7 +194,7 @@ public Torque getTorque(AngularVelocity angularVelocity, Voltage voltageInput) { * @param torqueNewtonMeters The torque produced by the gearbox. * @return The net current drawn by the all of the motors. */ - public double getCurrentAmps(double torqueNewtonMeters) { + public double getNetCurrentAmps(double torqueNewtonMeters) { return numMotors * dcMotor.getCurrentAmps(torqueNewtonMeters / numMotors / gearboxReduction); } @@ -204,7 +204,7 @@ public double getCurrentAmps(double torqueNewtonMeters) { * @param torque The torque produced by the gearbox. * @return The net current drawn by the all of the motors. */ - public Current getCurrent(Torque torque) { - return current.mut_setBaseUnitMagnitude(getCurrentAmps(torque.baseUnitMagnitude())); + public Current getNetCurrent(Torque torque) { + return current.mut_setBaseUnitMagnitude(getNetCurrentAmps(torque.baseUnitMagnitude())); } } From bea1a8965dd46a616a55d787137a36d70372a026 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Tue, 26 Nov 2024 03:23:40 +0000 Subject: [PATCH 4/7] Formatting fixes --- .../java/edu/wpi/first/math/system/plant/KnownDCMotor.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/KnownDCMotor.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/KnownDCMotor.java index 278dc43bc93..2db63b6a5a4 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/plant/KnownDCMotor.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/KnownDCMotor.java @@ -1,3 +1,7 @@ +// 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. + package edu.wpi.first.math.system.plant; import static edu.wpi.first.units.Units.Amps; From 1c78830642a7f797de38635f611028f0cae0b4ed Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Tue, 26 Nov 2024 01:04:59 -0500 Subject: [PATCH 5/7] doc fix --- .../src/main/java/edu/wpi/first/math/system/plant/Gearbox.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/Gearbox.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/Gearbox.java index 5f8e9998f65..112102e3566 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/plant/Gearbox.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/Gearbox.java @@ -20,8 +20,11 @@ /** Models the behavior of a gearbox. */ public class Gearbox { + /** The DCMotor for this gearbox. */ public final DCMotor dcMotor; + /** The number of motors for this gearbox. */ public final int numMotors; + /** The reduction of this gear box as a ratio of output rotations to input rotations. */ private double gearboxReduction; /** The returned measure of angular velocity. Exists to minimize calls to GC. */ From 1c068a7ec5995bae33ba273d257a5d80d0e5af92 Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Tue, 26 Nov 2024 09:34:41 -0500 Subject: [PATCH 6/7] doc cleanup --- .../src/main/java/edu/wpi/first/math/system/plant/Gearbox.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/Gearbox.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/Gearbox.java index 112102e3566..c46772b32d5 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/plant/Gearbox.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/Gearbox.java @@ -20,10 +20,13 @@ /** Models the behavior of a gearbox. */ public class Gearbox { + /** The DCMotor for this gearbox. */ public final DCMotor dcMotor; + /** The number of motors for this gearbox. */ public final int numMotors; + /** The reduction of this gear box as a ratio of output rotations to input rotations. */ private double gearboxReduction; From 90386b37e3a9e5e6731d222e8cfb3c764f8ba5a8 Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Tue, 26 Nov 2024 09:37:26 -0500 Subject: [PATCH 7/7] doc fixes --- .../src/main/java/edu/wpi/first/math/system/plant/Gearbox.java | 1 - 1 file changed, 1 deletion(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/Gearbox.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/Gearbox.java index c46772b32d5..b72184e6a23 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/plant/Gearbox.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/Gearbox.java @@ -20,7 +20,6 @@ /** Models the behavior of a gearbox. */ public class Gearbox { - /** The DCMotor for this gearbox. */ public final DCMotor dcMotor;