Miya Miyagawa / Mbed 2 deprecated AutoMaster_si-tu

lknds

Dependencies:   mbed TrapezoidControl Pulse QEI

Diff: Input/Ultrasonic/USS/USS.cpp

Revision:
22:7d93f79a3686
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Input/Ultrasonic/USS/USS.cpp	Tue Sep 17 04:40:17 2019 +0000
@@ -0,0 +1,25 @@
+#include "USS.h"
+#include "mbed.h"
+
+USS::USS(PinName echoPin, PinName trigPin, PinName tempPin) 
+                        :echo_(echoPin), trig_(trigPin), temp_(tempPin)  
+{
+    distance_ = 0;
+    
+}
+
+double USS::ReadDis(void) {
+    int temp = temp_.getTemperature();
+    double duration = echo_.read_high_us(5000);
+    trig_.write_us(1,10);
+    if(duration > 0){
+        duration /= 2;
+        double sspead = 331.5+0.6*temp;
+        distance_ = duration*sspead*100/1000000;
+    } else distance_ = 0;
+    return distance_;
+}
+
+double USS::GetDis(void) {
+    return distance_;
+}
\ No newline at end of file