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

[wpimath] Add tests for ArmFeedforward and ElevatorFeedforward (#5663)

Browse Source
This commit is contained in:
Colin Finn
2023-09-19 01:25:08 -04:00
committed by GitHub
parent c395b29fb4
commit 6ef94de9b5
4 changed files with 202 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

39
wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java Normal file
View File

@@ -0,0 +1,39 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.controller;
import static org.junit.jupiter.api.Assertions.assertEquals;
import org.junit.jupiter.api.Test;
class ArmFeedforwardTest {
private static final double ks = 0.5;
private static final double kg = 1;
private static final double kv = 1.5;
private static final double ka = 2;
private final ArmFeedforward m_armFF = new ArmFeedforward(ks, kg, kv, ka);
@Test
void testCalculate() {
assertEquals(0.5, m_armFF.calculate(Math.PI / 3, 0), 0.002);
assertEquals(2.5, m_armFF.calculate(Math.PI / 3, 1), 0.002);
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);
}
@Test
void testAcheviableVelocity() {
assertEquals(6, m_armFF.maxAchievableVelocity(12, Math.PI / 3, 1), 0.002);
assertEquals(-9, m_armFF.minAchievableVelocity(11.5, Math.PI / 3, 1), 0.002);
}
@Test
void testAcheviableAcceleration() {
assertEquals(4.75, m_armFF.maxAchievableAcceleration(12, Math.PI / 3, 1), 0.002);
assertEquals(6.75, m_armFF.maxAchievableAcceleration(12, Math.PI / 3, -1), 0.002);
assertEquals(-7.25, m_armFF.minAchievableAcceleration(12, Math.PI / 3, 1), 0.002);
assertEquals(-5.25, m_armFF.minAchievableAcceleration(12, Math.PI / 3, -1), 0.002);
}
}

40
wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java Normal file
View File

@@ -0,0 +1,40 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.controller;
import static org.junit.jupiter.api.Assertions.assertEquals;
import org.junit.jupiter.api.Test;
class ElevatorFeedforwardTest {
private static final double ks = 0.5;
private static final double kg = 1;
private static final double kv = 1.5;
private static final double ka = 2;
private final ElevatorFeedforward m_elevatorFF = new ElevatorFeedforward(ks, kg, kv, ka);
@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);
}
@Test
void testAcheviableVelocity() {
assertEquals(5, m_elevatorFF.maxAchievableVelocity(11, 1), 0.002);
assertEquals(-9, m_elevatorFF.minAchievableVelocity(11, 1), 0.002);
}
@Test
void testAcheviableAcceleration() {
assertEquals(3.75, m_elevatorFF.maxAchievableAcceleration(12, 2), 0.002);
assertEquals(7.25, m_elevatorFF.maxAchievableAcceleration(12, -2), 0.002);
assertEquals(-8.25, m_elevatorFF.minAchievableAcceleration(12, 2), 0.002);
assertEquals(-4.75, m_elevatorFF.minAchievableAcceleration(12, -2), 0.002);
}
}