File content as of revision 0:9efe13b5d868:

#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;
 
float correcaoPD;
double erroSensores; 

float velocidadeEsquerda;
float velocidadeDireita;
float velocidadeRealEsquerda;
float velocidadeRealDireita;
float velocidadeBase = 0.18;

Callback<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
    while(1){                   
        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:
                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 = velocidadeBase + correcaoPD;                                       //Ajusta a inicialmente a velocidade das rodas para manter na linha
                velocidadeDireita = velocidadeBase - correcaoPD;
                //velocidadeRealDireita = ;                                                             //Pega a velocidade do seguidor lida pelos encoderes
                //velocidadeRealEsquerda = ;
                velocidadeEsquerda = PID_rodaEsquerda(velocidadeEsquerda,velocidadeRealEsquerda);       //Faz o ajusta final da velocidade das rodas
                velocidadeDireita = PID_rodaDireita(velocidadeDireita,velocidadeRealDireita);
                ControleMotores(velocidadeEsquerda,velocidadeDireita);                                  //Muda a velocidade das rodas para as velocidades calculadas anteriormente
                //Mapear a pista

            break;

            case 2:
            break;

            case 3:
            break;

            default:
            ParadaFinal();
            break;
    }    


    }
    
}