test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Walk/OverCome/OverCome.cpp
- Committer:
- shimizuta
- Date:
- 2019-03-04
- Revision:
- 35:b4e1b8f25cd7
- Child:
- 43:2ed84f3558c1
File content as of revision 35:b4e1b8f25cd7:
#include "OverCome.h" #include "change_walk.h" 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], OneLeg legs[4]) :walk(legs) { 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++) SetOneLegFreeLinesParam(walk, 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(); }