RUTINA PRINCIPAL:
Esta rutina está encargada de realizar os cálculos comúns a tódolos modos de funcionamento do robot e de chamar as sub-rutinas que sexan necesarias en cada instante.
![rutinaPPAL](https://botybot.wordpress.com/wp-content/uploads/2014/04/rutinappal.png?w=428&h=394)
Cálculo da velocidade:
O cálculo da velocidade axúdase da sub-rutina “cálculo_v” a cal se lle pasan os valores necesarios para que poida realizar as operacións programadas. Despois de chamar a esta sub-rutina actualízanse e almacénanse os valores da velocidade. Esta operación realízase para o motor dereito e para o esquerdo.
//CALCULO DA VELOCIDADE
if(millis()-ultimo_calculo >tiempo_entre_calculos)//este tempo ten que ser igual a tiempo_entre_regulaciones
{
ultimo_calculo=millis();
v_realD=calculo_v(contadorD, nD, tiempoD);
contadorD=0; //Resetea contador
tiempoD=micros(); // Actualiza tempo para calculos
v_realI=calculo_v(contadorI, nI, tiempoI);
contadorI=0;
tiempoI=micros();
}
Sub-rutina calculo_v:
float calculo_v(int contadorZ, int nZ, float tiempoZ) //CALCULO DA VELOCIDADE
{
float v_real=0;
//calculo da velocidade en rpm
v_real=1000000*60*contadorZ/nZ; // devolve un valor en voltas/micros Seg
v_real=v_real/(micros()-tiempoZ);
return v_real;// Devolve a velocidade en rpm
}
Cálculo do regulador:
O fragmento de código encargado de calcular a velocidade execútase cunha frecuencia determinada pola variable “tiempo_entre_calculos”, isto faise así para que a velocidade calculada vaia acorde cos cálculos realizados na sub-rutina encargada de regular a tensión de alimentación dos motores, a cal se mostra a continuación xunto co fragmento de código da rutina principal encargado de chamala cunha determinada frecuencia e de pasarlle os valores adecuados.
//CALCULA REGULADOR
if(sigue_lineas==0)//O siguelineas ten seu propio regulador
{
if(millis()- ultima_regulacion > tiempo_entre_regulaciones)//Para chamar ao regulador cada ZZ segundos
{
rpmI=regulador(v_realI, v_realD, acelerando_izquierda);
rpmD=regulador(v_realD, v_realI, acelerando_derecha);
}
}
float regulador(float v_real,float v_otra, float aceleracion)// REGULADOR empregado sempre menos no caso do modo SIGUE_LINEA
{
//calculo de erros
float error_v_referencia = v_referencia - v_real;
float error_v_otra = v_otra - v_real;
//calculo da saida
float rpm=0;
rpm= v_referencia + error_v_referencia * kp1 + error_v_otra * kp2 + aceleracion *2 ;//A aceleracion
multiplicase *2 para anular o efecto de error_v_otra
return rpm;
}
A rutina “regulador” calcula a diferencia entre a velocidade marcada como v_referencia, que é a que debe de alcanzar o robot e a “v_real” calculada anteriormente e que obtén os datos do encoder de cada roda. Tamén calcula a diferencia de velocidade entre cada roda.
Coas diferenzas de velocidade calculadas, aplícase un logaritmo para corrixir o erro cometido, neste logaritmo entran en xogo os valores “kp1” e “kp2”.
A variable “aceleración” é a empregada para corrixir a posición do robot nos modos de funcionamento en que ten que seguir paredes.
Control dos motores:
O código empregado para administrar o sentido de xiro dos motores e regular a tensión que se lles entrega, controlando así a súa velocidade, sigue o seguinte diagrama.
![rutinamotores](https://botybot.wordpress.com/wp-content/uploads/2014/04/rutinamotores.png?w=495&h=332)
A sub-rutina “rpm_a_pwm()” encárgase de converter o valores calculados polo regulador que están en Revolución Por Minuto (rpm) en Modulación por Anchura de Pulso (Pulse-Width Modulation (PWM)). Para isto, emprega a función definida a anacos obtida na entrada anterior.
SUBRUTINAS DE COMPORTAMENTO:
As sub-rutinas encargadas do comportamento do robot, son: sigueLineas() automatico() e manual(). Estas a súa vez empregan outras sub_rutinas para realizar distintas tarefas. A continuación resúmese o seu funcionamento.
Subrutina S.Lineas()
De esta rutina se pode resaltar que as lecturas realizadas polo CNY-70 léense de forma analóxica, o que permite elixir os valores que determinarán a localización da liña. Isto é útil para acomodar os rangos de valores que representan a liña e os
que representan o resto da superficie, en función da luz, do tipo e do color de liña e de superficies empregadas, e incluso do nivel das baterías, xa que de estar moi baixas, poden non entregar a tensión adecuada aos sensores e facer que estes varíen a súa tensión de saída.
![rutinaSL](https://botybot.wordpress.com/wp-content/uploads/2014/04/rutinasl.png?w=300&h=213)
A localización da linea segue o esquema representado sobre a seguinte imaxen.
![sl_robot](https://botybot.wordpress.com/wp-content/uploads/2014/04/sl_robot.png?w=383&h=287)
Na imaxen anterior, a posición da linea está representada polas franxas negras. En función da posición da linea respecto aos sensores do robot, a variable error_sl varia o seu valor según marcan os números da imáxen.
O programa encargado de regular o movemento do robot pola línea é o seguinte:
Variar_V=Kp_sl*error_sl + Kd_sl*(error_sl-error_sl_ant);
error_sl_ant=error_sl;
ultima_regulacion_sl=millis();
rpmI=v_referencia + Variar_V;
rpmD=v_referencia - Variar_V;
O programa, emprega un regulador proporcional derivativo P-D. O regulador aplica un valor de corrección de ruta en función de onde se encontre a liña respecto aos sensores CNY-70.
Kp_sl e Kd_sl teñen uns valores asignados en función das probas realizadas e poden modificarse para cada circuito en función da súa dificultade.
Subrutina Automático()
A sub-rutina encargada do modo Automático do robot encárgase de xestionar outras sub-rutinas en función do estado do mesmo.
![rutinaautomatico](https://botybot.wordpress.com/wp-content/uploads/2014/04/rutinaautomatico.png?w=563&h=300)
Subrutina Dondeestoy()
A sub-rutina “dondeestoy()” emprégase para intentar localizar unha parede que seguir, consulta o estado dos sensores dianteiros e laterais do robot e se detecta algo diante ou o seu carón, vai na súa busca; se non detecta nada, fai que o robot continúe avanzando en liña recta.
![rutinaDondestoy](https://botybot.wordpress.com/wp-content/uploads/2014/04/rutinadondestoy.png?w=515&h=366)
Para traballar cos sensores de ultrasóns, o robot realiza a media das 20 derradeiras medidas de cada sensor, para así evitar modificar a súa ruta, por falsos ecos. As rutinas encargadas desta tareas son as chamadas smoothing.
int smoothing_izq(int nueva_lectura)// Funcion para calcular a media do US esquerdo
{
total_izq=total_izq-lecturas_izq[index_izq];//Elimina o valor existente nesa posicion
lecturas_izq[index_izq]=nueva_lectura;//Carga a lectura na taboa
total_izq=total_izq+lecturas_izq[index_izq];//Suma o valor desta nova lectura
index_izq=index_izq+1;//aumenta o punteiro da taboa
if(index_izq==numeroLecturas)//Si o punteiro desborda, volve a 0
{
index_izq=0;
}
int media=total_izq/numeroLecturas;//Calcula a media dos valores almaceados
delay(2);
return media;//Devolve la media
}
Sub-rutinas enparedizquierda() e enparedderecha()
As sub-rutinas “enparedizquierda()” e “enparedderecha()” son moi similares polo que unicamente considérase necesario a explicación dunha delas.
A sub-rutina “enparedizquierda()” despraza ao robot ao longo dunha parede situada na súa cara esquerda. Se mentres está executando esta rutina detecta algo diante do robot, regrésase á rutina “dondeestoy()” para así poder evitar a colisión. Como é moi complicado que o robot continúe totalmente recto ao longo das paredes, este algoritmo o que fai é forzar un desprazamento de acercamento e afastamento en función da distancia a que os sensores de ultrasóns detecten que se atopa a parede. Cando o robot aproxímase a menos de 6 cm da parede, o motor correspondente a mesma e acelerado forzando así o afastameno do robot da parede. Se o robot sobrepasa a distancia de 12 cm, o motor contrario a parede é o acelerado para que así se aproxime a parede. Se o sensor detecta que a distancia á parede é maior a 35 cm considérase que a parede foi perdida; isto ocorre no caso das portas abertas, o que se fai neste lugar é cambiar a rutina “dondeestoy()” a cal se encargará de que o robot xire na dirección da parede para así poder seguir rastreándoa.
![rutinaEnpared](https://botybot.wordpress.com/wp-content/uploads/2014/04/rutinaenpared.png?w=659&h=521)
Sub-rutina manual()
A sub-rutina “manual()” é empregada no control manual do robot a través da aplicación móbil (Que explicarei nunha próxima entrada). Esta rutina executa distintos algoritmos en función dos comandos enviados dende o teléfono.
Cada vez que se executa a rutina, que é cada 300 mS, fai unha lectura dos sensores dianteiros e avisa a aplicación móbil se existe perigo de colisión dianteira, enviando un “1” se se detecta un obstáculo preto e un “2” si non se detecta nada. A maiores, se a función Antichoque está activada e detecta un obstáculo diante do robot, bloquea o avance deste, permitindo unicamente os xiros e o retroceso.