Diff: main.cpp

Revision:

45:0654364226c9

Parent:

44:4aac39b8670b

Child:

46:621ea6492dea

--- a/main.cpp	Thu Mar 07 06:36:34 2019 +0000
+++ b/main.cpp	Thu Mar 07 08:49:57 2019 +0000
@@ -7,7 +7,6 @@
 SetWalkの引数はenum WalkWayの定数。SetWalk内でswitchしている。SetWalk内ではchange_walk.hの関数で軌道を設定。
 ROSありだと、ROSからのstateに従いswitchで切り替えている。
  */
-
 #include "pi.h"
 #include <math.h>
 #include <stdio.h>
@@ -32,7 +31,10 @@
     STOP,
     RUN,
     SANDDUNE,
-    ROPE_STATE
+    TURNRIGHT_STATE,
+    ROPE_STATE,
+    SLOPE_STATE,
+
 };
 int state_from_ros = 0;
 #ifdef USE_ROS
@@ -60,11 +62,13 @@
     OVERCOME,             //前足が乗った状態で進む
     BACKLEG_ON_SANDDUNE,  //後ろ脚を段差に載せる
     OVERCOME2,            //後ろ脚が乗った状態で進む
-    AFTER_OVERCOME, //段差を終了したら座って指示まち
+    AFTER_OVERCOME,       //段差を終了したら座って指示まち
     ROPE,                 //rope前足超える
     ROPE_BACK,            //rope後ろ足超える
     ROPE_DOWN,            //rope機体高さをLRF用に落とす
     SLOPE,
+    TRUNRIGHT,
+    TRUNLEFT,
     CHECK, //check用に最後に置いておく
 };
 //LRFPOSTUREだけspinOnceで変更するためグローバルで
@@ -78,6 +82,9 @@
 int SetWalk(Walk &walk, WalkWay way)
 {
     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;
     switch (way)
     {
     case STANDUP:
@@ -182,7 +189,7 @@
         walk.SetOffsetTime(0, 0, 0, 0);
         break;
     }
-     case ROPE:
+    case ROPE:
     {
         float offset_x_m[4] = {-0.05, 0, -0.05, 0};
         float d_x_m = 0.2;
@@ -237,6 +244,14 @@
         walk.SetOffsetTime(0, 0.5, 0.5, 0);
         break;
     }
+    case TRUNRIGHT:
+        Turn(walk, 1, turn_start_x_m, turn_start_y_m, turn_stride_m,
+             turn_height_m, turn_stridetime_s, turn_risetime_s);
+        break;
+    case TRUNLEFT:
+        Turn(walk, 0, turn_start_x_m, turn_start_y_m, turn_stride_m,
+             turn_height_m, turn_stridetime_s, turn_risetime_s);
+        break;
     default:
         printf("error: there is no WalkWay\r\n");
         return 1; //以上終了
@@ -273,30 +288,12 @@
 #endif
     /////立つ
     // SetWalk(walk, STANDUP);
-    SetWalk(walk, AFTER_OVERCOME);//段差越え後調整用に一時的に入れた
+    SetWalk(walk, AFTER_OVERCOME); //段差越え後調整用に一時的に入れた
     MoveOneCycle(walk, leg);
 #ifndef USE_ROS
     printf("Move?\r\n");
     WaitStdin('y'); //キーボード入力されるまでまで待つ
-#endif
-/*   
-///SLOPE
-    SetWalk(walk, SLOPE);
-    for (int i = 0; i < 30; i++)
-        MoveOneCycle(walk, leg);
-
-    /////////////ROPEを試す
-    SetWalk(walk, ROPE);
-    MoveOneCycle(walk, leg);
-    SetWalk(walk, ROPE_DOWN);
-    MoveOneCycle(walk, leg);
-    SetWalk(walk, LRFPOSTURE);
-    for (int i = 0; i < 3; i++)
-        MoveOneCycle(walk, leg);
-    SetWalk(walk, ROPE_BACK);
-    MoveOneCycle(walk, leg);
-*/
-#ifdef USE_ROS
+#else
     nh_mbed.getHardware()->setBaud(115200);
     nh_mbed.initNode();
     nh_mbed.subscribe(sub_vel);
@@ -318,7 +315,7 @@
             MoveOneCycle(walk, leg);
             break;
         case SANDDUNE:
-#endif
+            // #endif
             //2歩進んで当て処理
             //            for (int i = 0; i < 2; i++)
             //                MoveOneCycle(walk, leg);
@@ -340,7 +337,35 @@
             SetWalk(walk, OVERCOME2);
             for (int i = 0; i < 3; i++)
                 MoveOneCycle(walk, leg);
-#ifdef USE_ROS
+            SetWalk(walk, TRUNLEFT);
+            for (int i = 0; i < 2; i++)
+                MoveOneCycle(walk, leg);
+            state_from_ros = 1;
+            // #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);
+            for (int i = 0; i < 3; i++)
+                MoveOneCycle(walk, leg);
+            SetWalk(walk, ROPE_BACK);
+            MoveOneCycle(walk, leg);
+            state_from_ros = 1;
+            break;
+        case SLOPE_STATE:
+            SetWalk(walk, SLOPE);
+            for (int i = 0; i < 30; i++)
+                MoveOneCycle(walk, leg);
+            state_from_ros = 0;
             break;
         }
         nh_mbed.spinOnce();
@@ -362,10 +387,11 @@
     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 = cmd.data;
-    led[state_from_ros] = 1;
+    //    led[state_from_ros] = 1;
 }
 #endif
\ No newline at end of file