Diff: main.cpp

Revision:

48:1aad3cc386e8

Parent:

47:c2c579909787

Child:

49:198030e84936

--- a/main.cpp	Fri Mar 08 04:34:29 2019 +0000
+++ b/main.cpp	Fri Mar 08 06:09:31 2019 +0000
@@ -99,7 +99,7 @@
     case STANDUP:
     { //受け渡し用に待つ
         float offset_x_m[4] = {},
-              offset_y_m[4] = {0.275, 0.275, 0.275, 0.275};
+              offset_y_m[4] = {0.3, 0.3, 0.3, 0.3};
         for (int i = 0; i < 4; i++)
             SetOneLegStandParam(walk, i, offset_x_m[i], offset_y_m[i], 0.5);
         walk.SetOffsetTime(0, 0, 0, 0);
@@ -275,6 +275,7 @@
     walk.ResetPhase();
     return 0; //正常終了
 }
+void SmoothChange(Walk &walk, OneLeg leg[4], WalkWay nextway, float time_s);
 int main()
 {
     printf("program start\r\n");
@@ -312,15 +313,16 @@
     WaitStdin('y'); // キーボード入力されるまでまで待つ
 #endif
     /////立つ
-     SetWalk(walk, STANDUP);
-//    SetWalk(walk, AFTER_OVERCOME); //段差越え後調整用に一時的に入れた
+    SetWalk(walk, STANDUP);
+    // SetWalk(walk, AFTER_OVERCOME); //段差越え後調整用に一時的に入れた
     MoveOneCycle(walk, leg);
 #ifndef USE_ROS
     printf("Move?\r\n");
-    WaitStdin('y'); //キーボード入力されるまでまで待つ
+    WaitStdin('y');            //キーボード入力されるまでまで待つ
     state_from_ros = SANDDUNE; //遠してやらないならやりたいSTATEに変更
 #endif
-        int loopcount = 0;
+    int loopcount = 0;
+    ROS_STATE old_state = state_from_ros;
     //プログラムの最初のほうにあるstate_from_rosで動かすものを切り替える.caseはenum ROS_STATEで分ける
     while (1)
     {
@@ -329,70 +331,74 @@
         case STOP:
             break;
         case AREA1_LRFWALK_STATE:
-            SetWalk(walk, AREA1_LRFWALK);
+            if (old_state != state_from_ros)
+                SmoothChange(walk, leg, AREA1_LRFWALK, 0.5); //切り替え
+            else
+                SetWalk(walk, AREA1_LRFWALK);
             MoveOneCycle(walk, leg);
             break;
         case AREA2_LRFWALK_STATE:
-            SetWalk(walk, AREA2_LRFWALK);
+            if (old_state != state_from_ros)
+                SmoothChange(walk, leg, AREA2_LRFWALK, 0.5); //切り替え
+            else
+                SetWalk(walk, AREA2_LRFWALK);
             MoveOneCycle(walk, leg);
             break;
         case SANDDUNE:
-            // #endif
-            SetWalk(walk, STANDUP_SANDDUNE);
+            //前足を段差にかける
+            SmoothChange(walk, leg, FRONTLEG_ON_SANDDUNE, 0.5); //切り替えスムーズ
             MoveOneCycle(walk, leg);
-            //前足を段差にかける
-            SetWalk(walk, FRONTLEG_ON_SANDDUNE);
-            for (int i = 0; i < 1; i++)
-                MoveOneCycle(walk, leg);
             //前足が段差に乗った状態で進む
-            SetWalk(walk, OVERCOME);
+            SmoothChange(walk, leg, OVERCOME, 0.5); //切り替えスムーズ
             for (int i = 0; i < 5; i++)
                 MoveOneCycle(walk, leg);
             //後ろ脚載せる
-            SetWalk(walk, BACKLEG_ON_SANDDUNE);
+            SmoothChange(walk, leg, BACKLEG_ON_SANDDUNE, 0.5); //切り替えスムーズ
             for (int i = 0; i < 1; i++)
                 MoveOneCycle(walk, leg);
             //後ろ脚乗った状態で進む
-            SetWalk(walk, OVERCOME2);
+            SmoothChange(walk ,leg, OVERCOME2, 0.5); //切り替えスムーズ
             for (int i = 0; i < 3; i++)
                 MoveOneCycle(walk, leg);
-            SetWalk(walk, TRUNLEFT);
+            //数歩左に曲がる
+            SmoothChange(walk, leg, TRUNLEFT, 0.5); //切り替えスムーズ
             for (int i = 0; i < 2; i++)
                 MoveOneCycle(walk, leg);
             state_from_ros = AREA2_LRFWALK_STATE;
-            // #ifdef USE_ROS
             break;
         case ROPE_STATE:
-            SetWalk(walk, ROPE);
+        //前足だけ紐越え
+            SmoothChange(walk, leg, ROPE, 0.5);
             MoveOneCycle(walk, leg);
-            SetWalk(walk, ROPE_DOWN);
-            MoveOneCycle(walk, leg);
-            SetWalk(walk, AREA1_LRFWALK);
+            //LRFが取れる高さで歩行
+            SmoothChange(walk, leg, AREA1_LRFWALK, 0.5);
             for (int i = 0; i < 3; i++)
                 MoveOneCycle(walk, leg);
-            SetWalk(walk, ROPE_BACK);
+            //後ろ足越え
+            SmoothChange(walk,leg, ROPE_BACK, 0.5);
             MoveOneCycle(walk, leg);
             state_from_ros = AREA2_LRFWALK_STATE;
             break;
         case SLOPE_STATE:
-            SetWalk(walk, SLOPE);
+            SmoothChange(walk, leg, SLOPE, 0.5);
             for (int i = 0; i < 30; i++)
                 MoveOneCycle(walk, leg);
             state_from_ros = STOP;
             break;
         case TURNRIGHT_STATE:
-            SetWalk(walk, TRUNRIGHT);
+            SmoothChange(walk, leg, TRUNRIGHT, 0.5);
             for (int i = 0; i < 5; i++)
                 MoveOneCycle(walk, leg);
             state_from_ros = AREA2_LRFWALK_STATE;
             break;
         case TRUNLEFT_STATE:
-            SetWalk(walk, TRUNLEFT);
+            SmoothChange(walk, leg, TRUNLEFT, 0.5);
             for (int i = 0; i < 5; i++)
                 MoveOneCycle(walk, leg);
             state_from_ros = AREA2_LRFWALK_STATE;
             break;
         }
+        old_state = state_from_ros;
 #ifdef USE_ROS
         nh_mbed.spinOnce();
 #endif
@@ -418,11 +424,37 @@
     stride_m[RIGHT_B] = cmd_vel.y;
     back_vel = cmd_vel;
     pub_vel.publish(&back_vel);
-//    led[0] = !led[0];
 }
 void callback_state(const std_msgs::Int16 &cmd)
 {
     state_from_ros = (ROS_STATE)cmd.data;
     led[state_from_ros % 4] = 1;
 }
