test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Walk/change_walk.cpp@45:0654364226c9, 2019-03-07 (annotated)
- Committer:
- shimizuta
- Date:
- Thu Mar 07 08:49:57 2019 +0000
- Revision:
- 45:0654364226c9
- Parent:
- 44:4aac39b8670b
turn
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shimizuta | 44:4aac39b8670b | 1 | /*walk設定用関数. |
shimizuta | 44:4aac39b8670b | 2 | param walk:結果を入れる箱 |
shimizuta | 44:4aac39b8670b | 3 | 各パラメータの意味はOrbitクラスの基底クラスまで遡る必要があるので注意。 |
shimizuta | 44:4aac39b8670b | 4 | */ |
shimizuta | 35:b4e1b8f25cd7 | 5 | #include "change_walk.h" |
shimizuta | 45:0654364226c9 | 6 | #include "OverCome.h" |
shimizuta | 35:b4e1b8f25cd7 | 7 | void SetOneLegStandParam(Walk &walk, int legnum, float x_m, float y_m, float time_s) |
shimizuta | 35:b4e1b8f25cd7 | 8 | { |
shimizuta | 39:87dcdff27797 | 9 | Orbit freeline(FREELINES); |
shimizuta | 39:87dcdff27797 | 10 | LineParam line[]{ |
shimizuta | 42:982064594ba6 | 11 | {.time_s = 0, .x_m = x_m, .y_m = y_m, .is_point_to_point = 0}, |
shimizuta | 42:982064594ba6 | 12 | {.time_s = time_s, .x_m = x_m, .y_m = y_m, .is_point_to_point = 0}, |
shimizuta | 39:87dcdff27797 | 13 | }; |
shimizuta | 44:4aac39b8670b | 14 | freeline.SetFreeLinesParam(line, sizeof(line) / sizeof(line[0])); |
shimizuta | 39:87dcdff27797 | 15 | walk.orbit[legnum].Copy(freeline); |
shimizuta | 35:b4e1b8f25cd7 | 16 | } |
shimizuta | 35:b4e1b8f25cd7 | 17 | void SetOneLegTriangleParam(Walk &walk, int legnum, float offset_x_m, float offset_y_m, float stride_m, float height_m, float buffer_height_m, |
shimizuta | 35:b4e1b8f25cd7 | 18 | float stridetime_s, float toptime_s, float buffer_time_s) |
shimizuta | 35:b4e1b8f25cd7 | 19 | { |
shimizuta | 35:b4e1b8f25cd7 | 20 | Orbit triangle(TRIANGLE); |
shimizuta | 35:b4e1b8f25cd7 | 21 | triangle.SetTriangleParam(offset_x_m, offset_y_m, stride_m, height_m, buffer_height_m, |
shimizuta | 35:b4e1b8f25cd7 | 22 | stridetime_s, toptime_s, buffer_time_s); |
shimizuta | 35:b4e1b8f25cd7 | 23 | walk.orbit[legnum].Copy(triangle); |
shimizuta | 35:b4e1b8f25cd7 | 24 | } |
shimizuta | 39:87dcdff27797 | 25 | |
shimizuta | 39:87dcdff27797 | 26 | void SetOneLegFourPointParam(Walk &walk, int legnum, float offset_x_m, float offset_y_m, float stride_m, float height_m, float buffer_height_m, |
shimizuta | 44:4aac39b8670b | 27 | float stridetime_s, float toptime_s, float buffer_time_s) |
shimizuta | 39:87dcdff27797 | 28 | { |
shimizuta | 39:87dcdff27797 | 29 | Orbit four(FOURPOINT); |
shimizuta | 39:87dcdff27797 | 30 | four.SetFourPointParam(offset_x_m, offset_y_m, stride_m, height_m, buffer_height_m, |
shimizuta | 44:4aac39b8670b | 31 | stridetime_s, toptime_s, buffer_time_s); |
shimizuta | 39:87dcdff27797 | 32 | walk.orbit[legnum].Copy(four); |
shimizuta | 39:87dcdff27797 | 33 | } |
shimizuta | 35:b4e1b8f25cd7 | 34 | void SetOneLegFreeLinesParam(Walk &walk, int legnum, LineParam lineparams[], int point_num) |
shimizuta | 35:b4e1b8f25cd7 | 35 | { |
shimizuta | 35:b4e1b8f25cd7 | 36 | Orbit freeline(FREELINES); |
shimizuta | 35:b4e1b8f25cd7 | 37 | freeline.SetFreeLinesParam(lineparams, point_num); |
shimizuta | 35:b4e1b8f25cd7 | 38 | walk.orbit[legnum].Copy(freeline); |
shimizuta | 45:0654364226c9 | 39 | } |
shimizuta | 45:0654364226c9 | 40 | |
shimizuta | 45:0654364226c9 | 41 | void Turn(Walk &walk, int is_turnright, float start_x_m, float start_y_m, |
shimizuta | 45:0654364226c9 | 42 | float stride_m, float height_m, float stridetime_s, float risetime_s) |
shimizuta | 45:0654364226c9 | 43 | { |
shimizuta | 45:0654364226c9 | 44 | LineParam lines_forward[] = { |
shimizuta | 45:0654364226c9 | 45 | {.time_s = 0, .x_m = start_x_m, .y_m = start_y_m}, |
shimizuta | 45:0654364226c9 | 46 | {.time_s = stridetime_s, .x_m = start_x_m - stride_m, .y_m = start_y_m, .is_point_to_point = 0}, //進む |
shimizuta | 45:0654364226c9 | 47 | {.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}, //上げる |
shimizuta | 45:0654364226c9 | 48 | {.time_s = stridetime_s + risetime_s, .x_m = start_x_m, .y_m = start_y_m, .is_point_to_point = 0}, //おろす |
shimizuta | 45:0654364226c9 | 49 | {.time_s = stridetime_s + risetime_s + stridetime_s, .x_m = start_x_m, .y_m = start_y_m, .is_point_to_point = 1}, //着地したまま |
shimizuta | 45:0654364226c9 | 50 | {.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}, // 垂直上げ |
shimizuta | 45:0654364226c9 | 51 | {.time_s = (stridetime_s + risetime_s) * 2, .x_m = start_x_m, .y_m = start_y_m, .is_point_to_point = 0}, //おろす |
shimizuta | 45:0654364226c9 | 52 | }; |
shimizuta | 45:0654364226c9 | 53 | LineParam lines_back[7]; |
shimizuta | 45:0654364226c9 | 54 | for (unsigned int i = 0; i < sizeof(lines_back) / sizeof(lines_back[0]); i++) |
shimizuta | 45:0654364226c9 | 55 | { |
shimizuta | 45:0654364226c9 | 56 | lines_back[i].time_s = lines_forward[i].time_s; |
shimizuta | 45:0654364226c9 | 57 | lines_back[i].x_m = -lines_forward[i].x_m; |
shimizuta | 45:0654364226c9 | 58 | lines_back[i].y_m = lines_forward[i].y_m; |
shimizuta | 45:0654364226c9 | 59 | lines_back[i].is_point_to_point = lines_forward[i].is_point_to_point; |
shimizuta | 45:0654364226c9 | 60 | } |
shimizuta | 45:0654364226c9 | 61 | if (is_turnright) |
shimizuta | 45:0654364226c9 | 62 | { |
shimizuta | 45:0654364226c9 | 63 | SetOneLegFreeLinesParam(walk, LEFT_F, lines_forward, sizeof(lines_forward) / sizeof(lines_forward[0])); |
shimizuta | 45:0654364226c9 | 64 | SetOneLegFreeLinesParam(walk, LEFT_B, lines_forward, sizeof(lines_forward) / sizeof(lines_forward[0])); |
shimizuta | 45:0654364226c9 | 65 | SetOneLegFreeLinesParam(walk, RIGHT_F, lines_back, sizeof(lines_back) / sizeof(lines_back[0])); |
shimizuta | 45:0654364226c9 | 66 | SetOneLegFreeLinesParam(walk, RIGHT_B, lines_back, sizeof(lines_back) / sizeof(lines_back[0])); |
shimizuta | 45:0654364226c9 | 67 | } |
shimizuta | 45:0654364226c9 | 68 | else |
shimizuta | 45:0654364226c9 | 69 | { |
shimizuta | 45:0654364226c9 | 70 | SetOneLegFreeLinesParam(walk, RIGHT_F, lines_forward, sizeof(lines_forward) / sizeof(lines_forward[0])); |
shimizuta | 45:0654364226c9 | 71 | SetOneLegFreeLinesParam(walk, RIGHT_B, lines_forward, sizeof(lines_forward) / sizeof(lines_forward[0])); |
shimizuta | 45:0654364226c9 | 72 | SetOneLegFreeLinesParam(walk, LEFT_F, lines_back, sizeof(lines_back) / sizeof(lines_back[0])); |
shimizuta | 45:0654364226c9 | 73 | SetOneLegFreeLinesParam(walk, LEFT_B, lines_back, sizeof(lines_back) / sizeof(lines_back[0])); |
shimizuta | 45:0654364226c9 | 74 | } |
shimizuta | 45:0654364226c9 | 75 | walk.SetOffsetTime(0.5, 0.75, 0.25, 0); |
shimizuta | 45:0654364226c9 | 76 | } |