File content as of revision 6:5551834026ef:

#include "mbed.h"
#include "LineSensor.h"
#include "bluetooth.h"
#include "encoder.h"
#include "motors.h"
#include "PID.h"

#define TAMANHODOMAPA 50
#define PERIMETRODARODA 26

InterruptIn Botao (p5);
DigitalOut Led (p6);

InterruptIn OUTA_RIGHT(p26);
DigitalIn OUTB_RIGHT(p11);

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

int botao = 0;
int comando = 1;
int contadorAsaDireita = 0;
int posicaoNoMapa = 0;
float distanciaPraCurva;

float correcaoPD;
double erroSensores; 

float velocidadeEsquerda;
float velocidadeDireita;
float velocidadeRealEsquerda;
float velocidadeRealDireita;
float velocidadeAtual = 0.18;
float novaVelocidade;
float correcaoEsquerda;
float correcaoDireita;

bool estadoAsaDireita = false;
bool estadoAsaEsquerda = false;

float mapaPraCorrer[TAMANHODOMAPA][4];

void leArquivo ()
{
    FILE *mapaArquivo = fopen("/local/mapaTeste.txt","r");
    if(mapaArquivo == NULL){
        printf("Impossivel criar o arquivo");
        return;
    }
    for(int i = 0;i<TAMANHODOMAPA;i++)
    {
        fscanf(mapaArquivo,"%f-%f-%f-%f\n",&mapaPraCorrer[i][0],&mapaPraCorrer[i][1],&mapaPraCorrer[i][2],&mapaPraCorrer[i][3]); // talvez tenha q salvar numa variavel, igualar a um vetor
    }
    
    fclose(mapaArquivo);
}

void AlteraComando(){
    botao++;
}

void PiscaLed(){
    if(Led){
        Led = 0;
    }
    else{
        Led = 1;
    }
}

