Add tests for ArmFeedforward and ElevatorFeedforward

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);
+  }
+}

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);
+  }
+}

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 <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);
+}

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 <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);
+}