Diff: sensors/sensors.cpp

Revision:

45:6df20a8f4b21

Parent:

36:e1398ff45b12

Child:

47:b77796718a9b

--- a/sensors/sensors.cpp	Wed May 15 09:57:11 2019 +0000
+++ b/sensors/sensors.cpp	Fri May 17 05:43:51 2019 +0000
@@ -7,7 +7,7 @@
 //lowpass関係
 const float kOldWeight = 0;
 const float kOldWeightLight = 0.3; //filterの重み.軽いver
-const float kOutVal = 500; //<0.1が返ってきたときに返す値
+const float kOutVal = 5000; //<0.1が返ってきたときに返す値
 //filter [0]:forward, [1]: back
 LowPassFilter lowpassfilter[2] = {LowPassFilter(kOldWeight),LowPassFilter(kOldWeight),};
 
@@ -53,6 +53,7 @@
 LocalFileSystem local("local");
 const char kFileName[] = "/local/sensors.csv";
 Timer filetimer;
+void FileCloseFiltered();
 
 int FileOpen() //1:異常終了
 {
@@ -66,16 +67,21 @@
     filetimer.start();
     if(RightOrLeft) {
         //rightなら
-        switch_LR.fall(FileClose);//left(0)にしたらclose
+        switch_LR.fall(FileCloseFiltered);//left(0)にしたらclose
     } else {
         //left
-        switch_LR.rise(FileClose);//left(0)にしたらclose
+        switch_LR.rise(FileCloseFiltered);//left(0)にしたらclose
     }
     return 0;
 }
 
 void FileWrite()
 {
+    FileWrite("");
+}
+
+void FileWrite(const char *str)
+{
     static int is_first = 0;//初回はtimerを0にする
     if(is_first ==0) {
         filetimer.reset();
@@ -84,10 +90,11 @@
     if(fp == NULL) {
         is_first = 0;
     } else {
-        fprintf(fp, "%f, %f, %f, %f, %f, %f\r\n",
+        fprintf(fp, "%f, %f, %f, %f, %f, %f,%s\r\n",
                 filetimer.read(), degree0,
                 hcsr04_val[0], hcsr04_val[1],
-                motor_lo.getPosi(), motor_li.getPosi());
+                motor_lo.getPosi(), motor_li.getPosi(),
+                str);
     }
 }
 void FileClose()
@@ -96,6 +103,24 @@
     if(fp != NULL)
         fclose(fp);
 };
+void FileCloseFiltered()
+{
+    int is_ok = 0;
+    wait(0.01);
+    if(RightOrLeft) {
+        //rightならfallで取る
+        if(switch_LR == 0)
+            is_ok = 1;
+    } else {
+        //left
+        if(switch_LR == 1)
+            is_ok = 1;
+    }
+
+    if(is_ok == 1) {
+        FileClose();
+    }
+}
 
 
 
@@ -111,17 +136,12 @@
 }
 float GetFilteredDist_cm(HCSR04 & sensor, LowPassFilter & filter)
 {
-    wait_ms(90);//60
+    wait_ms(60);//60
     sensor.start();
     wait_ms(30);
     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;
-        */
     return raw_data;
 }
 
@@ -152,13 +172,13 @@
         if(hand.read()==0)i++;
     }
 }
-void can_send(int mode, float duty)
+void can_send(int mode, float duty, int id)
 {
     char data[2]= {0};
     int send=mode * 10 + (int)(duty * 10.0);
     data[0]=send & 0b11111111;
     data[1]=hand_mode;
-    if(can1.write(CANMessage(0,data,2)))led4=1;
+    if(can1.write(CANMessage(id,data,2)))led4=1;
     else led4=0;
 }
 
@@ -166,9 +186,12 @@
 {
 
     hcsr04_val[1] = GetFilteredDist_cm(sensor_back,lowpassfilter[1]);
-    FileWrite();
-    int is_ok = checkUW(hcsr04_val[1], degree0*kDeg2Rad, ec_li.getCount());//ECは内側を選んだ.適当
-    if(is_ok != 1) {
+    int is_out = checkUW(hcsr04_val[1], degree0*kDeg2Rad, ec_li.getCount());//ECは内側を選んだ.適当
+    //isokもlogに残す
+    char str[100] = {};
+    sprintf(str, "%d", is_out);
+    FileWrite(str);
+    if(is_out == 1) {
         //もう一度とる
         sensor_back.start();
         wait_ms(50);