change_walk.cpp

00001 /*walk設定用関数.
00002 param walk:結果を入れる箱
00003 各パラメータの意味はOrbitクラスの基底クラスまで遡る必要があるので注意。
00004 */
00005 #include "change_walk.h"
00006 #include "OverCome.h"
00007 void SetOneLegStandParam(Walk &walk, int legnum, float x_m, float y_m, float time_s)
00008 {
00009     Orbit freeline(FREELINES);
00010     LineParam line[]{
00011         {.time_s = 0, .x_m = x_m, .y_m = y_m, .is_point_to_point = 0},
00012         {.time_s = time_s, .x_m = x_m, .y_m = y_m, .is_point_to_point = 0},
00013     };
00014     freeline.SetFreeLinesParam(line, sizeof(line) / sizeof(line[0]));
00015     walk.orbit[legnum].Copy(freeline);
00016 }
00017 void SetOneLegTriangleParam(Walk &walk, int legnum, float offset_x_m, float offset_y_m, float stride_m, float height_m, float buffer_height_m,
00018                             float stridetime_s, float toptime_s, float buffer_time_s)
00019 {
00020     Orbit triangle(TRIANGLE);
00021     triangle.SetTriangleParam(offset_x_m, offset_y_m, stride_m, height_m, buffer_height_m,
00022                               stridetime_s, toptime_s, buffer_time_s);
00023     walk.orbit[legnum].Copy(triangle);
00024 }
00025 
00026 void SetOneLegFourPointParam(Walk &walk, int legnum, float offset_x_m, float offset_y_m, float stride_m, float height_m, float buffer_height_m,
00027                              float stridetime_s, float toptime_s, float buffer_time_s)
00028 {
00029     Orbit four(FOURPOINT);
00030     four.SetFourPointParam(offset_x_m, offset_y_m, stride_m, height_m, buffer_height_m,
00031                            stridetime_s, toptime_s, buffer_time_s);
00032     walk.orbit[legnum].Copy(four);
00033 }
00034 void SetOneLegFreeLinesParam(Walk &walk, int legnum, LineParam lineparams[], int point_num)
00035 {
00036     Orbit freeline(FREELINES);
00037     freeline.SetFreeLinesParam(lineparams, point_num);
00038     walk.orbit[legnum].Copy(freeline);
00039 }
00040 
00041 void Turn(Walk &walk, int is_turnright, float start_x_m, float start_y_m,
00042           float stride_m, float height_m, float stridetime_s, float risetime_s)
00043 {
00044     LineParam lines_forward[] = {
00045         {.time_s = 0, .x_m = start_x_m, .y_m = start_y_m},
00046         {.time_s = stridetime_s, .x_m = start_x_m - stride_m, .y_m = start_y_m, .is_point_to_point = 0},                                                 //進む
00047         {.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},                             //上げる
00048         {.time_s = stridetime_s + risetime_s, .x_m = start_x_m, .y_m = start_y_m, .is_point_to_point = 0},                                               //おろす
00049         {.time_s = stridetime_s + risetime_s + stridetime_s, .x_m = start_x_m, .y_m = start_y_m, .is_point_to_point = 1},                                //着地したまま
00050         {.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}, //   垂直上げ
00051         {.time_s = (stridetime_s + risetime_s) * 2, .x_m = start_x_m, .y_m = start_y_m, .is_point_to_point = 0},                                         //おろす
00052     };
00053     LineParam lines_back[7];
00054     for (unsigned int i = 0; i < sizeof(lines_back) / sizeof(lines_back[0]); i++)
00055     {
00056         lines_back[i].time_s = lines_forward[i].time_s;
00057         lines_back[i].x_m = -lines_forward[i].x_m;
00058         lines_back[i].y_m = lines_forward[i].y_m;
00059         lines_back[i].is_point_to_point = lines_forward[i].is_point_to_point;
00060     }
00061     if (is_turnright)
00062     {
00063         SetOneLegFreeLinesParam(walk, LEFT_F, lines_forward, sizeof(lines_forward) / sizeof(lines_forward[0]));
00064         SetOneLegFreeLinesParam(walk, LEFT_B, lines_forward, sizeof(lines_forward) / sizeof(lines_forward[0]));
00065         SetOneLegFreeLinesParam(walk, RIGHT_F, lines_back, sizeof(lines_back) / sizeof(lines_back[0]));
00066         SetOneLegFreeLinesParam(walk, RIGHT_B, lines_back, sizeof(lines_back) / sizeof(lines_back[0]));
00067     }
00068     else
00069     {
00070         SetOneLegFreeLinesParam(walk, RIGHT_F, lines_forward, sizeof(lines_forward) / sizeof(lines_forward[0]));
00071         SetOneLegFreeLinesParam(walk, RIGHT_B, lines_forward, sizeof(lines_forward) / sizeof(lines_forward[0]));
00072         SetOneLegFreeLinesParam(walk, LEFT_F, lines_back, sizeof(lines_back) / sizeof(lines_back[0]));
00073         SetOneLegFreeLinesParam(walk, LEFT_B, lines_back, sizeof(lines_back) / sizeof(lines_back[0]));
00074     }
00075     walk.SetOffsetTime(0.5, 0.75, 0.25, 0);
00076 }