CONTA DE FRANXAS:
Os encoders proporcionan a súa saída un cambio de nivel da tensión, por cada cambio de franxa das rodas.
As interrupción externas do microcontrolador configúranse para activarse cada vez que un dos pins asociados aos encoders pasan de nivel alto a nivel baixo e de nivel baixo a nivel alto.
Para o microcontrolador, nivel alto é unha tensión igual ou superior a 3 V e nivel baixo unha tensión igual ou menor a 2 V. A saída dos encoders varía a súa tensión o suficiente como para disparar estas interrupcións é así poder calcular canto tempo pasa entre cada unha.
O código relacionado co cálculo da velocidade é o seguitne:
// Declaración das rutinas de interrupción
main()
{
attachInterrupt(1, I_interrupt, CHANGE); //dig3
attachInterrupt(0, D_interrupt, CHANGE); //dig2
}
//Definición das rutinas de interrupción e aumento dos contadores
void I_interrupt() //Declaro rutina de interrupcion
{
contadorI++; //constante empregada para calcular a velocidade
contadorI2++; //constante empregada para ver si esta parado
contadorI3++;
}
void D_interrupt() //rutina de interrupcion
{
contadorD++;
contadorD2++;
}
Para calcular a velocidade a que xira cada roda é preciso coñecer o tempo que pasa entre cada interrupción e o número de interrupcións que ten un xiro completo de cada roda. O número de interrupcións por cada 360º vén dado polo número de franxas dos encoders, e estes teñen 24 marcas, polo que ao principio do programa defínese así:
int nI=24;/ /Numero de marcas dos encoders int nD=24;
CÁLCULO VELOCIDADE RODAS
Para coñecer o tempo que pasa entre cada interrupción, lánzase a rutina de cálculo da velocidade cada 333,3 mS. Tendo este tempo fixado solo queda dividir 333,3 entre o resultado almacenado nas variables contadorD e contadorI que corresponde co número de interrupcións lanzadas durante este tempo.
float tiempo_entre_calculos=333.3;
//CALCULO DA VELOCIDADE
if (millis()-ultimo_calculo > tiempo_entre_calculos)
{
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();
}
Para obter a velocidade de cada roda en revolucións por minuto, realízanse os seguintes cálculos:
V i =(1000000∗60∗númeroDeInterrupciónsDendeDerradeiroCálculo )/ númeroDeFranxasEncoder V f =V i /TempoDendeDerradeiroCálculo
O programa empregado para calcular esta velocidade é o seguinte:
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;// voltas/micros
v_real=v_real/(micros()-tiempoZ);
return v_real;// Devolve a velocidade en rpm
}
CÁLCULO VELOCIDADE ROBOT
Coa velocidade de cada roda e o seu diámetro, é posible calcular a velocidade de
desprazamento do robot coa seguinte fórmula:
V r =V f ∗DiámetroRoda
CÁLCULO XIRO ROBOT
Para calcular o que cada roda ten que xirar para que o robot xire sobre seu eixe vertical X grados, é preciso coñecer a distancia entre cada roda do robot, sendo esta de 8,7 cm os cálculos a realizar son os seguintes:
PulsosXirar i =((gradosXirar∗distanciaEntreRodas∗pi )/360) PulsosXirar f =PulsosXirar i /( DiámtroRoda∗pi / NumeroFranxasEncoder)
O resultado PulsosXirar f é o número de franxas que cada roda ten que xirar para que o robot xire X grados, definidos por gradosXirar . O programa encargado de realizar os cálculos anteriores é o mostrado a continuación:
void giro() //FUNCION QUE ENTREGA OS PULSOS A XIRAR EN FUNCION DO
ANGULO DESEXADO
{
pulsos_a_girar=((grados_a_girar*8.7*pi)/360)/(6.*pi/24);
}
BotyBot 