Eduard Medla / HCSR04

1 sensor using RTOS Timer, another 2 over RTOS Semaphore

Dependents:   Autonomous_quadcopter

Fork of HCSR04 by Antoniolinux B.

hcsr04.cpp@12:6f6469b2f016, 2018-05-15 (annotated)

Committer:
edy05
Date:
Tue May 15 10:32:08 2018 +0000
Revision:
12:6f6469b2f016
Parent:
11:49a0feca5d71
Child:
13:e9b99635aa2d
4 sonic sensors - front, left, right, back; rots semaphore

Who changed what in which revision?

UserRevisionLine numberNew contents of line
antoniolinux 0:86b2086be101 1 #include "hcsr04.h"
edy05 5:02948ae78be7 2 #include "definitions.h"
antoniolinux 0:86b2086be101 3 #include "mbed.h"
edy05 1:53657de3246f 4 #include "rtos.h"
antoniolinux 0:86b2086be101 5 /*
antoniolinux 0:86b2086be101 6 *HCSR04.cpp
antoniolinux 0:86b2086be101 7 */
edy05 12:6f6469b2f016 8 HCSR04::HCSR04(PinName t1, PinName e1) : trig1(t1), echo1(e1){
edy05 7:dcf0aa89bcd7 9
edy05 8:0a10bacaf501 10 int rate = (1.0 / FLIGHT_CONTROLLER_FREQUENCY) * 1000; //ms
edy05 7:dcf0aa89bcd7 11
edy05 7:dcf0aa89bcd7 12 limit = (rate * 1000 - rate * 200);
edy05 7:dcf0aa89bcd7 13
edy05 7:dcf0aa89bcd7 14 threadUpdateTimer1 = new RtosTimer(threadWorker1, osTimerPeriodic, this);
edy05 7:dcf0aa89bcd7 15 threadUpdateTimer1->start(rate);
edy05 7:dcf0aa89bcd7 16
edy05 7:dcf0aa89bcd7 17 //threadUpdateTimer2 = new RtosTimer(threadWorker2, osTimerPeriodic, this);
edy05 7:dcf0aa89bcd7 18 //threadUpdateTimer2->start(rate);
edy05 1:53657de3246f 19 }
edy05 1:53657de3246f 20
edy05 7:dcf0aa89bcd7 21 void HCSR04::threadWorker1(void const *p)
edy05 1:53657de3246f 22 {
edy05 1:53657de3246f 23 HCSR04* self = (HCSR04*)p;
edy05 1:53657de3246f 24
edy05 7:dcf0aa89bcd7 25 //printf("start\n\r");
edy05 7:dcf0aa89bcd7 26 self->trig1=1; // trigger high
edy05 7:dcf0aa89bcd7 27 //printf("trig\n\r");
edy05 1:53657de3246f 28 wait_us(10);
edy05 7:dcf0aa89bcd7 29 //printf("wait\n\r");
edy05 7:dcf0aa89bcd7 30 self->trig1=0; // trigger low
edy05 3:81512ca9a13c 31 // start pulseIN
edy05 8:0a10bacaf501 32 self->timerLimit.reset();
edy05 8:0a10bacaf501 33 self->timerLimit.start();
edy05 7:dcf0aa89bcd7 34 while(!self->echo1){
edy05 8:0a10bacaf501 35 if(self->timerLimit.read_us() > self->limit){
edy05 3:81512ca9a13c 36 //printf("moc1\n\r");
edy05 7:dcf0aa89bcd7 37 break;
edy05 3:81512ca9a13c 38 }
edy05 3:81512ca9a13c 39 }
edy05 8:0a10bacaf501 40 if(self->timerLimit.read_us() < self->limit){
edy05 8:0a10bacaf501 41 //self->timer1.stop();
edy05 7:dcf0aa89bcd7 42 self->timer1.reset(); //reset timer
edy05 7:dcf0aa89bcd7 43 self->timer1.start();
edy05 7:dcf0aa89bcd7 44 while(self->echo1){
edy05 8:0a10bacaf501 45 if(self->timerLimit.read_us() > self->limit){
edy05 3:81512ca9a13c 46 //printf("moc2\n\r");
edy05 7:dcf0aa89bcd7 47 break;
edy05 3:81512ca9a13c 48 }
edy05 3:81512ca9a13c 49 }
edy05 7:dcf0aa89bcd7 50 self->timer1.stop();
edy05 7:dcf0aa89bcd7 51 self->distan1 = self->timer1.read_us() / 58;
edy05 7:dcf0aa89bcd7 52 }
edy05 7:dcf0aa89bcd7 53 //printf("end\n\r");
edy05 7:dcf0aa89bcd7 54 }
edy05 7:dcf0aa89bcd7 55
edy05 1:53657de3246f 56
edy05 11:49a0feca5d71 57 float HCSR04::getDistan1(){
edy05 7:dcf0aa89bcd7 58 return distan1;
edy05 7:dcf0aa89bcd7 59 }
edy05 7:dcf0aa89bcd7 60
antoniolinux 0:86b2086be101 61