int main()
{
    Botao.rise(&AlteraComando);
    OUTA_RIGHT.rise(&contaPulso_direito);
    OUTA_LEFT.rise(&contaPulso_esquerdo);
    leArquivo();

    Setup_Motores();
    while(botao == 0){          //Espera o aperto do botao para iniciar a calibragem, o led permanecera acesso para indicar esta fase
        wait_us(10);
        Led = 1;
    }
    while (botao == 1)          //Inicialmente calibra os sensores ao ligar o seguidor,o led fica piscando para mostrar que a calibragem esta acontecendo
    {                           //Espera que se aperte o botao para terminar a calibragem
        CalibragemDosSensores();
        PiscaLed();
    }

    Led = 0;                    //Certificando que o led esta apagado depois da calibragem

    wait_us(5000);              //Espera 5 segundos para poder mudar a posição de calibrar os sensores para a de correr
    //Tocar o buzzer parar avisar q ai começar a correr                  
        switch(comando){        
                                //Comando = 1 -> Faz a pista devagar e mapeando 
                                //Comando = 2 -> Faz a pista usando a leitura do mapa
                                //Comando = 3 -> Faz a usando a leitura do mapa e ultrapassando os limites de velocidade
            case 1:
              while(1){ 
                velocidadeAtual = VelocidadeBateria(velocidadeAtual);                                     //Ajuste da velocidade por causa da descarga da bateria 
                LerSensoresFrontais();                                                                  //Leitura dos sensores frontais
                erroSensores = CalculaErro();                                                           //Calcula o arco do erro dos sensores
                correcaoPD = PID_Sensores(erroSensores);                                                //Calcula a correção do PD angular
                velocidadeEsquerda = velocidadeAtual + correcaoPD;                                       //Ajusta a inicialmente a velocidade das rodas para manter na linha
                velocidadeDireita = velocidadeAtual - correcaoPD;
                velocidadeRealDireita = retornaVelDireita();                                            //velocidade em /m/s
                velocidadeRealEsquerda = retornaVelEsquerda();
                velocidadeEsquerda = transformaPWM(velocidadeEsquerda);                                  //Converte velocidade PWM em m/s
                velocidadeDireita = transformaPWM(velocidadeDireita);
                correcaoEsquerda = PID_rodaEsquerda(velocidadeEsquerda,velocidadeRealEsquerda);       //Faz o ajusta final da velocidade das rodas
                correcaoDireita = PID_rodaDireita(velocidadeDireita,velocidadeRealDireita);
                velocidadeEsquerda += correcaoEsquerda;
                velocidadeDireita += correcaoDireita;
                ControleMotores(velocidadeEsquerda,velocidadeDireita);                                  //Muda a velocidade das rodas para as velocidades calculadas anteriormente
                estadoAsaDireita = LerAsaDireita();
                estadoAsaEsquerda = LerAsaEsquerda();
                if(estadoAsaEsquerda || estadoAsaDireita ){                                             //Checa os sensores laterais
                    mapeamento(estadoAsaEsquerda,estadoAsaDireita);                                     //faz o mapeamento da pista
                    if (estadoAsaDireita && !estadoAsaEsquerda){
                        contadorAsaDireita++;                                                           //Faz as contagens das marcacoes a direita
                        if(contadorAsaDireita >=2){//ARRUMAR UM JEITO DE DAR UM TEMPO ENTRE A LEITURA E O SEGUIDOR PARAR PARA ELE PASSAR TODO PELA LINHA DE CHAGADA
                            Aceleracao(0,0);
                            CriaParteQuatro();
                            criaArquivo();
                            ParadaFinal();                                                              //Para o seguidor na segunda marcacao
                        }
                    }
                }
              }

            break;

            case 2:                                                                           //Percorre a pista com o uso do mapa
              while(1){ 
                distanciaPraCurva =retornaPulsoDireito() * PERIMETRODARODA  ;
                if(distanciaPraCurva < mapaPraCorrer[posicaoNoMapa][1] - mapaPraCorrer[posicaoNoMapa][3]){                                     //Seleciona a velocidade
                    novaVelocidade = VelocidadeBateria (mapaPraCorrer[posicaoNoMapa+1][2]);
                }
                else{
                    novaVelocidade = VelocidadeBateria(mapaPraCorrer[posicaoNoMapa][2]);
                }  
                Aceleracao(novaVelocidade,1);                                                       //Acelera ou desacelera o robo
                LerSensoresFrontais();                                                                  //Leitura dos sensores frontais
                erroSensores = CalculaErro();                                                           //Calcula o arco do erro dos sensores
                correcaoPD = PID_Sensores(erroSensores);                                                //Calcula a correção do PD angular
                velocidadeEsquerda = velocidadeAtual + correcaoPD;                                       //Ajusta a inicialmente a velocidade das rodas para manter na linha
                velocidadeDireita = velocidadeAtual - correcaoPD;
                velocidadeRealDireita = retornaVelDireita(); 
                velocidadeRealEsquerda = retornaVelEsquerda();
                velocidadeEsquerda = transformaPWM(velocidadeEsquerda);                                  //Converte velocidade PWM em m/s
                velocidadeDireita = transformaPWM(velocidadeDireita);
                correcaoEsquerda = PID_rodaEsquerda(velocidadeEsquerda,velocidadeRealEsquerda);       //Faz o ajusta final da velocidade das rodas
                correcaoDireita = PID_rodaDireita(velocidadeDireita,velocidadeRealDireita);
                velocidadeEsquerda += correcaoEsquerda; 
                velocidadeDireita += correcaoDireita;
                ControleMotores(velocidadeEsquerda,velocidadeDireita);                                  //Muda a velocidade das rodas para as velocidades calculadas anteriormente
                estadoAsaDireita = LerAsaDireita();
                estadoAsaEsquerda = LerAsaEsquerda();
                if(estadoAsaEsquerda || estadoAsaDireita ){                                             //Checa os sensores laterais
                    if(!estadoAsaEsquerda || estadoAsaDireita){                                         //Leu somente o asa direita
                        posicaoNoMapa++;                                                             //Atualiza a etapa do mapa da pista que esta sendo lido
                    }                                                                                   
                    if(estadoAsaDireita && !estadoAsaEsquerda){                                        //Leu somente a asa esquerda
                        contadorAsaDireita++;                                                           //Faz as contagens das marcacoes a direita
                        if(contadorAsaDireita >=2){
                            Aceleracao(0,0);
                            ParadaFinal();                                                              //Para o seguidor na segunda marcacao
                        }
                    }
                }
              }

            break;

            case 3:  
                while(1){

                 }
            break;

            default:
            ParadaFinal();
            break;
    }    


    
    
}