Diff: ultrasonic.cpp

Revision:

0:62ac378df546

Child:

1:ed00e9c4ba6e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ultrasonic.cpp	Fri Apr 19 09:14:08 2013 +0000
@@ -0,0 +1,44 @@
+
+#include "mbed.h"
+#include "ultrasonic.h"
+ 
+extern Timer timer2;
+
+uint16_t ultrasonicVal[4];
+double ultrasonicValue[ALL_ULTRASONIC] = {0};
+
+
+void Ultrasonic()
+{
+    for(uint8_t i = 0 ; i < ALL_ULTRASONIC ; i++){
+        uint8_t flag = 0;
+    
+        DigitalOut PingPinOut(ultrasonic_pin[i]);
+        PingPinOut = 1;
+        wait_us(10);
+        PingPinOut = 0;
+        DigitalIn PingPin(ultrasonic_pin[i]);
+        timer2.reset();
+        while(PingPin == 0){
+            if(timer2.read_us() > 1000){   //1ms以上応答なし
+                ultrasonicValue[i] =  PING_ERR;
+                flag = 1;
+                break;
+            }
+        } 
+        timer2.reset();
+        while(PingPin == 1){
+            if((timer2.read_us() > 18500) || (flag == 1)){  //18.5ms以上のパルス
+                ultrasonicValue[i] =  PING_ERR;
+                flag = 1;
+                break;
+            }
+        }
+        if(flag == 0){
+            ultrasonicValue[i] = timer2.read_us() / 1000000.0 / 2.0 * 340.0 * 1000.0; //mm  MAX:3145
+            ultrasonicVal[i] = (int)(ultrasonicValue[i] * 10.0);
+        }else{
+            ultrasonicVal[i] = PING_ERR;
+        }
+    }
+} 
\ No newline at end of file