File content as of revision 6:5551834026ef:

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

#define TAMANHODOMAPA 50
#define ACELERACAO  2 // Deve ser alterado na fase de teste

//------------------------------------------------------------------------------

//---------------------------------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[TAMANHODOMAPA][4]; /* Matriz de dados:
mapa[i][0]-> distancia 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;
mapa[i][3]-> Distancia antes da curva em que precisa comecar a desacelerar*/

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

LocalFileSystem local("local"); // Cria o local fylesystem com o nome de "local"
//------------------------------------------------------------------------------

//------------------------------------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();
}

int retornaPulsoDireito ()
{
    return pulso_direito;
}

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;
    }
}

/*==============================================================================
Nome: DistanciaPraDesacelerar;
Objetivo: Calcula o 4o item do mapa, a distancia para desacelerar para chegar na proxima parte da pista (curva) com a velocidade certa;
==============================================================================*/
    

float DistanciaPraDesacelerar (float velAtual, float proxVel,float aceleracao)
{   
    if (proxVel == NULL)//para evitar problema na ultima parte da pista,em que nao teremos uma proxima velocidade
    {
        proxVel = 0;
    }
    float distancia = (pow(proxVel,2) - pow(velAtual,2))/2*aceleracao ;
    return distancia;
}

/*==============================================================================
Nome: CriaParteQuatro;
Objetivo: Adiciona a distancia para comecar a desaceleracao no mapa da pista;
==============================================================================*/

void CriaParteQuatro ()
{
    for(int i = 0;i<TAMANHODOMAPA;i++){
        if (mapa[i][0] > mapa[i][1] + 1 || mapa[i][0] + 1 < mapa[i][1] ) // +1 é um parametro arbitrario para evitar que seja uma diferenca minima dos periodos de cada roda
        {
            mapa[i][3] = DistanciaPraDesacelerar(velAtual_direita,mapa[i+1][2],ACELERACAO);
        }
        else 
        {
            mapa[i][3] = 0;
        }
    }
}
/*==============================================================================
Nome: criaArquivo;
Objetivo: Cria e coloca as informacoes do mapa da pista dentro de um arquivo;
==============================================================================*/
    
void criaArquivo ()
{

    FILE *mapaArquivo = fopen("/local/mapaTeste.txt","w");
    if(mapaArquivo == NULL){
        printf("Impossivel criar o arquivo");
        return;
    }
    //fprintf(mapaArquivo,"Teste\n");
    for(int i = 0;i<TAMANHODOMAPA;i++)
    {
        fprintf (mapaArquivo,"%f-%f-%f-%f\n",mapa[i][0],mapa[i][1],mapa[i][2],mapa[i][3]);
    }
    
    fclose(mapaArquivo);

}