test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
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 }
Generated on Thu Jul 14 2022 13:15:26 by 1.7.2