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

[wpimath] Add ElevatorFeedforward.calculate(currentV, nextV) overload (#5715)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
narmstro2020
2023-10-09 11:16:45 -04:00
committed by GitHub
parent a789632052
commit faa1e665ba
4 changed files with 122 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -6,7 +6,9 @@
#include <gtest/gtest.h>
#include "frc/EigenCore.h"
#include "frc/controller/ElevatorFeedforward.h"
#include "frc/controller/LinearPlantInversionFeedforward.h"
#include "units/acceleration.h"
#include "units/length.h"
#include "units/time.h"
@@ -18,12 +20,23 @@ static constexpr auto Kg = 1_V;
TEST(ElevatorFeedforwardTest, Calculate) {
frc::ElevatorFeedforward elevatorFF{Ks, Kg, Kv, Ka};
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()};
constexpr units::second_t dt = 20_ms;
frc::LinearPlantInversionFeedforward<1, 1> plantInversion{A, B, dt};
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);
}
TEST(ElevatorFeedforwardTest, AchievableVelocity) {