test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
freeline.cpp
00001 #include "freeline.h" 00002 void FreeLines::SetFreeLinesParam(LineParam lineparams[], int point_num) 00003 { 00004 point_num_ = point_num; 00005 for (int i = 0; i < point_num_; i++) 00006 lineparams_[i] = lineparams[i]; 00007 } 00008 int FreeLines::GetOrbit(OneLeg &leg, float phasetime_s) 00009 { 00010 //現在の最終到達pointを決定 00011 int next_point; 00012 for (next_point = 0; next_point < point_num_ - 1; next_point++) // point_num - 1が配列の最大index.これを超えないようにしている 00013 { 00014 if (phasetime_s < lineparams_[next_point].time_s) 00015 break; 00016 } 00017 float x_m, y_m; 00018 if (lineparams_[next_point].is_point_to_point == 1) 00019 { 00020 x_m = lineparams_[next_point].x_m; 00021 y_m = lineparams_[next_point].y_m; 00022 } 00023 else 00024 { 00025 //x,yを計算 00026 float d_phase = phasetime_s - lineparams_[next_point - 1].time_s; 00027 float d_time = lineparams_[next_point].time_s - lineparams_[next_point - 1].time_s; 00028 float phase_rate = d_phase / d_time; 00029 x_m = lineparams_[next_point - 1].x_m + 00030 (lineparams_[next_point].x_m - lineparams_[next_point - 1].x_m) * phase_rate; 00031 y_m = lineparams_[next_point - 1].y_m + 00032 (lineparams_[next_point].y_m - lineparams_[next_point - 1].y_m) * phase_rate; 00033 } 00034 return leg.SetXY_m(x_m, y_m); 00035 } 00036 float FreeLines::GetOneWalkTime() //足一周の時間 00037 { 00038 return lineparams_[point_num_ - 1].time_s; 00039 } 00040 void FreeLines::Copy(const FreeLines &origin) 00041 { 00042 point_num_ = origin.point_num_; //pointの数 00043 for (int i = 0; i < point_num_; i++) 00044 lineparams_[i] = origin.lineparams_[i]; 00045 }
Generated on Thu Jul 14 2022 13:15:26 by 1.7.2