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

[wpimath] Fully discretized ElevatorFF and ArmFF (#7024)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Nicholas Armstrong
2024-10-11 01:10:45 -04:00
committed by GitHub
parent 5d9a553104
commit 4adfa8bf64
31 changed files with 1004 additions and 425 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -62,7 +62,7 @@ void CalculateAndSimulate(const frc::ArmFeedforward& armFF,
units::radians_per_second_t currentVelocity,
units::radians_per_second_t nextVelocity,
units::second_t dt) {
auto input = armFF.Calculate(currentAngle, currentVelocity, nextVelocity, dt);
auto input = armFF.Calculate(currentAngle, currentVelocity, nextVelocity);
EXPECT_NEAR(nextVelocity.value(),
Simulate(currentAngle, currentVelocity, input, dt)(1), 1e-12);
}
@@ -80,17 +80,6 @@ TEST(ArmFeedforwardTest, Calculate) {
armFF.Calculate(std::numbers::pi / 3 * 1_rad, 1_rad_per_s).value(), 2.5,
0.002);
// Calculate(angle, angular velocity, angular acceleration)
EXPECT_NEAR(
armFF.Calculate(std::numbers::pi / 3 * 1_rad, 1_rad_per_s, 2_rad_per_s_sq)
.value(),
6.5, 0.002);
EXPECT_NEAR(
armFF
.Calculate(std::numbers::pi / 3 * 1_rad, -1_rad_per_s, 2_rad_per_s_sq)
.value(),
2.5, 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);

6
wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp
View File

@@ -23,10 +23,6 @@ TEST(ElevatorFeedforwardTest, Calculate) {
EXPECT_NEAR(elevatorFF.Calculate(0_m / 1_s).value(), Kg.value(), 0.002);
EXPECT_NEAR(elevatorFF.Calculate(2_m / 1_s).value(), 4.5, 0.002);
EXPECT_NEAR(elevatorFF.Calculate(2_m / 1_s, 1_m / 1_s / 1_s).value(), 6.5,
0.002);
EXPECT_NEAR(elevatorFF.Calculate(-2_m / 1_s, 1_m / 1_s / 1_s).value(), -0.5,
0.002);
frc::Matrixd<1, 1> A{-Kv.value() / Ka.value()};
frc::Matrixd<1, 1> B{1.0 / Ka.value()};
@@ -36,7 +32,7 @@ TEST(ElevatorFeedforwardTest, Calculate) {
frc::Vectord<1> r{2.0};
frc::Vectord<1> nextR{3.0};
EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks.value() + Kg.value(),
elevatorFF.Calculate(2_mps, 3_mps, dt).value(), 0.002);
elevatorFF.Calculate(2_mps, 3_mps).value(), 0.002);
}
TEST(ElevatorFeedforwardTest, AchievableVelocity) {