File content as of revision 0:2228de67a3ef:

#include "aldebaran_auto.h"
#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()
{
    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}; // We define variable for storing sensor output.
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);
Timer timer;
DigitalOut Led3(LED3);
int RC_value;
int RC_start;
int failsafe = 0;
#define FAILSAFE_SIGNAL 1400
void intHigh()
{
    RC_start = timer.read_us();
}
void intLow()
{
    RC_value = timer.read_us() - RC_start;
    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 * (60), 1500 + 5 * (60));
        break;

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

    case 1100: //vai reto
        Drive(1500 - 5 * (60), 1500 + 5 * (60));
        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), 150 + 5 * (30));
        break;

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

    default: //para////////////
        Drive(MED_PPM, MED_PPM);
        break;
    }
}

void AUTO_mode()
{
    timer.start();
    SWITCH.fall(&desligar);
    Failsafe.fall(&intLow);
    Failsafe.rise(&intHigh);
    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();
            //Line_check();
            Search();
        }
        else
        { // Failsafe ativado
            Led3 = 0;
            Drive(MED_PPM, MED_PPM);
        }
    }
}