Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41: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);
}
}

76
wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp Normal file
View File

@@ -0,0 +1,76 @@
// 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.
#include <cmath>
#include <numbers>
#include <gtest/gtest.h>
#include "frc/controller/ArmFeedforward.h"
#include "units/acceleration.h"
#include "units/length.h"
#include "units/time.h"
static constexpr auto Ks = 0.5_V;
static constexpr auto Kv = 1.5_V * 1_s / 1_rad;
static constexpr auto Ka = 2_V * 1_s * 1_s / 1_rad;
static constexpr auto Kg = 1_V;
TEST(ArmFeedforwardTest, Calculate) {
frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
EXPECT_NEAR(
armFF.Calculate(std::numbers::pi * 1_rad / 3, 0_rad / 1_s).value(), 0.5,
0.002);
EXPECT_NEAR(
armFF.Calculate(std::numbers::pi * 1_rad / 3, 1_rad / 1_s).value(), 2.5,
0.002);
EXPECT_NEAR(armFF
.Calculate(std::numbers::pi * 1_rad / 3, 1_rad / 1_s,
2_rad / 1_s / 1_s)
.value(),
6.5, 0.002);
EXPECT_NEAR(armFF
.Calculate(std::numbers::pi * 1_rad / 3, -1_rad / 1_s,
2_rad / 1_s / 1_s)
.value(),
2.5, 0.002);
}
TEST(ArmFeedforwardTest, AchievableVelocity) {
frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
EXPECT_NEAR(armFF
.MaxAchievableVelocity(12_V, std::numbers::pi * 1_rad / 3,
1_rad / 1_s / 1_s)
.value(),
6, 0.002);
EXPECT_NEAR(armFF
.MinAchievableVelocity(11.5_V, std::numbers::pi * 1_rad / 3,
1_rad / 1_s / 1_s)
.value(),
-9, 0.002);
}
TEST(ArmFeedforwardTest, AchievableAcceleration) {
frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
EXPECT_NEAR(armFF
.MaxAchievableAcceleration(12_V, std::numbers::pi * 1_rad / 3,
1_rad / 1_s)
.value(),
4.75, 0.002);
EXPECT_NEAR(armFF
.MaxAchievableAcceleration(12_V, std::numbers::pi * 1_rad / 3,
-1_rad / 1_s)
.value(),
6.75, 0.002);
EXPECT_NEAR(armFF
.MinAchievableAcceleration(12_V, std::numbers::pi * 1_rad / 3,
1_rad / 1_s)
.value(),
-7.25, 0.002);
EXPECT_NEAR(armFF
.MinAchievableAcceleration(12_V, std::numbers::pi * 1_rad / 3,
-1_rad / 1_s)
.value(),
-5.25, 0.002);
}

47
wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp Normal file
View File

@@ -0,0 +1,47 @@
// 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.
#include <cmath>
#include <gtest/gtest.h>
#include "frc/controller/ElevatorFeedforward.h"
#include "units/acceleration.h"
#include "units/length.h"
#include "units/time.h"
static constexpr auto Ks = 0.5_V;
static constexpr auto Kv = 1.5_V * 1_s / 1_m;
static constexpr auto Ka = 2_V * 1_s * 1_s / 1_m;
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);
}
TEST(ElevatorFeedforwardTest, AchievableVelocity) {
frc::ElevatorFeedforward elevatorFF{Ks, Kg, Kv, Ka};
EXPECT_NEAR(elevatorFF.MaxAchievableVelocity(11_V, 1_m / 1_s / 1_s).value(),
5, 0.002);
EXPECT_NEAR(elevatorFF.MinAchievableVelocity(11_V, 1_m / 1_s / 1_s).value(),
-9, 0.002);
}
TEST(ElevatorFeedforwardTest, AchievableAcceleration) {
frc::ElevatorFeedforward elevatorFF{Ks, Kg, Kv, Ka};
EXPECT_NEAR(elevatorFF.MaxAchievableAcceleration(12_V, 2_m / 1_s).value(),
3.75, 0.002);
EXPECT_NEAR(elevatorFF.MaxAchievableAcceleration(12_V, -2_m / 1_s).value(),
7.25, 0.002);
EXPECT_NEAR(elevatorFF.MinAchievableAcceleration(12_V, 2_m / 1_s).value(),
-8.25, 0.002);
EXPECT_NEAR(elevatorFF.MinAchievableAcceleration(12_V, -2_m / 1_s).value(),
-4.75, 0.002);
}