front_back_sensors.h

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "hardware.h"
00004 
00005 
00006 void front_sensor(void);
00007 void back_sensor(void);
00008 
00009 void semaphore_thread(void const *name){
00010     pc.printf("Starting semaphore thread\n\r");
00011     while (true) {
00012         _semaphore.wait();
00013         //pc.printf("%s\n\r", (const char*)name);
00014         if(strstr((const char*)name, "frontSensor") != NULL)
00015             front_sensor();    
00016         if(strstr((const char*)name, "backSensor") != NULL)
00017             back_sensor();    
00018         
00019         Thread::wait(100);
00020         _semaphore.release();
00021     }
00022 
00023 } 
00024 
00025 void front_sensor(){
00026     DigitalOut trig(p7);
00027     DigitalIn echo(p8);
00028     bool tooLong = false;
00029     Timer timer;
00030     
00031     timer.start();
00032     trig=1;
00033     wait_us(10);
00034     trig=0;
00035     while(!echo){
00036         if(timer.read_us() > 5000)
00037             tooLong = true;            
00038     }
00039     if(tooLong == false){
00040         timer.reset();
00041         while(echo){
00042            if(timer.read_us() > 5000)
00043                 break;    
00044         }
00045         _frontDistance = timer.read_us() / 58;
00046         //_frontWall = true;
00047         //pc.printf("front distance %d \n\n\r", _frontDistance);
00048     }
00049 }
00050 
00051 void back_sensor(){
00052     DigitalOut trig(p19);
00053     DigitalIn echo(p20);
00054     bool tooLong = false;
00055     Timer timer;
00056     
00057     //printf("left sensor started \n\r");
00058     timer.start();
00059     trig=1;
00060     wait_us(10);
00061     trig=0;
00062     while(!echo){
00063         if(timer.read_us() > 5000)
00064             tooLong = true;            
00065     }
00066     timer.reset();
00067     if(tooLong == false){
00068         timer.reset();
00069         while(echo){
00070            if(timer.read_us() > 5000)
00071                 break;    
00072         }
00073         _backDistance = timer.read_us() / 58;
00074         //_backWall = true;
00075         //pc.printf("                          back distance %d \n\n\r", _backDistance);
00076     }
00077 
00078 }
00079 
00080 /*
00081 void left_sensor(){
00082     DigitalOut trig(p19);
00083     DigitalIn echo(p20);
00084     bool tooLong = false;
00085     Timer timer;
00086     //printf("left sensor started \n\r");
00087     timer.start();
00088     //while(1){
00089         trig=1;   //  trigger high
00090         //printf("trig\n\r");
00091         wait_us(10);
00092         //printf("wait\n\r");
00093         trig=0;  // trigger low
00094         // start pulseIN
00095         while(!echo){
00096             if(timer.read_us() > 5000)
00097                 tooLong = true;            
00098         }
00099         if(tooLong == false){
00100             timer.reset();
00101             while(echo){
00102                if(timer.read_us() > 5000)
00103                     break;    
00104             }
00105             _leftDistance = timer.read_us() / 58;
00106             if(_leftDistance <= 30)
00107                 _leftWall = true;
00108             //pc.printf("left distance %d \n\n\r", _leftDistance);
00109         }
00110         //Thread::wait(100);
00111     //}
00112     
00113 }
00114 
00115 void right_sensor(){
00116     DigitalOut trig(p17);
00117     DigitalIn echo(p18);
00118     
00119     Timer timer;
00120     //printf("left sensor started \n\r");
00121     timer.start();
00122     //while(1){
00123         trig=1;   //  trigger high
00124         //printf("trig\n\r");
00125         wait_us(10);
00126         //printf("wait\n\r");
00127         trig=0;  // trigger low
00128         // start pulseIN
00129         while(!echo);
00130         timer.reset();
00131         while(echo);
00132         _rightDistance = timer.read_us() / 58;
00133         if(_rightDistance <= 40)
00134             _rightWall = true;
00135         pc.printf("right distance %d \n\n\r", _rightDistance);
00136         
00137         //Thread::wait(100);
00138     //}
00139     
00140 }
00141 */