#define echoPin 6 // Pin de echo del sensor ultrasonico #define trigPin 5 // Pin de trig del sensor ultrasonico int setPoint = 10; // Iniciamos con disctancia de 10 cm float Duracion, PID, Out, h, Distancia, tk; float tk_1 = 0; float proporcional = 0; float error_anterior = 0; float Derivada = 0; float Integral = 0; float kP = 8; float kD = 11; float kI = 0.0000001; void setup() { TCCR2B = TCCR2B & 0b11111000 | 0x07; // Reducimos el sonido de operacion afectando la frecuencia Serial.begin(9600); Serial.println("Introduce el Set point"); pinMode(trigPin, OUTPUT); pinMode(echoPin, INPUT); //Salidas motorreductor pinMode(2, OUTPUT); pinMode(4, OUTPUT); pinMode(8, OUTPUT); pinMode(9, OUTPUT); pinMode(11, OUTPUT); //ENA pinMode(10, OUTPUT); //ENB } void loop() { //-------------------------------------------------------- if (Serial.available() > 0){ if (Serial.peek() == ' ' || Serial.peek() == ' '){ Serial.read(); } else { setPoint = Serial.parseFloat(); Serial.println(setPoint, 4); } } //------------------------------------------------------------ //Control de pulsos del sensor digitalWrite(trigPin, LOW); delayMicroseconds(2); digitalWrite(trigPin, HIGH); delayMicroseconds(10); digitalWrite(trigPin, LOW); //------------------------------------------------------------ // Operacion del PID Duracion = pulseIn(echoPin, HIGH); Distancia = Duracion/58.4; // Calcula la distancia en cm Distancia = round(Distancia);// Se reduce el valor obtenido tk = millis();//Se guarda el tiempo que transcurrio desde que se encendio el arduino h = tk - tk_1; // Se guarda elperioda del ciclo proporcional = setPoint - Distancia; //Calcula el error Integral = Integral + (proporcional * h); //Calcula el integral del error Derivada = (proporcional - error_anterior) / h; //Calcula la derivada del error PID = (proporcional*kP) + (Integral*kI) + (Derivada*kD); //Ecuacion del PID error_anterior = proporcional; //Se manda el error actual al error anterior tk_1 = tk; // Se guarda el error actual en el error anterior //------------------------------------------------------------- //Limites del error minimo y del error maximo if(PID >= 85){ PID = 85; }// Valor maximo else if(PID
