Pues el algoritmo ha quedado básicamente así:
pError1 = ((float)VDeseada1 - (float)Velocidad1);
Potencia1 = PotenciaBase1 + (Kp * (float)pError1 + Kd*(pError1 - ErrorAnterior1) + Ki*Ierror1)/Ko;
if (Potencia1<0) Potencia1=0;
if (Potencia1>255) Potencia1=255;
ErrorAnterior1=pError1;
Ierror1 += pError1;
set_pwm1_duty((int8)Potencia1);
Como se puede ver, hay una variable llamada PotenciaBase1 que calculo mediante la fórmula que obtuve con Eureqa, y que en teoría es la potencia necesaria para conseguir la velocidad deseada. A partir de ese valor, el pid trabaja corrigiendo al alza o la baja las desviaciones para mantener la velocidad constante.
Para calcular las constantes Kp, Kd y Ki he utilizado el método de poner a Kd=Ki=0 y buscar el Kp más alto posible hasta que el sistema oscile. Una vez encontrado, lo he reducido al 60% y he incrementado Kd de la misma manera.
Creo que todavía se puede depurar más, pero de momento los valores con los que estoy trabajando, que son los de las gráficas anteriores, son estos:
#define Kp 3.0
#define Kd 1.5
#define Ki 0.0
#define Ko 1.0