RasPi/CopterController/PID.cpp

47 lines
803 B
C++
Raw Normal View History

2015-02-12 14:51:36 -07:00
/*
* pid.cpp
*/
#include "PID.h"
2015-02-12 14:51:36 -07:00
#include <ctime>
PID::PID( double p, double i, double d )
2015-02-12 14:51:36 -07:00
{
pGain = p;
iGain = i;
dGain = d;
lastPosition = 0;
integral = 0;
lastUpdate = clock();
}
double PID::update( double current, double desired )
2015-02-12 14:51:36 -07:00
{
double pAdjust, iAdjust, dAdjust;
double difference;
double posChange;
clock_t newTime;
clock_t deltaT;
2015-02-12 14:51:36 -07:00
newTime = clock();
deltaT = newTime - lastUpdate;
2015-02-12 14:51:36 -07:00
difference = desired - current;
// Porportional
2015-02-12 14:51:36 -07:00
pAdjust = pGain * difference;
// Integral
integral += difference * deltaT;
2015-02-12 14:51:36 -07:00
iAdjust = -iGain * integral;
// Differential
2015-02-12 14:51:36 -07:00
posChange = current - lastPosition;
dAdjust = -dGain * posChange / deltaT ;
2015-02-12 14:51:36 -07:00
return (1 + pAdjust) * (1 + iAdjust) * (1 + dAdjust) - 1;
2015-02-12 14:51:36 -07:00
}