mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-23 01:21:42 +00:00
77 lines
2.7 KiB
C++
77 lines
2.7 KiB
C++
|
// 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);
|
||
|
|
}
|