File content as of revision 2:4db1486af4d5:

//--------------------------------BIBLIOTECAS-----------------------------------
#include "mbed.h"
#include "encoder.h"
#include "LineSensor.h"
//------------------------------------------------------------------------------

//---------------------------------ENCODERS-------------------------------------
InterruptIn OUTA_RIGHT(p26);
DigitalIn OUTB_RIGHT(p11);

InterruptIn OUTA_LEFT(p21);
DigitalIn OUTB_LEFT(p7);
//------------------------------------------------------------------------------

//-----------------------------------VARIAVEIS----------------------------------
float diametro_roda = 26; // Diametro da roda
float reducao = 10; // Redução do motor 10:1
float perimetro_pulso = 3.14*diametro_roda/(12*reducao); // Perimetro da roda por pulso do encoder
float D = 149; // Distancia entre as rodas do line (mm)
float mi = 1.45; // Coeficiente de atrito (Valor aproximado, testar em lab)
float mapa[50][3]; /* Matriz de dados:
mapa[i][0]-> distância percorrida pela roda direita no trecho i;
mapa[i][1]-> distancia percorrida pela roda esquerda no trecho i;
mapa[i][2]-> velocidade maxima do line do trecho i;*/

int i = 0;
int pulso_direito = 0; // Contador de pulsos da roda direita
int pulso_esquerdo = 0; // Contador de pulsos da roda esquerda
float velAtual_direita = 0;
float velAtual_esquerda = 0;

Timer timer_direito; // Armazena o tempo de cada pulso na roda direita
Timer timer_esquerdo; // Armazena o tempo de cada pulso na roda esquerda
//------------------------------------------------------------------------------

//------------------------------------FUNÇÕES-----------------------------------
    
/*==============================================================================
Nome: contaPulso_direito; contaPulso_esquerdo;
Objetivo: Conta os pulsos do encoder e calcula a velocidade atual;
==============================================================================*/
void contaPulso_direito()
{
    pulso_direito++;
    timer_direito.stop();
    velAtual_direita = perimetro_pulso/timer_direito.read();
    timer_direito.reset();
    timer_direito.start();
}

void contaPulso_esquerdo()
{
    pulso_esquerdo++;
    timer_esquerdo.stop();
    velAtual_esquerda = perimetro_pulso/timer_esquerdo.read();
    timer_esquerdo.reset();
    timer_esquerdo.start();
}
/*==============================================================================
Nome: retornaVelDireita; retornaVelEsquerda;
Objetivo: Retorna as velocidades reais das rodas para podermos usa-las na main
==============================================================================*/
float retornaVelDireita (){
    return velAtual_direita;
}

float retornaVelEsquerda (){
    return velAtual_esquerda;
}
/*==============================================================================
Nome: calcula_velmax;
Objetivo: Calcula a velocidade máxima em cada trecho sem que o line derrape;
==============================================================================*/
float calcula_velmax()
{
    float raio; // Raio da curva
    float dif = mapa[i][0] - mapa[i][1]; // Diferença entra a distancia percorrida pela roda direita e a roda esquerda
    float soma = mapa[i][0] + mapa[i][1]; // Soma entra a distancia percorrida pela roda direita e a roda esquerda
    if (abs(dif) > 0.1) // Margem de erro de 0.1 na reta (A ser testada)
    {
        raio = abs((D/2) * (soma/dif));
        return sqrt(raio * 9810 * mi);
    }
    else
        return 0; // Se retornar 0 será feito um novo calculo maximo da velocidade
        
    
}   
/*==============================================================================
Nome: mapeamento;
Objetivo: Faz o mapeamento da pista e armazena na matriz de dados;
==============================================================================*/
void mapeamento(bool esq, bool dir) // Variaveis esq e dir estarão gravadas o estado do sensor da asa
{
    mapa[i][0] = pulso_direito*perimetro_pulso;
    mapa[i][1] = pulso_esquerdo*perimetro_pulso; 
    mapa[i][2] = calcula_velmax(); 
    if (esq && !dir) // Verifica se não está passando por um cruzamento
    {
        i++;
        pulso_direito = 0;
        pulso_esquerdo = 0; 
    }
    else if (dir && !esq) // Zera o contador ao passar pela marcação de inicio da pista
    {
        pulso_direito = 0;
        pulso_esquerdo = 0;
    }
}