Mirrors/overte
3
0
Fork 0
mirror of https://github.com/overte-org/overte.git synced 2025-08-08 16:58:09 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

Temporary debugging aids.

Browse source
This commit is contained in:
Howard Stearns 2015-12-07 09:31:06 -08:00
parent 188c477f7e
commit df413c0b14
2 changed files with 9 additions and 6 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

10
libraries/shared/src/PIDController.cpp
View file

@ -14,7 +14,7 @@
#include "SharedLogging.h" #include "SharedLogging.h"
#include "PIDController.h" #include "PIDController.h"
float PIDController::update(float measuredValue, float dt, bool resetAccumulator) { float PIDController::update(float measuredValue, float dt, bool resetAccumulator, float fixme1, float fixme2) {
const float error = getMeasuredValueSetpoint() - measuredValue; // Sign is the direction we want measuredValue to go. Positive means go higher. const float error = getMeasuredValueSetpoint() - measuredValue; // Sign is the direction we want measuredValue to go. Positive means go higher.
const float p = getKP() * error; // term is Proportional to error const float p = getKP() * error; // term is Proportional to error
@ -32,7 +32,7 @@ float PIDController::update(float measuredValue, float dt, bool resetAccumulator
getControlledValueHighLimit()); getControlledValueHighLimit());
if (getIsLogging()) { // if logging/reporting if (getIsLogging()) { // if logging/reporting
updateHistory(measuredValue, dt, error, accumulatedError, changeInError, p, i, d, computedValue); updateHistory(measuredValue, dt, error, accumulatedError, changeInError, p, i, d, computedValue, fixme1, fixme2);
} }
Q_ASSERT(!isnan(computedValue)); Q_ASSERT(!isnan(computedValue));
@ -43,7 +43,7 @@ float PIDController::update(float measuredValue, float dt, bool resetAccumulator
} }
// Just for logging/reporting. Used when picking/verifying the operational parameters. // Just for logging/reporting. Used when picking/verifying the operational parameters.
void PIDController::updateHistory(float measuredValue, float dt, float error, float accumulatedError, float changeInError, float p, float i, float d, float computedValue) { void PIDController::updateHistory(float measuredValue, float dt, float error, float accumulatedError, float changeInError, float p, float i, float d, float computedValue, float fixme1, float fixme2) {
// Don't report each update(), as the I/O messes with the results a lot. // Don't report each update(), as the I/O messes with the results a lot.
// Instead, add to history, and then dump out at once when full. // Instead, add to history, and then dump out at once when full.
// Typically, the first few values reported in each batch should be ignored. // Typically, the first few values reported in each batch should be ignored.
@ -59,16 +59,18 @@ void PIDController::updateHistory(float measuredValue, float dt, float error, fl
next.i = i; next.i = i;
next.d = d; next.d = d;
next.computed = computedValue; next.computed = computedValue;
next.fixme1 = fixme1; next.fixme2 = fixme2;
if (_history.size() == _history.capacity()) { // report when buffer is full if (_history.size() == _history.capacity()) { // report when buffer is full
reportHistory(); reportHistory();
_history.resize(0); _history.resize(0);
} }
} }
void PIDController::reportHistory() { void PIDController::reportHistory() {
qCDebug(shared) << _label << "measured dt FIXME || error accumulated changed || p i d controlled"; qCDebug(shared) << _label << "measured dt || error accumulated changed || p i d controlled";
for (int i = 0; i < _history.size(); i++) { for (int i = 0; i < _history.size(); i++) {
Row& row = _history[i]; Row& row = _history[i];
qCDebug(shared) << row.measured << row.dt << qCDebug(shared) << row.measured << row.dt <<
row.fixme1 << row.fixme2 <<
"||" << row.error << row.accumulated << row.changed << "||" << row.error << row.accumulated << row.changed <<
"||" << row.p << row.i << row.d << row.computed << 1.0f/row.computed; "||" << row.p << row.i << row.d << row.computed << 1.0f/row.computed;
} }

5
libraries/shared/src/PIDController.h
View file

@ -28,7 +28,7 @@ class PIDController {
public: public:
// These are the main interfaces: // These are the main interfaces:
void setMeasuredValueSetpoint(float newValue) { _measuredValueSetpoint = newValue; } void setMeasuredValueSetpoint(float newValue) { _measuredValueSetpoint = newValue; }
float update(float measuredValue, float dt, bool resetAccumulator = false); // returns the new computedValue float update(float measuredValue, float dt, bool resetAccumulator = false, float fixme1=0, float fixme2=0); // returns the new computedValue
void setHistorySize(QString label = QString(""), int size = 0) { _history.reserve(size); _history.resize(0); _label = label; } // non-empty does logging void setHistorySize(QString label = QString(""), int size = 0) { _history.reserve(size); _history.resize(0); _label = label; } // non-empty does logging
bool getIsLogging() { return _history.capacity(); } bool getIsLogging() { return _history.capacity(); }
@ -64,10 +64,11 @@ public:
float i; float i;
float d; float d;
float computed; float computed;
float fixme1; float fixme2;
}; };
protected: protected:
void reportHistory(); void reportHistory();
void updateHistory(float measured, float dt, float error, float accumulatedError, float changeInErro, float p, float i, float d, float computedValue); void updateHistory(float measured, float dt, float error, float accumulatedError, float changeInErro, float p, float i, float d, float computedValue, float fixme1, float fixme2);
float _measuredValueSetpoint { 0.0f }; float _measuredValueSetpoint { 0.0f };
float _controlledValueLowLimit { 0.0f }; float _controlledValueLowLimit { 0.0f };
float _controlledValueHighLimit { std::numeric_limits<float>::max() }; float _controlledValueHighLimit { std::numeric_limits<float>::max() };