岡本 陸
test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Walk/orbit/freeline/freeline.cpp@43:2ed84f3558c1, 2019-03-06 (annotated)
- Committer:
- shimizuta
- Date:
- Wed Mar 06 12:13:46 2019 +0000
- Revision:
- 43:2ed84f3558c1
- Parent:
- 42:982064594ba6
moved 3/06
Who changed what in which revision?
| User | Revision | Line number | New 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 | //現在の最終到達pointを決定 |
| shimizuta | 35:b4e1b8f25cd7 | 11 | int next_point; |
| shimizuta | 35:b4e1b8f25cd7 | 12 | for (next_point = 0; next_point < point_num_ - 1; next_point++) // point_num - 1が配列の最大index.これを超えないようにしている |
| shimizuta | 35:b4e1b8f25cd7 | 13 | { |
| shimizuta | 35:b4e1b8f25cd7 | 14 | if (phasetime_s < lineparams_[next_point].time_s) |
| shimizuta | 35:b4e1b8f25cd7 | 15 | break; |
| shimizuta | 35:b4e1b8f25cd7 | 16 | } |
| shimizuta | 35:b4e1b8f25cd7 | 17 | float x_m, y_m; |
| shimizuta | 43:2ed84f3558c1 | 18 | if (lineparams_[next_point].is_point_to_point == 1) |
| yuto17320508 | 41:38d79b6513c0 | 19 | { |
| yuto17320508 | 41:38d79b6513c0 | 20 | x_m = lineparams_[next_point].x_m; |
| yuto17320508 | 41:38d79b6513c0 | 21 | y_m = lineparams_[next_point].y_m; |
| yuto17320508 | 41:38d79b6513c0 | 22 | } |
| yuto17320508 | 41:38d79b6513c0 | 23 | else |
| yuto17320508 | 41:38d79b6513c0 | 24 | { |
| shimizuta | 42:982064594ba6 | 25 | //x,yを計算 |
| shimizuta | 42:982064594ba6 | 26 | float d_phase = phasetime_s - lineparams_[next_point - 1].time_s; |
| shimizuta | 42:982064594ba6 | 27 | float d_time = lineparams_[next_point].time_s - lineparams_[next_point - 1].time_s; |
| shimizuta | 42:982064594ba6 | 28 | float phase_rate = d_phase / d_time; |
| yuto17320508 | 41:38d79b6513c0 | 29 | x_m = lineparams_[next_point - 1].x_m + |
| yuto17320508 | 41:38d79b6513c0 | 30 | (lineparams_[next_point].x_m - lineparams_[next_point - 1].x_m) * phase_rate; |
| yuto17320508 | 41:38d79b6513c0 | 31 | y_m = lineparams_[next_point - 1].y_m + |
| yuto17320508 | 41:38d79b6513c0 | 32 | (lineparams_[next_point].y_m - lineparams_[next_point - 1].y_m) * phase_rate; |
| yuto17320508 | 41:38d79b6513c0 | 33 | } |
| shimizuta | 35:b4e1b8f25cd7 | 34 | return leg.SetXY_m(x_m, y_m); |
| shimizuta | 35:b4e1b8f25cd7 | 35 | } |
| shimizuta | 35:b4e1b8f25cd7 | 36 | float FreeLines::GetOneWalkTime() //足一周の時間 |
| shimizuta | 35:b4e1b8f25cd7 | 37 | { |
| shimizuta | 35:b4e1b8f25cd7 | 38 | return lineparams_[point_num_ - 1].time_s; |
| shimizuta | 35:b4e1b8f25cd7 | 39 | } |
| shimizuta | 35:b4e1b8f25cd7 | 40 | void FreeLines::Copy(const FreeLines &origin) |
| shimizuta | 35:b4e1b8f25cd7 | 41 | { |
| shimizuta | 42:982064594ba6 | 42 | point_num_ = origin.point_num_; //pointの数 |
| shimizuta | 35:b4e1b8f25cd7 | 43 | for (int i = 0; i < point_num_; i++) |
| shimizuta | 35:b4e1b8f25cd7 | 44 | lineparams_[i] = origin.lineparams_[i]; |
| shimizuta | 35:b4e1b8f25cd7 | 45 | } |