La implementación del PID es:
Término proporcional:
1) e_func = v_des - v_act; /* error function */
2) r_mot = Kp*e_func; /* motor output */
Término integrativo:
1 static int r_old=0, e_old=0;
2 ...
3 e_func = v_des - v_act;
4 r_mot = r_old + Kp*(e_func-e_old) + Ki*(e_func+e_old)/2;
5 r_mot = min(r_mot, +100); /* limit output */
6 r_mot = max(r_mot, -100); /* limit output */
7 r_old = r_mot;
8 e_old = e_func;
Término derivativo:
1 static int e_old=0;
2 ...
3 e_func = v_des - v_act; /* error function */
4 deriv = e_old - e_func; /* diff. of error fct. */
5 e_old = e_func; /* store error function */
6 r_mot = Kp*e_func + Kd*deriv; /* motor output */
7 r_mot = min(r_mot, +100); /* limit output */
8 r_mot = max(r_mot, -100); /* limit output */
Todo junto el PID al completo implementado en "C":
static int r_old=0, e_old=0, e_old2=0;
2 ...
3 e_func = v_des - v_act;
4 r_mot = r_old + Kp*(e_func-e_old) + Ki*(e_func+e_old)/2
5 + Kd*(e_func - 2* e_old + e_old2);
6 r_mot = min(r_mot, +100); /* limit output */
7 r_mot = max(r_mot, -100); /* limit output */
8 r_old = r_mot;
9 e_old2 = e_old;
10 e_old = e_func;
Ajuste de los parámetros o sintonización del PID:
1. Select a typical operating setting for the desired speed, turn off integral
and derivative parts, then increase KP to maximum or until oscillation
occurs.
2. If system oscillates, divide KP by 2.
3. Increase KD and observe behavior when increasing/decreasing the
desired speed by about 5%. Choose a value of KD which gives a damped
response.
4. Slowly increase KI until oscillation starts. Then divide KI by 2 or 3.
5. Check whether overall controller performance is satisfactorily under
typical system conditions.
Con esto más la información que has puesto, te tiene que permitir implementar el PID.
Un saludo