File content as of revision 0:2228de67a3ef:

#include "aldebaran_rc.h"
#include "mbed.h"
#define OFFSET 2

Serial bt(p9, p10);

Timer timer;

DigitalOut st_led(LED1);
DigitalOut th_led(LED2);
DigitalOut mid_led(LED3);

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

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

int max_st = 1500, max_th = 1500, min_st = 1500, min_th = 1500, mid_st, mid_th;

int RC_value[2];
int RC_start[2];

void ST_High() { RC_start[0] = timer.read_us(); } //start the time }
void ST_Low() { RC_value[0] = timer.read_us() - RC_start[0]; }

void TH_High() { RC_start[1] = timer.read_us(); } //start the time }
void TH_Low() { RC_value[1] = timer.read_us() - RC_start[1]; }

void RC_mode()
{
    int st_start, th_start;

    timer.start();

    ST.fall(&ST_Low);
    ST.rise(&ST_High);

    TH.fall(&TH_Low);
    TH.rise(&TH_High);

    Right.period(0.02);
    Left.period(0.02);

    bt.baud(9600);

    st_led = 1;
    wait(0.5);
    st_led = 0;

    st_start = timer.read_us();
    while (timer.read_us() - st_start < 5000000)
    {
        if (RC_value[0] > max_st)
        {
            max_st = RC_value[0];
        }
        if (RC_value[0] < min_st)
        {
            min_st = RC_value[0];
        }
    }
    st_led = 1;
    wait(1);
    th_led = 1;
    wait(0.5);
    th_led = 0;
    th_start = timer.read_us();
    while (timer.read_us() - th_start < 5000000)
    {
        if (RC_value[1] > max_th)
        {
            max_th = RC_value[1];
        }
        if (RC_value[1] < min_th)
        {
            min_th = RC_value[1];
        }
    }
    th_led = 1;

    mid_led = 1;
    wait(5);
    mid_led = 0;
    mid_st = RC_value[0];
    mid_th = RC_value[1];
    wait(1);
    mid_led = 1;

    bt.printf("Max-ST: %i Min-ST: %i Max-TH: %i Min-TH: %i\n\r", max_st, min_st, max_th, min_th);

    wait(5);
    while (1)
    {
        bt.printf("Left: %i  Right: %i\n\r", RC_value[1], RC_value[0]);

        Right.pulsewidth_us(mid_st + (mid_st - RC_value[1]) + (mid_th - RC_value[0]));
        Left.pulsewidth_us(mid_st + (mid_st - RC_value[1]) - (mid_th - RC_value[0]) - OFFSET);
    }
}