Diff: main.cpp

Revision:

46:621ea6492dea

Parent:

45:0654364226c9

Child:

47:c2c579909787

diff -r 0654364226c9 -r 621ea6492dea main.cpp
--- a/main.cpp	Thu Mar 07 08:49:57 2019 +0000
+++ b/main.cpp	Fri Mar 08 02:13:10 2019 +0000
@@ -29,14 +29,15 @@
 enum ROS_STATE
 {
     STOP,
-    RUN,
+    AREA1_LRFWALK_STATE,
     SANDDUNE,
-    TURNRIGHT_STATE,
+    AREA2_LRFWALK_STATE,
     ROPE_STATE,
     SLOPE_STATE,
-
+    TURNRIGHT_STATE,
+    TRUNLEFT_STATE,
 };
-int state_from_ros = 0;
+ROS_STATE state_from_ros = STOP; //ここを変えると動く方法が変わる
 #ifdef USE_ROS
 ros::NodeHandle nh_mbed;
 void callback(const geometry_msgs::Vector3 &cmd_vel);
@@ -55,7 +56,8 @@
 enum WalkWay //歩行軌道
 {
     STANDUP,              //受け渡し用に待つ
-    LRFPOSTURE,           //LRF用
+    AREA1_LRFWALK,        //段差までLRFありで歩く.元LRFPOSTURE
+    AREA2_LRFWALK,        //段差後からLRFありで歩く
     STANDUP_SANDDUNE,     //段差前で立つ
     BEFORE_SANDDUNE,      //段差に壁当て
     FRONTLEG_ON_SANDDUNE, //前足を段差にかける
@@ -71,17 +73,24 @@
     TRUNLEFT,
     CHECK, //check用に最後に置いておく
 };
-//LRFPOSTUREだけspinOnceで変更するためグローバルで
-float offset_x_m[4] = {-0.15, 0.15, -0.15, 0.15},
-      offset_y_m[4] = {0.27, 0.27, 0.27, 0.27};
+//LRF使うやつだけだけspinOnceで変更するためグローバルで
 float stride_m[4] = {0.2, 0.2, 0.2, 0.2}; //ropeのときは0.15だった
-float height_m[4] = {0.05, 0.05, 0.05, 0.05}, buffer_height_m = 0.02,
-      stridetime_s = 1, toptime_s = 0.2, buffer_time_s = 0.1;
 //各歩行軌道のパラメータ設定。switchで羅列している。新規に歩行パターンを増やすときはcaseを増やす。
 //param walk:結果を入れる箱。way:歩行軌道を指定するWalkWayの定数。
 int SetWalk(Walk &walk, WalkWay way)
 {
+    //複数歩行に共有するパラメータはここに書く。それ以外はcase内に入れる。
+    //LRFを使って歩行する際のパラメータ.stride_mだけグローバルにある。
+    float area1_offset_y_m[4] = {0.27, 0.27, 0.27, 0.27};
+    float area2_offset_y_m[4] = {0.275, 0.275, 0.275, 0.275};
+    float offset_x_m[4] = {-0.15, 0.15, -0.15, 0.15};
+    float height_m[4] = {0.05, 0.05, 0.05, 0.05}, buffer_height_m = 0.02,
+          stridetime_s = 1, toptime_s = 0.2, buffer_time_s = 0.1;
+
+    //段差
     float overcome_start_y_m[] = {0.41, 0.41, 0.41, 0.41};
+
+    //旋回
     float turn_start_x_m = 0, turn_start_y_m = 0.275,
           turn_stride_m = 0.05, turn_height_m = 0.05,
           turn_stridetime_s = 0.6, turn_risetime_s = 0.3;
@@ -96,9 +105,16 @@
         walk.SetOffsetTime(0, 0, 0, 0);
         break;
     }
-    case LRFPOSTURE: //LRF用に変数はグローバルにある
+    case AREA1_LRFWALK: //LRF用に変数はグローバルにある
         for (int i = 0; i < 4; i++)
-            SetOneLegTriangleParam(walk, i, offset_x_m[i], offset_y_m[i],
+            SetOneLegTriangleParam(walk, i, offset_x_m[i], area1_offset_y_m[i],
+                                   stride_m[i], height_m[i], buffer_height_m,
+                                   stridetime_s, toptime_s, buffer_time_s);
+        walk.SetOffsetTime(0, 0.5, 0.5, 0);
+        break;
+    case AREA2_LRFWALK:
+        for (int i = 0; i < 4; i++)
+            SetOneLegTriangleParam(walk, i, offset_x_m[i], area2_offset_y_m[i],
                                    stride_m[i], height_m[i], buffer_height_m,
                                    stridetime_s, toptime_s, buffer_time_s);
         walk.SetOffsetTime(0, 0.5, 0.5, 0);
@@ -185,7 +201,7 @@
     case AFTER_OVERCOME:
     {
         for (int i = 0; i < 4; i++)
-            SetOneLegStandParam(walk, i, offset_x_m[i], offset_y_m[i], 0.5);
+            SetOneLegStandParam(walk, i, offset_x_m[i], area1_offset_y_m[i], 0.5);
         walk.SetOffsetTime(0, 0, 0, 0);
         break;
     }
@@ -223,7 +239,7 @@
         {
             LineParam lines[] = {
                 {.time_s = 0, .x_m = walk.leg[i].GetX_m(), .y_m = walk.leg[i].GetY_m(), .is_point_to_point = 0},
-                {.time_s = 0.5, .x_m = offset_x_m[i], .y_m = offset_y_m[i], .is_point_to_point = 0},
+                {.time_s = 0.5, .x_m = offset_x_m[i], .y_m = area1_offset_y_m[i], .is_point_to_point = 0},
             };
             SetOneLegFreeLinesParam(walk, i, lines, sizeof(lines) / sizeof(lines[0]));
         }
@@ -266,6 +282,7 @@
     if (FileOpen()) //csv fileに書き込み
         return 1;   //異常なら強制終了
 #endif
+
     /////足の長さ、計算周期の設定
     OneLeg leg[4]; //各足の位置
     for (int i = 0; i < 4; i++)
@@ -282,7 +299,15 @@
             return 1; //強制終了.errorは内部の関数からprintfで知らせる
         }
     }
