Diff: hcsr04.cpp

Revision:

3:81512ca9a13c

Parent:

2:8801a3da8693

Child:

4:34023f76fc4f

Child:

5:02948ae78be7

--- a/hcsr04.cpp	Fri Dec 01 11:10:54 2017 +0000
+++ b/hcsr04.cpp	Sat Jan 20 11:44:21 2018 +0000
@@ -5,29 +5,40 @@
 *HCSR04.cpp
 */
 HCSR04::HCSR04(PinName t, PinName e) : trig(t), echo(e) {
-    thread = new Thread(threadWorker, this);
+    //thread = new Thread(threadWorker, this);
+    threadUpdateTimer = new RtosTimer(threadWorker, osTimerPeriodic, this);
+    threadUpdateTimer->start(5);
 }
 
 void HCSR04::threadWorker(void const *p)
 {
     HCSR04* self = (HCSR04*)p;
     
-    while(1){
-        
-        self->trig=0;   // trigger low 
-        wait_us(2); //  wait 
         self->trig=1;   //  trigger high
         wait_us(10);
         self->trig=0;  // trigger low
-        while(!self->echo); // start pulseIN
-        self->timer.reset();  //reset timer
+        // start pulseIN
+        self->timer.reset();
         self->timer.start();
-        while(self->echo);
-        self->timer.stop();
-        self->distan = self->timer.read_us() / 58;
-        
-        Thread::wait(2);
-    }
+        while(!self->echo){
+            if(self->timer.read_us() > 4500){
+                //printf("moc1\n\r");
+                break;
+            }    
+        }
+        if(self->timer.read_us() < 4500){
+            self->timer.stop();
+            self->timer.reset();  //reset timer
+            self->timer.start();
+            while(self->echo){
+                if(self->timer.read_us() > 4500){
+                    //printf("moc2\n\r");
+                    break;
+                }
+            }
+            self->timer.stop();
+            self->distan = self->timer.read_us() / 58;
+        }
 }
 
 uint16_t HCSR04::getDistan(){