Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'main' into 2027

Browse Source
This commit is contained in:
Peter Johnson
2025-04-25 23:45:43 -07:00
parent 2f0990e9d2 0695a4db89
commit 36811211be
79 changed files with 2093 additions and 415 deletions
Download Patch File Download Diff File Expand all files Collapse all files

59
wpimath/src/test/native/cpp/MathUtilTest.cpp
View File

@@ -3,11 +3,17 @@
// the WPILib BSD license file in the root directory of this project.
#include <limits>
#include <numbers>
#include <gtest/gtest.h>
#include "frc/MathUtil.h"
#include "frc/geometry/Translation2d.h"
#include "frc/geometry/Translation3d.h"
#include "units/angle.h"
#include "units/length.h"
#include "units/time.h"
#include "units/velocity.h"
#define EXPECT_UNITS_EQ(a, b) EXPECT_DOUBLE_EQ((a).value(), (b).value())
@@ -164,3 +170,56 @@ TEST(MathUtilTest, IsNear) {
EXPECT_FALSE(frc::IsNear(400, 395, 5, 0, 360));
EXPECT_FALSE(frc::IsNear(0_deg, -4_deg, 2.5_deg, 0_deg, 360_deg));
}
TEST(MathUtilTest, Translation2dSlewRateLimitUnchanged) {
const frc::Translation2d translation1{0_m, 0_m};
const frc::Translation2d translation2{2_m, 2_m};
const frc::Translation2d result1 =
frc::SlewRateLimit(translation1, translation2, 1_s, 50_mps);
const frc::Translation2d expected1{2_m, 2_m};
EXPECT_EQ(result1, expected1);
}
TEST(MathUtilTest, Translation2dSlewRateLimitChanged) {
const frc::Translation2d translation3{1_m, 1_m};
const frc::Translation2d translation4{3_m, 3_m};
const frc::Translation2d result2 =
frc::SlewRateLimit(translation3, translation4, 0.25_s, 2_mps);
const frc::Translation2d expected2{
units::meter_t{1.0 + 0.5 * (std::numbers::sqrt2 / 2)},
units::meter_t{1.0 + 0.5 * (std::numbers::sqrt2 / 2)}};
EXPECT_EQ(result2, expected2);
}
TEST(MathUtilTest, Translation3dSlewRateLimitUnchanged) {
const frc::Translation3d translation1{0_m, 0_m, 0_m};
const frc::Translation3d translation2{2_m, 2_m, 2_m};
const frc::Translation3d result1 =
frc::SlewRateLimit(translation1, translation2, 1_s, 50.0_mps);
const frc::Translation3d expected1{2_m, 2_m, 2_m};
EXPECT_EQ(result1, expected1);
}
TEST(MathUtilTest, Translation3dSlewRateLimitChanged) {
const frc::Translation3d translation3{1_m, 1_m, 1_m};
const frc::Translation3d translation4{3_m, 3_m, 3_m};
const frc::Translation3d result2 =
frc::SlewRateLimit(translation3, translation4, 0.25_s, 2.0_mps);
const frc::Translation3d expected2{
units::meter_t{1.0 + 0.5 * std::numbers::inv_sqrt3},
units::meter_t{1.0 + 0.5 * std::numbers::inv_sqrt3},
units::meter_t{1.0 + 0.5 * std::numbers::inv_sqrt3}};
EXPECT_EQ(result2, expected2);
}

100
wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp
View File

