Diff: main.cpp

Revision:

20:e30e6e175991

Parent:

19:b162e7f4cc06

Child:

21:14133581387b

--- a/main.cpp	Fri May 03 00:30:11 2019 +0000
+++ b/main.cpp	Fri May 03 01:46:30 2019 +0000
@@ -3,15 +3,6 @@
 #include "pin.h"
 #include "microinfinity.h"
 #include "robot.h"
-/*
-//#define DEBUG_ON
-
-#ifdef DEBUG_ON
-#define DEBUG(...) printf("" __VA_ARGS__);
-#else
-#define DEBUG(...)
-#endif
-*/
 #define Pi 3.14159265359 //円周率π
 
 //PIDcontroller, Motor, AirCylinderはOneLegのメンバクラスとして扱う
@@ -22,6 +13,10 @@
 OneLeg leg_lo, leg_li;
 Robot robot;
 
+//filter [0]:forward, [1]: back
+const float kOldWeight = 0.5;
+LowPassFilter lowpassfilter[2] = {LowPassFilter(kOldWeight),LowPassFilter(kOldWeight),};
+
 enum WalkMode {
     BACK,
     STOP,
@@ -50,9 +45,11 @@
 void turn(float target_degree);//回転角, 収束許容誤差
 void straightAndInfinity(float target_degree, float tolerance_degree);//角度を補正するのと前進
 void wait_gerege();
+float GetFilteredDist_cm(HCSR04 & sensor, LowPassFilter & filter);
 
 int main()
 {
+
     setup();
     printf("reset standby\n\r");
     reset();
@@ -70,15 +67,11 @@
     //直進スタート
     motor_lo.setDutyLimit(0.6);//0.5
     motor_li.setDutyLimit(0.6);
-    for(int i=0;i<3;++i){
-        while(get_dist_back() < 290)
-        {
+    for(int i=0; i<3; ++i) {
+        while(get_dist_back() < 290) {
             straightAndInfinity(0, 5);
-            //wait(0.01);
-//            printf("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
         }
     }
-    //printf("back dist:%.3f\r\n", get_dist_back());
     //段差前旋回
     motor_lo.setDutyLimit(0.3);//0.5
     motor_li.setDutyLimit(0.3);
@@ -86,19 +79,17 @@
     //段差乗り越え
     motor_lo.setDutyLimit(0.3);//0.5
     motor_li.setDutyLimit(0.3);
-    for(int i= 0;i<5;++i) straight();
+    for(int i= 0; i<5; ++i) straight();
     while(get_dist_back() > 40) straight();
     //段差後旋回
     printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
     turn(90.0);
     printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
     //前進
-    motor_lo.setDutyLimit(0.5);//0.5
+    motor_lo.setDutyLimit(0.5);
     motor_li.setDutyLimit(0.5);
-    for(int i=0;i<3;++i)
-    {
-        while(get_dist_forward() > 65)
-        {
+    for(int i=0; i<3; ++i) {
+        while(get_dist_forward() > 65) {
             straightAndInfinity(90.0, 5.0);
             printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
         }
@@ -108,17 +99,13 @@
     turn(0);
 
     //ロープ
-    while(mode4.read() == 0)
-    {
+    while(mode4.read() == 0) {
 
         straightAndInfinity(0, 5);
     }
-
     printf("go to uhai deg:%.3f forward dist:%.3f \n\r",degree0,get_dist_forward());
-    for(int i=0;i<3;++i)
-    {
-        while(get_dist_forward() > 65)//65
-        {
+    for(int i=0; i<3; ++i) {
+        while(get_dist_forward() > 65) { //65
             straightAndInfinity(0, 5);
             printf("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
         }
@@ -126,11 +113,11 @@
     printf("turn");
     turn(-90);
 
-    while(get_dist_back() > 10.0){}
-    while(get_dist_back() < 30.0){}
-    
+    while(get_dist_back() > 10.0) {}
+    while(get_dist_back() < 30.0) {}
+
     printf("last spart!!!!!!!!");
-    
+
     motor_lo.setDutyLimit(0.2);//0.5
     motor_li.setDutyLimit(0.2);
 
@@ -259,7 +246,7 @@
     switch_lo.mode(PullUp);
     switch_li.mode(PullUp);
     mode4.mode(PullUp);
-    
+
     pid_lo.setTolerance(10);
     pid_li.setTolerance(10);
     motor_lo.setEncoder(&ec_lo);
@@ -313,22 +300,27 @@
     ec_li.reset();
     motor_li.output(0.0);
 }
+
+
+float GetFilteredDist_cm(HCSR04 & sensor, LowPassFilter & filter)
+{
+    sensor.start();
+    float raw_data = sensor.get_dist_cm();
+    if(raw_data < 0.1)//0.1以下なら前回の値を返す
+        return filter.GetData();
+    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()
 {
-    sensor_back.start();
-    //wait_ms(20);//10           //sensor.start()で信号を送ったあと反射波が帰ってきてから距離データが更新されるため、少し待つ必要がある。
-    //何ループも回す場合は不要。また、時間は適当だから調整が必要。
-    float dist = sensor_back.get_dist_cm();
-    if(dist < 0.1) dist = 2000.0;
-    return dist;
-}
-double get_dist_forward()
-{
-    sensor_forward.start();
-    //wait_ms(20);//10
-    float dist = sensor_forward.get_dist_cm();
-    if(dist < 0.1) dist = 2000.0;
-    return dist;
+    return GetFilteredDist_cm(sensor_back,lowpassfilter[1]);
 }
 
 void wait_gerege()
@@ -337,4 +329,4 @@
     while(i!=100) {
         if(hand.read()==0)i++;
     }
-}
\ No newline at end of file
+}