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] Fix arm characterization crash (#6422)

Browse Source 
Fixes #6421.
This commit is contained in:
Tyler Veness
2024-03-09 09:54:37 -08:00
committed by GitHub
parent c19ee8b0fe
commit ccb4cbed63
1 changed files with 10 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
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.
@@ -56,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.