Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-05 03:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add DifferentialDriveAccelerationLimiter (#4091)

Browse Source
This commit is contained in:
Tyler Veness
2022-04-24 07:21:40 -07:00
committed by GitHub
parent 3919250da2
commit 5ebe911933
5 changed files with 622 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

104
wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiter.java Normal file
View File

@@ -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.
*
* <p>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<N2, N2, N2> 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<N2, N2, N2> 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));
}
}

60
wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp Normal file
View File

@@ -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 <utility>
#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<double, 2> u{leftVoltage.value(), rightVoltage.value()};
// Find unconstrained wheel accelerations
Eigen::Vector<double, 2> x{leftVelocity.value(), rightVelocity.value()};
Eigen::Vector<double, 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
Eigen::Matrix<double, 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
Eigen::Vector<double, 2> 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)}};
}

70
wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h Normal file
View File

@@ -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 <wpi/SymbolExports.h>
#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

209
wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java Normal file
View File

@@ -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);
}
}
}

179
wpimath/src/test/native/cpp/controller/DifferentialDriveAccelerationLimiterTest.cpp Normal file
View File

@@ -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 <gtest/gtest.h>
#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<double, 2> x{0.0, 0.0};
Eigen::Vector<double, 2> xAccelLimiter{0.0, 0.0};
// Ensure voltage exceeds acceleration before limiting
{
Eigen::Vector<double, 2> accels =
plant.A() * xAccelLimiter +
plant.B() * Eigen::Vector<double, 2>{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<double, 2> accels =
plant.A() * xAccelLimiter +
plant.B() * Eigen::Vector<double, 2>{-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<double, 2> 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<double, 2>{left, right}, dt);
Eigen::Vector<double, 2> accels =
plant.A() * xAccelLimiter +
plant.B() * Eigen::Vector<double, 2>{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<double, 2>{-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<double, 2>{left, right}, dt);
Eigen::Vector<double, 2> accels =
plant.A() * xAccelLimiter +
plant.B() * Eigen::Vector<double, 2>{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<double, 2>{-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<double, 2>{left, right}, dt);
Eigen::Vector<double, 2> accels =
plant.A() * xAccelLimiter +
plant.B() * Eigen::Vector<double, 2>{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<double, 2> x{0.0, 0.0};
Eigen::Vector<double, 2> xAccelLimiter{0.0, 0.0};
// Forward
Eigen::Vector<double, 2> 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<double, 2>{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<double, 2>{-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<double, 2>{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<double, 2>{-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<double, 2>{left, right}, dt);
EXPECT_DOUBLE_EQ(x(0), xAccelLimiter(0));
EXPECT_DOUBLE_EQ(x(1), xAccelLimiter(1));
}
}
} // namespace frc