기초적인 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.

}