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