File content as of revision 5:b07a3cfe017d:

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

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

int botao = 0;
int comando = 1;
int contadorAsaDireita = 0;
 
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;

void AlteraComando(){
    botao++;
}

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

int main()
{
    Botao.rise(&AlteraComando);
    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);                              
                            ParadaFinal();                                                              //Para o seguidor na segunda marcacao
                        }
                    }
                }
              }

            break;

            case 2:                                                                           //Percorre a pista com o uso do mapa
              while(1){ 
                /* 
                if(ditancia pra curva < distancia pra desacelerar){                                     //Seleciona a velocidade
                    novaVelocidade = VelocidadeBateria ( VELOCIDADE DO MAPA + 1)
                }
                else{
                    novaVelocidade = VelocidadeBateria(VELOCIDADE DO MAPA);
                }  
                Acelerando(float novaVelocidade);                                                       //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
                        //MAPA + 1 POSICAO                                                              //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;
    }    


    
    
}