Diff: hcsr04.cpp

Revision:

1:e01896ab28d7

Parent:

0:86b2086be101

--- a/hcsr04.cpp	Mon Apr 14 08:23:09 2014 +0000
+++ b/hcsr04.cpp	Mon Oct 24 12:59:41 2016 +0000
@@ -7,16 +7,55 @@
  long HCSR04::echo_duration() {
         
     timer.reset();  //reset timer
+    timeout_timer.reset();
+    
+    //Initiate Trigger
     trig=0;   // trigger low 
     wait_us(2); //  wait 
     trig=1;   //  trigger high
     wait_us(10);
     trig=0;  // trigger low
-         while(!echo); // start pulseIN
-      timer.start();
-     while(echo);
-      timer.stop();
-     return timer.read_us(); 
+    
+    timeout_timer.start();
+    
+    while(!echo){
+      //Wait for 5V/3.3V pulse to start
+      if(timeout_timer.read_ms()>=10){ //100 ms timeout
+          timedOut = true;
+          break;      
+          }
+      }
+      
+      //Pulse Started
+      if(timedOut == true){
+          //Error, pulse didn't start for some reason
+          timedOut = false;
+          return 999999;  //error pulse not started in time
+      }
+      else
+      {
+        timeout_timer.reset();
+        timer.start();
+        timeout_timer.start(); 
+        while(echo){
+            if(timeout_timer.read_ms()>=10){
+                timedOut = true;
+                break;
+            }        
+        }
+        if(timedOut == true){
+          timedOut = false;
+          return 999999;
+          }
+        else{
+          timer.stop();
+          return timer.read_us();     
+          }  
+      }
+      
+      
+      
+
  
 }