Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib] Add DCMotorSim (#3910)

Browse Source
This commit is contained in:
modelmat
2022-01-22 04:42:06 +00:00
committed by GitHub
parent a467392cbd
commit 42908126b9
9 changed files with 541 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

36
wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java
View File

@@ -89,6 +89,42 @@ public final class LinearSystemId {
new Matrix<>(Nat.N1(), Nat.N1()));
}
/**
* 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].
*
* @param motor The motor (or gearbox) attached to the DC motor.
* @param jKgMetersSquared The moment of inertia J of the DC motor.
* @param G The reduction between motor and drum, as a ratio of output to input.
* @return A LinearSystem representing the given characterized constants.
* @throws IllegalArgumentException if jKgMetersSquared &lt;= 0 or G &lt;= 0.
*/
@SuppressWarnings("ParameterName")
public static LinearSystem<N2, N1, N2> createDCMotorSystem(
DCMotor motor, double jKgMetersSquared, double G) {
if (jKgMetersSquared <= 0.0) {
throw new IllegalArgumentException("J must be greater than zero.");
}
if (G <= 0.0) {
throw new IllegalArgumentException("G must be greater than zero.");
}
return new LinearSystem<>(
Matrix.mat(Nat.N2(), Nat.N2())
.fill(
0,
1,
0,
-G
* G
* motor.KtNMPerAmp
/ (motor.KvRadPerSecPerVolt * motor.rOhms * jKgMetersSquared)),
VecBuilder.fill(0, G * motor.KtNMPerAmp / (motor.rOhms * jKgMetersSquared)),
Matrix.eye(Nat.N2()),
new Matrix<>(Nat.N2(), Nat.N1()));
}
/**
* Create a state-space model of a differential drive drivetrain. In this model, the states are
* [v_left, v_right]ᵀ, inputs are [V_left, V_right]ᵀ and outputs are [v_left, v_right]ᵀ.