-#endif
\ No newline at end of file
+#endif
+void SmoothChange(Walk &walk, OneLeg leg[4], WalkWay nextway, float time_s)
+{
+    LineParam lines[4][2];
+    //前回位置を保存
+    for (size_t i = 0; i < sizeof(lines) / sizeof(lines[0]); i++)
+    {
+        lines[i][0].time_s = 0;
+        lines[i][0].x_m = leg[i].GetX_m();
+        lines[i][0].y_m = leg[i].GetY_m();
+    }
+    Walk tempwalk(leg);
+    SetWalk(tempwalk, nextway);
+    tempwalk.Cal4LegsPosi(tempwalk.leg); //次の一歩目をwalk.legに書き込む
+    for (size_t i = 0; i < sizeof(lines) / sizeof(lines[0]); i++)
+    {
+        lines[i][1].time_s = time_s;
+        lines[i][1].x_m = tempwalk.leg[i].GetX_m();
+        lines[i][1].y_m = tempwalk.leg[i].GetY_m();
+        lines[i][1].is_point_to_point = 0;
+    }
+    //前回終点から次回始点まで直線移動
+    for (int i = 0; i < 4; i++)
+        SetOneLegFreeLinesParam(tempwalk, i, lines[i], sizeof(lines[i]) / sizeof(lines[i][0]));
+    tempwalk.ResetPhase();
+    MoveOneCycle(tempwalk, leg);
+    SetWalk(walk, nextway);
+};
\ No newline at end of file