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..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 @@ -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.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 95d01e16217..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 @@ -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.getNetCurrentAmps(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 51% 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..e2421950657 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; + return m_gearbox.getTorqueNewtonMeters(m_x.get(1, 0), m_u.get(0, 0)); } /** - * 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.getNetCurrentAmps(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..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 @@ -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.getNetCurrentAmps( + 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..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 @@ -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,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 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 +45,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..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 @@ -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,11 @@ 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..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 @@ -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,8 @@ 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 +57,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..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,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 +28,8 @@ 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..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 @@ -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,11 @@ 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..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 @@ -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,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(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..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 @@ -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,8 @@ @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 +51,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..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 @@ -4,37 +4,51 @@ 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 +59,106 @@ 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; + public double getVoltageInputVolts( + double torqueNewtonMeters, double angularVelocityRadiansPerSec) { + return 1.0 / kv.baseUnitMagnitude() * angularVelocityRadiansPerSec + + 1.0 + / kt.baseUnitMagnitude() + * internalResistance.baseUnitMagnitude() + * torqueNewtonMeters; } /** - * Calculates the angular speed 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 torqueNm The torque produced by the motor. + * @param torqueNewtonMeters 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); - } - - /** - * Return a gearbox of MiniCIM motors. - * - * @param numMotors Number of motors in the gearbox. - * @return A gearbox of MiniCIM motors. - */ - public static DCMotor getMiniCIM(int numMotors) { - return new DCMotor( - 12, 1.41, 89, 3, Units.rotationsPerMinuteToRadiansPerSecond(5840), numMotors); - } - - /** - * Return a gearbox of Bag motors. - * - * @param numMotors Number of motors in the gearbox. - * @return A gearbox of Bag motors. + * @return The angular velocity of the motor. */ - public static DCMotor getBag(int numMotors) { - return new DCMotor( - 12, 0.43, 53, 1.8, Units.rotationsPerMinuteToRadiansPerSecond(13180), 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 Andymark RS775-125 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 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..b72184e6a23 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/Gearbox.java @@ -0,0 +1,215 @@ +// 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 { + /** 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. */ + 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. + */ + 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 getNetCurrentAmps(double torqueNewtonMeters) { + return numMotors * dcMotor.getCurrentAmps(torqueNewtonMeters / numMotors / gearboxReduction); + } + + /** + * Calculate net current drawn by all of the motors for a given torque. + * + * @param torque The torque produced by the gearbox. + * @return The net current drawn by the all of the motors. + */ + public Current getNetCurrent(Torque torque) { + return current.mut_setBaseUnitMagnitude(getNetCurrentAmps(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..2db63b6a5a4 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/KnownDCMotor.java @@ -0,0 +1,99 @@ +// 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.RPM; +import static edu.wpi.first.units.Units.Volts; + +/** Holds the constants for known DC motors. */ +public enum KnownDCMotor { + /** 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), + + /** + * Falcon500 constants. + * + * @see Falcon 500 + * Documentation + */ + Falcon500(12, 4.69, 257, 1.5, 6380), + + /** + * Falcon 500 FOC constants. + * + * @see Falcon 500 + * FOC (Field-Oriented Control) Documentation + */ + Falcon500Foc(12, 5.84, 304, 1.5, 6080), + + /** + * Romi BuiltIn constants. + * + * @see Romi Built-In Documentation + */ + RomiBuiltIn(4.5, 0.1765, 1.25, 0.13, 150), + + /** + * Kraken X60 constants. + * + * @see Kraken X60 + * Documentation + */ + KrakenX60(12, 7.09, 366, 2, 6000), + + /** + * Kraken X60 FOC constants. + * + * @see Kraken X60 FOC + * (Field-Oriented Control) Documentation + */ + KrakenX60Foc(12, 9.37, 483, 2, 5800), + + /** + * 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; + + 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..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 @@ -22,137 +22,86 @@ private LinearSystemId() { * Create a state-space model of an elevator system. The states of the system are [position, * velocity]ᵀ, inputs are [voltage], and outputs are [position]. * - * @param motor The motor (or gearbox) attached to the carriage. + * @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 * 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); } /** @@ -160,23 +109,16 @@ public static LinearSystem createDCMotorSystem(double kV, double kA) * [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 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, J <= 0, or gearing - * <= 0. + * @throws IllegalArgumentException if m ≤ 0, r ≤ 0, rb ≤ 0, or J ≤ 0. */ public static LinearSystem createDrivetrainVelocitySystem( - DCMotor motor, - double massKg, - double rMeters, - double rbMeters, - double JKgMetersSquared, - double gearing) { + Gearbox gearbox, double massKg, double rMeters, double rbMeters, double JKgMetersSquared) { if (massKg <= 0.0) { throw new IllegalArgumentException("massKg must be greater than zero."); } @@ -189,15 +131,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 = n * G * ktNMPerAmp / (rOhms * rMeters); final double C3 = 1 / massKg + rbMeters * rbMeters / JKgMetersSquared; final double C4 = 1 / massKg - rbMeters * rbMeters / JKgMetersSquared; @@ -213,34 +155,27 @@ public static LinearSystem createDrivetrainVelocitySystem( * 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); } /** @@ -258,8 +193,8 @@ public static LinearSystem createSingleJointedArmSystem( * @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) { @@ -291,8 +226,8 @@ public static LinearSystem identifyVelocitySystem(double kV, double * @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) { @@ -325,7 +260,7 @@ public static LinearSystem identifyPositionSystem(double kV, double * @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 + * @see https://github.com/wpilibsuite/sysid */ public static LinearSystem identifyDrivetrainSystem( double kVLinear, double kALinear, double kVAngular, double kAAngular) { @@ -372,7 +307,7 @@ public static LinearSystem identifyDrivetrainSystem( * @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 + * @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..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,6 +4,11 @@ package edu.wpi.first.math.system.plant.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; @@ -37,15 +42,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..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 @@ -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,9 +43,12 @@ 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 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) @@ -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); @@ -159,9 +166,12 @@ 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); 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..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 @@ -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,8 +28,11 @@ 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 = @@ -94,8 +99,10 @@ void testStateSpaceEnabled() { @Test void testFlywheelEnabled() { - LinearSystem plant = - LinearSystemId.createFlywheelSystem(DCMotor.getNEO(2), 0.00289, 1.0); + 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); 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..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 @@ -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,22 @@ 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); + -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; 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..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,7 @@ 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.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 +31,26 @@ 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..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 @@ -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,8 @@ 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.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(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 +57,8 @@ 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 +68,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..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 @@ -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,14 @@ 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..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 @@ -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,14 @@ 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()); } }