a snippet to write your own PID controller
/*
output = Kp * err + (Ki * Ierr * dt) + (Kd * Derr /dt);
where
Kp = Proptional Constant.
Ki = Integral Constant.
Kd = Derivative Constant.
err = Expected Output - Actual Output ie. error;
Ierr = err + Ierr from previous loop; ( i.e. Integral error )
Derr = err - Derr from previous loop; ( i.e. Differential error)
dt = time interval between evaluations/
where initially 'Derr' and 'Ierr' would be zero.
*/
#define Kp 1
#define Ki 1
#define Kd 1
#define Dt 10 //time step (milliseconds)
#define SS_ERR 2 //steady steate error margin (percentage)
unsigned long oldtime=0;
signed int Vin,Vout,setpoint=128;
signed int err_i=0, err_d=0, err_calc=0;
void setup{
}
void loop(){
uint16_t ss_error;
Vin=AnalogRead(0);
err_calc = setpoint-Vin;
ss_error = (abs(err_calc)*100)/setpoint; //calculated percentage error
if((millis()-oldtime) > Dt && ss_error > SS_ERR){
millis = oldtime;
err_i = err_calc+err_i;
err_d = err_calc-err_d;
Vout = (Kp * err_calc) + (Ki * err_i * Dt) + ((Kd * err_d) /Dt);
}
}
you can check out wiki to see how to use it to then tune it. PID controller - Wikipedia 