File content as of revision 0:2228de67a3ef:

#include "aldebaran_semiauto.h"
#include "mbed.h"

#define MAX_PPM 2000       // Valor indicando que o motor deve ir para frente com 100% de acordo com o ppm
#define MED_PPM 1500       // Valor indicando que o motor deve ficar parado de acordo com o ppm
#define MIN_PPM 1000       // Valor indicando que o motor deve ir para tras com 100% de acordo com o ppm
#define REVERSE_LEFT 1550  //Signal to make the robot go reverse with the left wheels.
#define FORWARD_LEFT 1550  //Signal to make the robot go forward with the left wheels.
#define REVERSE_RIGHT 1550 //Signal to make the robot go reverse with the left wheels.
#define FORWARD_RIGHT 1550 //Signal to make the robot go forward with the left wheels.
#define BACKWARDS 10

Timer timer;
InterruptIn ST(p15);
InterruptIn TH(p14);

PwmOut Right(p21);
PwmOut Left(p22);

int ch1_start;
int ch2_start;
int ch1_in;
int ch2_in;

void ST_High() { ch1_start = timer.read_us(); } //start the time
void ST_Low() { ch1_in = timer.read_us() - ch1_start; }

void TH_High() { ch2_start = timer.read_us(); } //start the time
void TH_Low() { ch2_in = timer.read_us() - ch2_start; }
//================================================================================
//=====================  FAILSAFE - ==============================================
//================================================================================
InterruptIn Failsafe(p12);
DigitalOut Led4(LED4);
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 - =========================================
//================================================================================
DigitalOut led1(LED1);
DigitalOut led2(LED2);

#define RightSensor p18
#define LeftSensor p20
#define PARAMETER 0.9

AnalogIn RightRead(RightSensor);
AnalogIn LeftRead(LeftSensor);
float sensor_right;
float sensor_left;
int right_line;
int left_line;
int line;
int READ_SENSORS()
{
    sensor_right = RightRead;
    sensor_left = LeftRead;
    if (sensor_right > PARAMETER)
    {
        right_line = 1;
    } //black
    else
    {
        right_line = 0;
    } //white
    if (sensor_left > PARAMETER)
    {
        left_line = 1;
    }
    else
    {
        left_line = 0;
    }
    if (right_line)
    {
        led1 = 0;
    }
    else
    {
        led1 = 1;
    }
    if (left_line)
    {
        led2 = 0;
    }
    else
    {
        led2 = 1;
    }
    line = left_line * 10 + right_line;
    if (line != 11)
    {
        return 1;
    } //Sensors read something
    else
    {
        return 0;
    } //Sensors read nothing
}
void Drive(int LeftSpeed, int RightSpeed)
{
    Left.pulsewidth_us(LeftSpeed);
    Right.pulsewidth_us(RightSpeed);
}
void calibrate()
{
    Left.period_ms(16);
    Left.pulsewidth(MAX_PPM);

    Right.period_ms(16);
    Right.pulsewidth(MAX_PPM);

    wait(3);

    Drive(MED_PPM, MED_PPM);
    wait(3);
}

void semiauto()
{
    Drive(MED_PPM, MED_PPM); //Stop motors
    for (int i = 0; i < BACKWARDS; i++)
    {
        Drive(1550, 1450);
    }
}

void radio()
{
    Drive(ch1_in, ch2_in);
}

void SEMIAUTO_mode()
{
    timer.start();
    calibrate();
    Failsafe.fall(&intLow);
    Failsafe.rise(&intHigh);
    ST.fall(&ST_Low);
    ST.rise(&ST_High);
    TH.fall(&TH_Low);
    TH.rise(&TH_High);
    while (1)
    {
        if (!failsafe) //Failsafe não ativado
        {
            Led4 = 1;
            if (READ_SENSORS())
            {
                semiauto(); //line sensors read something
            }
            else
            {
                radio();
            }
        }
        else // Failsafe ativado
        {
            Led4 = 0;
            Drive(MED_PPM, MED_PPM);
        }
    }
}