1 sensor using RTOS Timer, another 2 over RTOS Semaphore
Dependents: Autonomous_quadcopter
Fork of HCSR04 by
front_back_sensors.h@13:e9b99635aa2d, 2018-05-22 (annotated)
- Committer:
- edy05
- Date:
- Tue May 22 19:35:58 2018 +0000
- Revision:
- 13:e9b99635aa2d
Final updates
Who changed what in which revision?
User | Revision | Line number | New 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 | */ |