////////////////////////////////////// ////// BLOQUE DE DECLARACIÓN ////////// /////////////////////////////////////// // Inclusión de bibliotecas externas #include <Servo.h> // Objetos empleados Servo obj_servo_frontal; Servo obj_servo_izquierdo; Servo obj_servo_derecho; // Variables empleadas int valor_sensor_izq ; int valor_sensor_cen ; int valor_sensor_der ; int error_anterior = 0; // Constantes genéricas #define MIN_SENSOR 60 #define MAX_SENSOR 400 // Constantes de los pines usados #define PIN_SENS_IZQ A2 #define PIN_SENS_CEN A0 #define PIN_SENS_DER A1 ///////////////////////////////////////// ////// BLOQUE DE INICIALIZACIÓN ///////// ///////////////////////////////////////// void setup() { Serial.begin(9600); // obj_servo_frontal.attach(3); // obj_servo_izquierdo.attach(6); // obj_servo_derecho.attach(5); } // fin de la función de INICIALIZACIÓN setup() //////////////////////////////////////// ////// BLOQUE DE PROGRAMA ////////////// //////////////////////////////////////// void loop() { // 1.- Lectura de los sensores. valor_sensor_izq = constrain(analogRead(PIN_SENS_IZQ), MIN_SENSOR, MAX_SENSOR); valor_sensor_der = constrain(analogRead(PIN_SENS_DER), MIN_SENSOR, MAX_SENSOR); valor_sensor_cen = constrain(analogRead(PIN_SENS_CEN), MIN_SENSOR, MAX_SENSOR); // 2.- Cálculo de la acción de control y escritura sobre los actuadores // Calculamos el error como un número entre -100 y 100 // Medimos la distancia al sensor 1 y al sensor 2 // En principio suponemos que se encuentra entre ellos. int distancia_sensor_izq = map(valor_sensor_izq, MIN_SENSOR, MAX_SENSOR, 0, 100); int distancia_sensor_der = map(valor_sensor_der, MIN_SENSOR, MAX_SENSOR, 0, 100); int distancia_sensor_cen = map(valor_sensor_cen, MIN_SENSOR, MAX_SENSOR, 0, 100); // si la suma de errores es muy alta es que esta en el centro y ninguno lo ve int suma_de_distancias = distancia_sensor_izq + distancia_sensor_der + distancia_sensor_cen; int error; int error_anterior; // añadido recurso de corrección del error. // si ninguno lo ve, es que se encuentra perdido y se queda con el maximo error //LAS LECTURAS AVANZADAS DEL SENSOR DE POSICION NO ME FUNCIONAN, LO DEEEJOOO // La parte derivativa funciona bien. // borrar y sustituir la parte de lectura del sensor if (valor_sensor_cen < -100) { // va recto si lo detecta muy bien el sensor central error = 0; } else { if (suma_de_distancias > 270) { // si ninguno lo ve está perdido error = error_anterior * 2; } else { if (distancia_sensor_izq < distancia_sensor_der) { error = map(distancia_sensor_izq, 0, 100, -100, 0); } else { error = map(distancia_sensor_der, 0, 100, 100, 0); } } } // el error va entre menos 100 y 100 // Calculamos además la derivada del error // Como el incremento de errores dividido por el tiempo //int derivada=0; int derivada = error - error_anterior; // hay que dividir por el tiempo de integracion constante. error_anterior = error; // Hace una corrección proporcional al error // la velocidad es proporcional al error que va en valor absoluto de cero a cien int velocidad = 102 - abs(error); // Multiplicamos el error por la constante de proporcionalidad KP // y ese efecto lo pasamos como corrección a la planta. int KP = 1; int kp_error = error * KP; int KD = 1; int kp_derivada = derivada * KD; // la inclusión de la derivada en la planta no está bien calibrada, porque deja de ser -100 100 la holgura del map int vel_dcha = map(velocidad - kp_error - kp_derivada, velocidad - 200, velocidad + 200, 90, 180); int vel_izq = map(velocidad + kp_error + kp_derivada, velocidad - 200, velocidad + 200, 90, 0); if (digitalRead(13)) { obj_servo_izquierdo.write(vel_izq); obj_servo_derecho.write(vel_dcha); } else { obj_servo_izquierdo.write(90); obj_servo_derecho.write(90); } // 3 .- Monitorización de valores obj_servo_frontal.write(map(error, -100, 100, 10, 170)); Serial.print(valor_sensor_izq); Serial.print(" "); Serial.print(valor_sensor_cen); Serial.print(" "); Serial.print(valor_sensor_der); Serial.println(" "); Serial.print(" distancia_sensor_izq "); Serial.print(distancia_sensor_izq); Serial.print(" "); Serial.print(" distancia_sensor_cen "); Serial.print(distancia_sensor_cen); Serial.print(" "); Serial.print(" distancia_sensor_der "); Serial.print(distancia_sensor_der); Serial.print(" "); Serial.print(" error "); Serial.print(error); Serial.print(" "); Serial.print(vel_dcha); Serial.print(" "); Serial.print(vel_izq); Serial.println(" "); /*  delay(3000);  */ // 4.- Control del ciclo de programa. Intervalo de integración. delay(150); // tiempo de integración //Espera la pulsacion del teclado para segir while (Serial.available() == 0) {} while (Serial.read() > 0) {} //vacía el buffer serie } // fin de la función de PROGRAMA loop()