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] Fix drivetrain system identification (#3406)

Browse Source 
The units for angular Kv and Ka were inconsistent with the derivation. A
second factory function overload was added for angular units that uses a
trackwidth to convert to the other form.

Notice how section 15.2 of https://file.tavsys.net/control/controls-engineering-in-frc.pdf
defines the angular feedforward as u = Kv,angular v instead of u = Kv,angular + omega.
The units cancel for elements of A but not B, so just the B matrix was incorrect in our code.

This breaks existing C++ code since the units are part of the function
signature.
This commit is contained in:
Tyler Veness
2021-06-05 11:22:05 -07:00
committed by GitHub
parent 989de4a1bf
commit 15c521a7fe
4 changed files with 104 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -177,6 +177,37 @@ public final class LinearSystemId {
VecBuilder.fill(0.0));
}
/**
* Identify a standard differential drive drivetrain, given the drivetrain's kV and kA in both
* linear (volts/(meter/sec) and volts/(meter/sec^2)) and angular (volts/(meter/sec) and
* volts/(meter/sec^2)) cases. This can be found using frc-characterization. The states of the
* system are [left velocity, right velocity]^T, inputs are [left voltage, right voltage]^T, and
* outputs are [left velocity, right velocity]^T.
*
* @param kVLinear The linear velocity gain, volts per (meter per second).
* @param kALinear The linear acceleration gain, volts per (meter per second squared).
* @param kVAngular The angular velocity gain, volts per (meter per second).
* @param kAAngular The angular acceleration gain, volts per (meter per second squared).
* @return A LinearSystem representing the given characterized constants.
* @see <a href="https://github.com/wpilibsuite/frc-characterization">
* https://github.com/wpilibsuite/frc-characterization</a>
*/
@SuppressWarnings("ParameterName")
public static LinearSystem<N2, N2, N2> identifyDrivetrainSystem(
double kVLinear, double kALinear, double kVAngular, double kAAngular) {
final double A1 = 0.5 * -(kVLinear / kALinear + kVAngular / kAAngular);
final double A2 = 0.5 * -(kVLinear / kALinear - kVAngular / kAAngular);
final double B1 = 0.5 * (1.0 / kALinear + 1.0 / kAAngular);
final double B2 = 0.5 * (1.0 / kALinear - 1.0 / kAAngular);
return new LinearSystem<>(
Matrix.mat(Nat.N2(), Nat.N2()).fill(A1, A2, A2, A1),
Matrix.mat(Nat.N2(), Nat.N2()).fill(B1, B2, B2, B1),
Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1),
Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 0, 0, 0));
}
/**
* Identify a standard differential drive drivetrain, given the drivetrain's kV and kA in both
* linear (volts/(meter/sec) and volts/(meter/sec^2)) and angular (volts/(radian/sec) and
@@ -188,24 +219,26 @@ public final class LinearSystemId {
* @param kALinear The linear acceleration gain, volts per (meter per second squared).
* @param kVAngular The angular velocity gain, volts per (radians per second).
* @param kAAngular The angular acceleration gain, volts per (radians per second squared).
* @param trackwidth The width of the drivetrain in meters.
* @return A LinearSystem representing the given characterized constants.
* @see <a href="https://github.com/wpilibsuite/frc-characterization">
* https://github.com/wpilibsuite/frc-characterization</a>
*/
@SuppressWarnings("ParameterName")
public static LinearSystem<N2, N2, N2> identifyDrivetrainSystem(
double kVLinear, double kALinear, double kVAngular, double kAAngular) {
final double c = 0.5 / (kALinear * kAAngular);
final double A1 = c * (-kALinear * kVAngular - kVLinear * kAAngular);
final double A2 = c * (kALinear * kVAngular - kVLinear * kAAngular);
final double B1 = c * (kALinear + kAAngular);
final double B2 = c * (kAAngular - kALinear);
return new LinearSystem<>(
Matrix.mat(Nat.N2(), Nat.N2()).fill(A1, A2, A2, A1),
Matrix.mat(Nat.N2(), Nat.N2()).fill(B1, B2, B2, B1),
Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1),
Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 0, 0, 0));
double kVLinear, double kALinear, double kVAngular, double kAAngular, double trackwidth) {
// We want to find a factor to include in Kv,angular that will convert
// `u = Kv,angular omega` to `u = Kv,angular v`.
//
// v = omega r
// omega = v/r
// omega = 1/r v
// omega = 1/(trackwidth/2) v
// omega = 2/trackwidth v
//
// So multiplying by 2/trackwidth converts the angular gains from V/(rad/s)
// to V/m/s).
return identifyDrivetrainSystem(
kVLinear, kALinear, kVAngular * 2.0 / trackwidth, kAAngular * 2.0 / trackwidth);
}
}