Diff: sensors/sensors.cpp

Revision:

47:b77796718a9b

Parent:

45:6df20a8f4b21

--- a/sensors/sensors.cpp	Fri May 17 06:07:59 2019 +0000
+++ b/sensors/sensors.cpp	Fri May 17 10:46:05 2019 +0000
@@ -17,9 +17,9 @@
 float hcsr04_val[2]  = {};
 float GetFilteredDist_cm(HCSR04 & sensor, LowPassFilter & filter);
 const float kDeg2Rad = M_pi/180.0;
-int checkUW(double uwdist,double theta,int eccount);
 
 //ec
+const double ecsliprate=0.8647f;
 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);
@@ -106,13 +106,11 @@
 void FileCloseFiltered()
 {
     int is_ok = 0;
-    wait(0.01);
-    if(RightOrLeft) {
-        //rightならfallで取る
+    wait(0.1);
+    if(RightOrLeft) {    //rightならfallで取る
         if(switch_LR == 0)
             is_ok = 1;
-    } else {
-        //left
+    } else {//left
         if(switch_LR == 1)
             is_ok = 1;
     }
@@ -186,39 +184,46 @@
 {
 
     hcsr04_val[1] = GetFilteredDist_cm(sensor_back,lowpassfilter[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) {
+    int is_ok = checkUW(hcsr04_val[1], degree0, ec_lo.getCount());//ECは内側を選んだ.適当
+    if(is_ok == 0) {
         //もう一度とる
         sensor_back.start();
         wait_ms(50);
         hcsr04_val[1] = sensor_back.get_dist_cm();
-        FileWrite();
+//        FileWrite();
     }
     return hcsr04_val[1];
 }
-int checkUW(double uwdist,double theta,int eccount)     //uwdist[cm],theta[rad],eccount[]
+int checkUW(double uwdist,double deg,int eccount, int is_getlog)     //uwdist[cm],theta[deg],eccount[]
 {
     const double oneloopdist=0.22f;//[m]
-    const double ecsliprate=0.7f;
     const double errrange=0.5;//収束判定[m]
     //const double D=10;//機体中心から足まで距離[m]
     static double X=0,Y=0;  //初期位置を代入
     static double pre_theta=M_pi*0.5f,pre_eccount=0;
+
+    double theta = deg * kDeg2Rad;
     theta+=M_pi*0.5f;
     double d_eccount=eccount-pre_eccount;
     double d_theta=theta-pre_theta;
     double ecdist=oneloopdist*d_eccount/1000*ecsliprate;
     double rho=ecdist/d_theta;
     double dx=-rho*(sin(pre_theta)-sin(theta));//[m]
+    //double dy= rho*(cos(pre_theta)-cos(theta));//[m]
     double dy= rho*(cos(pre_theta)-cos(theta));//[m]
     X+=dx;//[m]
     Y+=dy;//[m]
     pre_theta=theta;
     pre_eccount=eccount;
-    if(fabs(Y-uwdist*0.01f)<errrange)return 1;//Y-uwdist*0.01f*sin(theta)
-    else return 0;
-}
\ No newline at end of file
+//    printf("{X,Y}={%f,%f},uw=%f,",X,Y,uwdist);
+//    fprintf(result,"%f,%f,%f,",X,Y,uwdist);
+    int ret = 0;
+    if(fabs(Y-uwdist*0.01f)<errrange)ret = 1;//Y-uwdist*0.01f*sin(theta)
+    else ret = 0;
+    if(is_getlog == 1) {
+        char str[255];
+        sprintf(str,"%f,%f,%d,%f,%f,%d",uwdist,deg,eccount,X,Y, ret);
+        FileWrite(str);
+    }
+    return ret;
+}