Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21:42 +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