Diff: sensors/sensors.cpp

Revision:

34:0a8ae7f92262

Parent:

33:2dbbe198adaf

Child:

35:04699b49c463

--- a/sensors/sensors.cpp	Mon May 13 09:02:19 2019 +0000
+++ b/sensors/sensors.cpp	Mon May 13 11:55:08 2019 +0000
@@ -16,6 +16,8 @@
 HCSR04 sensor_back(pin_hcsr04[1][0], pin_hcsr04[1][1]);
 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 int kResolution=500;
@@ -160,3 +162,35 @@
     else led4=0;
 }
 
+float Hcsr04BackWithEc(){
+    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)
+    {
+        hcsr04_val[1] = GetFilteredDist_cm(sensor_back,lowpassfilter[1]);
+        FileWrite();
+    }
+    return hcsr04_val[1];
+}
+int checkUW(double uwdist,double theta,int eccount){    //uwdist[cm],theta[rad],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;
+    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]
+    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