diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiter.java b/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiter.java index 94d2c8fd33..7a0ff5e69d 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiter.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiter.java @@ -25,7 +25,8 @@ public class DifferentialDriveAccelerationLimiter { * Constructs a DifferentialDriveAccelerationLimiter. * * @param system The differential drive dynamics. - * @param trackwidth The trackwidth. + * @param trackwidth The distance between the differential drive's left and right wheels in + * meters. * @param maxLinearAccel The maximum linear acceleration in meters per second squared. * @param maxAngularAccel The maximum angular acceleration in radians per second squared. */ @@ -58,16 +59,24 @@ public class DifferentialDriveAccelerationLimiter { var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(leftVelocity, rightVelocity); var dxdt = m_system.getA().times(x).plus(m_system.getB().times(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] var M = new MatBuilder<>(Nat.N2(), Nat.N2()) .fill(0.5, 0.5, -1.0 / m_trackwidth, 1.0 / m_trackwidth); - - // Convert to linear and angular accelerations, constrain them, then convert - // back var accels = M.times(dxdt); + + // Constrain the linear and angular accelerations if (accels.get(0, 0) > m_maxLinearAccel) { accels.set(0, 0, m_maxLinearAccel); } else if (accels.get(0, 0) < -m_maxLinearAccel) { @@ -78,9 +87,13 @@ public class DifferentialDriveAccelerationLimiter { } else if (accels.get(1, 0) < -m_maxAngularAccel) { accels.set(1, 0, -m_maxAngularAccel); } + + // Convert the constrained linear and angular accelerations back to wheel + // accelerations dxdt = M.solve(accels); // Find voltages for the given wheel accelerations + // // dx/dt = Ax + Bu // u = B⁻¹(dx/dt - Ax) u = m_system.getB().solve(dxdt.minus(m_system.getA().times(x))); diff --git a/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp b/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp index b0eb440b34..5729e00955 100644 --- a/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp +++ b/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp @@ -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); diff --git a/wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h b/wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h index 34370f4b15..8891c7a599 100644 --- a/wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h +++ b/wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h @@ -30,7 +30,8 @@ class WPILIB_DLLEXPORT DifferentialDriveAccelerationLimiter { * Constructs a DifferentialDriveAccelerationLimiter. * * @param system The differential drive dynamics. - * @param trackwidth The trackwidth. + * @param trackwidth The distance between the differential drive's left and + * right wheels. * @param maxLinearAccel The maximum linear acceleration. * @param maxAngularAccel The maximum angular acceleration. */