岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

Walk/orbit/freeline/freeline.cpp@41:38d79b6513c0, 2019-03-05 (annotated)

Committer:
yuto17320508
Date:
Tue Mar 05 01:45:03 2019 +0000
Revision:
41:38d79b6513c0
Parent:
35:b4e1b8f25cd7
Child:
42:982064594ba6
devide PointToPoint and LineKeep;

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;
yuto17320508 41:38d79b6513c0 23
yuto17320508 41:38d79b6513c0 24 if(lineparams_[next_point - 1].line_state == 1)
yuto17320508 41:38d79b6513c0 25 {
yuto17320508 41:38d79b6513c0 26 x_m = lineparams_[next_point].x_m;
yuto17320508 41:38d79b6513c0 27 y_m = lineparams_[next_point].y_m;
yuto17320508 41:38d79b6513c0 28 }
yuto17320508 41:38d79b6513c0 29 else
yuto17320508 41:38d79b6513c0 30 {
yuto17320508 41:38d79b6513c0 31 x_m = lineparams_[next_point - 1].x_m +
yuto17320508 41:38d79b6513c0 32 (lineparams_[next_point].x_m - lineparams_[next_point - 1].x_m) * phase_rate;
yuto17320508 41:38d79b6513c0 33 y_m = lineparams_[next_point - 1].y_m +
yuto17320508 41:38d79b6513c0 34 (lineparams_[next_point].y_m - lineparams_[next_point - 1].y_m) * phase_rate;
yuto17320508 41:38d79b6513c0 35 }
yuto17320508 41:38d79b6513c0 36
shimizuta 35:b4e1b8f25cd7 37 return leg.SetXY_m(x_m, y_m);
shimizuta 35:b4e1b8f25cd7 38 }
shimizuta 35:b4e1b8f25cd7 39 float FreeLines::GetOneWalkTime() //足一周の時間
shimizuta 35:b4e1b8f25cd7 40 {
shimizuta 35:b4e1b8f25cd7 41 return lineparams_[point_num_ - 1].time_s;
shimizuta 35:b4e1b8f25cd7 42 }
shimizuta 35:b4e1b8f25cd7 43 void FreeLines::Copy(const FreeLines &origin)
shimizuta 35:b4e1b8f25cd7 44 {
shimizuta 35:b4e1b8f25cd7 45 point_num_ = origin.point_num_; //pointの数
shimizuta 35:b4e1b8f25cd7 46 for (int i = 0; i < point_num_; i++)
shimizuta 35:b4e1b8f25cd7 47 lineparams_[i] = origin.lineparams_[i];
shimizuta 35:b4e1b8f25cd7 48 }