-#ifndef USE_ROS
+//軌道チェックしてからROS完全起動
+#ifdef USE_ROS
+    nh_mbed.getHardware()->setBaud(115200);
+    nh_mbed.initNode();
+    nh_mbed.subscribe(sub_vel);
+    nh_mbed.subscribe(sub_state);
+    nh_mbed.advertise(pub_vel);
+    nh_mbed.spinOnce(); //一度受信
+#else
     printf("Stand up?\r\n");
     WaitStdin('y'); // キーボード入力されるまでまで待つ
 #endif
@@ -293,32 +318,26 @@
 #ifndef USE_ROS
     printf("Move?\r\n");
     WaitStdin('y'); //キーボード入力されるまでまで待つ
-#else
-    nh_mbed.getHardware()->setBaud(115200);
-    nh_mbed.initNode();
-    nh_mbed.subscribe(sub_vel);
-    nh_mbed.subscribe(sub_state);
-    nh_mbed.advertise(pub_vel);
-    nh_mbed.spinOnce(); //一度受信
-    SetWalk(walk, LRFPOSTURE);
+    state_from_ros = SANDDUNE; //遠してやらないならやりたいSTATEに変更
+#endif
+        int loopcount = 0;
+    //プログラムの最初のほうにあるstate_from_rosで動かすものを切り替える.caseはenum ROS_STATEで分ける
     while (1)
     {
         switch (state_from_ros)
         {
         case STOP:
             break;
-        case RUN:
-            for (int i = 0; i < 4; i++)
-                SetOneLegTriangleParam(walk, i, offset_x_m[i], offset_y_m[i],
-                                       stride_m[i], height_m[i], buffer_height_m,
-                                       stridetime_s, toptime_s, buffer_time_s);
+        case AREA1_LRFWALK_STATE:
+            SetWalk(walk, AREA1_LRFWALK);
+            MoveOneCycle(walk, leg);
+            break;
+        case AREA2_LRFWALK_STATE:
+            SetWalk(walk, AREA2_LRFWALK);
             MoveOneCycle(walk, leg);
             break;
         case SANDDUNE:
             // #endif
-            //2歩進んで当て処理
-            //            for (int i = 0; i < 2; i++)
-            //                MoveOneCycle(walk, leg);
             SetWalk(walk, STANDUP_SANDDUNE);
             MoveOneCycle(walk, leg);
             //前足を段差にかける
@@ -340,37 +359,49 @@
             SetWalk(walk, TRUNLEFT);
             for (int i = 0; i < 2; i++)
                 MoveOneCycle(walk, leg);
-            state_from_ros = 1;
+            state_from_ros = AREA2_LRFWALK_STATE;
             // #ifdef USE_ROS
             break;
-        case TURNRIGHT_STATE:
-            SetWalk(walk, TRUNRIGHT);
-            for (int i = 0; i < 5; i++)
-                MoveOneCycle(walk, leg);
-            state_from_ros = 1;
-            break;
         case ROPE_STATE:
             SetWalk(walk, ROPE);
             MoveOneCycle(walk, leg);
             SetWalk(walk, ROPE_DOWN);
             MoveOneCycle(walk, leg);
-            SetWalk(walk, LRFPOSTURE);
+            SetWalk(walk, AREA1_LRFWALK);
             for (int i = 0; i < 3; i++)
                 MoveOneCycle(walk, leg);
             SetWalk(walk, ROPE_BACK);
             MoveOneCycle(walk, leg);
-            state_from_ros = 1;
+            state_from_ros = AREA2_LRFWALK_STATE;
             break;
         case SLOPE_STATE:
             SetWalk(walk, SLOPE);
             for (int i = 0; i < 30; i++)
                 MoveOneCycle(walk, leg);
-            state_from_ros = 0;
+            state_from_ros = STOP;
+            break;
+        case TURNRIGHT_STATE:
+            SetWalk(walk, TRUNRIGHT);
+            for (int i = 0; i < 5; i++)
+                MoveOneCycle(walk, leg);
+            state_from_ros = AREA2_LRFWALK_STATE;
+            break;
+        case TRUNLEFT_STATE:
+            SetWalk(walk, TRUNLEFT);
+            for (int i = 0; i < 5; i++)
+                MoveOneCycle(walk, leg);
+            state_from_ros = AREA2_LRFWALK_STATE;
             break;
         }
+#ifdef USE_ROS
         nh_mbed.spinOnce();
+#endif
+#ifdef VSCODE
+        ++loopcount;
+        if (loopcount > 1)
+            break; //VSCODEでデバックする際は一ループで抜ける
+#endif
     }
-#endif
     printf("program end\r\n");
 #ifdef VSCODE
     fclose(fp);
@@ -379,8 +410,8 @@
 #ifdef USE_ROS
 void callback(const geometry_msgs::Vector3 &cmd_vel)
 {
-    if (state_from_ros == 0)
-        state_from_ros = 1;
+    if (state_from_ros == STOP)
+        state_from_ros = AREA1_LRFWALK_STATE;
     stride_m[LEFT_F] = cmd_vel.x;
     stride_m[LEFT_B] = cmd_vel.x;
     stride_m[RIGHT_F] = cmd_vel.y;