File content as of revision 0:98cd95fbb541:

#include "mbed.h"
#include "rtos.h"
#include "esc.h"
#include "Servo.h"

#define RightIR     A3
#define LeftIR      A1
#define RearIR      A2

#define SERVO1      PB_6 //D10
#define SERVO2      PC_7 //D9

#define MotorLeft   SERVO1
#define MotorRight  SERVO2

#define BtnStart    A5
#define BtnCalib    A4

#define LeftBit     2
#define MidBit      1
#define RightBit    0


const double threshold = 0.2;

Serial pc(SERIAL_TX, SERIAL_RX);

DigitalOut led(LED1);

DigitalIn StartButton(BtnStart, PullUp);
DigitalIn CalibButton(BtnCalib, PullUp);

AnalogIn LeftSensor(LeftIR); 
AnalogIn RightSensor(RightIR);
AnalogIn RearSensor(RearIR);

Servo LeftESC(MotorLeft);
Servo RightESC(MotorRight);

volatile double SensorRightVal, SensorLeftVal, SensorRearVal;
volatile unsigned int SensorFlag;

void initMotor()
{
    LeftESC=0.5;
    RightESC=0.5;
}
void setMotor(double L, double R)
{
    L /=(double)2;
    L+=(double)0.5;
    R /=(double)2;
    R+=(double)0.5;
    
    LeftESC=L;
    RightESC=R;
    Thread::wait(20);
}
void SensorReader(void const *argument)
{
    while(true)
    {
        SensorRightVal = RightSensor.read();
        SensorLeftVal = LeftSensor.read();
        SensorRearVal = RearSensor.read();
        if(SensorRightVal>threshold)
        {
            SensorFlag |=(1<<RightBit);
        }
        else
        {
            SensorFlag &=~(1<<RightBit);
        }
        if(SensorLeftVal>threshold)
        {
            SensorFlag |=(1<<LeftBit);
        }
        else
        {
            SensorFlag &=~(1<<LeftBit);
        }
        if (SensorRearVal>threshold)
        {
            SensorFlag |=(1<<MidBit);
        }
        else
        {
            SensorFlag &=~(1<<MidBit);
        }
        Thread::wait(20); //20ms
    }
}
void DataReporter(void const *argument)
{
   while(true)
   {
       pc.printf("Sensor Flag 0x%X L = %10.5f R = %10.5f B = %10.5f BtnStart %d BtnCalib %d\r\n", SensorFlag, LeftSensor.read(), RightSensor.read(), RearSensor.read(), StartButton.read(), CalibButton.read());
       Thread::wait(500);
   }
       
}
void MotorController(void const *argument)
{
    while(true)
    {
        switch(SensorFlag)
        {
            case 0b000:
                setMotor(0.25,-0.25);
            break;
            case 0b001:
                setMotor(0.25,-0.25);
            break;
            case 0b010:
                setMotor(0.5,-0.5);
            break;
            case 0b011:
                
            break;
            case 0b100:
                setMotor(-0.25,0.25);
            break;
            case 0b101:
                setMotor(1,1);
            break;
            case 0b110:
                
            break;
            case 0b111:
                
            break;
               
        }
        Thread::wait(1);
    }
}
void enterCalibrationMode(void)
{
    pc.printf("Entering Calibration Mode...\r\n");
    pc.printf("Calib Button is Full Throttle\r\n");
    pc.printf("Start Button is Full Reverse\r\n");
    pc.printf("Please Restart Nucleo to exit from this mode\r\n");
    while(true)
    {
        //pc.printf("entering loop\r\n");
        if((CalibButton.read()==0)&&(StartButton.read()==1)) //full throttle
        {
            pc.printf("FullThrottle\r\n");
            LeftESC=1;
            RightESC=1;
        }
        else if ((StartButton.read()==0)&&(CalibButton.read()==1)) //Full Reverse
        {
            pc.printf("FullReverse\r\n");
            LeftESC=0;
            RightESC=0;
        }
        else //Neutral
        {
            pc.printf("Neutral\r\n");
            LeftESC=0.5;
            RightESC=0.5;
        }
        wait(0.1);
    }
}
int main(void)
{
    led=1;
    initMotor();
    pc.baud(115200);
    Thread::wait(500);
    pc.printf("*** RTOS Sumo ROBOT SMADA ***\r\n");
    pc.printf("Press Calib button then power on to enter Calibration Mode\r\n");
    if(CalibButton.read()==0)
    {
        pc.printf("Please Release Calib Button\r\n");
        while(CalibButton.read()==0);
        enterCalibrationMode();
    }
    pc.printf("Release Start button To begin the match...\r\n ");
    while(StartButton.read()!=0);
    Thread::wait(5000);
    pc.printf("Bismillah.. Semoga kita menang, Amiinn...\r\n");
    Thread SensorThread(SensorReader, NULL, osPriorityNormal, DEFAULT_STACK_SIZE);
    Thread DataThread(DataReporter, NULL, osPriorityNormal, DEFAULT_STACK_SIZE);
    Thread MotorThread(MotorController, NULL, osPriorityNormal, DEFAULT_STACK_SIZE);
    while(1)
    {
       led=0;
       Thread::wait(100);
       led=1;
       Thread::wait(100);
       if(CalibButton.read()==0)
       {
           SensorThread.terminate();
           DataThread.terminate();
           MotorThread.terminate();
           setMotor(0,0);
           while(1);
        }
    }
}