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

[wpilib] Add physics simulation support with state-space (#2615)

Browse Source 
This includes physics simulation support for arms/elevator models, as well as differential drivetrains.

Swerve might be added at a later date.

Co-authored-by: Claudius Tewari <cttewari@gmail.com>
Co-authored-by: Prateek Machiraju <prateek.machiraju@gmail.com>
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Matt
2020-09-20 09:39:52 -07:00
committed by GitHub
parent 0503225928
commit b61f08d3fa
43 changed files with 3787 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/DCMotor.java
View File

@@ -48,10 +48,21 @@ public class DCMotor {
this.m_rOhms = nominalVoltageVolts / stallCurrentAmps;
this.m_KvRadPerSecPerVolt = freeSpeedRadPerSec / (nominalVoltageVolts - m_rOhms
* freeCurrentAmps);
* freeCurrentAmps);
this.m_KtNMPerAmp = stallTorqueNewtonMeters / stallCurrentAmps;
}
/**
* Estimate the current being drawn by this motor.
*
* @param speedRadiansPerSec The speed of the rotor.
* @param voltageInputVolts The input voltage.
*/
public double getCurrent(double speedRadiansPerSec, double voltageInputVolts) {
return -1.0 / m_KvRadPerSecPerVolt / m_rOhms * speedRadiansPerSec
+ 1.0 / m_rOhms * voltageInputVolts;
}
/**
* Return a gearbox of CIM motors.
*
@@ -59,8 +70,8 @@ public class DCMotor {
*/
public static DCMotor getCIM(int numMotors) {
return new DCMotor(12,
2.42 * numMotors, 133,
2.7, Units.rotationsPerMinuteToRadiansPerSecond(5310));
2.42 * numMotors, 133,
2.7, Units.rotationsPerMinuteToRadiansPerSecond(5310));
}
/**
@@ -70,8 +81,8 @@ public class DCMotor {
*/
public static DCMotor getVex775Pro(int numMotors) {
return gearbox(new DCMotor(12,
0.71, 134,
0.7, Units.rotationsPerMinuteToRadiansPerSecond(18730)), numMotors);
0.71, 134,
0.7, Units.rotationsPerMinuteToRadiansPerSecond(18730)), numMotors);
}
/**
@@ -81,7 +92,7 @@ public class DCMotor {
*/
public static DCMotor getNEO(int numMotors) {
return gearbox(new DCMotor(12, 2.6,
105, 1.8, Units.rotationsPerMinuteToRadiansPerSecond(5676)), numMotors);
105, 1.8, Units.rotationsPerMinuteToRadiansPerSecond(5676)), numMotors);
}
/**
@@ -91,7 +102,7 @@ public class DCMotor {
*/
public static DCMotor getMiniCIM(int numMotors) {
return gearbox(new DCMotor(12, 1.41, 89, 3,
Units.rotationsPerMinuteToRadiansPerSecond(5840)), numMotors);
Units.rotationsPerMinuteToRadiansPerSecond(5840)), numMotors);
}
/**
@@ -101,7 +112,7 @@ public class DCMotor {
*/
public static DCMotor getBag(int numMotors) {
return gearbox(new DCMotor(12, 0.43, 53, 1.8,
Units.rotationsPerMinuteToRadiansPerSecond(13180)), numMotors);
Units.rotationsPerMinuteToRadiansPerSecond(13180)), numMotors);
}
/**
@@ -111,7 +122,7 @@ public class DCMotor {
*/
public static DCMotor getAndymarkRs775_125(int numMotors) {
return gearbox(new DCMotor(12, 0.28, 18, 1.6,
Units.rotationsPerMinuteToRadiansPerSecond(5800.0)), numMotors);
Units.rotationsPerMinuteToRadiansPerSecond(5800.0)), numMotors);
}
/**
@@ -121,7 +132,7 @@ public class DCMotor {
*/
public static DCMotor getBanebotsRs775(int numMotors) {
return gearbox(new DCMotor(12, 0.72, 97, 2.7,
Units.rotationsPerMinuteToRadiansPerSecond(13050.0)), numMotors);
Units.rotationsPerMinuteToRadiansPerSecond(13050.0)), numMotors);
}
/**
@@ -131,7 +142,7 @@ public class DCMotor {
*/
public static DCMotor getAndymark9015(int numMotors) {
return gearbox(new DCMotor(12, 0.36, 71, 3.7,
Units.rotationsPerMinuteToRadiansPerSecond(14270.0)), numMotors);
Units.rotationsPerMinuteToRadiansPerSecond(14270.0)), numMotors);
}
/**
@@ -141,7 +152,7 @@ public class DCMotor {
*/
public static DCMotor getBanebotsRs550(int numMotors) {
return gearbox(new DCMotor(12, 0.38, 84, 0.4,
Units.rotationsPerMinuteToRadiansPerSecond(19000.0)), numMotors);
Units.rotationsPerMinuteToRadiansPerSecond(19000.0)), numMotors);
}
/**
@@ -151,7 +162,7 @@ public class DCMotor {
*/
public static DCMotor getNeo550(int numMotors) {
return gearbox(new DCMotor(12, 0.97, 100, 1.4,
Units.rotationsPerMinuteToRadiansPerSecond(11000.0)), numMotors);
Units.rotationsPerMinuteToRadiansPerSecond(11000.0)), numMotors);
}
/**
@@ -161,11 +172,11 @@ public class DCMotor {
*/
public static DCMotor getFalcon500(int numMotors) {
return gearbox(new DCMotor(12, 4.69, 257, 1.5,
Units.rotationsPerMinuteToRadiansPerSecond(6380.0)), numMotors);
Units.rotationsPerMinuteToRadiansPerSecond(6380.0)), numMotors);
}
private static DCMotor gearbox(DCMotor motor, double numMotors) {
return new DCMotor(motor.m_nominalVoltageVolts, motor.m_stallTorqueNewtonMeters * numMotors,
motor.m_stallCurrentAmps, motor.m_freeCurrentAmps, motor.m_freeSpeedRadPerSec);
motor.m_stallCurrentAmps, motor.m_freeCurrentAmps, motor.m_freeSpeedRadPerSec);
}
}

4
wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/LinearSystemId.java
View File

@@ -65,7 +65,9 @@ public final class LinearSystemId {
}
/**
* Create a state-space model of a differential drive drivetrain.
* Create a state-space model of a differential drive drivetrain. In this model, the
* states are [v_left, v_right]^T, inputs are [V_left, V_right]^T and outputs are
* [v_left, v_right]^T.
*
* @param motor the gearbox representing the motors driving the drivetrain.
* @param massKg the mass of the robot.