test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Diff: Walk/Walk.cpp
- Revision:
- 34:89d701e15cdf
- Parent:
- 32:dc684a0b8448
- Child:
- 35:b4e1b8f25cd7
--- a/Walk/Walk.cpp Thu Feb 28 09:39:02 2019 +0000 +++ b/Walk/Walk.cpp Fri Mar 01 12:07:23 2019 +0000 @@ -3,7 +3,7 @@ #include <stdio.h> #include "stdlib.h" #include "Walk.h" - #include "pi.h" +#include "pi.h" //Orbitは足毎の軌道をあらわす。 const float kGravity = 9.8; @@ -81,7 +81,7 @@ { //軌道が値域の外に出ないか計算で確かめる float one_walk_time = GetOneWalkTime(); - int step = one_walk_time / calctime_s_ * 2; + float step = calctime_s_ * 0.1; for (float j = 0; j < one_walk_time; j += step) { if (orbit_[i].GetOrbit(templateleg, j) == 1) @@ -159,9 +159,13 @@ buffer_time_s_ = buffer_time_s; //頂点から一時停止点までの時間. //事前に計算しておく - top_x_m_ = offset_x_m_ + stride_m * 0.5 - height_m * reverse_tanbeta_; + CalOtherParam(); +} +void TriangleOrbit::CalOtherParam() +{ + top_x_m_ = offset_x_m_ + stride_m_ * 0.5 - height_m_ * reverse_tanbeta_; top_y_m_ = -height_m_ + offset_y_m_; - buffer_x_m_ = offset_x_m_ + stride_m * 0.5 - buffer_height_m_ * reverse_tanbeta_; + buffer_x_m_ = offset_x_m_ + stride_m_ * 0.5 - buffer_height_m_ * reverse_tanbeta_; buffer_y_m_ = -buffer_height_m_ + offset_y_m_; } //足一周の時間 @@ -190,7 +194,7 @@ { ///////////x,yを計算 float s0 = stride_m_ * 0.5 + offset_x_m_; - float s1 = offset_x_m_ -stride_m_ * 0.5; + float s1 = offset_x_m_ - stride_m_ * 0.5; float g_h = sqrtf(kGravity / offset_y_m_); float t = phasetime_s / stridetime_s_; float denominator = expf(g_h) - expf(-g_h); //分母 @@ -220,17 +224,12 @@ buffer_height_m_ = val; break; } + CalOtherParam(); } void FreeLineOrbit::SetFreeLinesParam(LineParam lineparams[], int point_num) { point_num_ = point_num; - if (point_num_ == 1) - { - printf("error:point_num = 1. You should put >=2 to make line."); - while (1) - ; - } for (int i = 0; i < point_num_; i++) lineparams_[i] = lineparams[i]; } @@ -238,38 +237,148 @@ { ; //現在の最終到達pointを決定 - int arrived_point; - float sum_time = 0; - for (arrived_point = 0; arrived_point < point_num_ - 2; arrived_point++) //arrived_point = point_num - 1(これが配列の最大index)の状態で終わらないように-2している + int next_point; + for (next_point = 0; next_point < point_num_ - 1; next_point++) // point_num - 1が配列の最大index.これを超えないようにしている { - sum_time += lineparams_[arrived_point].time_s; - if (phasetime_s < sum_time) + if (phasetime_s < lineparams_[next_point].time_s) break; } //x,yを計算 float x_m, y_m; - x_m = lineparams_[arrived_point].x_m + - (lineparams_[arrived_point + 1].x_m - lineparams_[arrived_point].x_m) * - (phasetime_s - sum_time + lineparams_[arrived_point].time_s) / lineparams_[arrived_point].time_s; - y_m = lineparams_[arrived_point].y_m + - (lineparams_[arrived_point + 1].y_m - lineparams_[arrived_point].y_m) * - (phasetime_s - sum_time + lineparams_[arrived_point].time_s) / lineparams_[arrived_point].time_s; + float d_phase = phasetime_s - lineparams_[next_point - 1].time_s; + float d_time = lineparams_[next_point].time_s - lineparams_[next_point - 1].time_s; + float phase_rate = d_phase / d_time; + x_m = lineparams_[next_point - 1].x_m + + (lineparams_[next_point].x_m - lineparams_[next_point - 1].x_m) * phase_rate; + y_m = lineparams_[next_point - 1].y_m + + (lineparams_[next_point].y_m - lineparams_[next_point - 1].y_m) * phase_rate; return leg.SetXY_m(x_m, y_m); } float FreeLineOrbit::GetOneWalkTime() //足一周の時間 { - float sum_time = 0; - for (int i = 0; i < point_num_; i++) - sum_time += lineparams_[i].time_s; - return sum_time; + return lineparams_[point_num_ - 1].time_s; } void FreeLineOrbit::SetStandParam(float x_m, float y_m, float time_s) { point_num_ = 2; lineparams_[0].x_m = x_m; lineparams_[0].y_m = y_m; - lineparams_[0].time_s = time_s; + lineparams_[0].time_s = 0; lineparams_[1].x_m = x_m; lineparams_[1].y_m = y_m; - lineparams_[1].time_s = 0; + lineparams_[1].time_s = time_s; +} +OverCome::OverCome(float start_x_m[4], float start_y_m[4], + float d_x_m, float goal_y_m[4], float height_m[4], float gravity_dist[4]) +{ + for (int i = 0; i < 4; i++) + { + start_x_m_[i] = start_x_m[i]; //足のスタートx + start_y_m_[i] = start_y_m[i]; //足のスタートy + goal_y_m_[i] = goal_y_m[i]; //目標地点までのy + gravity_dist_[i] = gravity_dist[i]; + height_m_[i] = height_m[i]; + } + d_x_m_ = d_x_m; //目標地点までのx + + d_time_ = 0.5; //各動きの時間 + d_time_slow_ = 0.5; + next_point_ = 0; //次のparamのindex + GetLine(); + for (int i = 0; i < 4; i++) + walk.SetOneLegFreeLinesParam(i, legs_[i], next_point_); +} + +void OverCome::Rise(int legnum) +{ + for (int i = 0; i < 4; i++) + { + legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_; + legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m; + legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m; + } + legs_[legnum][next_point_].y_m -= height_m_[legnum]; + next_point_++; +} +void OverCome::Forward(int legnum) +{ + for (int i = 0; i < 4; i++) + { + legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_; + legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m; + legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m; + } + legs_[legnum][next_point_].x_m += d_x_m_; + next_point_++; +} +void OverCome::Land(int legnum) +{ + for (int i = 0; i < 4; i++) + { + legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_slow_; + legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m; + legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m; + } + legs_[legnum][next_point_].y_m = goal_y_m_[legnum]; //着地 + next_point_++; } +void OverCome::Step(int legnum) +{ + Rise(legnum); + Forward(legnum); + Land(legnum); +} +//重心移動、param direct:動く正負。+-1を入れる +void OverCome::GravityMove(int legnum) +{ + for (int i = 0; i < 4; i++) + { + legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_; + legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m - gravity_dist_[legnum]; + legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m; + } + ++next_point_; +} +void OverCome::StartPoint() +{ + for (int i = 0; i < 4; i++) + { + legs_[i][next_point_].time_s = 0; + legs_[i][next_point_].x_m = start_x_m_[i]; + legs_[i][next_point_].y_m = start_y_m_[i]; + } + ++next_point_; +} +void OverCome::GoalPoint() +{ + for (int i = 0; i < 4; i++) + { + legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_; + legs_[i][next_point_].x_m = start_x_m_[i]; + legs_[i][next_point_].y_m = goal_y_m_[i]; + } + ++next_point_; +} +void OverCome::GetLine() +{ + StartPoint(); //スタート時点 + GravityMove(RIGHT_F); + Step(RIGHT_F); + GravityMove(LEFT_B); + Step(LEFT_B); + GravityMove(LEFT_F); + Step(LEFT_F); + GravityMove(RIGHT_B); + Step(RIGHT_B); + GoalPoint(); + /* GravityMove(1); + Step(RIGHT_B); + GravityMove(-1); + Step(RIGHT_F); + GravityMove(1); + Step(LEFT_B); + GravityMove(-1); + Step(LEFT_F); + GravityMove(1); +*/ +} \ No newline at end of file