Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-02 02:51:42 +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

16
wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java
View File

@@ -6,6 +6,10 @@ package edu.wpi.first.math.controller;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.numbers.N1;
import org.junit.jupiter.api.Test;
class ElevatorFeedforwardTest {
@@ -22,6 +26,18 @@ class ElevatorFeedforwardTest {
assertEquals(4.5, m_elevatorFF.calculate(2), 0.002);
assertEquals(6.5, m_elevatorFF.calculate(2, 1), 0.002);
assertEquals(-0.5, m_elevatorFF.calculate(-2, 1), 0.002);
var A = Matrix.mat(Nat.N1(), Nat.N1()).fill(-kv / ka);
var B = Matrix.mat(Nat.N1(), Nat.N1()).fill(1.0 / ka);
final double dt = 0.02;
var plantInversion = new LinearPlantInversionFeedforward<N1, N1, N1>(A, B, dt);
var r = VecBuilder.fill(2.0);
var nextR = VecBuilder.fill(3.0);
assertEquals(
plantInversion.calculate(r, nextR).get(0, 0) + ks + kg,
m_elevatorFF.calculate(2.0, 3.0, dt),
0.002);
}
@Test

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) {