test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Walk/change_walk.cpp
- Committer:
- shimizuta
- Date:
- 2019-03-11
- Revision:
- 50:36741e8ab197
- Parent:
- 45:0654364226c9
File content as of revision 50:36741e8ab197:
/*walk設定用関数. param walk:結果を入れる箱 各パラメータの意味は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); LineParam line[]{ {.time_s = 0, .x_m = x_m, .y_m = y_m, .is_point_to_point = 0}, {.time_s = time_s, .x_m = x_m, .y_m = y_m, .is_point_to_point = 0}, }; freeline.SetFreeLinesParam(line, sizeof(line) / sizeof(line[0])); walk.orbit[legnum].Copy(freeline); } void SetOneLegTriangleParam(Walk &walk, int legnum, float offset_x_m, float offset_y_m, float stride_m, float height_m, float buffer_height_m, float stridetime_s, float toptime_s, float buffer_time_s) { Orbit triangle(TRIANGLE); triangle.SetTriangleParam(offset_x_m, offset_y_m, stride_m, height_m, buffer_height_m, stridetime_s, toptime_s, buffer_time_s); walk.orbit[legnum].Copy(triangle); } void SetOneLegFourPointParam(Walk &walk, int legnum, float offset_x_m, float offset_y_m, float stride_m, float height_m, float buffer_height_m, float stridetime_s, float toptime_s, float buffer_time_s) { Orbit four(FOURPOINT); four.SetFourPointParam(offset_x_m, offset_y_m, stride_m, height_m, buffer_height_m, stridetime_s, toptime_s, buffer_time_s); walk.orbit[legnum].Copy(four); } void SetOneLegFreeLinesParam(Walk &walk, int legnum, LineParam lineparams[], int point_num) { Orbit freeline(FREELINES); freeline.SetFreeLinesParam(lineparams, point_num); walk.orbit[legnum].Copy(freeline); } 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); }