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

21
wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java
View File

@@ -4,6 +4,9 @@
package edu.wpi.first.math.controller;
import static edu.wpi.first.units.Units.Radians;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.Volts;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertThrows;
@@ -68,19 +71,23 @@ class ArmFeedforwardTest {
*/
private void calculateAndSimulate(
double currentAngle, double currentVelocity, double nextVelocity, double dt) {
final double input = m_armFF.calculate(currentAngle, currentVelocity, nextVelocity, dt);
final double input =
m_armFF
.calculate(
Radians.of(currentAngle),
RadiansPerSecond.of(currentVelocity),
RadiansPerSecond.of(nextVelocity))
.in(Volts);
assertEquals(nextVelocity, simulate(currentAngle, currentVelocity, input, dt).get(1, 0), 1e-12);
}
@Test
void testCalculate() {
// calculate(angle, angular velocity)
assertEquals(0.5, m_armFF.calculate(Math.PI / 3, 0), 0.002);
assertEquals(2.5, m_armFF.calculate(Math.PI / 3, 1), 0.002);
// calculate(angle, angular velocity, angular acceleration)
assertEquals(6.5, m_armFF.calculate(Math.PI / 3, 1, 2), 0.002);
assertEquals(2.5, m_armFF.calculate(Math.PI / 3, -1, 2), 0.002);
assertEquals(
0.5, m_armFF.calculate(Radians.of(Math.PI / 3), RadiansPerSecond.of(0)).in(Volts), 0.002);
assertEquals(
2.5, m_armFF.calculate(Radians.of(Math.PI / 3), RadiansPerSecond.of(1)).in(Volts), 0.002);
// calculate(currentAngle, currentVelocity, nextAngle, dt)
calculateAndSimulate(Math.PI / 3, 1.0, 1.05, 0.020);

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

@@ -4,6 +4,8 @@
package edu.wpi.first.math.controller;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.Volts;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertThrows;
@@ -24,10 +26,8 @@ class ElevatorFeedforwardTest {
@Test
void testCalculate() {
assertEquals(1, m_elevatorFF.calculate(0), 0.002);
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);
assertEquals(1, m_elevatorFF.calculate(MetersPerSecond.of(0)).in(Volts), 0.002);
assertEquals(4.5, m_elevatorFF.calculate(MetersPerSecond.of(2)).in(Volts), 0.002);
var A = MatBuilder.fill(Nat.N1(), Nat.N1(), -kv / ka);
var B = MatBuilder.fill(Nat.N1(), Nat.N1(), 1.0 / ka);
@@ -38,7 +38,7 @@ class ElevatorFeedforwardTest {
var nextR = VecBuilder.fill(3.0);
assertEquals(
plantInversion.calculate(r, nextR).get(0, 0) + ks + kg,
m_elevatorFF.calculate(2.0, 3.0, dt),
m_elevatorFF.calculate(MetersPerSecond.of(2.0), MetersPerSecond.of(3.0)).in(Volts),
0.002);
}