test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Diff: Walk/change_walk.cpp
- Revision:
- 45:0654364226c9
- Parent:
- 44:4aac39b8670b
diff -r 4aac39b8670b -r 0654364226c9 Walk/change_walk.cpp --- a/Walk/change_walk.cpp Thu Mar 07 06:36:34 2019 +0000 +++ b/Walk/change_walk.cpp Thu Mar 07 08:49:57 2019 +0000 @@ -3,6 +3,7 @@ 各パラメータの意味はOrbitクラスの基底クラスまで遡る必要があるので注意。 */ #include "change_walk.h" +#include "OverCome.h" void SetOneLegStandParam(Walk &walk, int legnum, float x_m, float y_m, float time_s) { Orbit freeline(FREELINES); @@ -35,4 +36,41 @@ Orbit freeline(FREELINES); freeline.SetFreeLinesParam(lineparams, point_num); walk.orbit[legnum].Copy(freeline); -} \ No newline at end of file +} + +void Turn(Walk &walk, int is_turnright, float start_x_m, float start_y_m, + float stride_m, float height_m, float stridetime_s, float risetime_s) +{ + LineParam lines_forward[] = { + {.time_s = 0, .x_m = start_x_m, .y_m = start_y_m}, + {.time_s = stridetime_s, .x_m = start_x_m - stride_m, .y_m = start_y_m, .is_point_to_point = 0}, //進む + {.time_s = stridetime_s + risetime_s * 0.66f, .x_m = start_x_m, .y_m = start_y_m - height_m, .is_point_to_point = 1}, //上げる + {.time_s = stridetime_s + risetime_s, .x_m = start_x_m, .y_m = start_y_m, .is_point_to_point = 0}, //おろす + {.time_s = stridetime_s + risetime_s + stridetime_s, .x_m = start_x_m, .y_m = start_y_m, .is_point_to_point = 1}, //着地したまま + {.time_s = stridetime_s + risetime_s + stridetime_s + risetime_s * 0.66f, .x_m = start_x_m, .y_m = start_y_m - height_m, .is_point_to_point = 1}, // 垂直上げ + {.time_s = (stridetime_s + risetime_s) * 2, .x_m = start_x_m, .y_m = start_y_m, .is_point_to_point = 0}, //おろす + }; + LineParam lines_back[7]; + for (unsigned int i = 0; i < sizeof(lines_back) / sizeof(lines_back[0]); i++) + { + lines_back[i].time_s = lines_forward[i].time_s; + lines_back[i].x_m = -lines_forward[i].x_m; + lines_back[i].y_m = lines_forward[i].y_m; + lines_back[i].is_point_to_point = lines_forward[i].is_point_to_point; + } + if (is_turnright) + { + SetOneLegFreeLinesParam(walk, LEFT_F, lines_forward, sizeof(lines_forward) / sizeof(lines_forward[0])); + SetOneLegFreeLinesParam(walk, LEFT_B, lines_forward, sizeof(lines_forward) / sizeof(lines_forward[0])); + SetOneLegFreeLinesParam(walk, RIGHT_F, lines_back, sizeof(lines_back) / sizeof(lines_back[0])); + SetOneLegFreeLinesParam(walk, RIGHT_B, lines_back, sizeof(lines_back) / sizeof(lines_back[0])); + } + else + { + SetOneLegFreeLinesParam(walk, RIGHT_F, lines_forward, sizeof(lines_forward) / sizeof(lines_forward[0])); + SetOneLegFreeLinesParam(walk, RIGHT_B, lines_forward, sizeof(lines_forward) / sizeof(lines_forward[0])); + SetOneLegFreeLinesParam(walk, LEFT_F, lines_back, sizeof(lines_back) / sizeof(lines_back[0])); + SetOneLegFreeLinesParam(walk, LEFT_B, lines_back, sizeof(lines_back) / sizeof(lines_back[0])); + } + walk.SetOffsetTime(0.5, 0.75, 0.25, 0); +}