@@ -15,27 +15,32 @@
#include "units/time.h"
#include "units/voltage.h"
static constexpr auto Ks = 0.5_V;
static constexpr auto Kv = 1.5_V / 1_rad_per_s;
static constexpr auto Ka = 2_V / 1_rad_per_s_sq;
static constexpr auto Kg = 1_V;
namespace {
using Ks_unit = decltype(1_V);
using Kv_unit = decltype(1_V / 1_rad_per_s);
using Ka_unit = decltype(1_V / 1_rad_per_s_sq);
using Kg_unit = decltype(1_V);
/**
* Simulates a single-jointed arm and returns the final state.
*
* @param Ks The static gain, in volts.
* @param Kv The velocity gain, in volt seconds per radian.
* @param Ka The acceleration gain, in volt seconds² per radian.
* @param Kg The gravity gain, in volts.
* @param currentAngle The starting angle.
* @param currentVelocity The starting angular velocity.
* @param input The input voltage.
* @param dt The simulation time.
* @return The final state as a 2-vector of angle and angular velocity.
*/
frc::Matrixd<2, 1> Simulate(units::radian_t currentAngle,
frc::Matrixd<2, 1> Simulate(Ks_unit Ks, Kv_unit Kv, Ka_unit Ka, Kg_unit Kg,
units::radian_t currentAngle,
units::radians_per_second_t currentVelocity,
units::volt_t input, units::second_t dt) {
constexpr frc::Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -Kv.value() / Ka.value()}};
constexpr frc::Matrixd<2, 1> B{{0.0}, {1.0 / Ka.value()}};
frc::Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -Kv.value() / Ka.value()}};
frc::Matrixd<2, 1> B{{0.0}, {1.0 / Ka.value()}};
return frc::RK4(
[&](const frc::Matrixd<2, 1>& x,
@@ -52,24 +57,35 @@ frc::Matrixd<2, 1> Simulate(units::radian_t currentAngle,
* Simulates a single-jointed arm and returns the final state.
*
* @param armFF The feedforward object.
* @param Ks The static gain, in volts.
* @param Kv The velocity gain, in volt seconds per radian.
* @param Ka The acceleration gain, in volt seconds² per radian.
* @param Kg The gravity gain, in volts.
* @param currentAngle The starting angle.
* @param currentVelocity The starting angular velocity.
* @param input The input voltage.
* @param dt The simulation time.
*/
void CalculateAndSimulate(const frc::ArmFeedforward& armFF,
void CalculateAndSimulate(const frc::ArmFeedforward& armFF, Ks_unit Ks,
Kv_unit Kv, Ka_unit Ka, Kg_unit Kg,
units::radian_t currentAngle,
units::radians_per_second_t currentVelocity,
units::radians_per_second_t nextVelocity,
units::second_t dt) {
auto input = armFF.Calculate(currentAngle, currentVelocity, nextVelocity);
EXPECT_NEAR(nextVelocity.value(),
Simulate(currentAngle, currentVelocity, input, dt)(1), 1e-12);
EXPECT_NEAR(
nextVelocity.value(),
Simulate(Ks, Kv, Ka, Kg, currentAngle, currentVelocity, input, dt)(1),
1e-4);
}
} // namespace
TEST(ArmFeedforwardTest, Calculate) {
constexpr auto Ks = 0.5_V;
constexpr auto Kv = 1.5_V / 1_rad_per_s;
constexpr auto Ka = 2_V / 1_rad_per_s_sq;
constexpr auto Kg = 1_V;
frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
// Calculate(angle, angular velocity)
@@ -81,18 +97,54 @@ TEST(ArmFeedforwardTest, Calculate) {
0.002);
// Calculate(currentAngle, currentVelocity, nextAngle, dt)
CalculateAndSimulate(armFF, std::numbers::pi / 3 * 1_rad, 1_rad_per_s,
1.05_rad_per_s, 20_ms);
CalculateAndSimulate(armFF, std::numbers::pi / 3 * 1_rad, 1_rad_per_s,
0.95_rad_per_s, 20_ms);
CalculateAndSimulate(armFF, -std::numbers::pi / 3 * 1_rad, 1_rad_per_s,
1.05_rad_per_s, 20_ms);
CalculateAndSimulate(armFF, -std::numbers::pi / 3 * 1_rad, 1_rad_per_s,
0.95_rad_per_s, 20_ms);
CalculateAndSimulate(armFF, Ks, Kv, Ka, Kg, std::numbers::pi / 3 * 1_rad,
1_rad_per_s, 1.05_rad_per_s, 20_ms);
CalculateAndSimulate(armFF, Ks, Kv, Ka, Kg, std::numbers::pi / 3 * 1_rad,
1_rad_per_s, 0.95_rad_per_s, 20_ms);
CalculateAndSimulate(armFF, Ks, Kv, Ka, Kg, -std::numbers::pi / 3 * 1_rad,
1_rad_per_s, 1.05_rad_per_s, 20_ms);
CalculateAndSimulate(armFF, Ks, Kv, Ka, Kg, -std::numbers::pi / 3 * 1_rad,
1_rad_per_s, 0.95_rad_per_s, 20_ms);
}
TEST(ArmFeedforwardTest, CalculateIllConditionedModel) {
constexpr auto Ks = 0.39671_V;
constexpr auto Kv = 2.7167_V / 1_rad_per_s;
constexpr auto Ka = 1e-2_V / 1_rad_per_s_sq;
constexpr auto Kg = 0.2708_V;
frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
constexpr auto currentAngle = 1_rad;
constexpr auto currentVelocity = 0.02_rad_per_s;
constexpr auto nextVelocity = 0_rad_per_s;
constexpr auto averageAccel = (nextVelocity - currentVelocity) / 20_ms;
EXPECT_DOUBLE_EQ(
armFF.Calculate(currentAngle, currentVelocity, nextVelocity).value(),
(Ks + Kv * currentVelocity + Ka * averageAccel +
Kg * units::math::cos(currentAngle))
.value());
}
TEST(ArmFeedforwardTest, CalculateIllConditionedGradient) {
constexpr auto Ks = 0.39671_V;
constexpr auto Kv = 2.7167_V / 1_rad_per_s;
constexpr auto Ka = 0.50799_V / 1_rad_per_s_sq;
constexpr auto Kg = 0.2708_V;
frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
CalculateAndSimulate(armFF, Ks, Kv, Ka, Kg, 1_rad, 0.02_rad_per_s,
0_rad_per_s, 20_ms);
}
TEST(ArmFeedforwardTest, AchievableVelocity) {
constexpr auto Ks = 0.5_V;
constexpr auto Kv = 1.5_V / 1_rad_per_s;
constexpr auto Ka = 2_V / 1_rad_per_s_sq;
constexpr auto Kg = 1_V;
frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
EXPECT_NEAR(armFF
.MaxAchievableVelocity(12_V, std::numbers::pi / 3 * 1_rad,
1_rad_per_s_sq)
@@ -106,7 +158,12 @@ TEST(ArmFeedforwardTest, AchievableVelocity) {
}
TEST(ArmFeedforwardTest, AchievableAcceleration) {
constexpr auto Ks = 0.5_V;
constexpr auto Kv = 1.5_V / 1_rad_per_s;
constexpr auto Ka = 2_V / 1_rad_per_s_sq;
constexpr auto Kg = 1_V;
frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
EXPECT_NEAR(armFF
.MaxAchievableAcceleration(12_V, std::numbers::pi / 3 * 1_rad,
1_rad_per_s)
@@ -130,7 +187,12 @@ TEST(ArmFeedforwardTest, AchievableAcceleration) {
}
TEST(ArmFeedforwardTest, NegativeGains) {
constexpr auto Ks = 0.5_V;
constexpr auto Kv = 1.5_V / 1_rad_per_s;
constexpr auto Ka = 2_V / 1_rad_per_s_sq;
constexpr auto Kg = 1_V;
frc::ArmFeedforward armFF{Ks, Kg, -Kv, -Ka};
EXPECT_EQ(armFF.GetKv().value(), 0);
EXPECT_EQ(armFF.GetKa().value(), 0);
}

166
wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
View File

@@ -2,7 +2,9 @@
// 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 <algorithm>
#include <cmath>
#include <numbers>
#include <vector>
#include <Eigen/QR>
@@ -12,15 +14,19 @@
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/AngleStatistics.h"
#include "frc/estimator/UnscentedKalmanFilter.h"
#include "frc/system/Discretization.h"
#include "frc/system/NumericalIntegration.h"
#include "frc/system/NumericalJacobian.h"
#include "frc/system/plant/DCMotor.h"
#include "frc/system/plant/LinearSystemId.h"
#include "frc/trajectory/TrajectoryGenerator.h"
#include "units/moment_of_inertia.h"
namespace {
frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) {
// First test system, differential drive
frc::Vectord<5> DriveDynamics(const frc::Vectord<5>& x,
const frc::Vectord<2>& u) {
auto motors = frc::DCMotor::CIM(2);
// constexpr double Glow = 15.32; // Low gear ratio
@@ -51,22 +57,21 @@ frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) {
k1.value() * ((C1 * vr).value() + (C2 * Vr).value())};
}
frc::Vectord<3> LocalMeasurementModel(
frc::Vectord<3> DriveLocalMeasurementModel(
const frc::Vectord<5>& x, [[maybe_unused]] const frc::Vectord<2>& u) {
return frc::Vectord<3>{x(2), x(3), x(4)};
}
frc::Vectord<5> GlobalMeasurementModel(
frc::Vectord<5> DriveGlobalMeasurementModel(
const frc::Vectord<5>& x, [[maybe_unused]] const frc::Vectord<2>& u) {
return frc::Vectord<5>{x(0), x(1), x(2), x(3), x(4)};
}
} // namespace
TEST(UnscentedKalmanFilterTest, Init) {
TEST(UnscentedKalmanFilterTest, DriveInit) {
constexpr auto dt = 5_ms;
frc::UnscentedKalmanFilter<5, 2, 3> observer{Dynamics,
LocalMeasurementModel,
frc::UnscentedKalmanFilter<5, 2, 3> observer{DriveDynamics,
DriveLocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.01, 0.01},
frc::AngleMean<5, 5>(2),
@@ -78,22 +83,22 @@ TEST(UnscentedKalmanFilterTest, Init) {
frc::Vectord<2> u{12.0, 12.0};
observer.Predict(u, dt);
auto localY = LocalMeasurementModel(observer.Xhat(), u);
auto localY = DriveLocalMeasurementModel(observer.Xhat(), u);
observer.Correct(u, localY);
auto globalY = GlobalMeasurementModel(observer.Xhat(), u);
auto globalY = DriveGlobalMeasurementModel(observer.Xhat(), u);
auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
observer.Correct<5>(u, globalY, GlobalMeasurementModel, R,
observer.Correct<5>(u, globalY, DriveGlobalMeasurementModel, R,
frc::AngleMean<5, 5>(2), frc::AngleResidual<5>(2),
frc::AngleResidual<5>(2), frc::AngleAdd<5>(2));
}
TEST(UnscentedKalmanFilterTest, Convergence) {
TEST(UnscentedKalmanFilterTest, DriveConvergence) {
constexpr auto dt = 5_ms;
constexpr auto rb = 0.8382_m / 2.0; // Robot radius
frc::UnscentedKalmanFilter<5, 2, 3> observer{Dynamics,
LocalMeasurementModel,
frc::UnscentedKalmanFilter<5, 2, 3> observer{DriveDynamics,
DriveLocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.5, 0.5},
frc::AngleMean<5, 5>(2),
@@ -112,8 +117,8 @@ TEST(UnscentedKalmanFilterTest, Convergence) {
frc::Vectord<5> r = frc::Vectord<5>::Zero();
frc::Vectord<2> u = frc::Vectord<2>::Zero();
auto B = frc::NumericalJacobianU<5, 5, 2>(Dynamics, frc::Vectord<5>::Zero(),
frc::Vectord<2>::Zero());
auto B = frc::NumericalJacobianU<5, 5, 2>(
DriveDynamics, frc::Vectord<5>::Zero(), frc::Vectord<2>::Zero());
observer.SetXhat(frc::Vectord<5>{
trajectory.InitialPose().Translation().X().value(),
@@ -134,24 +139,25 @@ TEST(UnscentedKalmanFilterTest, Convergence) {
ref.pose.Translation().X().value(), ref.pose.Translation().Y().value(),
ref.pose.Rotation().Radians().value(), vl.value(), vr.value()};
auto localY = LocalMeasurementModel(trueXhat, frc::Vectord<2>::Zero());
auto localY = DriveLocalMeasurementModel(trueXhat, frc::Vectord<2>::Zero());
observer.Correct(u, localY + frc::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
frc::Vectord<5> rdot = (nextR - r) / dt.value();
u = B.householderQr().solve(rdot - Dynamics(r, frc::Vectord<2>::Zero()));
u = B.householderQr().solve(rdot -
DriveDynamics(r, frc::Vectord<2>::Zero()));
observer.Predict(u, dt);
r = nextR;
trueXhat = frc::RK4(Dynamics, trueXhat, u, dt);
trueXhat = frc::RK4(DriveDynamics, trueXhat, u, dt);
}
auto localY = LocalMeasurementModel(trueXhat, u);
auto localY = DriveLocalMeasurementModel(trueXhat, u);
observer.Correct(u, localY);
auto globalY = GlobalMeasurementModel(trueXhat, u);
auto globalY = DriveGlobalMeasurementModel(trueXhat, u);
auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
observer.Correct<5>(u, globalY, GlobalMeasurementModel, R,
observer.Correct<5>(u, globalY, DriveGlobalMeasurementModel, R,
frc::AngleMean<5, 5>(2), frc::AngleResidual<5>(2),
frc::AngleResidual<5>(2), frc::AngleAdd<5>(2)
@@ -168,6 +174,37 @@ TEST(UnscentedKalmanFilterTest, Convergence) {
EXPECT_NEAR(0.0, observer.Xhat(4), 0.1);
}
TEST(UnscentedKalmanFilterTest, LinearUKF) {
constexpr units::second_t dt = 20_ms;
auto plant = frc::LinearSystemId::IdentifyVelocitySystem<units::meters>(
0.02_V / 1_mps, 0.006_V / 1_mps_sq);
frc::UnscentedKalmanFilter<1, 1, 1> observer{
[&](const frc::Vectord<1>& x, const frc::Vectord<1>& u) {
return plant.A() * x + plant.B() * u;
},
[&](const frc::Vectord<1>& x, const frc::Vectord<1>& u) {
return plant.CalculateY(x, u);
},
{0.05},
{1.0},
dt};
frc::Matrixd<1, 1> discA;
frc::Matrixd<1, 1> discB;
frc::DiscretizeAB<1, 1>(plant.A(), plant.B(), dt, &discA, &discB);
frc::Vectord<1> ref{100.0};
frc::Vectord<1> u{0.0};
for (int i = 0; i < 2.0 / dt.value(); ++i) {
observer.Predict(u, dt);
u = discB.householderQr().solve(ref - discA * ref);
}
EXPECT_NEAR(ref(0, 0), observer.Xhat(0), 5);
}
TEST(UnscentedKalmanFilterTest, RoundTripP) {
constexpr auto dt = 5_ms;
@@ -183,3 +220,90 @@ TEST(UnscentedKalmanFilterTest, RoundTripP) {
ASSERT_TRUE(observer.P().isApprox(P));
}
// Second system, single motor feedforward estimator
frc::Vectord<4> MotorDynamics(const frc::Vectord<4>& x,
const frc::Vectord<1>& u) {
double v = x(1);
double kV = x(2);
double kA = x(3);
double V = u(0);
double a = -kV / kA * v + 1.0 / kA * V;
return frc::Vectord<4>{v, a, 0.0, 0.0};
}
frc::Vectord<3> MotorMeasurementModel(const frc::Vectord<4>& x,
const frc::Vectord<1>& u) {
double p = x(0);
double v = x(1);
double kV = x(2);
double kA = x(3);
double V = u(0);
double a = -kV / kA * v + 1.0 / kA * V;
return frc::Vectord<3>{p, v, a};
}
frc::Vectord<1> MotorControlInput(double t) {
return frc::Vectord<1>{
std::clamp(8 * std::sin(std::numbers::pi * std::sqrt(2.0) * t) +
6 * std::sin(std::numbers::pi * std::sqrt(3.0) * t) +
4 * std::sin(std::numbers::pi * std::sqrt(5.0) * t),
-12.0, 12.0)};
}
TEST(UnscentedKalmanFilterTest, MotorConvergence) {
constexpr units::second_t dt = 10_ms;
constexpr int steps = 500;
constexpr double true_kV = 3;
constexpr double true_kA = 0.2;
constexpr double pos_stddev = 0.02;
constexpr double vel_stddev = 0.1;
constexpr double accel_stddev = 0.1;
std::vector<frc::Vectord<4>> states(steps + 1);
std::vector<frc::Vectord<1>> inputs(steps);
std::vector<frc::Vectord<3>> measurements(steps);
states[0] = frc::Vectord<4>{{0.0}, {0.0}, {true_kV}, {true_kA}};
constexpr frc::Matrixd<4, 4> A{{0.0, 1.0, 0.0, 0.0},
{0.0, -true_kV / true_kA, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0}};
constexpr frc::Matrixd<4, 1> B{{0.0}, {1.0 / true_kA}, {0.0}, {0.0}};
frc::Matrixd<4, 4> discA;
frc::Matrixd<4, 1> discB;
frc::DiscretizeAB(A, B, dt, &discA, &discB);
for (int i = 0; i < steps; ++i) {
inputs[i] = MotorControlInput(i * dt.value());
states[i + 1] = discA * states[i] + discB * inputs[i];
measurements[i] =
MotorMeasurementModel(states[i + 1], inputs[i]) +
frc::MakeWhiteNoiseVector(pos_stddev, vel_stddev, accel_stddev);
}
frc::Vectord<4> P0{0.001, 0.001, 10, 10};
frc::UnscentedKalmanFilter<4, 1, 3> observer{
MotorDynamics, MotorMeasurementModel, wpi::array{0.1, 1.0, 1e-10, 1e-10},
wpi::array{pos_stddev, vel_stddev, accel_stddev}, dt};
observer.SetXhat(frc::Vectord<4>{0.0, 0.0, 2.0, 2.0});
observer.SetP(P0.asDiagonal());
for (int i = 0; i < steps; ++i) {
observer.Predict(inputs[i], dt);
observer.Correct(inputs[i], measurements[i]);
}
EXPECT_NEAR(true_kV, observer.Xhat(2), true_kV * 0.5);
EXPECT_NEAR(true_kA, observer.Xhat(3), true_kA * 0.5);
}
} // namespace

19
wpimath/src/test/native/cpp/filter/DebouncerTest.cpp
View File

@@ -56,3 +56,22 @@ TEST_F(DebouncerTest, DebounceBoth) {
EXPECT_FALSE(debouncer.Calculate(false));
}
TEST_F(DebouncerTest, DebounceParams) {
frc::Debouncer debouncer{20_ms, frc::Debouncer::DebounceType::kBoth};
EXPECT_TRUE(debouncer.GetDebounceTime() == 20_ms);
EXPECT_TRUE(debouncer.GetDebounceType() ==
frc::Debouncer::DebounceType::kBoth);
debouncer.SetDebounceTime(100_ms);
EXPECT_TRUE(debouncer.GetDebounceTime() == 100_ms);
debouncer.SetDebounceType(frc::Debouncer::DebounceType::kFalling);
EXPECT_TRUE(debouncer.GetDebounceType() ==
frc::Debouncer::DebounceType::kFalling);
EXPECT_TRUE(debouncer.Calculate(false));
}

7
wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
View File

@@ -234,3 +234,10 @@ TEST(TrapezoidProfileTest, TimingBeforeNegativeGoal) {
}
}
}
TEST(TrapezoidProfileTest, InitalizationOfCurrentState) {
frc::TrapezoidProfile<units::meter>::Constraints constraints{1_mps, 1_mps_sq};
frc::TrapezoidProfile<units::meter> profile{constraints};
EXPECT_NEAR_UNITS(profile.TimeLeftUntil(0_m), 0_s, 1e-10_s);
EXPECT_NEAR_UNITS(profile.TotalTime(), 0_s, 1e-10_s);
}