Código: Funcionamiento de Seguidor de Línea PDF

//\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*// //\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- CÓDIGO: FUNCIONAMIENTO DE SEGUIDOR DE LÍNEA \-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-...

//\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*// //\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- CÓDIGO: FUNCIONAMIENTO DE SEGUIDOR DE LÍNEA \-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\--// //\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*// //\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- Librerias\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- \#include \ //\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- Declaración de variables. \-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- //Defino pines de START \#define PIN\_LedStart 46 \#define PIN\_SwStart 47 //Defino pines de DIP Switch/selector de codigos \#define PIN\_Sw\_Dip1 51 \#define PIN\_Sw\_Dip2 50 \#define PIN\_Sw\_Dip3 49 \#define PIN\_Sw\_Dip4 48 //Defino pines Sensor ultrasonido \#define PIN\_TriggerUs 21 \#define PIN\_EchoUs 20 //Defino pines IN del driver L298N (Dan sentido de giro a motores) \#define IN1 43 \#define IN2 42 \#define IN3 41 \#define IN4 40 //Defino pines EN del driver L298N (Dan velocidad a motores) \#define ENA 44 \#define ENB 45 \#define PIN\_Rele 39 //Defino pines de los IR frontales \#define PIN\_IRF0 11 \#define PIN\_IRF1 10 \#define PIN\_IRF2 9 \#define PIN\_IRF3 8 //Defino Variables relacionadas con Sensores IR Frontales \#define NumSensores\_Front 4 bool IR\_F\[NumSensores\_Front\]={}; //Defino variable donde almacenar la corrección a efectuar. float correccion = 0; //Defino el número de lecturas que quiero realizar para cada sensor. A partir de este número luego se realizará el promedio. EJEMPLO: PISO\_NumeroLecturas = 10 =\> Cada valor almacenado en el array «PISO\_arrayPROMEDIOS» habrá sido el resultado de promediar diez lecturas de cada sensor. \#define PISO\_NumeroLecturas 5 //Defino cuántos sensores IR\_P voy a estar utilizando en el robot. Sirve para crear estructuras iterativas en base a ese valor. \#define numeroSensores 7 //Defino matriz, array multidimensional para generar una cola circular. Esta matriz tendrá numeroSensores filas y PISO\_NumeroLecturas columnas. int BufferCircIRP\_Val\[numeroSensores\]\[PISO\_NumeroLecturas\] = {}; //Defino índices para recorrer la cola circular. int BufferCircIRP\_IndiceVal = 0; int BufferCircIRP\_IndiceSens = 0; //Defino el array que almacenará mis X (7) promedios. Las lecturas promediadas de cada sensor se almacenarán en una posición distinta del vector. int IR\_P\[numeroSensores\]={}; //Defino Valores ideales para calculo de SetPoint float ValoresIdeales\_IZQ\[7\] = {0,0,0,0,0,0,1000}; float ValoresIdeales\_DER\[7\] = {1000,0,0,0,0,0,0}; //Defino los valores máximos que se visualizaron manualmente para cada sensor int Val\_IRP\_Max\[numeroSensores\]={730,460,590,670,500,780,460}; //Porcentaje del valor maximo de cada sensor a partir del cual se considera negro pleno para el calculo. Evaluar subir a 0.9 si funciona bien lo del enganche de abajo float Margen\_Negro=0.88; //en porcentaje 100%=1 bool BanderaIRF\_Alguno = false; //Valor a partir del cual se considera negro para enganche de lado. float EngancheNegro = 500; //en porcentaje 100%=1 //Defino arrays de pesos según derecha o izquierda float PesosDerecha\[7\] = {15, 10, 10 ,10, 10, 10, 10}; //{2, 2, 4, 8, 12, 16, 21} float PesosIzquierda\[7\] = {10, 10, 10 ,10, 10, 10, 15}; float SumaPesos\_IZQ = 0; float SumaPesos\_DER = 0; float Velocidad = 255; //Seteo de Velocidad que se usa en el Robot float Veloc = Velocidad; //Variable que se le pasa a la función Adelante, se va modificando durante el programa float Mult\_HayObjeto = 1.2; //En caso de detectar Objeto en el camino incrementa la velocidad usando este factor float Correccion\_Max = 255; //Correccion\_Max es una variable y va a valer 255 o Correccion\_MaxBlanco segun lo determine el codigo luego //Defino variables globales de la funcion ModificacionVeloc char ModoAcel=1; unsigned long RampaAcel\_Inicio=0; unsigned long RampaAcel\_Fin=0; unsigned long Timeout\_RampaAcel = 800; unsigned long Timeout\_RampaAcel\_Bloqueo = 500; bool Bandera\_RampaAcel\_Habilitada=1; const float Correccion\_MaxBlanco = 100; // Ver NOTA abajo // NOTA: Correccion\_MaxBlanco es fijo y tiene q ser menor que Velocidad pero a su vez menor a 125 // para que el robot no gire sobre su propio eje // QUEDO VIEJO, NO SE USA. CHEQUEAR //Defino variables de PID float errorActual = 0; float errorAnterior = 0; float integral = 0; float derivativa = 0; const float S\_kp = 1.3; // ganancia proporcional //kp=2 const float S\_ki = 1.8; // ganancia integral //ki=3 const float S\_kd = 0.15; // ganancia derivativa //kd=0.2 float kp = S\_kp; // ganancia proporcional //kp=2 float ki = S\_ki; // ganancia integral //ki=3 float kd = S\_kd; // ganancia derivativa //kd=0.2 float Mult\_enBlancoPID = 2.55; // Pensar si lo hacemos depender de otras variables unsigned long tiempoAnterior = 0; //Variables que nos permiten arrancar desde todo blanco buscando lentamente el lado seteado en UltimoLado unsigned long Timeout\_SinPegarLado = 1000; //2s bool Bandera\_PegarladoAct = false; //Variables para arranque suave bool Bandera\_ArranqueSuave = false; unsigned long Timeout\_ArranqueSuave = 400; //Intento de giro abrupto en Blanco unsigned long TodoBlanco\_Inicio = 0; unsigned long Timeout\_DerivativoElevado = 200; //100ms, considerar aumentarlo o disminuirlo en función de respuesta de robot. bool Bandera\_DerivativoElevado = 0; float FactorDerivativoE = 0.2; // Debería ser un poco mayor a 0.2, pero no cerca de 0.2 ya //0.3 unsigned long DerivElevado\_Fin = 0; unsigned long Timeout\_DerivElev\_Bloqueo = 200; //200 ms //Defino variables para setpoint en cada lado float SetPoint\_IZQ = 0; float SetPoint\_DER = 0; //Defino variables para determinar el lado. int LadoActual = 2; //IZQ=0 DER=1 TODOBLACO=2 TODONEGRO=3 (sin implementar, no deberiamos llegar a esa situación) int UltimoLado = 0; //Si se posiciona el Robot sobre todo blanco,osea dentro de pista, se debe setear AQUI el Lado al que nos queremos enganchar. IZQ=0 DER=1 bool Bandera\_CambioLado = 0; //Creo variables para trabajar el PWM y sentido de giro de cada motor, que con el negado (!) voy a poder ajustar el IN (en la funcion Motores). int sentidoGiroM1 = HIGH; int sentidoGiroM2 = HIGH; int pwmENA = 0; int pwmENB = 0; //Defino Variables para Timer1 y Sensor ultrasonido \#define TIMER\_US 50 // 50 uS Timer1 duracion \#define CONT\_TIMEOUT\_TRIGGER 2000 // 50\*2000=100 mS Timer1 volatile long echo\_start = 0; // Records start of echo pulse volatile long echo\_end = 0; // Records end of echo pulse volatile long echo\_duration = 0; // Duration - difference between end and start volatile int trigger\_time\_count = 0; // Count down counter to trigger pulse time volatile long distance\_cm = 0; int DistDeteccObjeto = 15; //Defino variables para el Inicio por reglamento bool Bandera\_Arrancamos = 0; bool Bandera\_SwStartYaPres = 0; bool Boton\_Start=0; unsigned long TempoStart = 0; unsigned long TempoStart\_Inicio = 0; unsigned long Bandera\_ArranqueConDemoraReglamentaria=1; //\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- Fin Declaración de variables. \-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- void setup() { //Defino Pines de Start pinMode(PIN\_LedStart, OUTPUT); pinMode(PIN\_SwStart, INPUT); //Defino pines de DIP Switch/selector de codigos pinMode(PIN\_Sw\_Dip1, INPUT); pinMode(PIN\_Sw\_Dip2, INPUT); pinMode(PIN\_Sw\_Dip3, INPUT); pinMode(PIN\_Sw\_Dip4, INPUT); //Defino salidas de los motores pinMode(IN1, OUTPUT); pinMode(IN2, OUTPUT); pinMode(ENA, OUTPUT); pinMode(IN3, OUTPUT); pinMode(IN4, OUTPUT); pinMode(ENB, OUTPUT); pinMode (PIN\_Rele, OUTPUT); //Defino entradas de los sensores IR Frontales pinMode(PIN\_IRF0, INPUT); pinMode(PIN\_IRF1, INPUT); pinMode(PIN\_IRF2, INPUT); pinMode(PIN\_IRF3, INPUT); //Inicializo Pines y Timer para Sensor ultrasonido pinMode(PIN\_TriggerUs, OUTPUT); pinMode(PIN\_EchoUs, INPUT); Timer1.initialize(TIMER\_US); // Initialise timer 1 Timer1.attachInterrupt( TimerISR ); // Attach interrupt a la rutina de servicio del timer attachInterrupt(digitalPinToInterrupt(PIN\_EchoUs), Echo\_ISR, CHANGE); // Attach interrupt to the sensor echo input //Inicializo serial a 9600 bits por segundo! Serial.begin(9600); //Calculo SumaPesos\_DER/IZQ y los SetPointDER/IZQ de cada lado segun posicion ideal deseada CalcularSumaPesos(); CalcularSetpoint(); } void loop() { delay(10); //Es necesario un valor de compromiso que permita al PID calcular un DT y funcionar bien. InicioPorReglamento(); //Funcion bloqueante por while hasta que se presiona SwStart. Si está Sw\_DIP1 en 1 arranca de una, sino hace el inicio a los 5 segundos //Llamo a la función para leer los valores de los sensores y almacenarlos en un buffer circular. Lectura\_IRP\_Val(); //Llama a la función para calcular el promedio de las lecturas almacenadas en el buffer circular. ActualizarPromedioIRP\_Val(); //Llamo a la función para leer los sensores frontales y almacenarlos en un buffer. Lectura\_IRF(); //Llama a la función que evalua el lado al que esta enganchado o al que tiene que engancharse. EvaluarLado(); //Llama a la función que calcula el error (segun el lado de enganche) como diferencia entre el setpoint y la medición actual. CalcularErrorActual(); //Llama a la funcion que modifica los valores de Kp, Ki, Kd y CorreccionMax, segun circunstancias de la pista ModificacionValoresK (); //Llama a la función que calcula la correción segun los parametros seteados CorreccionPID(); //Llamo a la funcion que modifica el valor de Veloc, segun circunstancias de la pista ModificacionVeloc(); //Llama a la función que calcula los abs(PWM) y los sentidos de giro de cada motor PWMySentido(Veloc, correccion); //Llama a la función que interviene en los valores de PWM ya calculados segun circunstancias de la pista IntervencionPWM (); //Ya le paso PWM final a los motores y su sentido de giro correspondiente Motores(); } //\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- FUNCIÓN INICIO POR REGLAMENTO \-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- //Resumen: Funcion bloqueante, por el while, hasta que se presione SwStart. Si está Sw\_DIP1 en 1 arranca de una, sino hace el inicio a los 5 segundos void InicioPorReglamento() { while(!Bandera\_Arrancamos) // Este while se ejecuta ciclicamente hasta que ocurra la largada, luego nunca mas se ejecuta. { Lectura\_IRP\_Val(); ActualizarPromedioIRP\_Val(); Lectura\_IRF(); EvaluarLado(); CalcularErrorActual(); if(Bandera\_SwStartYaPres==0) // Chequeo continuamente si apretan el BotonStart y una vez que lo \... { // detecto, guardo el tiempo en que ocurre y no entro mas a este IF. Boton\_Start = digitalRead(PIN\_SwStart); // El led de Start se prende al bootear y se apaga al presionar el boton e iniciar conteo. digitalWrite(PIN\_LedStart, HIGH ); if(Boton\_Start) { Bandera\_SwStartYaPres=1; TempoStart\_Inicio=millis(); digitalWrite(PIN\_LedStart, LOW ); } } else if(Bandera\_SwStartYaPres==1) // Si ya fue presionado el Boton\_Start, contabilizamos 5 segundos para largar. { // Durante los ultimos 2 segundos de esos 5, prendo el Led\_Start. TempoStart=millis()-TempoStart\_Inicio; if(TempoStart\>=3000 && TempoStart\=5000) { digitalWrite(PIN\_LedStart, LOW ); Bandera\_Arrancamos=1; tiempoAnterior = millis(); } if(digitalRead(PIN\_Sw\_Dip1)) // Si el SW\_Dip1 esta activado no espero los 5 segundos, arranco ni bien presionan SwStart. {Bandera\_Arrancamos=1; tiempoAnterior = millis(); Bandera\_ArranqueConDemoraReglamentaria=0; digitalWrite(PIN\_LedStart, LOW ); } } //Serial.println(\"esperando\"); } //Serial.println(\"Largamos\"); } //\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- FIN FUNCIÓN INICIO POR REGLAMENTO \-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- //\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- FUNCIÓN LECTURA SENSORES DE PISO \-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- //Esta función lee los valores de los sensores y los guarda en un buffer circular. void Lectura\_IRP\_Val(void) { //Creo estructura iterativa que recorre todos los sensores. Recordar que BufferIndiceVal comienza siendo cero. for(int i = 0; i \< numeroSensores; i++) { //Leo el valor analógico de cada sensor y lo guardo en el buffer circular correspondiente. BufferCircIRP\_Val\[i\]\[BufferCircIRP\_IndiceVal\] = analogRead(A0+i); } // Incrementa el índice del buffer circular BufferCircIRP\_IndiceVal++; //Si el índice del buffer circular alcanza el número de lecturas deseadas, lo reseteo. if (BufferCircIRP\_IndiceVal \>= PISO\_NumeroLecturas) { BufferCircIRP\_IndiceVal = 0; } } //\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- FIN FUNCIÓN ACTUALIZAR SENSORES DE PISO \-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- //\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- FUNCIÓN ACTUALIZAR PROMEDIO SENSORES DE PISO \-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- //Esta función calcula el promedio de cada sensor y lo almacena en el array IR\_P\[i\] void ActualizarPromedioIRP\_Val(void) { //Itero sobre todos los sensores. for(int i = 0 ; i \< numeroSensores; i++) { //Realizo la operación de obtener promedios PISO\_NumeroLecturas veces. IR\_P\[i\]=0; for(int j = 0 ; j \< PISO\_NumeroLecturas; j++) { //Acumulador que va almacenando en variable IR\_P\[\] el valor leído y almacenado en cada posición del buffer circular. IR\_P\[i\]+=BufferCircIRP\_Val\[i\]\[j\]; } //Obtengo el promedio de mis sensores de piso, al dividir el acumulador por el contador. IR\_P\[i\]=IR\_P\[i\]/PISO\_NumeroLecturas; } //Recorro todos los sensores para aplicar un filtro. for(int i = 0; i \< numeroSensores; i++) { //Si el sensor es menor que el umbral blanco, lo considero blanco. if(IR\_P\[i\]\ (Margen\_Negro\*Val\_IRP\_Max\[i\])) { IR\_P\[i\] = Val\_IRP\_Max\[i\]; } //Añado línea de seguridad para limitar las lecturas de los sensores a, como máximo, sus valores MÁX. IR\_P\[i\] = constrain(IR\_P\[i\],0,Val\_IRP\_Max\[i\]); //Mapeo las lecturas para trabajar en una escala común. IR\_P\[i\] = map(IR\_P\[i\],0,Val\_IRP\_Max\[i\],0,1000); } } //\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- FIN FUNCIÓN ACTUALIZAR SENSORES DE PISO \-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- //\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- FUNCIÓN ACTUALIZAR SENSORES FRONTALES \-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- void Lectura\_IRF() { for(int i=0 ; i\= EngancheNegro) \|\| (IR\_P\[5\] \>= EngancheNegro) \|\| (IR\_P\[4\] \>= EngancheNegro) ) && (IR\_P\[1\] == 0) && (IR\_P\[0\] == 0) ) { //Determino lado IZQ. LadoActual = 0; if(LadoActual!=UltimoLado) Bandera\_CambioLado=1; //Se utilizara para resetear valor del PID UltimoLado = 0; //se podría poner 2? Lo mismo en el siguiente if } if((LadoActual == 2) && ((IR\_P\[0\] \>= EngancheNegro) \|\|(IR\_P\[1\] \>= EngancheNegro) \|\| (IR\_P\[2\] \>= EngancheNegro)) && (IR\_P\[5\] == 0) && (IR\_P\[6\] == 0) ) { //Determino lado derecha. LadoActual = 1; if(LadoActual!=UltimoLado) Bandera\_CambioLado=1; //Se utilizara para resetear valor del PID UltimoLado = 1; //se podría poner 2? Lo mismo en el anterior if } } //\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- FIN FUNCIÓN EVALUAR LADO \-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- //\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- FUNCIÓN CÁLCULO DE ERROR ACTUAL \-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- void CalcularErrorActual() { float sumaPonderada = 0; float medicionActual = 0; //Analizo caso según lado izquierdo, con pesos izquierda. if(LadoActual == 0 \|\| UltimoLado== 0) { for(int i = 0; i \< numeroSensores; i++) { sumaPonderada += IR\_P\[i\] \* PesosIzquierda\[i\]; } //Serial.println(\"Estoy en el lado IZQUIERDO\"); medicionActual = sumaPonderada / SumaPesos\_IZQ; errorActual = SetPoint\_IZQ - medicionActual; } if(LadoActual == 1 \|\| UltimoLado== 1) { for(int i = 0; i \< numeroSensores; i++) { sumaPonderada += IR\_P\[i\] \* PesosDerecha\[i\]; } //Serial.println(\"Estoy en el lado DERECHO\"); medicionActual = sumaPonderada / SumaPesos\_DER; errorActual =SetPoint\_DER - medicionActual; errorActual = -errorActual; } } //\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- FIN FUNCIÓN CÁLCULO DE ERROR ACTUAL \-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- //\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- FUNCIÓN MODIFICACION VALORES K \-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- void ModificacionValoresK (void) { //Lógica para estabilizar el arranque; arranque suave. if((millis() - TempoStart\_Inicio) \> (Timeout\_ArranqueSuave + 5000\*Bandera\_ArranqueConDemoraReglamentaria)) { Bandera\_ArranqueSuave = true; } if (LadoActual==0 \|\| LadoActual==1) { //Intento de suavizar inicio 1: avanzar con kp y kd hasta que algún sensor detecte negro, y ahí los desactiva por x ms. if(Bandera\_ArranqueSuave) { kp = S\_kp; ki = S\_ki; kd = S\_kd; } else { kp = 0; kd = 0; ki = S\_ki; //probar multiplicar por constante para aumentar la potencia de la corrección. } } if (LadoActual==2) { //Si pasaron Timeout\_SinPegarLado segundos desde que salí de cronómetro de 5s, inicio el comportamiento de pegarme de manera extrema a un lado. if((millis() - TempoStart\_Inicio) \> (Timeout\_SinPegarLado + 5000\*Bandera\_ArranqueConDemoraReglamentaria)) { Bandera\_PegarladoAct = true; } if(Bandera\_PegarladoAct) { kp = S\_kp\*1.7; //Valor: 80, 3, 1.7 ki = S\_ki\*2.4; //Valor: 0, 3, 4 kd = S\_kd; //Valor: 8, 5, 10 //Dejar siempre igual a condición default; considerar perjuicio en función CambioDerivativoElevado } else //Antes del tiempo de inicio \"PegarLado\_Start\", la corrección debe ser muy baja para no efectuar un giro muy abrupto. { kp = S\_kp\*0.5; ki = 0; kd = 0; } } } //\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- FIN FUNCIÓN MODIFICACION VALORES K \-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- //\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- FUNCIÓN CORRECCIONPID \-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- void CorreccionPID() { //Funcionamiento PID unsigned long tiempoActual = millis(); float dt = (tiempoActual - tiempoAnterior) / 1000.0; // convierto a segundos (variación de tiempo en un loop). NO ES NECESARIO, pero ayuda a trabajar en una escala acorde. if (dt == 0) dt = 0.001; // Evitar división por cero tiempoAnterior = tiempoActual; // Condición para resetear Parametros Ki y Kd if(Bandera\_CambioLado == 1) { Bandera\_CambioLado=0; //Si hay cambio de lado se resetean ambos integral=0; derivativa=0; } // Si el error es cero o cruza por cero(o sea cambia signo) se resetea el acumulador de integral else if ( (errorActual \> 0 && errorAnterior \< 0) \|\| (errorActual \< 0 && errorAnterior \> 0) \|\| (errorActual==0) ) { integral = 0; derivativa = (errorActual - errorAnterior) / dt; } else { integral += (errorActual \* dt); //Considerar no multiplicarlo por tiempo. derivativa = (errorActual - errorAnterior) / dt; } correccion = kp \* errorActual + ki \* integral + kd \* derivativa; //errorActual podría tener un maximo de -1000 o +1000 pero correccion depende de esta cuenta. errorAnterior = errorActual; float aux = 1000\*PesosDerecha\[0\]/SumaPesos\_DER; aux = (aux/dt)\* FactorDerivativoE; // Este Factor por el cual se multiplica hay q evaluarlo para que entre al siguiente if en la situacion de giro abrupto pasando por todo blanco if(LadoActual == 2 && abs(derivativa) \> aux && ((millis() - DerivElevado\_Fin) \> Timeout\_DerivElev\_Bloqueo)) { Bandera\_DerivativoElevado=1; digitalWrite(PIN\_LedStart, HIGH); } } //\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- FIN FUNCIÓN CORRECCIONPID \-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- void ModificacionVeloc(void) { //Acá vamos a evaluar agregar una funcionalidad que si el ABS(correccion) menor a cierto valor haga una rampa de aceleracion durante por ejemplo 500ms y no la repita en esa recta supongamos float AuxRefErrorIRP1 = 1000\*PesosDerecha\[1\]/SumaPesos\_DER; bool Bandera\_YaPasePorCurva=0; bool Bandera\_Timeout\_RampaAcel\_Bloqueo=0; bool Bandera\_SeDesacomodo=0; bool Bandera\_Timeout\_RampaAcel=0; switch (ModoAcel) { case 1: // Velocidad Seteada por Default //ACCIONES // Definir un comportamiento al detectar objeto delante, modificar Mult\_HayObjeto (siempre 1 o mayor), subir o bajar segun si la velocidad Normal tiene o no suficiente fuerza para empujar if(distance\_cm \=255) Veloc = 255; //Me aseguro de no pasar el limite Maximo del PWM } else Veloc = Velocidad; // Vuelvo la velocidad al valor normal seteado //CONDICIONES para cambio de estado // Evaluo sino esta habilitada y pasa el tiempo de bloqueo, levanto bandera Timeout if(Bandera\_RampaAcel\_Habilitada==0 && (millis()-RampaAcel\_Fin)\>Timeout\_RampaAcel\_Bloqueo) Bandera\_Timeout\_RampaAcel\_Bloqueo=1; //Evalúo si hay un cambio fuerte en correccion que pueda ser considerado una curva, y dar aviso de ello. if (abs(correccion) \> (3\*AuxRefErrorIRP1)) Bandera\_YaPasePorCurva=0; //1 Se puso en cero con la intención de deshabilitarlo y que sólo se utilice el timeout. //Si suceden alguna de las dos situaciones anteriores Habilito que pueda entrar en rampa de aceleración. if ( Bandera\_YaPasePorCurva \|\| Bandera\_Timeout\_RampaAcel\_Bloqueo ) { Bandera\_RampaAcel\_Habilitada=1; } //Si está habilitada y se cumple que la correccion es baja, voy a Acelerar if ( (abs(correccion) \< (1\*AuxRefErrorIRP1)) && (Bandera\_RampaAcel\_Habilitada) ) { ModoAcel=2; RampaAcel\_Inicio=millis(); //digitalWrite(PIN\_LedStart, HIGH); } break; case 2: //Rampa de Velocidad, ya en movimiento //ACCIONES Veloc=Veloc+1; //Evaluar cuanto incrementar en cada ciclo de Loop if (Veloc\>=255) Veloc = 255; //CONDICIONES para cambio de estado //Evalúo si ya estuve mucho en rampa como para llegar a la proxima curva mas despacio if((millis()-RampaAcel\_Inicio)\>Timeout\_RampaAcel) Bandera\_Timeout\_RampaAcel=1; //Evalúo si el auto supera un valor de correccion en el que se vuelva inestable por culpa del aumento de Veloc if(correccion \> 1.6\*AuxRefErrorIRP1) Bandera\_SeDesacomodo=1; //Si suceden alguna de las dos situaciones anteriores, quitamos la aceleracion if (Bandera\_Timeout\_RampaAcel \|\| Bandera\_SeDesacomodo ) { ModoAcel=1; Bandera\_RampaAcel\_Habilitada=0; RampaAcel\_Fin=millis(); //digitalWrite(PIN\_LedStart, LOW); } break; default: // if nothing else matches, do the default. Default is optional ModoAcel=1; Bandera\_RampaAcel\_Habilitada=1; break; } } //\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- FUNCIÓN AVANCE \-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- void PWMySentido(int vel, float correccion) { //Revisar correccion = constrain(correccion, -1000, 1000); //determino algun limite máximo, REVISAR. correccion = map(correccion, -1000, 1000, -Correccion\_Max, Correccion\_Max); //Correccion\_MAX = 255. //Inicializo las variables globales en su estado default, para que sean modificadas segun la logica de esta función. sentidoGiroM1 = HIGH; sentidoGiroM2 = HIGH; pwmENA = 0; pwmENB = 0; // Obtener el excedente que supera el límite de velocidad de 255 int excedente = abs(vel) + abs(correccion) - 255; // Ajustar la velocidad si hay excedente if (excedente \> 0) { if (vel\>=0) vel = vel - excedente; else if (vel\= 0) { pwmENA = vel - correccion; pwmENB = vel + correccion; } else { pwmENA = vel + abs(correccion); pwmENB = vel - abs(correccion); } //Invertir el sentido de giro si la velocidad es negativa if (pwmENA \< 0) { pwmENA = -pwmENA; sentidoGiroM1 = LOW; } if (pwmENB \< 0) { pwmENB = -pwmENB; sentidoGiroM2 = LOW; } } //\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- FIN FUNCIÓN PWMySentido \-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- //\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- FUNCIÓN INTERVENCION DE PWM \-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- void IntervencionPWM (void) { //Timer para giros abruptos\... TodoBlanco\_Inicio comienza en EvaluarLado() if(Bandera\_DerivativoElevado==1 && ((millis() - TodoBlanco\_Inicio) \> Timeout\_DerivativoElevado)) { Bandera\_DerivativoElevado=0; DerivElevado\_Fin = millis(); digitalWrite(PIN\_LedStart, LOW); } if (sentidoGiroM1 == LOW) // Si estoy en blanco y el giro del motor debiera ser negativo y no hay cambio bruzco, limito PWM \"negativo\". Caso contrario no limito nada { if(LadoActual==2 && pwmENA \> 50 && !Bandera\_DerivativoElevado) { pwmENA = 50; } } if (sentidoGiroM2 == LOW) { if(LadoActual==2 && pwmENB \> 50 && !Bandera\_DerivativoElevado) { pwmENB = 50; } } } //\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- FUNCIÓN INTERVENCION DE PWM \-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- //\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- FUNCIÓN MOTORES \-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- void Motores(void) { //Dar sentido de giro a los motores digitalWrite(IN1, sentidoGiroM1); digitalWrite(IN2, !sentidoGiroM1); digitalWrite(IN3, sentidoGiroM2); digitalWrite(IN4, !sentidoGiroM2); //Dar velocidad a los motores según todas las condiciones evaluadas analogWrite(ENA, pwmENA); analogWrite(ENB, pwmENB); } //\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- FIN FUNCIÓN MOTORES \-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- //\-\-\-\-\-\-\-\-\-\-\-- FUNCIÓN CALCULAR SUMA PESOS (EN SETUP - SE EJECUTA UNA SOLA VEZ) \-\-\-\-\-\-\-\-\-\-- void CalcularSumaPesos() { for(int i = 0; i \< numeroSensores; i++) { SumaPesos\_IZQ += PesosIzquierda\[i\]; } for(int i = 0; i \< numeroSensores; i++) { SumaPesos\_DER += PesosDerecha\[i\]; } } //\-\-\-\-\-\-\-\-- FIN FUNCIÓN CALCULAR SUMA PESOS (EN SETUP - SE EJECUTA UNA SOLA VEZ) \-\-\-\-\-\-\-\-\-- //\-\-\-\-\-\-\-\-\-- FUNCIÓN CALCULAR SETPOINT (EN SETUP - SE EJECUTA UNA SOLA VEZ) \-\-\-\-\-\-\-\-\-\-- void CalcularSetpoint() { //Calculo la suma ponderada para el lado izquierdo float SumaPonderada = 0; for(int i = 0; i \< numeroSensores; i++) { SumaPonderada += ValoresIdeales\_IZQ\[i\] \* PesosIzquierda\[i\]; } SetPoint\_IZQ = SumaPonderada / SumaPesos\_IZQ; SumaPonderada = 0; //calculo la otra condición, para lado derecho for(int i = 0; i \< numeroSensores; i++) { SumaPonderada += ValoresIdeales\_DER\[i\] \* PesosDerecha\[i\]; } SetPoint\_DER = SumaPonderada / SumaPesos\_DER; } //\-\-\-\-\-\-\-\-\-- FIN FUNCIÓN CALCULAR SETPOINT (EN SETUP - SE EJECUTA UNA SOLA VEZ) \-\-\-\-\-\-\-\-\-\-- //\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- SERVICIO DE INTERRUPCION TIMER1 \-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- void TimerISR() { trigger\_pulse(); // Manejo del pulso del trigger del sensor ultrasonido //Aqui se puede aprovechar el timer para incrementar algun contador para alguna otro conteo de tiempo } // trigger\_pulse() called every 50 uS to schedule trigger pulses. // Generates a pulse one timer1 tick long. // Minimum trigger pulse width for the HC-SR04 is 10 us. This system // delivers a 50 uS pulse. void trigger\_pulse() { static volatile int state = 0; // State machine variable if (!(\--trigger\_time\_count)) // Count to 100mS { // Time out - Initiate trigger pulse trigger\_time\_count = CONT\_TIMEOUT\_TRIGGER; // Reload state = 1; // Changing to state 1 initiates a pulse } switch(state) // State machine handles delivery of trigger pulse { case 0: // Normal state does nothing break; case 1: // Initiate pulse digitalWrite(PIN\_TriggerUs, HIGH); // Set the trigger output high state = 2; // and set state to 2 break; case 2: // Complete the pulse default: digitalWrite(PIN\_TriggerUs, LOW); // Set the trigger output low state = 0; // and return state to normal 0 break; } } //\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- FIN SERVICIO DE INTERRUPCION TIMER1 \-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- //\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- SERVICIOS DE INTERRUPCION DE ECHO DEL S-ULTRASONIDO \-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- // Echo\_ISR() External interrupt from HC-SR04 echo signal. // Called every time the echo signal changes state. // Note: this routine does not handle the case where the timer // counter overflows which will result in the occassional error. void Echo\_ISR() { switch (digitalRead(PIN\_EchoUs)) // Test to see if the signal is high or low { case HIGH: // High so must be the start of the echo pulse echo\_end = 0; // Clear the end time echo\_start = micros(); // Save the start time break; case LOW: // Low so must be the end of hte echo pulse echo\_end = micros(); // Save the end time echo\_duration = echo\_end - echo\_start; // Calculate the pulse duration if(echo\_duration\>=23200)echo\_duration=23200; // Max duracion de echo esperada= 400cm\*58=23200 distance\_cm=echo\_duration/58; break; } } //\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- FIN SERVICIO DE INTERRUPCION ECHO DEL S-ULTRASONIDO \-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- //\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- CAJA DE COPY PASTE \-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-\-- //Leer sensores\... /\* Serial.println(\"\"); for(int i = 0; i \< numeroSensores; i++) { Serial.println(IR\_P\[i\]); } Serial.println(\"\"); \*/ //Para ver los valores medidos en un ciclo en la función Lectura\_IRP\_Val();\... /\*for(int i = 0; i \< numeroSensores; i++) {Serial.print(BufferCircIRP\_Val\[i\]\[BufferCircIRP\_IndiceVal\]); Serial.print(\"\\t\"); } Serial.println(\"\"); \*/