Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-04 03:11:43 +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-09 09:57:55 -08:00
parent 9ed0631ec9 18e57f7872
commit 38c128fe6a
88 changed files with 941 additions and 230 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -4,6 +4,8 @@
#include "sysid/analysis/FeedbackAnalysis.h"
#include <cmath>
#include <frc/controller/LinearQuadraticRegulator.h>
#include <frc/system/LinearSystem.h>
#include <frc/system/plant/LinearSystemId.h>
@@ -21,6 +23,10 @@ using Ka_t = decltype(1_V / 1_mps_sq);
FeedbackGains sysid::CalculatePositionFeedbackGains(
const FeedbackControllerPreset& preset, const LQRParameters& params,
double Kv, double Ka) {
if (!std::isfinite(Kv) || !std::isfinite(Ka)) {
return {0.0, 0.0};
}
// If acceleration requires no effort, velocity becomes an input for position
// control. We choose an appropriate model in this case to avoid numerical
// instabilities in the LQR.
@@ -32,7 +38,8 @@ FeedbackGains sysid::CalculatePositionFeedbackGains(
frc::LinearQuadraticRegulator<2, 1> controller{
system, {params.qp, params.qv}, {params.r}, preset.period};
// Compensate for any latency from sensor measurements, filtering, etc.
controller.LatencyCompensate(system, preset.period, 0.0_s);
controller.LatencyCompensate(system, preset.period,
preset.measurementDelay);
return {controller.K(0, 0) * preset.outputConversionFactor,
controller.K(0, 1) * preset.outputConversionFactor /
@@ -47,7 +54,7 @@ FeedbackGains sysid::CalculatePositionFeedbackGains(
frc::LinearQuadraticRegulator<1, 1> controller{
system, {params.qp}, {params.r}, preset.period};
// Compensate for any latency from sensor measurements, filtering, etc.
controller.LatencyCompensate(system, preset.period, 0.0_s);
controller.LatencyCompensate(system, preset.period, preset.measurementDelay);
return {Kv * controller.K(0, 0) * preset.outputConversionFactor, 0.0};
}
@@ -55,6 +62,10 @@ FeedbackGains sysid::CalculatePositionFeedbackGains(
FeedbackGains sysid::CalculateVelocityFeedbackGains(
const FeedbackControllerPreset& preset, const LQRParameters& params,
double Kv, double Ka, double encFactor) {
if (!std::isfinite(Kv) || !std::isfinite(Ka)) {
return {0.0, 0.0};
}
// If acceleration for velocity control requires no effort, the feedback
// control gains approach zero. We special-case it here because numerical
// instabilities arise in LQR otherwise.

13
sysid/src/main/native/cpp/analysis/FilteringUtils.cpp
View File

@@ -146,9 +146,18 @@ sysid::TrimStepVoltageData(std::vector<PreparedData>* data,
wpi::sgn(b.velocity) * b.acceleration;
});
// Current limiting can delay onset of the peak acceleration, so we need to
// find the first acceleration *near* the max. Magic number tolerance here
// because this whole file is tech debt already
auto accelBegins = std::find_if(
data->begin(), data->end(), [&maxAccel](const auto& measurement) {
return wpi::sgn(measurement.velocity) * measurement.acceleration >
0.8 * wpi::sgn(maxAccel->velocity) * maxAccel->acceleration;
});
units::second_t velocityDelay;
if (maxAccel != data->end()) {
velocityDelay = maxAccel->timestamp - firstTimestamp;
if (accelBegins != data->end()) {
velocityDelay = accelBegins->timestamp - firstTimestamp;
// Trim data before max acceleration
data->erase(data->begin(), maxAccel);

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

@@ -103,3 +103,30 @@ TEST(FeedbackAnalysisTest, VelocityREVConversion) {
EXPECT_NEAR(Kp, 0.00241 / 3, 0.005);
EXPECT_NEAR(Kd, 0.00, 0.05);
}
TEST(FeedbackAnalysisTest, Position) {
auto Kv = 3.060;
auto Ka = 0.327;
sysid::LQRParameters params{1, 1.5, 7};
auto [Kp, Kd] = sysid::CalculatePositionFeedbackGains(
sysid::presets::kDefault, params, Kv, Ka);
EXPECT_NEAR(Kp, 6.41, 0.05);
EXPECT_NEAR(Kd, 2.48, 0.05);
}
TEST(FeedbackAnalysisTest, PositionWithLatencyCompensation) {
auto Kv = 3.060;
auto Ka = 0.327;
sysid::LQRParameters params{1, 1.5, 7};
sysid::FeedbackControllerPreset preset{sysid::presets::kDefault};
preset.measurementDelay = 10_ms;
auto [Kp, Kd] = sysid::CalculatePositionFeedbackGains(preset, params, Kv, Ka);
EXPECT_NEAR(Kp, 5.92, 0.05);
EXPECT_NEAR(Kd, 2.12, 0.05);
}