/* Create a simple PID controller with compile time optimizations
Arguments:
- name will be the function name
- intype and outtype are the input and output types
- timetype is the type of dt
- Kp is the constant for the proportional component
- Ki is the constant for the integral component
- Kd is the constant for the derivative component
Components can be turned off by setting their constant to 0 (recudes code size)
*/
#define create_pid_control(name, intype, outtype, timetype, Kp, Ki, Kd) \
outtype name (timetype dt, intype setpoint, intype measured_value) \
{ \
outtype ret; \
intype error = setpoint - measured_value; \
ret = 0; \
\
if (Kp != 0) \
ret += Kp * error; \
\
if (Ki != 0) { \
static intype integral = 0; \
integral += error * dt; \
ret += Ki * integral; \
} \
\
if (Kd != 0) { \
static intype previous_error = 0; \
ret += Kd * (error - previous_error)/dt; \
previous_error = error; \
} \
\
return ret; \
}