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

[wpimath] Improve DifferentialDriveAccelerationLimiter docs (NFC) (#4323)

Browse Source 
Defined trackwidth, added skipped steps to the algorithm's internal
proof, and grouped the algorithm steps more logically with blank lines.
This commit is contained in:
Tyler Veness
2022-07-01 06:43:57 -07:00
committed by GitHub
parent 82fac41244
commit add00a96ed
3 changed files with 40 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp
View File

@@ -29,15 +29,23 @@ DifferentialDriveWheelVoltages DifferentialDriveAccelerationLimiter::Calculate(
Vectord<2> x{leftVelocity.value(), rightVelocity.value()};
Vectord<2> dxdt = m_system.A() * x + m_system.B() * u;
// Converts from wheel accelerations to linear and angular acceleration
// a = (dxdt(0) + dxdt(1)) / 2.0
// alpha = (dxdt(1) - dxdt(0)) / trackwidth
// Convert from wheel accelerations to linear and angular accelerations
//
// a = (dxdt(0) + dx/dt(1)) / 2
// = 0.5 dxdt(0) + 0.5 dxdt(1)
//
// α = (dxdt(1) - dxdt(0)) / trackwidth
// = -1/trackwidth dxdt(0) + 1/trackwidth dxdt(1)
//
// [a] = [ 0.5 0.5][dxdt(0)]
// [α] [-1/trackwidth 1/trackwidth][dxdt(1)]
//
// accels = M dxdt where M = [0.5, 0.5; -1/trackwidth, 1/trackwidth]
Matrixd<2, 2> M{{0.5, 0.5},
{-1.0 / m_trackwidth.value(), 1.0 / m_trackwidth.value()}};
// Convert to linear and angular accelerations, constrain them, then convert
// back
Vectord<2> accels = M * dxdt;
// Constrain the linear and angular accelerations
if (accels(0) > m_maxLinearAccel.value()) {
accels(0) = m_maxLinearAccel.value();
} else if (accels(0) < -m_maxLinearAccel.value()) {
@@ -48,9 +56,13 @@ DifferentialDriveWheelVoltages DifferentialDriveAccelerationLimiter::Calculate(
} else if (accels(1) < -m_maxAngularAccel.value()) {
accels(1) = -m_maxAngularAccel.value();
}
// Convert the constrained linear and angular accelerations back to wheel
// accelerations
dxdt = M.householderQr().solve(accels);
// Find voltages for the given wheel accelerations
//
// dx/dt = Ax + Bu
// u = B⁻¹(dx/dt - Ax)
u = m_system.B().householderQr().solve(dxdt - m_system.A() * x);