Diff: front_back_sensors.h

Revision:

13:e9b99635aa2d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/front_back_sensors.h	Tue May 22 19:35:58 2018 +0000
@@ -0,0 +1,141 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "hardware.h"
+
+
+void front_sensor(void);
+void back_sensor(void);
+
+void semaphore_thread(void const *name){
+    pc.printf("Starting semaphore thread\n\r");
+    while (true) {
+        _semaphore.wait();
+        //pc.printf("%s\n\r", (const char*)name);
+        if(strstr((const char*)name, "frontSensor") != NULL)
+            front_sensor();    
+        if(strstr((const char*)name, "backSensor") != NULL)
+            back_sensor();    
+        
+        Thread::wait(100);
+        _semaphore.release();
+    }
+
+} 
+
+void front_sensor(){
+    DigitalOut trig(p7);
+    DigitalIn echo(p8);
+    bool tooLong = false;
+    Timer timer;
+    
+    timer.start();
+    trig=1;
+    wait_us(10);
+    trig=0;
+    while(!echo){
+        if(timer.read_us() > 5000)
+            tooLong = true;            
+    }
+    if(tooLong == false){
+        timer.reset();
+        while(echo){
+           if(timer.read_us() > 5000)
+                break;    
+        }
+        _frontDistance = timer.read_us() / 58;
+        //_frontWall = true;
+        //pc.printf("front distance %d \n\n\r", _frontDistance);
+    }
+}
+
+void back_sensor(){
+    DigitalOut trig(p19);
+    DigitalIn echo(p20);
+    bool tooLong = false;
+    Timer timer;
+    
+    //printf("left sensor started \n\r");
+    timer.start();
+    trig=1;
+    wait_us(10);
+    trig=0;
+    while(!echo){
+        if(timer.read_us() > 5000)
+            tooLong = true;            
+    }
+    timer.reset();
+    if(tooLong == false){
+        timer.reset();
+        while(echo){
+           if(timer.read_us() > 5000)
+                break;    
+        }
+        _backDistance = timer.read_us() / 58;
+        //_backWall = true;
+        //pc.printf("                          back distance %d \n\n\r", _backDistance);
+    }
+
+}
+
+/*
+void left_sensor(){
+    DigitalOut trig(p19);
+    DigitalIn echo(p20);
+    bool tooLong = false;
+    Timer timer;
+    //printf("left sensor started \n\r");
+    timer.start();
+    //while(1){
+        trig=1;   //  trigger high
+        //printf("trig\n\r");
+        wait_us(10);
+        //printf("wait\n\r");
+        trig=0;  // trigger low
+        // start pulseIN
+        while(!echo){
+            if(timer.read_us() > 5000)
+                tooLong = true;            
+        }
+        if(tooLong == false){
+            timer.reset();
+            while(echo){
+               if(timer.read_us() > 5000)
+                    break;    
+            }
+            _leftDistance = timer.read_us() / 58;
+            if(_leftDistance <= 30)
+                _leftWall = true;
+            //pc.printf("left distance %d \n\n\r", _leftDistance);
+        }
+        //Thread::wait(100);
+    //}
+    
+}
+
+void right_sensor(){
+    DigitalOut trig(p17);
+    DigitalIn echo(p18);
+    
+    Timer timer;
+    //printf("left sensor started \n\r");
+    timer.start();
+    //while(1){
+        trig=1;   //  trigger high
+        //printf("trig\n\r");
+        wait_us(10);
+        //printf("wait\n\r");
+        trig=0;  // trigger low
+        // start pulseIN
+        while(!echo);
+        timer.reset();
+        while(echo);
+        _rightDistance = timer.read_us() / 58;
+        if(_rightDistance <= 40)
+            _rightWall = true;
+        pc.printf("right distance %d \n\n\r", _rightDistance);
+        
+        //Thread::wait(100);
+    //}
+    
+}
+*/