Mirrors/overte
3
0
Fork 0
mirror of https://github.com/overte-org/overte.git synced 2025-08-14 11:39:54 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions 2

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 "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 p = getKP() * error; // term is Proportional to error
@ -32,7 +32,7 @@ float PIDController::update(float measuredValue, float dt, bool resetAccumulator
getControlledValueHighLimit());
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));
@ -43,7 +43,7 @@ float PIDController::update(float measuredValue, float dt, bool resetAccumulator
}
// 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.
// Instead, add to history, and then dump out at once when full.
// 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.d = d;
next.computed = computedValue;
next.fixme1 = fixme1; next.fixme2 = fixme2;
if (_history.size() == _history.capacity()) { // report when buffer is full
reportHistory();
_history.resize(0);
}
}
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++) {
Row& row = _history[i];
qCDebug(shared) << row.measured << row.dt <<
row.fixme1 << row.fixme2 <<
"||" << row.error << row.accumulated << row.changed <<
"||" << 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:
// These are the main interfaces:
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
bool getIsLogging() { return _history.capacity(); }
@ -64,10 +64,11 @@ public:
float i;
float d;
float computed;
float fixme1; float fixme2;
};
protected:
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 _controlledValueLowLimit { 0.0f };
float _controlledValueHighLimit { std::numeric_limits<float>::max() };