File content as of revision 1:60ad9c191ea9:

#include <L298_H.h>
#include <mbed.h>

 
Timer t1;      //Timer used to record the time duration

DigitalOut trigpin(PTD4);
DigitalIn echopin(PTE20);
DigitalOut trigpin2(PTD1);
DigitalIn echopin2(PTC9);
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 getDis (DigitalIn echopin) {
Timer t;
int distance;
float duration;
while (!echopin);         // read the duration time
    t.start();
    while (echopin);
    t.stop();
    duration = t.read_us(); 
    distance = (duration/2) / 29.1;  
    t.reset();
    printf("distance: %d\n",distance); //read the distance
    test = 1;
    wait_ms(100);
    test=0;
    return distance;
}


int main() {
L298 controlLeft(PTA12,PTA4,PTA5);
L298 controlRight(PTA13,PTD5,PTD0);
dirR=1;
dirL=1;
speed=0.2;
int distance;
int distance2;
int totalT;
controlLeft.SetDirection(dirL);
controlRight.SetDirection(dirR);
controlLeft.SetSpeed(speed);
controlRight.SetSpeed(speed);
test =1;
led1=1;
led2 =1;

  while (1) {
    t1.start();
    distance = 0;
    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 */
    
    distance = getDis(echopin);
    distance2 = getDis(echopin2);
        

    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=1;
      dirL=1;
      controlLeft.SetDirection(dirL);
      controlRight.SetDirection(dirR);
    }
    
    
    wait_us(10);
    t1.stop();
    totalT= t1.read_us();
    printf("totalTime: %d \n",totalT);
    t1.reset();
             }
            }