Diff: main.cpp

Revision:

39:87dcdff27797

Parent:

38:e664b1c12da9

Child:

41:38d79b6513c0

--- a/main.cpp	Mon Mar 04 10:51:46 2019 +0000
+++ b/main.cpp	Mon Mar 04 23:41:48 2019 +0000
@@ -68,6 +68,7 @@
 {
     STANDUP,
     LRFPOSTURE,
+    OVERCOME,
     FRONTLEG_ON_SANDDUNE, //前足かける
 };
 
@@ -106,20 +107,25 @@
                                    stridetime_s, toptime_s, buffer_time_s);
         walk.SetOffsetTime(0, 0.5, 0.5, 0);
         break;
-
-    case FRONTLEG_ON_SANDDUNE: //前足かける
-                               //                                   for(int q = 0; q < 4; q++)
-                               //            SetOneLegFreeLinesParam(walk, q, over_come_front, sizeof(over_come_front) / sizeof(over_come_front[0]));
-                               //        walk.SetOffsetTime(0, 0.5, 0.5, 0);
-
+    case FRONTLEG_ON_SANDDUNE:
+    {   //前足かける
+        /*   for (int q = 0; q < 4; q++)
+            SetOneLegFreeLinesParam(walk, q, over_come_front, sizeof(over_come_front) / sizeof(over_come_front[0]));
+        walk.SetOffsetTime(0, 0.5, 0.5, 0); */
         OverCome overcome(offset_x_m, start_y_m, d_x_m, goal_y_m, overcome_height_m, gravity_dist, walk.leg);
         walk.Copy(overcome.walk);
+    }
+    break;
+    case OVERCOME: //4点歩行でトロットし続ける
+        for (int i = 0; i < 4; i++)
+            SetOneLegFourPointParam(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);
+        walk.SetOffsetTime(0, 0.5, 0.5, 0);
         break;
     }
     return walk.CheckOrbit();
 }
-Walk* walk;
-OneLeg leg[4]; //各足の位置
 
 int main()
 {
@@ -127,64 +133,60 @@
     if (FileOpen()) //csv fileに書き込み
         return 1;   //異常終了したら強制終了
 #endif
-    
+    OneLeg leg[4]; //各足の位置
     for (int i = 0; i < 4; i++)
         leg[i] = OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2);
-    walk = new Walk(leg); //歩行法はここに入れていく
+    Walk walk(leg); //歩行法はここに入れていく
     Walk::calctime_s_ = 0.03;
     printf("Stand up?\r\n");
     WaitStdin('y'); // ボタンを押したら立つ
-    if (SetWalk(*walk, STANDUP) == 1)
+    if (SetWalk(walk, STANDUP) == 1)
     {
         printf("error: stand move\r\n");
         return 1; //強制終了.errorは内部の関数からprintfで知らせる
     }
-    if (MoveOneCycle(*walk, leg) == 1)
-    {
-        printf("error: stand move\r\n");
-        return 1; //強制終了.errorは内部の関数からprintfで知らせる
-    }
+    MoveOneCycle(walk, leg);
     printf("Move?\r\n");
     WaitStdin('y'); // ボタンを押したらスタート
 #ifdef USE_ROS
     nh_mbed.getHardware()->setBaud(115200);
     nh_mbed.initNode();
     nh_mbed.subscribe(sub_vel);
-    while (1){
+    while (1)
+    {
+        SetWalk(walk, LRFPOSTURE);
+        MoveOneCycle(walk, leg);
         nh_mbed.spinOnce();
-            }
+    }
 #else
-
-    if (SetWalk(*walk, FRONTLEG_ON_SANDDUNE) == 1)
+    if (SetWalk(walk, OVERCOME) == 1)
+    {
+        printf("error: OVERCOME\r\n");
+        return 1; //強制終了
+    }
+    for (int i = 0; i < 2; i++) //歩数
+        MoveOneCycle(walk, leg);
+    /*
+    if (SetWalk(walk, FRONTLEG_ON_SANDDUNE) == 1)
     {
         printf("error: triangle\r\n");
         return 1; //強制終了.errorは内部の関数からprintfで知らせる
     }
-
     for (int i = 0; i < 2; i++)
-    {
-        if (MoveOneCycle(*walk, leg) == 1)
-        {
-            printf("error: triangle move\r\n");
-            return 1; //強制終了.errorは内部の関数からprintfで知らせる
-        }
-    }
+        MoveOneCycle(walk, leg);
+    
     offset_y_m[LEFT_F] = 0.29;
     offset_y_m[RIGHT_F] = 0.29;
-    if (SetWalk(*walk, LRFPOSTURE) == 1)
+    */
+    /* 
+    if (SetWalk(walk, LRFPOSTURE) == 1)
     {
         printf("error: triangle\r\n");
         return 1; //強制終了.errorは内部の関数からprintfで知らせる
     }
-
-    for (int i = 0; i < 20; i++)
-    {
-        if (MoveOneCycle(*walk, leg) == 1)
-        {
-            printf("error: triangle move\r\n");
-            return 1; //強制終了.errorは内部の関数からprintfで知らせる
-        }
-    }
+    for (int i = 0; i < 2; i++)
+        MoveOneCycle(walk, leg);
+ */
 
     printf("program end\r\n");
 #ifdef VSCODE
@@ -278,7 +280,7 @@
 {
     float left_vel = cmd_vel.x;
     float right_vel = cmd_vel.y;
-    int state = cmd_vel.z;
+//    int state = cmd_vel.z;
     //閾値は要検討
     if (right_vel < left_vel)
     {
@@ -294,7 +296,5 @@
         stride_m[LEFT_B] = 0.2;
         stride_m[RIGHT_B] = 0.2;
     }
-    SetWalk(*walk, LRFPOSTURE);
-    MoveOneCycle(*walk, leg);
 }
 #endif
\ No newline at end of file