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

[sysid] Add SysId (#5672)

Browse Source 
The source is copied from this commit:
625ff04784.
This commit is contained in:
Tyler Veness
2023-10-01 15:09:09 -07:00
committed by GitHub
parent 8d2cbfce16
commit a331ed2374
67 changed files with 7568 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

105
sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp Normal file
View File

@@ -0,0 +1,105 @@
// 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 <gtest/gtest.h>
#include "sysid/analysis/FeedbackAnalysis.h"
#include "sysid/analysis/FeedbackControllerPreset.h"
TEST(FeedbackAnalysisTest, Velocity1) {
auto Kv = 3.060;
auto Ka = 0.327;
sysid::LQRParameters params{1, 1.5, 7};
auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains(
sysid::presets::kDefault, params, Kv, Ka);
EXPECT_NEAR(Kp, 2.11, 0.05);
EXPECT_NEAR(Kd, 0.00, 0.05);
}
TEST(FeedbackAnalysisTest, Velocity2) {
auto Kv = 0.0693;
auto Ka = 0.1170;
sysid::LQRParameters params{1, 1.5, 7};
auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains(
sysid::presets::kDefault, params, Kv, Ka);
EXPECT_NEAR(Kp, 3.11, 0.05);
EXPECT_NEAR(Kd, 0.00, 0.05);
}
TEST(FeedbackAnalysisTest, VelocityConversion) {
auto Kv = 0.0693;
auto Ka = 0.1170;
sysid::LQRParameters params{1, 1.5, 7};
auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains(
sysid::presets::kDefault, params, Kv, Ka, 3.0 * 1023);
// This should have the same Kp as the test above, but scaled by a factor of 3
// * 1023.
EXPECT_NEAR(Kp, 3.11 / (3 * 1023), 0.005);
EXPECT_NEAR(Kd, 0.00, 0.05);
}
TEST(FeedbackAnalysisTest, VelocityCTRE) {
auto Kv = 1.97;
auto Ka = 0.179;
sysid::LQRParameters params{1, 1.5, 7};
auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains(
sysid::presets::kCTRECANCoder, params, Kv, Ka);
EXPECT_NEAR(Kp, 0.000417, 0.00005);
EXPECT_NEAR(Kd, 0.00, 0.05);
}
TEST(FeedbackAnalysisTest, VelocityCTREConversion) {
auto Kv = 1.97;
auto Ka = 0.179;
sysid::LQRParameters params{1, 1.5, 7};
auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains(
sysid::presets::kCTRECANCoder, params, Kv, Ka, 3.0);
// This should have the same Kp as the test above, but scaled by a factor
// of 3.
EXPECT_NEAR(Kp, 0.000417 / 3, 0.00005);
EXPECT_NEAR(Kd, 0.00, 0.05);
}
TEST(FeedbackAnalysisTest, VelocityREV) {
auto Kv = 1.97;
auto Ka = 0.179;
sysid::LQRParameters params{1, 1.5, 7};
auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains(
sysid::presets::kREVNEOBuiltIn, params, Kv, Ka);
EXPECT_NEAR(Kp, 0.00241, 0.005);
EXPECT_NEAR(Kd, 0.00, 0.05);
}
TEST(FeedbackAnalysisTest, VelocityREVConversion) {
auto Kv = 1.97;
auto Ka = 0.179;
sysid::LQRParameters params{1, 1.5, 7};
auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains(
sysid::presets::kREVNEOBuiltIn, params, Kv, Ka, 3.0);
// This should have the same Kp as the test above, but scaled by a factor
// of 3.
EXPECT_NEAR(Kp, 0.00241 / 3, 0.005);
EXPECT_NEAR(Kd, 0.00, 0.05);
}