Diff: sensors/sensors.cpp

Revision:

26:5fb1aa9cb7f0

Parent:

25:c740e6fd5dab

Child:

27:d392a95f4799

--- a/sensors/sensors.cpp	Mon May 06 04:03:00 2019 +0000
+++ b/sensors/sensors.cpp	Mon May 06 06:28:41 2019 +0000
@@ -1,40 +1,89 @@
 #include "sensors.h"
 #include "microinfinity.h"
 #include "pinnames.h"
+#include "moves.h"
+//lowpass関係
 const float kOldWeight = 0;
 const float kOldWeightLight = 0.3; //filterの重み.軽いver
-
 const float kOutVal = 500; //<0.1が返ってきたときに返す値
 //filter [0]:forward, [1]: back
 LowPassFilter lowpassfilter[2] = {LowPassFilter(kOldWeight),LowPassFilter(kOldWeight),};
 
+//超音波
 HCSR04 sensor_forward(pin_hcsr04[0][0],pin_hcsr04[0][1]);
 HCSR04 sensor_back(pin_hcsr04[1][0], pin_hcsr04[1][1]);
+float hcsr04_val[2]  = {};
+float GetFilteredDist_cm(HCSR04 & sensor, LowPassFilter & filter);
 
+//ec
 const int kResolution=500;
 Ec ec_lo(pin_ec[0][0],pin_ec[0][1],NC,kResolution,0.01);
 Ec ec_li(pin_ec[1][0],pin_ec[1][1],NC,kResolution,0.01);
 
-
+//スイッチ
 DigitalIn bus_in(pin_bus);
 DigitalIn hand(pin_hand,PullUp);
 DigitalIn switch_lo(pin_switch_lo,PullUp);
 DigitalIn switch_li(pin_switch_li,PullUp);
-DigitalIn mode4(pin_mode4,PullUp);
+InterruptIn mode4(pin_mode4,PullUp);
 DigitalOut led4(LED4);
-
 int hand_mode=NORMAL;
 
+//can
 CAN can1(pin_can_rd,pin_can_td);
 
+//ticker
 Ticker ticker;
-const float kTicker_s = 0.2;
-void Hcsr04Start();
-float GetFilteredDist_cm(HCSR04 & sensor, LowPassFilter & filter);
+//const float kTicker_s = 0.2;
+void Hcsr04Start();//超音波をtickerでとる用
+
+
+//file操作
+FILE *fp = NULL;
+LocalFileSystem local("local");
+const char kFileName[] = "/local/sensors.csv";
+Timer filetimer;
+
+int FileOpen() //1:異常終了
+{
+    if ((fp = fopen(kFileName, "w")) == NULL) {
+        printf("error : FileSave()\r\n");
+        return 1;
+    }
+    fprintf(fp, "time[s], gyro[deg], hcsr04_forward[cm], hcsr04_back[cm], motor_lo[deg], motor_li[deg]\r\n");
+    filetimer.reset();
+    filetimer.start();
+    mode4.rise(FileClose);//mode4ピンを上げげればfileclose
+    return 0;
+}
+
+void FileWrite()
+{
+    static int is_first = 0;//初回はtimerを0にする
+    if(is_first ==0) {
+        filetimer.reset();
+        ++is_first;
+    }
+    if(fp == NULL) {
+        is_first = 0;
+    } else {
+        fprintf(fp, "%f, %f, %f, %f, %f, %f\r\n",
+                filetimer.read(), degree0,
+                hcsr04_val[0], hcsr04_val[1],
+                motor_lo.getPosi(), motor_li.getPosi());
+    }
+}
+void FileClose()
+{
+    filetimer.stop();
+    if(fp != NULL)
+        fclose(fp);
+};
+
 
 void TickerSetUp()
 {
-    ticker.attach(Hcsr04Start,kTicker_s);
+//    ticker.attach(FileWriteTicker,kTicker_s);
 }
 
 void Hcsr04Start()
@@ -50,21 +99,23 @@
     float raw_data = sensor.get_dist_cm();
     if(raw_data < 20)//0.1以下なら前回の値を返す
         return kOutVal;
-/*    filter.SetData(raw_data);
-    float dist = filter.GetData();
-//    printf("raw %.3f, filter %.3f\r\n", raw_data,dist);
-    return dist;
-    */
+    /*    filter.SetData(raw_data);
+        float dist = filter.GetData();
+    //    printf("raw %.3f, filter %.3f\r\n", raw_data,dist);
+        return dist;
+        */
     return raw_data;
 }
 
 float get_dist_forward()
 {
-    return GetFilteredDist_cm(sensor_forward,lowpassfilter[0]);
+    hcsr04_val[0] = GetFilteredDist_cm(sensor_forward,lowpassfilter[0]);
+    return hcsr04_val[0];
 }
 float get_dist_back()
 {
-    return GetFilteredDist_cm(sensor_back,lowpassfilter[1]);
+    hcsr04_val[1] = GetFilteredDist_cm(sensor_back,lowpassfilter[1]);
+    return hcsr04_val[1];
 }
 
 void set_gyro()
@@ -92,3 +143,4 @@
     if(can1.write(CANMessage(0,data,2)))led4=1;
     else led4=0;
 }
+