test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Walk/orbit/freeline/freeline.cpp
- Committer:
- shimizuta
- Date:
- 2019-03-05
- Revision:
- 42:982064594ba6
- Parent:
- 41:38d79b6513c0
- Child:
- 43:2ed84f3558c1
File content as of revision 42:982064594ba6:
#include "freeline.h" void FreeLines::SetFreeLinesParam(LineParam lineparams[], int point_num) { point_num_ = point_num; for (int i = 0; i < point_num_; i++) lineparams_[i] = lineparams[i]; } int FreeLines::GetOrbit(OneLeg &leg, float phasetime_s) { //現在の最終到達pointを決定 int next_point; for (next_point = 0; next_point < point_num_ - 1; next_point++) // point_num - 1が配列の最大index.これを超えないようにしている { if (phasetime_s < lineparams_[next_point].time_s) break; } float x_m, y_m; if (lineparams_[next_point - 1].is_point_to_point == 1) { x_m = lineparams_[next_point].x_m; y_m = lineparams_[next_point].y_m; } else { //x,yを計算 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 FreeLines::GetOneWalkTime() //足一周の時間 { return lineparams_[point_num_ - 1].time_s; } void FreeLines::Copy(const FreeLines &origin) { point_num_ = origin.point_num_; //pointの数 for (int i = 0; i < point_num_; i++) lineparams_[i] = origin.lineparams_[i]; }