1 sensor using RTOS Timer, another 2 over RTOS Semaphore
Dependents: Autonomous_quadcopter
Fork of HCSR04 by
Diff: front_back_sensors.h
- Revision:
- 13:e9b99635aa2d
diff -r 6f6469b2f016 -r e9b99635aa2d front_back_sensors.h --- /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); + //} + +} +*/