File content as of revision 0:aed6099bd419:

#include <RobotControl_H.h>
#include <mbed.h>

Timer t; 
Timer t1;      //Timer used to record the time duration
Timer t2;
DigitalOut trigpin(PTD4);
DigitalIn echopin(PTE20);
DigitalOut trigpin2();
DigitalIn echopin2();
DigitalOut test(PTE30);
DigitalOut led1(PTD2);
DigitalOut led2(PTD3);  //two LED indicates if there's any obstacles nearby
//initialise the class twice, one controls the left part and the other the right
bool dirL;
bool dirR;
float speed;

int main() {
RobotControl controlLeft(PTA12,PTA4,PTA5);
RobotControl controlRight(PTA13,PTD5,PTD0);
dirR=0;
dirL=0;
speed=0.2;
controlLeft.SetDirection(dirL);
controlRight.SetDirection(dirR);
controlLeft.SetSpeed(speed);
controlRight.SetSpeed(speed);

  while (1) {
    t1.start();
    int totalT;
    float duration;
    float duration2;
    int distance = 0;
    int distance2 = 0;
    test = 0;
    trigpin = 0; // low trig signal for 2us
    trigpin2 = 0;
    wait_us(2);
    trigpin = 1;
    trigpin2= 1; // high trig signal for 10us
    wait_us(10);
    trigpin = 0; // stay low trig level
    trigpin2 = 0;
    while (!echopin);         // read the duration time
    t.start();
    while (echopin);
    t.stop();
    duration = t.read_us(); 
    distance = (duration/2) / 29.1;    //Measure the Digital Signal high level
    while (!echopin2);         // read the duration time of second sensor
    t2.start();
    while (echopin2);
    t2.stop();
    duration2 = t2.read_us(); 
    distance2 = (duration2/2) / 29.1;    //Measure the Digital Signal high level
 
    t.reset ();
    t2.rest();
    printf("distance: %d\n",distance); //read the distance
    printf("distance2: %d\n",distance2); //read the distance
    test = 1;
    wait_ms(100);
    test=0;
   
        

    if (distance < 20|| distance2 <20) { // This is where the LED On/Off happens
      led1 = 1;
      led2 = 0;// When the Red condition is met, the Green LED should turn off
      speed = 0;
      controlLeft.SetSpeed(speed);
      controlRight.SetSpeed(speed);
      if (distance>distance2){
          speed = 0.2;
          controlLeft.SetSpeed(speed);
          controlRight.SetSpeed(speed);
          dirR=1;
          dirL=0;
          controlLeft.SetDirection(dirl);
          controlRight.SetDirection(dirR);   //obstacle on the right, turn left
          }
      else {                 //obstacle on the left or in the middle, turn right
          speed = 0.2;
          controlLeft.SetSpeed(speed);
          controlRight.SetSpeed(speed);
          dirR=0;
          dirL=1;          
          controlLeft.SetDirection(dirL);
          controlRight.SetDirection(dirR); 
          }
    }
    else {
      led1 = 0;
      led2 = 1;
      speed = 0.2;
      controlLeft.SetSpeed(speed);
      controlRight.SetSpeed(speed);
      dirR=0;
      dirL=0;
      controlLeft.SetDirection(dirL);
      controlRight.SetDirection(dirR);
    }
    
    
    wait_us(10);
    t1.stop();
    totalT= t1.read_us();
    printf("totalTime: %d \n",totalT);
    t1.reset();
             }
            }