1 sensor using RTOS Timer, another 2 over RTOS Semaphore

Dependents:   Autonomous_quadcopter

Fork of HCSR04 by Antoniolinux B.

Committer:
edy05
Date:
Tue May 22 19:35:58 2018 +0000
Revision:
13:e9b99635aa2d
Final updates

Who changed what in which revision?

UserRevisionLine numberNew contents of line
edy05 13:e9b99635aa2d 1 #include "mbed.h"
edy05 13:e9b99635aa2d 2 #include "rtos.h"
edy05 13:e9b99635aa2d 3 #include "hardware.h"
edy05 13:e9b99635aa2d 4
edy05 13:e9b99635aa2d 5
edy05 13:e9b99635aa2d 6 void front_sensor(void);
edy05 13:e9b99635aa2d 7 void back_sensor(void);
edy05 13:e9b99635aa2d 8
edy05 13:e9b99635aa2d 9 void semaphore_thread(void const *name){
edy05 13:e9b99635aa2d 10 pc.printf("Starting semaphore thread\n\r");
edy05 13:e9b99635aa2d 11 while (true) {
edy05 13:e9b99635aa2d 12 _semaphore.wait();
edy05 13:e9b99635aa2d 13 //pc.printf("%s\n\r", (const char*)name);
edy05 13:e9b99635aa2d 14 if(strstr((const char*)name, "frontSensor") != NULL)
edy05 13:e9b99635aa2d 15 front_sensor();
edy05 13:e9b99635aa2d 16 if(strstr((const char*)name, "backSensor") != NULL)
edy05 13:e9b99635aa2d 17 back_sensor();
edy05 13:e9b99635aa2d 18
edy05 13:e9b99635aa2d 19 Thread::wait(100);
edy05 13:e9b99635aa2d 20 _semaphore.release();
edy05 13:e9b99635aa2d 21 }
edy05 13:e9b99635aa2d 22
edy05 13:e9b99635aa2d 23 }
edy05 13:e9b99635aa2d 24
edy05 13:e9b99635aa2d 25 void front_sensor(){
edy05 13:e9b99635aa2d 26 DigitalOut trig(p7);
edy05 13:e9b99635aa2d 27 DigitalIn echo(p8);
edy05 13:e9b99635aa2d 28 bool tooLong = false;
edy05 13:e9b99635aa2d 29 Timer timer;
edy05 13:e9b99635aa2d 30
edy05 13:e9b99635aa2d 31 timer.start();
edy05 13:e9b99635aa2d 32 trig=1;
edy05 13:e9b99635aa2d 33 wait_us(10);
edy05 13:e9b99635aa2d 34 trig=0;
edy05 13:e9b99635aa2d 35 while(!echo){
edy05 13:e9b99635aa2d 36 if(timer.read_us() > 5000)
edy05 13:e9b99635aa2d 37 tooLong = true;
edy05 13:e9b99635aa2d 38 }
edy05 13:e9b99635aa2d 39 if(tooLong == false){
edy05 13:e9b99635aa2d 40 timer.reset();
edy05 13:e9b99635aa2d 41 while(echo){
edy05 13:e9b99635aa2d 42 if(timer.read_us() > 5000)
edy05 13:e9b99635aa2d 43 break;
edy05 13:e9b99635aa2d 44 }
edy05 13:e9b99635aa2d 45 _frontDistance = timer.read_us() / 58;
edy05 13:e9b99635aa2d 46 //_frontWall = true;
edy05 13:e9b99635aa2d 47 //pc.printf("front distance %d \n\n\r", _frontDistance);
edy05 13:e9b99635aa2d 48 }
edy05 13:e9b99635aa2d 49 }
edy05 13:e9b99635aa2d 50
edy05 13:e9b99635aa2d 51 void back_sensor(){
edy05 13:e9b99635aa2d 52 DigitalOut trig(p19);
edy05 13:e9b99635aa2d 53 DigitalIn echo(p20);
edy05 13:e9b99635aa2d 54 bool tooLong = false;
edy05 13:e9b99635aa2d 55 Timer timer;
edy05 13:e9b99635aa2d 56
edy05 13:e9b99635aa2d 57 //printf("left sensor started \n\r");
edy05 13:e9b99635aa2d 58 timer.start();
edy05 13:e9b99635aa2d 59 trig=1;
edy05 13:e9b99635aa2d 60 wait_us(10);
edy05 13:e9b99635aa2d 61 trig=0;
edy05 13:e9b99635aa2d 62 while(!echo){
edy05 13:e9b99635aa2d 63 if(timer.read_us() > 5000)
edy05 13:e9b99635aa2d 64 tooLong = true;
edy05 13:e9b99635aa2d 65 }
edy05 13:e9b99635aa2d 66 timer.reset();
edy05 13:e9b99635aa2d 67 if(tooLong == false){
edy05 13:e9b99635aa2d 68 timer.reset();
edy05 13:e9b99635aa2d 69 while(echo){
edy05 13:e9b99635aa2d 70 if(timer.read_us() > 5000)
edy05 13:e9b99635aa2d 71 break;
edy05 13:e9b99635aa2d 72 }
edy05 13:e9b99635aa2d 73 _backDistance = timer.read_us() / 58;
edy05 13:e9b99635aa2d 74 //_backWall = true;
edy05 13:e9b99635aa2d 75 //pc.printf(" back distance %d \n\n\r", _backDistance);
edy05 13:e9b99635aa2d 76 }
edy05 13:e9b99635aa2d 77
edy05 13:e9b99635aa2d 78 }
edy05 13:e9b99635aa2d 79
edy05 13:e9b99635aa2d 80 /*
edy05 13:e9b99635aa2d 81 void left_sensor(){
edy05 13:e9b99635aa2d 82 DigitalOut trig(p19);
edy05 13:e9b99635aa2d 83 DigitalIn echo(p20);
edy05 13:e9b99635aa2d 84 bool tooLong = false;
edy05 13:e9b99635aa2d 85 Timer timer;
edy05 13:e9b99635aa2d 86 //printf("left sensor started \n\r");
edy05 13:e9b99635aa2d 87 timer.start();
edy05 13:e9b99635aa2d 88 //while(1){
edy05 13:e9b99635aa2d 89 trig=1; // trigger high
edy05 13:e9b99635aa2d 90 //printf("trig\n\r");
edy05 13:e9b99635aa2d 91 wait_us(10);
edy05 13:e9b99635aa2d 92 //printf("wait\n\r");
edy05 13:e9b99635aa2d 93 trig=0; // trigger low
edy05 13:e9b99635aa2d 94 // start pulseIN
edy05 13:e9b99635aa2d 95 while(!echo){
edy05 13:e9b99635aa2d 96 if(timer.read_us() > 5000)
edy05 13:e9b99635aa2d 97 tooLong = true;
edy05 13:e9b99635aa2d 98 }
edy05 13:e9b99635aa2d 99 if(tooLong == false){
edy05 13:e9b99635aa2d 100 timer.reset();
edy05 13:e9b99635aa2d 101 while(echo){
edy05 13:e9b99635aa2d 102 if(timer.read_us() > 5000)
edy05 13:e9b99635aa2d 103 break;
edy05 13:e9b99635aa2d 104 }
edy05 13:e9b99635aa2d 105 _leftDistance = timer.read_us() / 58;
edy05 13:e9b99635aa2d 106 if(_leftDistance <= 30)
edy05 13:e9b99635aa2d 107 _leftWall = true;
edy05 13:e9b99635aa2d 108 //pc.printf("left distance %d \n\n\r", _leftDistance);
edy05 13:e9b99635aa2d 109 }
edy05 13:e9b99635aa2d 110 //Thread::wait(100);
edy05 13:e9b99635aa2d 111 //}
edy05 13:e9b99635aa2d 112
edy05 13:e9b99635aa2d 113 }
edy05 13:e9b99635aa2d 114
edy05 13:e9b99635aa2d 115 void right_sensor(){
edy05 13:e9b99635aa2d 116 DigitalOut trig(p17);
edy05 13:e9b99635aa2d 117 DigitalIn echo(p18);
edy05 13:e9b99635aa2d 118
edy05 13:e9b99635aa2d 119 Timer timer;
edy05 13:e9b99635aa2d 120 //printf("left sensor started \n\r");
edy05 13:e9b99635aa2d 121 timer.start();
edy05 13:e9b99635aa2d 122 //while(1){
edy05 13:e9b99635aa2d 123 trig=1; // trigger high
edy05 13:e9b99635aa2d 124 //printf("trig\n\r");
edy05 13:e9b99635aa2d 125 wait_us(10);
edy05 13:e9b99635aa2d 126 //printf("wait\n\r");
edy05 13:e9b99635aa2d 127 trig=0; // trigger low
edy05 13:e9b99635aa2d 128 // start pulseIN
edy05 13:e9b99635aa2d 129 while(!echo);
edy05 13:e9b99635aa2d 130 timer.reset();
edy05 13:e9b99635aa2d 131 while(echo);
edy05 13:e9b99635aa2d 132 _rightDistance = timer.read_us() / 58;
edy05 13:e9b99635aa2d 133 if(_rightDistance <= 40)
edy05 13:e9b99635aa2d 134 _rightWall = true;
edy05 13:e9b99635aa2d 135 pc.printf("right distance %d \n\n\r", _rightDistance);
edy05 13:e9b99635aa2d 136
edy05 13:e9b99635aa2d 137 //Thread::wait(100);
edy05 13:e9b99635aa2d 138 //}
edy05 13:e9b99635aa2d 139
edy05 13:e9b99635aa2d 140 }
edy05 13:e9b99635aa2d 141 */