volatile unsigned long tiempoAnterior;

volatile double dT;

int trigPin =9;
int echoPin=8;
long duracion;
double kp=4.26;
double ki=0.016;
double kd=0.00041;
double ref=10;
double error=0;
double errorAnt=0;
double errorSuma=0;
double errorDer=0;

void setup() {
Serial.begin(9600);
pinMode(trigPin, OUTPUT); // define el pin 6 como entrada (echo)
pinMode(echoPin, INPUT); // define el pin 7 como salida (triger)
digitalWrite(trigPin, LOW); //inicializar el sensor en 0
tiempoAnterior=0;
}

void loop() {
unsigned long tiempoActual;
tiempoActual= millis();
dT= (double)(tiempoActual-tiempoAnterior);

if(dT>=10) {
digitalWrite(trigPin, HIGH); // genera el pulso de triger por 10us
delayMicroseconds(100); //delay para el pulso generado
digitalWrite(trigPin, LOW); //deja de medir
duracion=pulseIn(echoPin, HIGH); //Devuelve la longitud del pulso del pin Echo en
us
long distancia = duracion/58;
distancia=58-distancia;
error=ref-distancia;

errorDer=error-errorAnt;
errorSuma+=(ki*error);
double U=kp*error+(errorSuma)+kd*errorDer;

if (U>255){
U=255;
}
if (U<0){
U=0;
}
Serial.print(ref);
Serial.print("I");
Serial.print(distancia);
Serial.print("B");
Serial.println(U);
analogWrite(6,floor (U));
errorAnt=error;
Serial.println(distancia);
tiempoAnterior=tiempoActual;
}
}