diff --git a/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.h index 213b5a51c6..7dd9d9788c 100644 --- a/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.h @@ -98,8 +98,7 @@ class Drivetrain { frc::Field2d m_fieldSim; frc::LinearSystem<2, 2, 2> m_drivetrainSystem = frc::LinearSystemId::IdentifyDrivetrainSystem( - 1.98_V / 1_mps, 0.2_V / 1_mps_sq, 1.5_V / 1_rad_per_s, - 0.3_V / 1_rad_per_s_sq); + 1.98_V / 1_mps, 0.2_V / 1_mps_sq, 1.5_V / 1_mps, 0.3_V / 1_mps_sq); frc::sim::DifferentialDrivetrainSim m_drivetrainSimulator{ m_drivetrainSystem, kTrackWidth, frc::DCMotor::CIM(2), 8, 2_in}; }; diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Constants.h index 8a47ff8bb0..ed68cf4f78 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Constants.h +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Constants.h @@ -52,11 +52,11 @@ constexpr double kEncoderDistancePerPulse = // Toolsuite provides a convenient tool for obtaining these values for your // robot. constexpr auto ks = 0.22_V; -constexpr auto kv = 1.98 * 1_V * 1_s / 1_m; -constexpr auto ka = 0.2 * 1_V * 1_s * 1_s / 1_m; +constexpr auto kv = 1.98 * 1_V / 1_mps; +constexpr auto ka = 0.2 * 1_V / 1_mps_sq; -constexpr auto kvAngular = 1.5 * 1_V * 1_s / 1_rad; -constexpr auto kaAngular = 0.3 * 1_V * 1_s * 1_s / 1_rad; +constexpr auto kvAngular = 1.5 * 1_V / 1_mps; +constexpr auto kaAngular = 0.3 * 1_V / 1_mps_sq; extern const frc::LinearSystem<2, 2, 2> kDrivetrainPlant; diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java index fc517f4928..12e0b0fbea 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java @@ -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 + * https://github.com/wpilibsuite/frc-characterization + */ + @SuppressWarnings("ParameterName") + public static LinearSystem 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 * https://github.com/wpilibsuite/frc-characterization */ @SuppressWarnings("ParameterName") public static LinearSystem 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); } } diff --git a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h index cd30ca653e..dcc6786e13 100644 --- a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h +++ b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h @@ -10,6 +10,7 @@ #include "units/acceleration.h" #include "units/angular_acceleration.h" #include "units/angular_velocity.h" +#include "units/length.h" #include "units/moment_of_inertia.h" #include "units/velocity.h" #include "units/voltage.h" @@ -156,33 +157,68 @@ class LinearSystemId { * Inputs: [[left voltage], [right voltage]] * Outputs: [[left velocity], [right velocity]] * - * @param kVlinear The linear velocity gain, in volt seconds per distance. - * @param kAlinear The linear acceleration gain, in volt seconds^2 per - * distance. - * @param kVangular The angular velocity gain, in volt seconds per angle. - * @param kAangular The angular acceleration gain, in volt seconds^2 per - * angle. + * @param kVlinear The linear velocity gain in volts per (meter per second). + * @param kAlinear The linear acceleration gain in volts per (meter per + * second squared). + * @param kVangular The angular velocity gain in volts per (meter per second). + * @param kAangular The angular acceleration gain in volts per (meter per + * second squared). */ static LinearSystem<2, 2, 2> IdentifyDrivetrainSystem( decltype(1_V / 1_mps) kVlinear, decltype(1_V / 1_mps_sq) kAlinear, - decltype(1_V / 1_rad_per_s) kVangular, - decltype(1_V / 1_rad_per_s_sq) kAangular) { - double c = 0.5 / (kAlinear.to() * kAangular.to()); - double A1 = c * (-kAlinear.to() * kVangular.to() - - kVlinear.to() * kAangular.to()); - double A2 = c * (kAlinear.to() * kVangular.to() - - kVlinear.to() * kAangular.to()); - double B1 = c * (kAlinear.to() + kAangular.to()); - double B2 = c * (kAangular.to() - kAlinear.to()); + decltype(1_V / 1_mps) kVangular, decltype(1_V / 1_mps_sq) kAangular) { + double A1 = -(kVlinear.to() / kAlinear.to() + + kVangular.to() / kAangular.to()); + double A2 = -(kVlinear.to() / kAlinear.to() - + kVangular.to() / kAangular.to()); + double B1 = 1.0 / kAlinear.to() + 1.0 / kAangular.to(); + double B2 = 1.0 / kAlinear.to() - 1.0 / kAangular.to(); - auto A = frc::MakeMatrix<2, 2>(A1, A2, A2, A1); - auto B = frc::MakeMatrix<2, 2>(B1, B2, B2, B1); + auto A = 0.5 * frc::MakeMatrix<2, 2>(A1, A2, A2, A1); + auto B = 0.5 * frc::MakeMatrix<2, 2>(B1, B2, B2, B1); auto C = frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0); auto D = frc::MakeMatrix<2, 2>(0.0, 0.0, 0.0, 0.0); return LinearSystem<2, 2, 2>(A, B, C, D); } + /** + * Constructs the state-space model for a 2 DOF drivetrain velocity system + * from system identification data. + * + * States: [[left velocity], [right velocity]] + * Inputs: [[left voltage], [right voltage]] + * Outputs: [[left velocity], [right velocity]] + * + * @param kVlinear The linear velocity gain in volts per (meter per second). + * @param kAlinear The linear acceleration gain in volts per (meter per + * second squared). + * @param kVangular The angular velocity gain in volts per (radian per + * second). + * @param kAangular The angular acceleration gain in volts per (radian per + * second squared). + * @param trackwidth The width of the drivetrain. + */ + static LinearSystem<2, 2, 2> IdentifyDrivetrainSystem( + decltype(1_V / 1_mps) kVlinear, decltype(1_V / 1_mps_sq) kAlinear, + decltype(1_V / 1_rad_per_s) kVangular, + decltype(1_V / 1_rad_per_s_sq) kAangular, units::meter_t 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 * 1_rad, + kAangular * 2.0 / trackwidth * 1_rad); + } + /** * Constructs the state-space model for a flywheel. *