Newer
Older
#include "pidFilterAngle.h"
const float PIDFilterAngle::Kp = FILTER_ANGLE_KP;
const float PIDFilterAngle::Kd = FILTER_ANGLE_KD;
const float PIDFilterAngle::Ki = FILTER_ANGLE_KI;
PIDFilterAngle::PIDFilterAngle() :
sommeErreurs(0),
erreurPrecedente(0)
{
}
float PIDFilterAngle::getFilteredValue(Angle erreur){
sommeErreurs=sommeErreurs*FILTER_ANGLE_COEF+erreur;
Angle proportionnel = erreur;
Angle integrale = sommeErreurs;
Angle derivee = erreur - erreurPrecedente;
erreurPrecedente=erreur;
return proportionnel*Kp
+ integrale*Ki
+ derivee*Kd;
}
void PIDFilterAngle::resetErrors(){
sommeErreurs = 0;
erreurPrecedente = 0;
}