main.cpp

00001 
00002 #include <L298_H.h>
00003 #include <mbed.h>
00004 
00005  
00006 Timer t1;      //Timer used to record the time duration
00007 
00008 DigitalOut trigpin(PTD4);
00009 DigitalIn echopin(PTE20);
00010 DigitalOut trigpin2(PTD1);
00011 DigitalIn echopin2(PTC9);
00012 DigitalOut test(PTE30);
00013 DigitalOut led1(PTD2);
00014 DigitalOut led2(PTD3);  //two LED indicates if there's any obstacles nearby
00015 //initialise the class twice, one controls the left part and the other the right
00016 bool dirL;
00017 bool dirR;
00018 float speed;
00019 
00020 int getDis (DigitalIn echopin) {
00021 Timer t;
00022 int distance;
00023 float duration;
00024 while (!echopin);         // read the duration time
00025     t.start();
00026     while (echopin);
00027     t.stop();
00028     duration = t.read_us(); 
00029     distance = (duration/2) / 29.1;  
00030     t.reset();
00031     printf("distance: %d\n",distance); //read the distance
00032     test = 1;
00033     wait_ms(100);
00034     test=0;
00035     return distance;
00036 }
00037 
00038 
00039 int main() {
00040 L298 controlLeft(PTA12,PTA4,PTA5);
00041 L298 controlRight(PTA13,PTD5,PTD0);
00042 dirR=1;
00043 dirL=1;
00044 speed=0.2;
00045 int distance;
00046 int distance2;
00047 int totalT;
00048 controlLeft.SetDirection(dirL);
00049 controlRight.SetDirection(dirR);
00050 controlLeft.SetSpeed(speed);
00051 controlRight.SetSpeed(speed);
00052 test =1;
00053 led1=1;
00054 led2 =1;
00055 
00056   while (1) {
00057     t1.start();
00058     distance = 0;
00059     distance2 = 0;
00060     test = 0;
00061     trigpin = 0; // low trig signal for 2us
00062     trigpin2 = 0;
00063     wait_us(2);
00064     trigpin = 1;
00065     trigpin2= 1; // high trig signal for 10us
00066     wait_us(10);
00067     trigpin = 0; // stay low trig level
00068     trigpin2 = 0;
00069    /* while (!echopin);         // read the duration time
00070     t.start();
00071     while (echopin);
00072     t.stop();
00073     duration = t.read_us(); 
00074     distance = (duration/2) / 29.1;    //Measure the Digital Signal high level
00075     while (!echopin2);         // read the duration time of second sensor
00076     t2.start();
00077     while (echopin2);
00078     t2.stop();
00079     duration2 = t2.read_us(); 
00080     distance2 = (duration2/2) / 29.1;    //Measure the Digital Signal high level
00081  
00082     t.reset ();
00083     t2.rest();
00084     printf("distance: %d\n",distance); //read the distance
00085     printf("distance2: %d\n",distance2); //read the distance */
00086     
00087     distance = getDis(echopin);
00088     distance2 = getDis(echopin2);
00089         
00090 
00091     if (distance < 20|| distance2 <20) { // This is where the LED On/Off happens
00092       led1 = 1;
00093       led2 = 0;// When the Red condition is met, the Green LED should turn off
00094       speed = 0;
00095       controlLeft.SetSpeed(speed);
00096       controlRight.SetSpeed(speed);
00097       if (distance>distance2){
00098           speed = 0.2;
00099           controlLeft.SetSpeed(speed);
00100           controlRight.SetSpeed(speed);
00101           dirR=1;
00102           dirL=0;
00103           controlLeft.SetDirection(dirL);
00104           controlRight.SetDirection(dirR);   //obstacle on the right, turn left
00105           }
00106       else {                 //obstacle on the left or in the middle, turn right
00107           speed = 0.2;
00108           controlLeft.SetSpeed(speed);
00109           controlRight.SetSpeed(speed);
00110           dirR=0;
00111           dirL=1;          
00112           controlLeft.SetDirection(dirL);
00113           controlRight.SetDirection(dirR); 
00114           }
00115     }
00116     else {
00117       led1 = 0;
00118       led2 = 1;
00119       speed = 0.2;
00120       controlLeft.SetSpeed(speed);
00121       controlRight.SetSpeed(speed);
00122       dirR=1;
00123       dirL=1;
00124       controlLeft.SetDirection(dirL);
00125       controlRight.SetDirection(dirR);
00126     }
00127     
00128     
00129     wait_us(10);
00130     t1.stop();
00131     totalT= t1.read_us();
00132     printf("totalTime: %d \n",totalT);
00133     t1.reset();
00134              }
00135             }
00136    
00137     
00138 
00139