File content as of revision 0:3e0de9e209d1:

#include"mbed.h"
#include"PwmOut.h"
Serial bt(p9, p10);
//================================================================================
//=========================  MOTORS CONTROL - ====================================
//================================================================================
PwmOut Right(p21);
PwmOut Left(p22);
#define MED_PPM 1500 // Valor indicando que o motor deve ficar parado de acordo com o ppm
int LSpeedGlobal;
int RSpeedGlobal;

void Drive(int LeftSpeed,int RightSpeed)
{
    Left.pulsewidth_us(LeftSpeed);
    Right.pulsewidth_us(RightSpeed);
    LSpeedGlobal = LeftSpeed;
    RSpeedGlobal = RightSpeed;
}
void calibrate()// Função necessária para dar o PPM inicial nos controladores. 
{
    Drive(MED_PPM, MED_PPM);
    wait(3);
}
//================================================================================
//=====================  DISTANCE  SENSORS - =====================================
//================================================================================
//****************************  DEFINICOES - ************************************
DigitalIn Sensor1 (p26);// Sensor digital localizado na esquerda do robo
DigitalIn Sensor2 (p27);//Sensor digital localizado na diagonal esquerda do robo
DigitalIn Sensor3 (p28);//Sensor digital localizado na frente do robo
DigitalIn Sensor4 (p25); //Sensor digital localizado na diagonal direita do robo
DigitalIn Sensor5 (p30); //Sensor digital localizado na direita do robo

int measure[5]= {0,0,0,0,0}; // Variável para guardar os valores dos sensores de distância.
int total = 0; //{left, left_diagonal, front, right_diagonal, right}

void Search ()
{
    measure[0] = Sensor1.read();
    measure[1] = Sensor2.read();
    measure[2] = Sensor3.read();
    measure[3] = Sensor4.read();
    measure[4] = Sensor5.read();

    total = measure[4]*10000 + measure[3]*1000 + measure[2]*100 + measure[1]*10 + measure[0];
    return;
}
//================================================================================
//=====================  SWITCH INICIALIZATION - =================================
//================================================================================
InterruptIn SWITCH(p11);
DigitalOut Led4(LED4);
void desligar()
{
    Drive(MED_PPM,MED_PPM);
}
//================================================================================
//=====================  FAILSAFE - ==============================================
//================================================================================
InterruptIn Failsafe(p12); // Interrupt on digital pushbutton input p12
Timer timer;
DigitalOut Led3(LED3);
int RC_value;
int RC_start;
int failsafe=0;
#define FAILSAFE_SIGNAL 1400 //Valor em microssegundos do sinal do failsafe
void intHigh()
{
    RC_start=timer.read_us();//Começa o timer
}
void intLow()
{
    RC_value=timer.read_us()- RC_start;//Reads ppm signal from RC receiver.  
    if(RC_value > FAILSAFE_SIGNAL) {
        failsafe=0;   //failsafe não ativado
    } else {
        failsafe=1;   //failsafe ativado
    }
}

//================================================================================
//=====================  LINE  SENSORS - =========================================
//================================================================================
#define PARAMETER 0.908
#define RightSensor p18
#define LeftSensor p20
DigitalOut Led1 (LED1);
DigitalOut Led2 (LED2);
AnalogIn RightRead(RightSensor);
AnalogIn LeftRead(LeftSensor);
bool right_line=1,left_line=1;
int result=0,anterior=0;
int Line_check ()
{
    if(RightRead.read()>PARAMETER) {
        right_line=1;
    }
    else {
        right_line=0;   //white
    }
    if(LeftRead.read()>PARAMETER) {
        left_line=1;
    } else {
        left_line=0;
    }
    result=right_line*10+left_line;
    if(result==0 && anterior==0) {
        return 0;
    }
    if(result==0 && anterior==1) {
        anterior=0;
        wait_ms(0);
        return 0;
    }
    switch(result) {
        case 10: { //Sensor de linha da direita viu a linha
            Led2=1;
            Led1=0;
            Drive(1500+5*(50),1500-5*(50)); //Para tras com velocidade máxima por 50ms
            wait(1);
            Drive(1500-5*(35),1500-5*(35));//Para esquerda com velocidade máxima por 100ms
            wait(0.5);
            anterior=1;
            break;
        }

        case 1: { //Sensor de linha da esquerda viu a linha
            Led2=0;
            Led1=1;
            Drive(1500+5*(50),1500-5*(50));//Para tras com velocidade máxima por 100ms
            wait(1);
            Drive(1500+5*(35),1500+5*(35)); //Para direita com velocidade máxima por 50ms
            wait(0.5);
            anterior=1;
            break;
        }

        case 11: { //Sensor de linha da esquerda e da direita viram a linha
            Led2=1;
            Led1=1;
            Drive(1500+5*(50),1500-5*(50));//Para tras 
            wait(1);
            Drive(1500+5*(35),1500+5*(35)); //Para direita 
            wait(0.5);
            anterior=1;
            break;

        }
        default: {
            anterior=1;
            Led2=0;
            Led1=0;
            break;
        }
    }
    return 1;
}
void Action ()
{

    switch(total) {

        case 0: //caso nao veja nenhum dos sensores
            Drive(LSpeedGlobal,RSpeedGlobal);
            break;

        case 1: //vira para a esquerda
            Drive(1500-5*(40),1500-5*(40));
            break;

        case 10: //vira para a esquerda
            Drive(1500-5*(30),1500-5*(30));
            break;

        case 11://vira para a esquerda
            Drive(1500-5*(30),1500-5*(30));
            break;

        case 100: //vai reto
            Drive(1500-5*(60),1500+5*(60));
            break;

        case 110://vai reto
            Drive(1500-5*(50),1500+5*(50));
            break;

        case 1000: //vira para a direita
            Drive(1500+5*(30),1500+5*(30));
            break;//

        case 1100://vai reto
            Drive(1500-5*(50),1500+5*(50));
            break;

        case 1110://vai reto (GRANDE CHIFRE)
            Drive(1500-5*(100),1500+5*(100));
            break;

        case 10000: //vira para direita
            Drive(1500+5*(40),1500+5*(40));
            break;

        case 11000://vira para a direita
            Drive (1500+5*(30),1500+5*(30));
            break;

        //todos os outros casos que são impossíveis

        default://para////////////
            Drive(MED_PPM,MED_PPM);
            break;
    }
}
int main()
{
    timer.start();
    SWITCH.fall(&desligar);//Attach interrupt function request when falling edge is occurred
    Failsafe.fall(&intLow);//Attach interrupt function request when falling edge is occurred
    Failsafe.rise(&intHigh);//Attach interrupt function request when rising edge is occurred
    calibrate();
    LSpeedGlobal = 1500-5*(30);
    RSpeedGlobal = 1500+5*(30);

    while(SWITCH.read()==0) {
        Search();
        Led4=1;
    }
    Led4=0;
    while(1) {
        if(!failsafe && SWITCH.read()) { //Failsafe não ativado
            Led3=1;
            Action();
            Search();
            }
        else { // Failsafe ativado
            Led3=0;
            Drive(MED_PPM,MED_PPM);
        }
    }
}