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

[wpimath] Add full state support to LinearSystemId functions (#6554)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Nicholas Armstrong
2024-05-15 09:23:22 -04:00
committed by GitHub
parent 7fbbecb5b7
commit 7fc17811fa
29 changed files with 343 additions and 88 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSim.java
View File

@@ -141,7 +141,7 @@ public class DCMotorSim extends LinearSystemSim<N2, N1, N2> {
// 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(getAngularVelocityRadPerSec() * m_gearing, m_u.get(0, 0))
return m_gearbox.getCurrent(m_x.get(1, 0) * m_gearing, m_u.get(0, 0))
* Math.signum(m_u.get(0, 0));
}

17
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java
View File

@@ -15,7 +15,7 @@ import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.system.plant.LinearSystemId;
/** Represents a simulated elevator mechanism. */
public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
public class ElevatorSim extends LinearSystemSim<N2, N1, N2> {
// Gearbox for the elevator.
private final DCMotor m_gearbox;
@@ -43,13 +43,13 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
*/
@SuppressWarnings("this-escape")
public ElevatorSim(
LinearSystem<N2, N1, N1> plant,
LinearSystem<N2, N1, N2> plant,
DCMotor gearbox,
double minHeightMeters,
double maxHeightMeters,
boolean simulateGravity,
double startingHeightMeters,
Matrix<N1, N1> measurementStdDevs) {
Matrix<N2, N1> measurementStdDevs) {
super(plant, measurementStdDevs);
m_gearbox = gearbox;
m_minHeight = minHeightMeters;
@@ -72,7 +72,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
* @param simulateGravity Whether gravity should be simulated or not.
*/
public ElevatorSim(
LinearSystem<N2, N1, N1> plant,
LinearSystem<N2, N1, N2> plant,
DCMotor gearbox,
double minHeightMeters,
double maxHeightMeters,
@@ -138,7 +138,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
double maxHeightMeters,
boolean simulateGravity,
double startingHeightMeters,
Matrix<N1, N1> measurementStdDevs) {
Matrix<N2, N1> measurementStdDevs) {
this(
LinearSystemId.identifyPositionSystem(kV, kA),
gearbox,
@@ -171,7 +171,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
double maxHeightMeters,
boolean simulateGravity,
double startingHeightMeters,
Matrix<N1, N1> measurementStdDevs) {
Matrix<N2, N1> measurementStdDevs) {
this(
LinearSystemId.createElevatorSystem(gearbox, carriageMassKg, drumRadiusMeters, gearing),
gearbox,
@@ -281,7 +281,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
* @return The velocity of the elevator.
*/
public double getVelocityMetersPerSecond() {
return m_x.get(1, 0);
return getOutput(1);
}
/**
@@ -297,8 +297,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
// 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 =
getVelocityMetersPerSecond() * kV * m_gearbox.KvRadPerSecPerVolt;
double motorVelocityRadPerSec = m_x.get(1, 0) * kV * m_gearbox.KvRadPerSecPerVolt;
var appliedVoltage = m_u.get(0, 0);
return m_gearbox.getCurrent(motorVelocityRadPerSec, appliedVoltage)
* Math.signum(appliedVoltage);

14
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java
View File

@@ -15,7 +15,7 @@ import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.system.plant.LinearSystemId;
/** Represents a simulated single jointed arm mechanism. */
public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N2> {
// The gearbox for the arm.
private final DCMotor m_gearbox;
@@ -51,7 +51,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
*/
@SuppressWarnings("this-escape")
public SingleJointedArmSim(
LinearSystem<N2, N1, N1> plant,
LinearSystem<N2, N1, N2> plant,
DCMotor gearbox,
double gearing,
double armLengthMeters,
@@ -59,7 +59,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
double maxAngleRads,
boolean simulateGravity,
double startingAngleRads,
Matrix<N1, N1> measurementStdDevs) {
Matrix<N2, N1> measurementStdDevs) {
super(plant, measurementStdDevs);
m_gearbox = gearbox;
m_gearing = gearing;
@@ -86,7 +86,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
* @param startingAngleRads The initial position of the Arm simulation in radians.
*/
public SingleJointedArmSim(
LinearSystem<N2, N1, N1> plant,
LinearSystem<N2, N1, N2> plant,
DCMotor gearbox,
double gearing,
double armLengthMeters,
@@ -161,7 +161,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
double maxAngleRads,
boolean simulateGravity,
double startingAngleRads,
Matrix<N1, N1> measurementStdDevs) {
Matrix<N2, N1> measurementStdDevs) {
this(
LinearSystemId.createSingleJointedArmSystem(gearbox, jKgMetersSquared, gearing),
gearbox,
@@ -230,7 +230,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
* @return The current arm angle.
*/
public double getAngleRads() {
return m_y.get(0, 0);
return getOutput(0);
}
/**
@@ -239,7 +239,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
* @return The current arm velocity.
*/
public double getVelocityRadPerSec() {
return m_x.get(1, 0);
return getOutput(1);
}
/**