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

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