OverCome.cpp

00001 #include "OverCome.h"
00002 #include "change_walk.h"
00003 #include "math.h"
00004 OverCome::OverCome(float start_x_m[4], float start_y_m[4],
00005                    float d_x_m, float goal_y_m[4], float height_m[4], float gravity_dist[4],
00006                    OneLeg legs[4], float raise_offset_x_m[4], float d_time, float d_time_slow) : walk(legs)
00007 {
00008     for (int i = 0; i < 4; i++)
00009     {
00010         start_x_m_[i] = start_x_m[i]; //足のスタートx
00011         start_y_m_[i] = start_y_m[i]; //足のスタートy
00012         goal_y_m_[i] = goal_y_m[i];   //目標地点までのy
00013         gravity_dist_[i] = gravity_dist[i];
00014         height_m_[i] = height_m[i];
00015         raise_offset_x_m_[i] = raise_offset_x_m[i];
00016     }
00017     d_x_m_ = d_x_m; //目標地点までのx
00018 
00019     d_time_ = d_time;//0 .2; //各動きの時間
00020     d_time_slow_ = d_time_slow;// 0.3;
00021 
00022     next_point_ = 0; //次のparamのindex
00023     GetLine();
00024     for (int i = 0; i < 4; i++)
00025         SetOneLegFreeLinesParam(walk, i, legs_[i], next_point_);
00026 }
00027 
00028 void OverCome::Rise(int legnum)
00029 {
00030     for (int i = 0; i < 4; i++)
00031     {
00032         legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_;
00033         legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m;
00034         legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m;
00035         legs_[i][next_point_].is_point_to_point = 0; //直線軌道を維持
00036     }
00037     legs_[legnum][next_point_].x_m += raise_offset_x_m_[legnum];
00038     legs_[legnum][next_point_].y_m -= height_m_[legnum];
00039     next_point_++;
00040 }
00041 void OverCome::Forward(int legnum)
00042 {
00043     for (int i = 0; i < 4; i++)
00044     {
00045         legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_;
00046         legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m;
00047         legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m;
00048         legs_[i][next_point_].is_point_to_point = 0; //直線軌道を維持
00049     }
00050     legs_[legnum][next_point_].x_m += d_x_m_ - raise_offset_x_m_[legnum];
00051     next_point_++;
00052 }
00053 void OverCome::Land(int legnum)
00054 {
00055     for (int i = 0; i < 4; i++)
00056     {
00057         legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_slow_;
00058         legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m;
00059         legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m;
00060         legs_[i][next_point_].is_point_to_point = 0; //ゆっくり着地するために直線軌道を維持
00061     }
00062     legs_[legnum][next_point_].y_m = goal_y_m_[legnum]; //着地
00063     next_point_++;
00064 }
00065 void OverCome::Step(int legnum)
00066 {
00067     Rise(legnum);
00068     Forward(legnum);
00069     Land(legnum);
00070 }
00071 //重心移動、param direct:動く正負。+-1を入れる
00072 void OverCome::GravityMove(int legnum)
00073 {
00074     if (fabs(gravity_dist_[legnum]) < 0.01)
00075         return;
00076     for (int i = 0; i < 4; i++)
00077     {
00078         legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_;
00079         legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m - gravity_dist_[legnum];
00080         legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m;
00081         legs_[i][next_point_].is_point_to_point = 0; //ゆっくり移動するために直線軌道を維持
00082     }
00083     ++next_point_;
00084 }
00085 void OverCome::StartPoint()
00086 {
00087     for (int i = 0; i < 4; i++)
00088     {
00089         legs_[i][next_point_].time_s = 0;
00090         legs_[i][next_point_].x_m = start_x_m_[i];
00091         legs_[i][next_point_].y_m = start_y_m_[i];
00092     }
00093     ++next_point_;
00094 }
00095 void OverCome::GoalPoint()
00096 {
00097     for (int i = 0; i < 4; i++)
00098     {
00099         legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_;
00100         legs_[i][next_point_].x_m = start_x_m_[i];
00101         legs_[i][next_point_].y_m = goal_y_m_[i];
00102         legs_[i][next_point_].is_point_to_point = 0; //ゆっくり移動するために直線軌道を維持
00103     }
00104     ++next_point_;
00105 }
00106 void OverCome::GetLine()
00107 {
00108     StartPoint(); //スタート時点
00109     GravityMove(RIGHT_F);
00110     Step(RIGHT_F);
00111     GravityMove(LEFT_B);
00112     Step(LEFT_B);
00113     GravityMove(LEFT_F);
00114     Step(LEFT_F);
00115     GravityMove(RIGHT_B);
00116     Step(RIGHT_B);
00117     // GoalPoint();
00118 }