From 6ef94de9b5e1342651c9bc34be801fd5e1e46354 Mon Sep 17 00:00:00 2001 From: Colin Finn <56322557+cfinn68916@users.noreply.github.com> Date: Tue, 19 Sep 2023 01:25:08 -0400 Subject: [PATCH] [wpimath] Add tests for ArmFeedforward and ElevatorFeedforward (#5663) --- .../math/controller/ArmFeedforwardTest.java | 39 ++++++++++ .../controller/ElevatorFeedforwardTest.java | 40 ++++++++++ .../cpp/controller/ArmFeedforwardTest.cpp | 76 +++++++++++++++++++ .../controller/ElevatorFeedforwardTest.cpp | 47 ++++++++++++ 4 files changed, 202 insertions(+) create mode 100644 wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java create mode 100644 wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java create mode 100644 wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp create mode 100644 wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java new file mode 100644 index 0000000000..7b5cf77380 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java @@ -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); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java new file mode 100644 index 0000000000..c701f38d1b --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java @@ -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); + } +} diff --git a/wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp new file mode 100644 index 0000000000..b7853b19ba --- /dev/null +++ b/wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp @@ -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 +#include + +#include + +#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); +} diff --git a/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp new file mode 100644 index 0000000000..bf2c1ba3b0 --- /dev/null +++ b/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp @@ -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 + +#include + +#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); +}