Diff: sensors/sensors.cpp

Revision:

21:14133581387b

Child:

22:0ed9de464f40

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sensors/sensors.cpp	Fri May 03 02:21:47 2019 +0000
@@ -0,0 +1,29 @@
+#include "sensors.h"
+//filter [0]:forward, [1]: back
+const float kOldWeight = 0.5;
+const float kOutVal = 500; //<0.1が返ってきたときに返す値
+LowPassFilter lowpassfilter[2] = {LowPassFilter(kOldWeight),LowPassFilter(kOldWeight),};
+
+HCSR04 sensor_forward(p11,p12);
+HCSR04 sensor_back(p17, p18);
+
+float GetFilteredDist_cm(HCSR04 & sensor, LowPassFilter & filter)
+{
+    sensor.start();
+    float raw_data = sensor.get_dist_cm();
+    if(raw_data < 0.1)//0.1以下なら前回の値を返す
+        return kOutVal;
+    filter.SetData(raw_data);
+    float dist = filter.GetData();
+//    DEBUG("raw %.3f, filter %.3f\r\n", raw_data,dist);
+    return dist;
+}
+
+double get_dist_forward()
+{
+    return GetFilteredDist_cm(sensor_forward,lowpassfilter[0]);
+}
+double get_dist_back()
+{
+    return GetFilteredDist_cm(sensor_back,lowpassfilter[1]);
+}