Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-02 02:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'main' into development

Browse Source
This commit is contained in:
Peter Johnson
2024-03-16 11:06:44 -07:00
parent 38c128fe6a c10f7d91b7
commit fd4424eb89
28 changed files with 781 additions and 107 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp
View File

@@ -8,7 +8,6 @@
#include <frc/controller/LinearQuadraticRegulator.h>
#include <frc/system/LinearSystem.h>
#include <frc/system/plant/LinearSystemId.h>
#include <units/acceleration.h>
#include <units/velocity.h>
#include <units/voltage.h>
@@ -32,8 +31,10 @@ FeedbackGains sysid::CalculatePositionFeedbackGains(
// instabilities in the LQR.
if (Ka > 1E-7) {
// Create a position system from our feedforward gains.
auto system = frc::LinearSystemId::IdentifyPositionSystem<units::meter>(
Kv_t(Kv), Ka_t(Ka));
frc::LinearSystem<2, 1, 1> system{
frc::Matrixd<2, 2>{{0.0, 1.0}, {0.0, -Kv / Ka}},
frc::Matrixd<2, 1>{0.0, 1.0 / Ka}, frc::Matrixd<1, 2>{1.0, 0.0},
frc::Matrixd<1, 1>{0.0}};
// Create an LQR with 2 states to control -- position and velocity.
frc::LinearQuadraticRegulator<2, 1> controller{
system, {params.qp, params.qv}, {params.r}, preset.period};
@@ -41,9 +42,10 @@ FeedbackGains sysid::CalculatePositionFeedbackGains(
controller.LatencyCompensate(system, preset.period,
preset.measurementDelay);
return {controller.K(0, 0) * preset.outputConversionFactor,
controller.K(0, 1) * preset.outputConversionFactor /
(preset.normalized ? 1 : preset.period.value())};
return {
controller.K(0, 0) * preset.outputConversionFactor,
controller.K(0, 1) * preset.outputConversionFactor /
(preset.normalized ? 1 : units::second_t{preset.period}.value())};
}
// This is our special model to avoid instabilities in the LQR.
@@ -74,8 +76,9 @@ FeedbackGains sysid::CalculateVelocityFeedbackGains(
}
// Create a velocity system from our feedforward gains.
auto system = frc::LinearSystemId::IdentifyVelocitySystem<units::meter>(
Kv_t(Kv), Ka_t(Ka));
frc::LinearSystem<1, 1, 1> system{
frc::Matrixd<1, 1>{-Kv / Ka}, frc::Matrixd<1, 1>{1.0 / Ka},
frc::Matrixd<1, 1>{1.0}, frc::Matrixd<1, 1>{0.0}};
// Create an LQR controller with 1 state -- velocity.
frc::LinearQuadraticRegulator<1, 1> controller{
system, {params.qv}, {params.r}, preset.period};

2
sysid/src/main/native/include/sysid/analysis/ArmSim.h
View File

@@ -33,7 +33,7 @@ class ArmSim {
* forward dt seconds.
*
* @param voltage Voltage to apply over the timestep.
* @param dt Sammple period.
* @param dt Sample period.
*/
void Update(units::volt_t voltage, units::second_t dt);

2
sysid/src/main/native/include/sysid/analysis/ElevatorSim.h
View File

@@ -31,7 +31,7 @@ class ElevatorSim {
* dt seconds.
*
* @param voltage Voltage to apply over the timestep.
* @param dt Sammple period.
* @param dt Sample period.
*/
void Update(units::volt_t voltage, units::second_t dt);

2
sysid/src/main/native/include/sysid/analysis/SimpleMotorSim.h
View File

@@ -30,7 +30,7 @@ class SimpleMotorSim {
* seconds.
*
* @param voltage Voltage to apply over the timestep.
* @param dt Sammple period.
* @param dt Sample period.
*/
void Update(units::volt_t voltage, units::second_t dt);

13
sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp
View File

@@ -130,3 +130,16 @@ TEST(FeedbackAnalysisTest, PositionWithLatencyCompensation) {
EXPECT_NEAR(Kp, 5.92, 0.05);
EXPECT_NEAR(Kd, 2.12, 0.05);
}
TEST(FeedbackAnalysisTest, PositionREV) {
auto Kv = 3.060;
auto Ka = 0.327;
sysid::LQRParameters params{1, 1.5, 7};
auto [Kp, Kd] = sysid::CalculatePositionFeedbackGains(
sysid::presets::kREVNEOBuiltIn, params, Kv, Ka);
EXPECT_NEAR(Kp, 0.30202, 0.05);
EXPECT_NEAR(Kd, 48.518, 0.05);
}