Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-29 02:21:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Remove Units class from Elevator and Arm Feedforwards (#7570)

Browse Source
This commit is contained in:
Nicholas Armstrong
2024-12-26 21:21:19 -05:00
committed by GitHub
parent 0470e51569
commit fe49cbe429
7 changed files with 26 additions and 110 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -4,9 +4,6 @@
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;
@@ -72,22 +69,15 @@ class ArmFeedforwardTest {
private void calculateAndSimulate(
double currentAngle, double currentVelocity, double nextVelocity, double dt) {
final double input =
m_armFF
.calculate(
Radians.of(currentAngle),
RadiansPerSecond.of(currentVelocity),
RadiansPerSecond.of(nextVelocity))
.in(Volts);
m_armFF.calculateWithVelocities(currentAngle, currentVelocity, nextVelocity);
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(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);
assertEquals(0.5, m_armFF.calculate(Math.PI / 3, 0.0), 0.002);
assertEquals(2.5, m_armFF.calculate(Math.PI / 3, 1.0), 0.002);
// calculate(currentAngle, currentVelocity, nextAngle, dt)
calculateAndSimulate(Math.PI / 3, 1.0, 1.05, 0.020);

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

@@ -4,8 +4,6 @@
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;
@@ -26,8 +24,8 @@ class ElevatorFeedforwardTest {
@Test
void testCalculate() {
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);
assertEquals(1, m_elevatorFF.calculate(0.0), 0.002);
assertEquals(4.5, m_elevatorFF.calculate(2.0), 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 +36,7 @@ class ElevatorFeedforwardTest {
var nextR = VecBuilder.fill(3.0);
assertEquals(
plantInversion.calculate(r, nextR).get(0, 0) + ks + kg,
m_elevatorFF.calculate(MetersPerSecond.of(2.0), MetersPerSecond.of(3.0)).in(Volts),
m_elevatorFF.calculateWithVelocities(2.0, 3.0),
0.002);
}