Diff: main.cpp

Revision:

14:1a3a673d85e3

Parent:

13:29e71a2fd11e

Child:

15:1098bf926b5b

--- a/main.cpp	Thu May 02 07:20:17 2019 +0000
+++ b/main.cpp	Thu May 02 09:08:08 2019 +0000
@@ -12,17 +12,15 @@
 
 #define Pi 3.14159265359 //円周率π
 
-enum WalkMode
-{
+enum WalkMode {
     BACK,
     STOP,
     FORWARD,
 };
-enum EVENT
-{
+enum EVENT {
     NORMAL,
     GEREGE,
-    GOAL,    
+    GOAL,
 };
 float accel_max = 0.01; //これグローバルにしたのはごめん。set関数多すぎてめんどくなった。
 
@@ -66,7 +64,10 @@
     {
         duty_limit_ = limit;
     };
-    float getDutyLimit(){return duty_limit_;};
+    float getDutyLimit()
+    {
+        return duty_limit_;
+    };
     float getPosi();                   //ポジをエンコーダから取得
     void calcDuty(PIDcontroller *pid); //Duty比を計算
     void setEncoder(Ec *ec)
@@ -283,7 +284,7 @@
 ////////////定数
 
 ////////////変数
-bool hand_mode=NORMAL;
+int hand_mode=NORMAL;
 
 //PIDcontroller, Motor, AirCylinderはOneLegのメンバクラスとして扱う
 //しかし変更を多々行うためポインタ渡しにしてある
@@ -303,7 +304,7 @@
 int main()
 {
     setup();
-    
+
     pid_lo.setTolerance(10);
     pid_li.setTolerance(10);
     motor_lo.setEncoder(&ec_lo);
@@ -321,30 +322,31 @@
     printf("reset standby\n\r");
     reset();
     printf("bus standby\n\r");
+
     while(1) {
         if(bus_in.read() == 1) break;
     }
     printf("bus is %d\n\r", bus_in.read());
-    
+
     wait(0.5);
-    
+
     straight();
-    
+
     wait_gerege();
-    
+
     hand_mode = GEREGE;
-    
+
     set_gyro();
     //Sample
     //スタート直進
     printf("dist:%.3f\r\n", get_dist_forward());
-    /*
+
     for(int i=0;i<3;++i){
         while(get_dist_back() < 280)
         {
             straightAndInfinity(0, 5);
             //wait(0.01);
-            printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
+            printf("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
         }
     }
     //printf("back dist:%.3f\r\n", get_dist_back());
@@ -364,7 +366,7 @@
     motor_li.setDutyLimit(0.5);
     for(int i=0;i<3;++i)
     {
-        while(get_dist_forward() > 65) 
+        while(get_dist_forward() > 65)
         {
             straightAndInfinity(90.0, 5.0);
             printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
@@ -373,54 +375,56 @@
     //ロープ前旋回
     printf("before roop deg:%.3f\n\r",degree0);
     turn(0);
+
     //ロープ
     while(mode4.read() == 0)
     {
-        
+
         straightAndInfinity(0, 5);
     }
-    */
-    printf("go to uhai deg:%.3f\n\r",degree0);
+
+    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)
+        while(get_dist_forward() > 65)//65
         {
             straightAndInfinity(0, 5);
-        //printf("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
-        }
-    }
-    turn(-90);
-    
-    
-    motor_lo.setDutyLimit(0.3);//0.5
-    motor_li.setDutyLimit(0.3);
-    
-    for(int i=0;i<15;++i)straightAndInfinity(-90, 5);
-    printf("wall standby");
-    for(int i=0;i<3;++i)
-    {
-        while(get_dist_forward() > 60)
-        {
-            straightAndInfinity(-90, 5);
             printf("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
         }
     }
+    printf("turn");
+    turn(-90);
+
     
+    motor_lo.setDutyLimit(0.2);//0.5
+    motor_li.setDutyLimit(0.2);
+
+    for(int i=0; i<15; ++i)straightAndInfinity(0, 5);
+    printf("wall standby");
+    while(get_dist_back() < 250) {
+        straightAndInfinity(0, 5);
+        printf("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
+    }
+    for(int i=0; i<2; ++i) {
+        while(get_dist_forward() > 65) {
+            straightAndInfinity(0, 5);
+            printf("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
+        }
+    }
     hand_mode = GOAL;
     straight();
     printf("uhai!!!!!!!!!!!!!!!");
+
+    
 }
 void turn(float target_degree)//turn_degreeを超えるまで旋回し続ける関数
 {
-    if(target_degree - degree0 > 0)
-    {
+    if(target_degree - degree0 > 0) {
         while(target_degree - degree0 > 0)
-        turnLeft();   
-    }
-    else
-    {
+            turnLeft();
+    } else {
         while(target_degree - degree0 < 0)
-        turnRight();
+            turnRight();
     }
     printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
 }
@@ -564,21 +568,25 @@
 double get_dist_back()
 {
     sensor_back.start();
-    //wait_ms(50);           //sensor.start()で信号を送ったあと反射波が帰ってきてから距離データが更新されるため、少し待つ必要がある。
+    wait_ms(10);//10           //sensor.start()で信号を送ったあと反射波が帰ってきてから距離データが更新されるため、少し待つ必要がある。
     //何ループも回す場合は不要。また、時間は適当だから調整が必要。
-    return sensor_back.get_dist_cm();
+    float dist = sensor_back.get_dist_cm();
+    if(dist < 0.1) dist = 2000.0;
+    return dist;
 }
 double get_dist_forward()
 {
     sensor_forward.start();
-    return sensor_forward.get_dist_cm();
+    wait_ms(10);//10
+    float dist = sensor_forward.get_dist_cm();
+    if(dist < 0.1) dist = 2000.0;
+    return dist;
 }
 
 void wait_gerege()
 {
     int i = 0;
-    while(i!=100)
-    {
-        if(hand.read()==0)i++;    
-    }    
+    while(i!=100) {
+        if(hand.read()==0)i++;
+    }
 }
\ No newline at end of file