static long last_time; //Guardo valor tiempo entre llamadas
static double last_error_p; //Guardo error de iteración anterior
double error_p,error_d,error_i;
unsigned long now=millis();
double dt=now-last_time;
error_p=referencia-contador; //Error proporcional
error_d=(error_p-last_error_p)/dt; //Error derivativo
error_i=error_i+error_p*dt; //Error integral
salida=Kp*error_p+Ki*error_i+Kd*error_d; // Calculo salida
last_error_p=error_p;
last_time=now;
// Saturación a máxima velocidad
if (salida >max_vel) salida =maxvel
else if(salida<-max_vel) salida=-maxvel;
return(salida);