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

[wpimath] Make SimpleMotorFeedforward only support discrete feedforward (#6647)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Nicholas Armstrong
2024-07-16 20:23:11 -04:00
committed by GitHub
parent 5f261a88af
commit 30c7632ab8
31 changed files with 540 additions and 218 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp
View File

@@ -12,35 +12,34 @@
#include "units/acceleration.h"
#include "units/length.h"
#include "units/time.h"
#include "units/velocity.h"
namespace frc {
TEST(SimpleMotorFeedforwardTest, Calculate) {
double Ks = 0.5;
double Kv = 3.0;
double Ka = 0.6;
auto dt = 0.02_s;
constexpr auto Ks = 0.5_V;
constexpr auto Kv = 3_V / 1_mps;
constexpr auto Ka = 0.6_V / 1_mps_sq;
constexpr units::second_t dt = 20_ms;
Matrixd<1, 1> A{-Kv / Ka};
Matrixd<1, 1> B{1.0 / Ka};
constexpr Matrixd<1, 1> A{{-Kv.value() / Ka.value()}};
constexpr Matrixd<1, 1> B{{1.0 / Ka.value()}};
frc::LinearPlantInversionFeedforward<1, 1> plantInversion{A, B, dt};
frc::SimpleMotorFeedforward<units::meter> simpleMotor{
units::volt_t{Ks}, units::volt_t{Kv} / 1_mps,
units::volt_t{Ka} / 1_mps_sq};
frc::SimpleMotorFeedforward<units::meter> simpleMotor{Ks, Kv, Ka};
Vectord<1> r{2.0};
Vectord<1> nextR{3.0};
constexpr Vectord<1> r{{2.0}};
constexpr Vectord<1> nextR{{3.0}};
EXPECT_NEAR(37.524995834325161 + Ks,
simpleMotor.Calculate(2_mps, 3_mps, dt).value(), 0.002);
EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks,
simpleMotor.Calculate(2_mps, 3_mps, dt).value(), 0.002);
EXPECT_NEAR((37.524995834325161_V + Ks).value(),
simpleMotor.Calculate(2_mps, 3_mps).value(), 0.002);
EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks.value(),
simpleMotor.Calculate(2_mps, 3_mps).value(), 0.002);
// These won't match exactly. It's just an approximation to make sure they're
// in the same ballpark.
EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks,
simpleMotor.Calculate(2_mps, 1_mps / dt).value(), 2.0);
EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks.value(),
simpleMotor.Calculate(2_mps, 3_mps).value(), 2.0);
}
} // namespace frc

12
wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
View File

