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 new file mode 100644 index 0000000000..8e3a35b618 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiter.java @@ -0,0 +1,104 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.controller; + +import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.numbers.N2; +import edu.wpi.first.math.system.LinearSystem; + +/** + * Filters the provided voltages to limit a differential drive's linear and angular acceleration. + * + *

The differential drive model can be created via the functions in {@link + * edu.wpi.first.math.system.plant.LinearSystemId}. + */ +public class DifferentialDriveAccelerationLimiter { + private final LinearSystem m_system; + private final double m_trackwidth; + private final double m_maxLinearAccel; + private final double m_maxAngularAccel; + + /** Motor voltages for a differential drive. */ + @SuppressWarnings("MemberName") + public static class WheelVoltages { + public double left; + public double right; + + private WheelVoltages() {} + + public WheelVoltages(double left, double right) { + this.left = left; + this.right = right; + } + } + + /** + * Constructs a DifferentialDriveAccelerationLimiter. + * + * @param system The differential drive dynamics. + * @param trackwidth The trackwidth. + * @param maxLinearAccel The maximum linear acceleration in meters per second squared. + * @param maxAngularAccel The maximum angular acceleration in radians per second squared. + */ + public DifferentialDriveAccelerationLimiter( + LinearSystem system, + double trackwidth, + double maxLinearAccel, + double maxAngularAccel) { + m_system = system; + m_trackwidth = trackwidth; + m_maxLinearAccel = maxLinearAccel; + m_maxAngularAccel = maxAngularAccel; + } + + /** + * Returns the next voltage pair subject to acceleraiton constraints. + * + * @param leftVelocity The left wheel velocity in meters per second. + * @param rightVelocity The right wheel velocity in meters per second. + * @param leftVoltage The unconstrained left motor voltage. + * @param rightVoltage The unconstrained right motor voltage. + * @return The constrained wheel voltages. + */ + @SuppressWarnings("LocalVariableName") + public WheelVoltages calculate( + double leftVelocity, double rightVelocity, double leftVoltage, double rightVoltage) { + var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(leftVoltage, rightVoltage); + + // Find unconstrained wheel accelerations + 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 + 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); + if (accels.get(0, 0) > m_maxLinearAccel) { + accels.set(0, 0, m_maxLinearAccel); + } else if (accels.get(0, 0) < -m_maxLinearAccel) { + accels.set(0, 0, -m_maxLinearAccel); + } + if (accels.get(1, 0) > m_maxAngularAccel) { + accels.set(1, 0, m_maxAngularAccel); + } else if (accels.get(1, 0) < -m_maxAngularAccel) { + accels.set(1, 0, -m_maxAngularAccel); + } + 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))); + + return new WheelVoltages(u.get(0, 0), u.get(1, 0)); + } +} diff --git a/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp b/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp new file mode 100644 index 0000000000..1d03ffd47a --- /dev/null +++ b/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp @@ -0,0 +1,60 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/controller/DifferentialDriveAccelerationLimiter.h" + +#include + +#include "Eigen/QR" + +using namespace frc; + +DifferentialDriveAccelerationLimiter::DifferentialDriveAccelerationLimiter( + LinearSystem<2, 2, 2> system, units::meter_t trackwidth, + units::meters_per_second_squared_t maxLinearAccel, + units::radians_per_second_squared_t maxAngularAccel) + : m_system{std::move(system)}, + m_trackwidth{trackwidth}, + m_maxLinearAccel{maxLinearAccel}, + m_maxAngularAccel{maxAngularAccel} {} + +DifferentialDriveAccelerationLimiter::WheelVoltages +DifferentialDriveAccelerationLimiter::Calculate( + units::meters_per_second_t leftVelocity, + units::meters_per_second_t rightVelocity, units::volt_t leftVoltage, + units::volt_t rightVoltage) { + Eigen::Vector u{leftVoltage.value(), rightVoltage.value()}; + + // Find unconstrained wheel accelerations + Eigen::Vector x{leftVelocity.value(), rightVelocity.value()}; + Eigen::Vector 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 + Eigen::Matrix 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 + Eigen::Vector accels = M * dxdt; + if (accels(0) > m_maxLinearAccel.value()) { + accels(0) = m_maxLinearAccel.value(); + } else if (accels(0) < -m_maxLinearAccel.value()) { + accels(0) = -m_maxLinearAccel.value(); + } + if (accels(1) > m_maxAngularAccel.value()) { + accels(1) = m_maxAngularAccel.value(); + } else if (accels(1) < -m_maxAngularAccel.value()) { + accels(1) = -m_maxAngularAccel.value(); + } + 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); + + return {units::volt_t{u(0)}, units::volt_t{u(1)}}; +} diff --git a/wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h b/wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h new file mode 100644 index 0000000000..d2173c59ba --- /dev/null +++ b/wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h @@ -0,0 +1,70 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include "Eigen/Core" +#include "frc/system/LinearSystem.h" +#include "units/acceleration.h" +#include "units/angular_acceleration.h" +#include "units/length.h" +#include "units/velocity.h" +#include "units/voltage.h" + +namespace frc { + +/** + * Filters the provided voltages to limit a differential drive's linear and + * angular acceleration. + * + * The differential drive model can be created via the functions in + * LinearSystemId. + */ +class WPILIB_DLLEXPORT DifferentialDriveAccelerationLimiter { + public: + /** + * Motor voltages for a differential drive. + */ + struct WheelVoltages { + units::volt_t left = 0_V; + units::volt_t right = 0_V; + }; + + /** + * Constructs a DifferentialDriveAccelerationLimiter. + * + * @param system The differential drive dynamics. + * @param trackwidth The trackwidth. + * @param maxLinearAccel The maximum linear acceleration. + * @param maxAngularAccel The maximum angular acceleration. + */ + DifferentialDriveAccelerationLimiter( + LinearSystem<2, 2, 2> system, units::meter_t trackwidth, + units::meters_per_second_squared_t maxLinearAccel, + units::radians_per_second_squared_t maxAngularAccel); + + /** + * Returns the next voltage pair subject to acceleraiton constraints. + * + * @param leftVelocity The left wheel velocity. + * @param rightVelocity The right wheel velocity. + * @param leftVoltage The unconstrained left motor voltage. + * @param rightVoltage The unconstrained right motor voltage. + * @return The constrained wheel voltages. + */ + WheelVoltages Calculate(units::meters_per_second_t leftVelocity, + units::meters_per_second_t rightVelocity, + units::volt_t leftVoltage, + units::volt_t rightVoltage); + + private: + LinearSystem<2, 2, 2> m_system; + units::meter_t m_trackwidth; + units::meters_per_second_squared_t m_maxLinearAccel; + units::radians_per_second_squared_t m_maxAngularAccel; +}; + +} // namespace frc diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java new file mode 100644 index 0000000000..7ae700a34c --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java @@ -0,0 +1,209 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.controller; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.system.plant.LinearSystemId; +import org.junit.jupiter.api.Test; + +class DifferentialDriveAccelerationLimiterTest { + @Test + @SuppressWarnings("LocalVariableName") + void testLowLimits() { + final double trackwidth = 0.9; + final double dt = 0.005; + final double maxA = 2.0; + final double maxAlpha = 2.0; + + var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0); + + var accelLimiter = new DifferentialDriveAccelerationLimiter(plant, trackwidth, maxA, maxAlpha); + + var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); + var xAccelLimiter = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); + + // Ensure voltage exceeds acceleration before limiting + { + final var accels = + plant + .getA() + .times(xAccelLimiter) + .plus(plant.getB().times(new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0))); + final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0; + assertTrue(Math.abs(a) > maxA); + } + { + final var accels = + plant + .getA() + .times(xAccelLimiter) + .plus(plant.getB().times(new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0))); + final double alpha = (accels.get(1, 0) - accels.get(0, 0)) / trackwidth; + assertTrue(Math.abs(alpha) > maxAlpha); + } + + // Forward + var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0); + for (double t = 0.0; t < 3.0; t += dt) { + x = plant.calculateX(x, u, dt); + final var voltages = + accelLimiter.calculate( + xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0)); + xAccelLimiter = + plant.calculateX( + xAccelLimiter, + new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right), + dt); + + final var accels = + plant + .getA() + .times(xAccelLimiter) + .plus( + plant + .getB() + .times( + new MatBuilder<>(Nat.N2(), Nat.N1()) + .fill(voltages.left, voltages.right))); + final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0; + final double alpha = (accels.get(1, 0) - accels.get(0, 0)) / trackwidth; + assertTrue(Math.abs(a) <= maxA); + assertTrue(Math.abs(alpha) <= maxAlpha); + } + + // Backward + u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0); + for (double t = 0.0; t < 3.0; t += dt) { + x = plant.calculateX(x, u, dt); + final var voltages = + accelLimiter.calculate( + xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0)); + xAccelLimiter = + plant.calculateX( + xAccelLimiter, + new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right), + dt); + + final var accels = + plant + .getA() + .times(xAccelLimiter) + .plus( + plant + .getB() + .times( + new MatBuilder<>(Nat.N2(), Nat.N1()) + .fill(voltages.left, voltages.right))); + final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0; + final double alpha = (accels.get(1, 0) - accels.get(0, 0)) / trackwidth; + assertTrue(Math.abs(a) <= maxA); + assertTrue(Math.abs(alpha) <= maxAlpha); + } + + // Rotate CCW + u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0); + for (double t = 0.0; t < 3.0; t += dt) { + x = plant.calculateX(x, u, dt); + final var voltages = + accelLimiter.calculate( + xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0)); + xAccelLimiter = + plant.calculateX( + xAccelLimiter, + new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right), + dt); + + final var accels = + plant + .getA() + .times(xAccelLimiter) + .plus( + plant + .getB() + .times( + new MatBuilder<>(Nat.N2(), Nat.N1()) + .fill(voltages.left, voltages.right))); + final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0; + final double alpha = (accels.get(1, 0) - accels.get(0, 0)) / trackwidth; + assertTrue(Math.abs(a) <= maxA); + assertTrue(Math.abs(alpha) <= maxAlpha); + } + } + + @Test + @SuppressWarnings("LocalVariableName") + void testHighLimits() { + final double trackwidth = 0.9; + final double dt = 0.005; + + var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0); + + // Limits are so high, they don't get hit, so states of constrained and + // unconstrained systems should match + var accelLimiter = new DifferentialDriveAccelerationLimiter(plant, trackwidth, 1e3, 1e3); + + var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); + var xAccelLimiter = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); + + // Forward + var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0); + for (double t = 0.0; t < 3.0; t += dt) { + x = plant.calculateX(x, u, dt); + final var voltages = + accelLimiter.calculate( + xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0)); + xAccelLimiter = + plant.calculateX( + xAccelLimiter, + new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right), + dt); + + assertEquals(x.get(0, 0), xAccelLimiter.get(0, 0), 1e-5); + assertEquals(x.get(1, 0), xAccelLimiter.get(1, 0), 1e-5); + } + + // Backward + x.fill(0.0); + xAccelLimiter.fill(0.0); + u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0); + for (double t = 0.0; t < 3.0; t += dt) { + x = plant.calculateX(x, u, dt); + final var voltages = + accelLimiter.calculate( + xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0)); + xAccelLimiter = + plant.calculateX( + xAccelLimiter, + new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right), + dt); + + assertEquals(x.get(0, 0), xAccelLimiter.get(0, 0), 1e-5); + assertEquals(x.get(1, 0), xAccelLimiter.get(1, 0), 1e-5); + } + + // Rotate CCW + x.fill(0.0); + xAccelLimiter.fill(0.0); + u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0); + for (double t = 0.0; t < 3.0; t += dt) { + x = plant.calculateX(x, u, dt); + final var voltages = + accelLimiter.calculate( + xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0)); + xAccelLimiter = + plant.calculateX( + xAccelLimiter, + new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right), + dt); + + assertEquals(x.get(0, 0), xAccelLimiter.get(0, 0), 1e-5); + assertEquals(x.get(1, 0), xAccelLimiter.get(1, 0), 1e-5); + } + } +} diff --git a/wpimath/src/test/native/cpp/controller/DifferentialDriveAccelerationLimiterTest.cpp b/wpimath/src/test/native/cpp/controller/DifferentialDriveAccelerationLimiterTest.cpp new file mode 100644 index 0000000000..31927ff2f8 --- /dev/null +++ b/wpimath/src/test/native/cpp/controller/DifferentialDriveAccelerationLimiterTest.cpp @@ -0,0 +1,179 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include + +#include "frc/controller/DifferentialDriveAccelerationLimiter.h" +#include "frc/system/plant/LinearSystemId.h" +#include "units/math.h" + +namespace frc { + +TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) { + constexpr auto trackwidth = 0.9_m; + constexpr auto dt = 5_ms; + constexpr auto maxA = 2_mps_sq; + constexpr auto maxAlpha = 2_rad_per_s_sq; + + using Kv_t = decltype(1_V / 1_mps); + using Ka_t = decltype(1_V / 1_mps_sq); + auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0}, + Kv_t{1.0}, Ka_t{1.0}); + + DifferentialDriveAccelerationLimiter accelLimiter{plant, trackwidth, maxA, + maxAlpha}; + + Eigen::Vector x{0.0, 0.0}; + Eigen::Vector xAccelLimiter{0.0, 0.0}; + + // Ensure voltage exceeds acceleration before limiting + { + Eigen::Vector accels = + plant.A() * xAccelLimiter + + plant.B() * Eigen::Vector{12.0, 12.0}; + units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0}; + EXPECT_GT(units::math::abs(a), maxA); + } + { + Eigen::Vector accels = + plant.A() * xAccelLimiter + + plant.B() * Eigen::Vector{-12.0, 12.0}; + units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) / + trackwidth.value()}; + EXPECT_GT(units::math::abs(alpha), maxAlpha); + } + + // Forward + Eigen::Vector u{12.0, 12.0}; + for (auto t = 0_s; t < 3_s; t += dt) { + x = plant.CalculateX(x, u, dt); + auto [left, right] = + accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)}, + units::meters_per_second_t{xAccelLimiter(1)}, + units::volt_t{u(0)}, units::volt_t{u(1)}); + xAccelLimiter = plant.CalculateX(xAccelLimiter, + Eigen::Vector{left, right}, dt); + + Eigen::Vector accels = + plant.A() * xAccelLimiter + + plant.B() * Eigen::Vector{left, right}; + units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0}; + units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) / + trackwidth.value()}; + EXPECT_LE(units::math::abs(a), maxA); + EXPECT_LE(units::math::abs(alpha), maxAlpha); + } + + // Backward + u = Eigen::Vector{-12.0, -12.0}; + for (auto t = 0_s; t < 3_s; t += dt) { + x = plant.CalculateX(x, u, dt); + auto [left, right] = + accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)}, + units::meters_per_second_t{xAccelLimiter(1)}, + units::volt_t{u(0)}, units::volt_t{u(1)}); + xAccelLimiter = plant.CalculateX(xAccelLimiter, + Eigen::Vector{left, right}, dt); + + Eigen::Vector accels = + plant.A() * xAccelLimiter + + plant.B() * Eigen::Vector{left, right}; + units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0}; + units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) / + trackwidth.value()}; + EXPECT_LE(units::math::abs(a), maxA); + EXPECT_LE(units::math::abs(alpha), maxAlpha); + } + + // Rotate CCW + u = Eigen::Vector{-12.0, 12.0}; + for (auto t = 0_s; t < 3_s; t += dt) { + x = plant.CalculateX(x, u, dt); + auto [left, right] = + accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)}, + units::meters_per_second_t{xAccelLimiter(1)}, + units::volt_t{u(0)}, units::volt_t{u(1)}); + xAccelLimiter = plant.CalculateX(xAccelLimiter, + Eigen::Vector{left, right}, dt); + + Eigen::Vector accels = + plant.A() * xAccelLimiter + + plant.B() * Eigen::Vector{left, right}; + units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0}; + units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) / + trackwidth.value()}; + EXPECT_LE(units::math::abs(a), maxA); + EXPECT_LE(units::math::abs(alpha), maxAlpha); + } +} + +TEST(DifferentialDriveAccelerationLimiterTest, HighLimits) { + constexpr auto trackwidth = 0.9_m; + constexpr auto dt = 5_ms; + + using Kv_t = decltype(1_V / 1_mps); + using Ka_t = decltype(1_V / 1_mps_sq); + + auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0}, + Kv_t{1.0}, Ka_t{1.0}); + + // Limits are so high, they don't get hit, so states of constrained and + // unconstrained systems should match + DifferentialDriveAccelerationLimiter accelLimiter{ + plant, trackwidth, 1e3_mps_sq, 1e3_rad_per_s_sq}; + + Eigen::Vector x{0.0, 0.0}; + Eigen::Vector xAccelLimiter{0.0, 0.0}; + + // Forward + Eigen::Vector u{12.0, 12.0}; + for (auto t = 0_s; t < 3_s; t += dt) { + x = plant.CalculateX(x, u, dt); + auto [left, right] = + accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)}, + units::meters_per_second_t{xAccelLimiter(1)}, + units::volt_t{u(0)}, units::volt_t{u(1)}); + xAccelLimiter = plant.CalculateX(xAccelLimiter, + Eigen::Vector{left, right}, dt); + + EXPECT_DOUBLE_EQ(x(0), xAccelLimiter(0)); + EXPECT_DOUBLE_EQ(x(1), xAccelLimiter(1)); + } + + // Backward + x.setZero(); + xAccelLimiter.setZero(); + u = Eigen::Vector{-12.0, -12.0}; + for (auto t = 0_s; t < 3_s; t += dt) { + x = plant.CalculateX(x, u, dt); + auto [left, right] = + accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)}, + units::meters_per_second_t{xAccelLimiter(1)}, + units::volt_t{u(0)}, units::volt_t{u(1)}); + xAccelLimiter = plant.CalculateX(xAccelLimiter, + Eigen::Vector{left, right}, dt); + + EXPECT_DOUBLE_EQ(x(0), xAccelLimiter(0)); + EXPECT_DOUBLE_EQ(x(1), xAccelLimiter(1)); + } + + // Rotate CCW + x.setZero(); + xAccelLimiter.setZero(); + u = Eigen::Vector{-12.0, 12.0}; + for (auto t = 0_s; t < 3_s; t += dt) { + x = plant.CalculateX(x, u, dt); + auto [left, right] = + accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)}, + units::meters_per_second_t{xAccelLimiter(1)}, + units::volt_t{u(0)}, units::volt_t{u(1)}); + xAccelLimiter = plant.CalculateX(xAccelLimiter, + Eigen::Vector{left, right}, dt); + + EXPECT_DOUBLE_EQ(x(0), xAccelLimiter(0)); + EXPECT_DOUBLE_EQ(x(1), xAccelLimiter(1)); + } +} + +} // namespace frc