44 lines
920 B
C++
44 lines
920 B
C++
/**
|
|
* @file pid.cpp
|
|
*
|
|
* http://en.wikipedia.org/wiki/PID_controller
|
|
*
|
|
* @date Sep 16, 2014
|
|
* @author Andrey Belomutskiy, (c) 2012-2014
|
|
*/
|
|
|
|
#include "pid.h"
|
|
|
|
Pid::Pid(float pFactor, float iFactor, float dFactor, float minResult, float maxResult) {
|
|
this->pFactor = pFactor;
|
|
this->iFactor = iFactor;
|
|
this->dFactor = dFactor;
|
|
this->minResult = minResult;
|
|
this->maxResult = maxResult;
|
|
|
|
integration = 0;
|
|
prevError = 0;
|
|
}
|
|
|
|
float Pid::getValue(float target, float input, float dTime) {
|
|
float error = target - input;
|
|
|
|
float pTerm = pFactor * error;
|
|
integration += iFactor * dTime * error;
|
|
float dTerm = dFactor / dTime * (error - prevError);
|
|
|
|
prevError = error;
|
|
|
|
float result = pTerm + integration + dTerm;
|
|
if (result > maxResult) {
|
|
integration -= result - maxResult;
|
|
result = maxResult;
|
|
} else if (result < minResult) {
|
|
integration += minResult - result;
|
|
result = minResult;
|
|
|
|
}
|
|
return result;
|
|
}
|
|
|