기초적인 PID 제어 코드..

기초: PID 제어

#define Kp (2) // configurable value.
#define Ki (1) // configurable value.
#define Kd (1) // configurable value.
#define dt   0.01

double pid_con(double val_in)
{
 static double P_err_prv; // Prev P error to get D_err.
 double P_err, I_err, D_err; // Basic Error values.

 P_err = AIM - val_in;
 I_err += P_err * dt;
 D_err = (P_err - P_err_prv) / dt;

 P_err_prv = P_err; // backup P_err.
 val_out = (Kp * P_err) + (Ki * I_err) + (Kd * D_err);
 return val_out; // Calculated.
}