岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

Walk/change_walk.cpp@50:36741e8ab197, 2019-03-11 (annotated)

Committer:
shimizuta
Date:
Mon Mar 11 10:38:07 2019 +0000
Revision:
50:36741e8ab197
Parent:
45:0654364226c9 
a

Who changed what in which revision?

UserRevisionLine numberNew 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 }