#define BAUD 115200
class PID
{
private:
double _prev_time, _now, _dt;
double _prev_error, _error;
double _Kp, _Ki, _Kd;
int _direction;
double _setpoint;
double _integral;
double _output, _prev_output;
public:
PID()
{
_prev_time=_now=micros()*1e-6;
_prev_error=_error=0.0;
_dt=1e9;
_setpoint=0.0;
_integral=0.0;
_Kp=_Ki=_Kd=0.0;
_output=0.0;
_direction=1.0;
}
void reverse()
{
int _direction=-1;
}
void setPoint(double setpoint)
{
_setpoint=setpoint;
}
void setTunings(double Kp, double Ki, double Kd)
{
if(Ki!=0.0)
_integral*=_Ki / Ki;
_Kp=Kp;
_Ki=Ki;
_Kd=Kd;
}
double Compute(double input)
{
_now=micros() 1e-6;
if(_now>_prev_time)
_dt=_now-_prev_time;
_error=input-_setpoint;
_integral+=_error_dt;
_output= _Kp*_error+_Ki*_integral+_Kd*(_error-_prev_error)/_dt;
_prev_time=_now;
_prev_error=_error;
_prev_output=_output;
return (unsigned int) (_output*_direction)&0x00000000ff;
}
double getIntegral()
{
return _integral;
}
};
double Setpoint=500, Input, Output, Error;
double Kp_f = 0.4;
double Ki_f = 8;
double Kd_f = 0.0;
double Kp = 1;
double Ki = 20;
double Kd = 0.002;
unsigned long ii = 0;
PID myPID;
void setup() {
pinMode(6, OUTPUT);
TCCR1A = 0b00000001;
TCCR1B = 0b00001001;
pinMode (A0, INPUT);
Serial.begin (BAUD);
myPID.setTunings (Kp, Ki, Kd);
myPID.setPoint (Setpoint);
myPID.reverse ();
}
void loop()
{
Input=analogRead(A0); // put your main code here, to run repeatedly:
Error = Input-Setpoint;
if (abs(Error)<3)
myPID.setTunings (Kp_f,Ki_f,Kd_f);
else if(abs(Error)>5)
myPID.setTunings(Kp,Ki,Kd);
Input=analogRead(A0);
Output = myPID.Compute(Input);
analogWrite(6, Output);
if(ii++%100==0)
{
Serial.print(Input);
Serial.print(',');
Serial.print(Output);
Serial.print(',');
Serial.print(abs(Error)/Setpoint*100);
Serial.print(',');
Serial.print(myPID.getIntegral());
}
}