Diff: main.cpp

Revision:

33:945d634d4c9b

Parent:

32:dc684a0b8448

Child:

34:89d701e15cdf

diff -r dc684a0b8448 -r 945d634d4c9b main.cpp
--- a/main.cpp	Thu Feb 28 08:48:21 2019 +0000
+++ b/main.cpp	Thu Feb 28 09:39:02 2019 +0000
@@ -12,8 +12,6 @@
 #include "can.h"
 #define USE_CAN //can通信するならdefine.しないなら切らないとエラー出る
 //#define USE_ROS
-#include <ros.h>
-#include <geometry_msgs/Vector3.h>
 #endif
 //#define DEBUG_ON //デバッグ用。使わないときはコメントアウト
 #include "debug.h"
@@ -49,6 +47,8 @@
 DigitalOut led[4] = {DigitalOut(LED1), DigitalOut(LED2), DigitalOut(LED3), DigitalOut(LED4)};
 #endif
 #ifdef USE_ROS
+#include <ros.h>
+#include <geometry_msgs/Vector3.h>
 ros::NodeHandle nh_mbed;
 //ROSからのコールバック関数
 void callback(const geometry_msgs::Vector3 &cmd_vel);
@@ -137,29 +137,54 @@
     // OVERCOME 前足を乗せる
     
     LineParam over_come_front_r[] = {
+        {.time_s = 0.2, .x_m = 0, .y_m = 0.32},
+        {.time_s = 0.2, .x_m = -0.13, .y_m = 0.15},
+        {.time_s = 0.2, .x_m = 0.12, .y_m = 0.15},
+        {.time_s = 0.6, .x_m = 0.12, .y_m = 0.2},
+        {.time_s = 0, 
+        .x_m = 0.05, .y_m = 0.2}};
+    LineParam over_come_front_l[] = {
         {.time_s = 0.6, .x_m = -0, .y_m = 0.32},
         {.time_s = 0.2, .x_m = -0, .y_m = 0.32},
         {.time_s = 0.2, .x_m = -0.13, .y_m = 0.15},
         {.time_s = 0.2, .x_m = 0.12, .y_m = 0.15},
-        {.time_s = 0.6, .x_m = 0.12, .y_m = 0.15},
         {.time_s = 0, .x_m = 0.12, .y_m = 0.2}};
-    LineParam over_come_front_l[] = {
-        {.time_s = 0.6, .x_m = -0, .y_m = 0.32},
-        {.time_s = 0.6, .x_m = -0, .y_m = 0.32},
-        {.time_s = 0.2, .x_m = -0, .y_m = 0.32},
-        {.time_s = 0.2, .x_m = -0.13, .y_m = 0.15},
-        {.time_s = 0.2, .x_m = 0.2, .y_m = 0.15},
-        {.time_s = 0, .x_m = 0.2, .y_m = 0.2}};
-    LineParam over_come_back[] = {
+    LineParam over_come_backleg[] = {
         {.time_s = 0.6, .x_m = 0, .y_m = 0.3},
         {.time_s = 0.2, .x_m = 0, .y_m = 0.3},
-        {.time_s = 1, .x_m = -0.1,.y_m = 0.3},
+        {.time_s = 0.4, .x_m = -0.1,.y_m = 0.3},
         {.time_s = 0, .x_m = -0.1,.y_m = 0.3},
     };
     walks[OVERCOME].SetOneLegFreeLinesParam(RIGHT_F,  over_come_front_r, sizeof(over_come_front_r)/ sizeof(over_come_front_r[0]));
     walks[OVERCOME].SetOneLegFreeLinesParam(LEFT_F,  over_come_front_l, sizeof(over_come_front_l)/sizeof(over_come_front_l[0]));
-    walks[OVERCOME].SetOneLegFreeLinesParam(LEFT_B,  over_come_back, sizeof(over_come_back)/sizeof(over_come_back[0]));
-    walks[OVERCOME].SetOneLegFreeLinesParam(RIGHT_B,  over_come_back, sizeof(over_come_back)/sizeof(over_come_back[0]));
+    walks[OVERCOME].SetOneLegFreeLinesParam(LEFT_B,  over_come_backleg, sizeof(over_come_backleg)/sizeof(over_come_backleg[0]));
+    walks[OVERCOME].SetOneLegFreeLinesParam(RIGHT_B,  over_come_backleg, sizeof(over_come_backleg)/sizeof(over_come_backleg[0]));
+
+    
+    LineParam over_come_front_r[] = {
+        {.time_s = 0.2, .x_m = 0, .y_m = 0.32},
+        {.time_s = 0.2, .x_m = -0.13, .y_m = 0.15},
+        {.time_s = 0.2, .x_m = 0.12, .y_m = 0.15},
+        {.time_s = 0.6, .x_m = 0.12, .y_m = 0.2},
+        {.time_s = 0, 
+        .x_m = 0.05, .y_m = 0.2}};
+    LineParam over_come_front_l[] = {
+        {.time_s = 0.6, .x_m = -0, .y_m = 0.32},
+        {.time_s = 0.2, .x_m = -0, .y_m = 0.32},
+        {.time_s = 0.2, .x_m = -0.13, .y_m = 0.15},
+        {.time_s = 0.2, .x_m = 0.12, .y_m = 0.15},
+        {.time_s = 0, .x_m = 0.12, .y_m = 0.2}};
+    LineParam over_come_backleg[] = {
+        {.time_s = 0.6, .x_m = 0, .y_m = 0.3},
+        {.time_s = 0.2, .x_m = 0, .y_m = 0.3},
+        {.time_s = 0.4, .x_m = -0.1,.y_m = 0.3},
+        {.time_s = 0, .x_m = -0.1,.y_m = 0.3},
+    };
+    walks[OVERCOME].SetOneLegFreeLinesParam(RIGHT_F,  over_come_front_r, sizeof(over_come_front_r)/ sizeof(over_come_front_r[0]));
+    walks[OVERCOME].SetOneLegFreeLinesParam(LEFT_F,  over_come_front_l, sizeof(over_come_front_l)/sizeof(over_come_front_l[0]));
+    walks[OVERCOME].SetOneLegFreeLinesParam(LEFT_B,  over_come_backleg, sizeof(over_come_backleg)/sizeof(over_come_backleg[0]));
+    walks[OVERCOME].SetOneLegFreeLinesParam(RIGHT_B,  over_come_backleg, sizeof(over_come_backleg)/sizeof(over_come_backleg[0]));
+    
 
     // SANDDUNE, //前足だけがsandduneに乗った状態で進む
     offset_x_m = -0.03, offset_y_m = 0.3, stride_m = 0.1, height_m = 0.03, buffer_height_m = 0.01,
@@ -258,8 +283,8 @@
         for(int k=0;k<1;k++)MoveOneCycle(walks[UP], leg);
         for(int k=0;k<1;k++)MoveOneCycle(walks[OVERCOME],leg);
         for(int k=0;k<5;k++) MoveOneCycle(walks[SANDDUNE],leg);
-        for(int k=0;k<1;k++)MoveOneCycle(walks[OVERCOME_BACK],leg);
-        for(int k=0;k<3;k++)MoveOneCycle(walks[STRAIGHT_W], leg);
+//        for(int k=0;k<1;k++)MoveOneCycle(walks[OVERCOME_BACK],leg);
+//        for(int k=0;k<3;k++)MoveOneCycle(walks[STRAIGHT_W], leg);
 //        for(int k=0;k<5;k++)Move(*straight_w2, leg,(*straight_w2).GetTheta(0.1));
 //        for(int k=0;k<3;k++)Move(*straight, leg);
 /*