Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[wpimath] Cleanup and Enhancement of DCMotor and gearboxes #7435

Open
wants to merge 8 commits into
base: main
Choose a base branch
from
+817 −727
Open

[wpimath] Cleanup and Enhancement of DCMotor and gearboxes #7435

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
db56d09
initial commit
narmstro2020 Nov 26, 2024
844135e
java first draft complete
narmstro2020 Nov 26, 2024
0d048de
slight name change to getCurrent methods in Gearbox
narmstro2020 Nov 26, 2024
bea1a89
Formatting fixes
github-actions[bot] Nov 26, 2024
1c78830
doc fix
narmstro2020 Nov 26, 2024
248978b
Merge branch 'Motor-Gearbox' of https://github.com/narmstro2020/allwp…
narmstro2020 Nov 26, 2024
1c068a7
doc cleanup
narmstro2020 Nov 26, 2024
90386b3
doc fixes
narmstro2020 Nov 26, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
71 changes: 32 additions & 39 deletions wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -38,7 +39,7 @@
* <p>y = x
*/
public class DifferentialDrivetrainSim {
private final DCMotor m_motor;
private final Gearbox m_gearbox;
private final double m_originalGearing;
private final Matrix<N7, N1> m_measurementStdDevs;
private double m_currentGearing;
Expand All @@ -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.
Expand All @@ -68,23 +67,16 @@ public class DifferentialDrivetrainSim {
* point.
*/
public DifferentialDrivetrainSim(
DCMotor driveMotor,
double gearing,
Gearbox gearbox,
double jKgMetersSquared,
double massKg,
double wheelRadiusMeters,
double trackWidthMeters,
Matrix<N7, N1> 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);
Expand All @@ -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.
Expand All @@ -113,15 +103,14 @@ public DifferentialDrivetrainSim(
*/
public DifferentialDrivetrainSim(
LinearSystem<N2, N2, N2> plant,
DCMotor driveMotor,
double gearing,
Gearbox gearbox,
double trackWidthMeters,
double wheelRadiusMeters,
Matrix<N7, N1> 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;
Expand Down Expand Up @@ -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));
}

Expand All @@ -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));
}

Expand Down Expand Up @@ -309,12 +300,14 @@ public void setPose(Pose2d pose) {
* @return The state derivative with respect to time.
*/
protected Matrix<N7, N1> getDynamics(Matrix<N7, N1> x, Matrix<N2, N1> 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(
Expand Down Expand Up @@ -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;
}
}
Expand Down Expand Up @@ -487,9 +480,9 @@ public static DifferentialDrivetrainSim createKitbotSim(
KitbotWheelSize wheelSize,
double jKgMetersSquared,
Matrix<N7, N1> measurementStdDevs) {
motor.value.withReduction(gearing.value);
return new DifferentialDrivetrainSim(
motor.value,
gearing.value,
jKgMetersSquared,
Units.lbsToKilograms(60),
wheelSize.value / 2.0,
Expand Down
27 changes: 11 additions & 16 deletions wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<N2, N1, N2> {
// 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;
Expand All @@ -33,8 +33,8 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N2> {
* 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.
Expand All @@ -46,7 +46,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N2> {
@SuppressWarnings("this-escape")
public ElevatorSim(
LinearSystem<N2, N1, N2> plant,
DCMotor gearbox,
Gearbox gearbox,
double minHeightMeters,
double maxHeightMeters,
boolean simulateGravity,
Expand Down Expand Up @@ -77,7 +77,7 @@ public ElevatorSim(
public ElevatorSim(
double kV,
double kA,
DCMotor gearbox,
Gearbox gearbox,
double minHeightMeters,
double maxHeightMeters,
boolean simulateGravity,
Expand All @@ -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.
Expand All @@ -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,
Expand All @@ -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,
Expand Down Expand Up @@ -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);
}

Expand Down
51 changes: 16 additions & 35 deletions wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -20,10 +20,7 @@
/** Represents a simulated flywheel mechanism. */
public class FlywheelSim extends LinearSystemSim<N1, N1, N1> {
// 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;
Expand All @@ -32,15 +29,14 @@ public class FlywheelSim extends LinearSystemSim<N1, N1, N1> {
* 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<N1, N1, N1> plant, DCMotor gearbox, double... measurementStdDevs) {
LinearSystem<N1, N1, N1> plant, Gearbox gearbox, double... measurementStdDevs) {
super(plant, measurementStdDevs);
m_gearbox = gearbox;

Expand All @@ -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));
}

/**
Expand All @@ -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.
*
Expand All @@ -96,7 +81,7 @@ public double getJKgMetersSquared() {
*
* @return The flywheel's gearbox.
*/
public DCMotor getGearbox() {
public Gearbox getGearbox() {
return m_gearbox;
}

Expand Down Expand Up @@ -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));
}

/**
Expand Down
Loading
Loading