@@ -45,17 +45,19 @@ TEST(DifferentialDriveVoltageConstraintTest, Constraint) {
point.velocity * point.curvature};
auto [left, right] = kinematics.ToWheelSpeeds(chassisSpeeds);
auto acceleration = point.acceleration;
// Not really a strictly-correct test as we're using the chassis accel
// instead of the wheel accel, but much easier than doing it "properly" and
// a reasonable check anyway
EXPECT_TRUE(feedforward.Calculate(left, point.acceleration) <
EXPECT_TRUE(feedforward.Calculate(left, left + acceleration * dt) <
maxVoltage + 0.05_V);
EXPECT_TRUE(feedforward.Calculate(left, point.acceleration) >
EXPECT_TRUE(feedforward.Calculate(left, left + acceleration * dt) >
-maxVoltage - 0.05_V);
EXPECT_TRUE(feedforward.Calculate(right, point.acceleration) <
EXPECT_TRUE(feedforward.Calculate(right,
right + acceleration * dt) <
maxVoltage + 0.05_V);
EXPECT_TRUE(feedforward.Calculate(right, point.acceleration) >
EXPECT_TRUE(feedforward.Calculate(right, right + acceleration * dt) >
-maxVoltage - 0.05_V);
}
}

52
wpimath/src/test/native/cpp/trajectory/ExponentialProfileTest.cpp
View File

@@ -41,9 +41,9 @@ frc::ExponentialProfile<units::meter, units::volts>::State CheckDynamics(
frc::ExponentialProfile<units::meter, units::volts>::State current,
frc::ExponentialProfile<units::meter, units::volts>::State goal) {
auto next = profile.Calculate(kDt, current, goal);
auto signal = feedforward.Calculate(current.velocity, next.velocity, kDt);
auto signal = feedforward.Calculate(current.velocity, next.velocity);
EXPECT_LE(units::math::abs(signal), constraints.maxInput + 1e-9_V);
EXPECT_LE(units::math::abs(signal), (constraints.maxInput + 1e-9_V));
return next;
}
@@ -52,8 +52,8 @@ TEST(ExponentialProfileTest, ReachesGoal) {
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
0.43277_V / 1_mps_sq};
frc::SimpleMotorFeedforward<units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
frc::ExponentialProfile<units::meter, units::volts>::State goal{10_m, 0_mps};
frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
@@ -69,8 +69,8 @@ TEST(ExponentialProfileTest, PosContinousUnderVelChange) {
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
0.43277_V / 1_mps_sq};
frc::SimpleMotorFeedforward<units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
frc::ExponentialProfile<units::meter, units::volts>::State goal{10_m, 0_mps};
frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
@@ -92,8 +92,8 @@ TEST(ExponentialProfileTest, PosContinousUnderVelChangeBackward) {
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
0.43277_V / 1_mps_sq};
frc::SimpleMotorFeedforward<units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
frc::ExponentialProfile<units::meter, units::volts>::State goal{-10_m, 0_mps};
frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
@@ -114,8 +114,8 @@ TEST(ExponentialProfileTest, Backwards) {
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
0.43277_V / 1_mps_sq};
frc::SimpleMotorFeedforward<units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
frc::ExponentialProfile<units::meter, units::volts>::State goal{-10_m, 0_mps};
frc::ExponentialProfile<units::meter, units::volts>::State state;
@@ -129,8 +129,8 @@ TEST(ExponentialProfileTest, SwitchGoalInMiddle) {
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
0.43277_V / 1_mps_sq};
frc::SimpleMotorFeedforward<units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
frc::ExponentialProfile<units::meter, units::volts>::State goal{-10_m, 0_mps};
frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
@@ -151,8 +151,8 @@ TEST(ExponentialProfileTest, TopSpeed) {
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
0.43277_V / 1_mps_sq};
frc::SimpleMotorFeedforward<units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
frc::ExponentialProfile<units::meter, units::volts>::State goal{40_m, 0_mps};
frc::ExponentialProfile<units::meter, units::volts>::State state;
@@ -172,8 +172,8 @@ TEST(ExponentialProfileTest, TopSpeedBackward) {
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
0.43277_V / 1_mps_sq};
frc::SimpleMotorFeedforward<units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
frc::ExponentialProfile<units::meter, units::volts>::State goal{-40_m, 0_mps};
frc::ExponentialProfile<units::meter, units::volts>::State state;
@@ -193,8 +193,8 @@ TEST(ExponentialProfileTest, HighInitialSpeed) {
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
0.43277_V / 1_mps_sq};
frc::SimpleMotorFeedforward<units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
frc::ExponentialProfile<units::meter, units::volts>::State goal{40_m, 0_mps};
frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 8_mps};
@@ -210,8 +210,8 @@ TEST(ExponentialProfileTest, HighInitialSpeedBackward) {
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
0.43277_V / 1_mps_sq};
frc::SimpleMotorFeedforward<units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
frc::ExponentialProfile<units::meter, units::volts>::State goal{-40_m, 0_mps};
frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, -8_mps};
@@ -278,8 +278,8 @@ TEST(ExponentialProfileTest, TimingToCurrent) {
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
0.43277_V / 1_mps_sq};
frc::SimpleMotorFeedforward<units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
frc::ExponentialProfile<units::meter, units::volts>::State goal{2_m, 0_mps};
frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
@@ -295,8 +295,8 @@ TEST(ExponentialProfileTest, TimingToGoal) {
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
0.43277_V / 1_mps_sq};
frc::SimpleMotorFeedforward<units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
frc::ExponentialProfile<units::meter, units::volts>::State goal{2_m, 0_mps};
frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
@@ -318,8 +318,8 @@ TEST(ExponentialProfileTest, TimingToNegativeGoal) {
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
0.43277_V / 1_mps_sq};
frc::SimpleMotorFeedforward<units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
frc::ExponentialProfile<units::meter, units::volts>::State goal{-2_m, 0_mps};
frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};