岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

Walk/orbit/freeline/freeline.cpp@35:b4e1b8f25cd7, 2019-03-04 (annotated)

Committer:
shimizuta
Date:
Mon Mar 04 09:54:47 2019 +0000
Revision:
35:b4e1b8f25cd7
Child:
41:38d79b6513c0
new MR2;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimizuta 35:b4e1b8f25cd7 1 #include "freeline.h"
shimizuta 35:b4e1b8f25cd7 2 void FreeLines::SetFreeLinesParam(LineParam lineparams[], int point_num)
shimizuta 35:b4e1b8f25cd7 3 {
shimizuta 35:b4e1b8f25cd7 4 point_num_ = point_num;
shimizuta 35:b4e1b8f25cd7 5 for (int i = 0; i < point_num_; i++)
shimizuta 35:b4e1b8f25cd7 6 lineparams_[i] = lineparams[i];
shimizuta 35:b4e1b8f25cd7 7 }
shimizuta 35:b4e1b8f25cd7 8 int FreeLines::GetOrbit(OneLeg &leg, float phasetime_s)
shimizuta 35:b4e1b8f25cd7 9 {
shimizuta 35:b4e1b8f25cd7 10 ;
shimizuta 35:b4e1b8f25cd7 11 //現在の最終到達pointを決定
shimizuta 35:b4e1b8f25cd7 12 int next_point;
shimizuta 35:b4e1b8f25cd7 13 for (next_point = 0; next_point < point_num_ - 1; next_point++) // point_num - 1が配列の最大index.これを超えないようにしている
shimizuta 35:b4e1b8f25cd7 14 {
shimizuta 35:b4e1b8f25cd7 15 if (phasetime_s < lineparams_[next_point].time_s)
shimizuta 35:b4e1b8f25cd7 16 break;
shimizuta 35:b4e1b8f25cd7 17 }
shimizuta 35:b4e1b8f25cd7 18 //x,yを計算
shimizuta 35:b4e1b8f25cd7 19 float x_m, y_m;
shimizuta 35:b4e1b8f25cd7 20 float d_phase = phasetime_s - lineparams_[next_point - 1].time_s;
shimizuta 35:b4e1b8f25cd7 21 float d_time = lineparams_[next_point].time_s - lineparams_[next_point - 1].time_s;
shimizuta 35:b4e1b8f25cd7 22 float phase_rate = d_phase / d_time;
shimizuta 35:b4e1b8f25cd7 23 x_m = lineparams_[next_point - 1].x_m +
shimizuta 35:b4e1b8f25cd7 24 (lineparams_[next_point].x_m - lineparams_[next_point - 1].x_m) * phase_rate;
shimizuta 35:b4e1b8f25cd7 25 y_m = lineparams_[next_point - 1].y_m +
shimizuta 35:b4e1b8f25cd7 26 (lineparams_[next_point].y_m - lineparams_[next_point - 1].y_m) * phase_rate;
shimizuta 35:b4e1b8f25cd7 27 return leg.SetXY_m(x_m, y_m);
shimizuta 35:b4e1b8f25cd7 28 }
shimizuta 35:b4e1b8f25cd7 29 float FreeLines::GetOneWalkTime() //足一周の時間
shimizuta 35:b4e1b8f25cd7 30 {
shimizuta 35:b4e1b8f25cd7 31 return lineparams_[point_num_ - 1].time_s;
shimizuta 35:b4e1b8f25cd7 32 }
shimizuta 35:b4e1b8f25cd7 33 void FreeLines::Copy(const FreeLines &origin)
shimizuta 35:b4e1b8f25cd7 34 {
shimizuta 35:b4e1b8f25cd7 35 point_num_ = origin.point_num_; //pointの数
shimizuta 35:b4e1b8f25cd7 36 for (int i = 0; i < point_num_; i++)
shimizuta 35:b4e1b8f25cd7 37 lineparams_[i] = origin.lineparams_[i];
shimizuta 35:b4e1b8f25cd7 38 }