File content as of revision 11:8d71a1e1e947:

//================================================================================
//=========================  BLOCO PRINCIPAL - MAIN.CPP  =========================
//================================================================================


//*****************************  INCLUDES - INICIO  ******************************

#include "mbed.h"
#include "PID.h"
#include "Motor.h"
#include "Sensor.h"

Serial bt(p28, p27);

//Definição das variaveis para as velocidades dos motores
float LMotor = 0;
float RMotor = 0;

//Variavel que faz a contagem do para a parada
//int FIM = 0;
Timeout END; //Timer para fazer o LF parar

//#define DEBUG

#ifdef DEBUG
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
#endif

//*****************************  INCLUDES -  FINAL  ******************************
//################################################################################
//################################################################################
//******************************* FUNCOES - INICIO  ******************************
void bluetooth (float left, float right) {
    bt.printf("%d, %.2f, %.2f, %d, %d\n\r", Get_Position(), left, right,Get_Left(),Get_Right());  
      
}

//Funcao que inicializa a interrupcao do sensor lateral direito

int main(void) {
    //int i;
    Motors_Setup();  //Chama a função para inicializar os motores
    Sensors_Setup(); //Chama a função para inicializar os sensores
    Meio_Setup();    //Chama a função para inicializar o sensor traseiro
    
    #ifdef DEBUG
    led1 = 0;
    led2 = 0;
    led3 = 0;
    led4 = 0;
    #endif
    
    //END.attach(&Stop, 20.0); //Inicia END para executar a função Stop depois de 20s
    
    //Loop infinito
    while (1) { // Valor correto: 17/18
        
        /*if (Check_Final()) { 
            FIM++; 
            #ifdef DEBUG
            if(FIM == 1){led1 = 1;}
            else if(FIM == 2){led2 = 1;}
            else if(FIM == 3){led3 = 1;}
            else if(FIM == 4){led4 = 1;}
            else if(FIM == 16){led2 = 0; led4 = 0; }
            #endif
            }*/
        Read_Sensors();                                 //Lê os sensores
        PID_Control(Get_Position(), &LMotor, &RMotor);  //Calcula o controle
        Drive(LMotor, RMotor); 
        bluetooth(LMotor,RMotor);                         //Passa os valores pro motor
        //wait(0.0035);                                   //Se não tiver delay motor fica sem controle
    } 
    
    /*for (i=0; i < 4200; i++) {
        Read_Sensors();
        PID_Control(Get_Position(), &LMotor, &RMotor);
        Drive(LMotor, RMotor);
    }
    Stop();*/  
    
    return 0;
}

//******************************** FUNCOES - FINAL  ******************************