test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Walk/Walk.cpp@50:36741e8ab197, 2019-03-11 (annotated)
- Committer:
- shimizuta
- Date:
- Mon Mar 11 10:38:07 2019 +0000
- Revision:
- 50:36741e8ab197
- Parent:
- 44:4aac39b8670b
a
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shimizuta | 35:b4e1b8f25cd7 | 1 | #include "Walk.h" |
shimizuta | 27:79b4b932a6dd | 2 | #include <stdio.h> |
shimizuta | 35:b4e1b8f25cd7 | 3 | Walk::Walk(OneLeg legs[4]) |
shimizuta | 11:e81425872740 | 4 | { |
shimizuta | 44:4aac39b8670b | 5 | for (int i = 0; i < 4; i++) |
shimizuta | 44:4aac39b8670b | 6 | { |
shimizuta | 35:b4e1b8f25cd7 | 7 | offset_multi[i] = 0; |
shimizuta | 35:b4e1b8f25cd7 | 8 | leg[i] = legs[i]; |
shimizuta | 27:79b4b932a6dd | 9 | } |
shimizuta | 27:79b4b932a6dd | 10 | } |
shimizuta | 27:79b4b932a6dd | 11 | float Walk::calctime_s_; |
shimizuta | 35:b4e1b8f25cd7 | 12 | int Walk::Cal4LegsPosi(OneLeg legs[4]) //失敗したら1を返す。成功なら0 |
shimizuta | 11:e81425872740 | 13 | { |
shimizuta | 29:7d8b8011a88d | 14 | int is_out = 0; |
shimizuta | 11:e81425872740 | 15 | for (int i = 0; i < 4; i++) |
shimizuta | 11:e81425872740 | 16 | { |
shimizuta | 35:b4e1b8f25cd7 | 17 | float one_walk_time = orbit[i].GetOneWalkTime(); |
shimizuta | 27:79b4b932a6dd | 18 | phasetime_s_[i] += calctime_s_; |
shimizuta | 27:79b4b932a6dd | 19 | while (phasetime_s_[i] > one_walk_time) |
shimizuta | 27:79b4b932a6dd | 20 | phasetime_s_[i] -= one_walk_time; |
shimizuta | 35:b4e1b8f25cd7 | 21 | if (orbit[i].GetOrbit(legs[i], phasetime_s_[i]) == 1) |
shimizuta | 29:7d8b8011a88d | 22 | { |
shimizuta | 29:7d8b8011a88d | 23 | printf("error:leg %d in Cal4LegsPosi\r\n", i); |
shimizuta | 29:7d8b8011a88d | 24 | is_out = 1; |
shimizuta | 29:7d8b8011a88d | 25 | } |
shimizuta | 11:e81425872740 | 26 | } |
shimizuta | 29:7d8b8011a88d | 27 | return is_out; |
shimizuta | 11:e81425872740 | 28 | } |
shimizuta | 29:7d8b8011a88d | 29 | void Walk::SetOffsetTime(float offset_multi0, float offset_multi1, float offset_multi2, float offset_multi3) |
shimizuta | 11:e81425872740 | 30 | { |
shimizuta | 35:b4e1b8f25cd7 | 31 | offset_multi[0] = offset_multi0; |
shimizuta | 35:b4e1b8f25cd7 | 32 | offset_multi[1] = offset_multi1; |
shimizuta | 35:b4e1b8f25cd7 | 33 | offset_multi[2] = offset_multi2; |
shimizuta | 35:b4e1b8f25cd7 | 34 | offset_multi[3] = offset_multi3; |
shimizuta | 27:79b4b932a6dd | 35 | } |
shimizuta | 27:79b4b932a6dd | 36 | //軌道がリンク定義外になっていないかチェック。reutn 0:ok 1:out |
shimizuta | 35:b4e1b8f25cd7 | 37 | int Walk::CheckOrbit() |
shimizuta | 27:79b4b932a6dd | 38 | { |
shimizuta | 11:e81425872740 | 39 | for (int i = 0; i < 4; i++) |
shimizuta | 11:e81425872740 | 40 | { |
shimizuta | 27:79b4b932a6dd | 41 | //軌道が値域の外に出ないか計算で確かめる |
shimizuta | 35:b4e1b8f25cd7 | 42 | float one_walk_time = orbit[i].GetOneWalkTime(); |
shimizuta | 34:89d701e15cdf | 43 | float step = calctime_s_ * 0.1; |
shimizuta | 27:79b4b932a6dd | 44 | for (float j = 0; j < one_walk_time; j += step) |
shimizuta | 27:79b4b932a6dd | 45 | { |
shimizuta | 35:b4e1b8f25cd7 | 46 | if (orbit[i].GetOrbit(leg[i], j) == 1) |
shimizuta | 29:7d8b8011a88d | 47 | { |
shimizuta | 43:2ed84f3558c1 | 48 | printf("error:leg %d, time %f\r\n", i, j); |
shimizuta | 27:79b4b932a6dd | 49 | return 1; //解が出ないときは1を返す |
shimizuta | 29:7d8b8011a88d | 50 | } |
shimizuta | 27:79b4b932a6dd | 51 | } |
shimizuta | 11:e81425872740 | 52 | } |
shimizuta | 44:4aac39b8670b | 53 | ResetPhase(); |
shimizuta | 27:79b4b932a6dd | 54 | return 0; |
shimizuta | 11:e81425872740 | 55 | } |
shimizuta | 35:b4e1b8f25cd7 | 56 | void Walk::Copy(Walk &walk) |
shimizuta | 32:dc684a0b8448 | 57 | { |
shimizuta | 32:dc684a0b8448 | 58 | for (int i = 0; i < 4; i++) |
shimizuta | 44:4aac39b8670b | 59 | { |
shimizuta | 44:4aac39b8670b | 60 | orbit[i].Copy(walk.orbit[i]); |
shimizuta | 35:b4e1b8f25cd7 | 61 | phasetime_s_[i] = walk.phasetime_s_[i]; |
shimizuta | 35:b4e1b8f25cd7 | 62 | offset_multi[i] = walk.offset_multi[i]; |
shimizuta | 35:b4e1b8f25cd7 | 63 | leg[i] = walk.leg[i]; |
shimizuta | 44:4aac39b8670b | 64 | } |
shimizuta | 44:4aac39b8670b | 65 | } |
shimizuta | 44:4aac39b8670b | 66 | void Walk::ResetPhase(){ |
shimizuta | 44:4aac39b8670b | 67 | for (int i = 0; i < 4; i++) |
shimizuta | 44:4aac39b8670b | 68 | phasetime_s_[i] = orbit[i].GetOneWalkTime() * offset_multi[i]; |
shimizuta | 34:89d701e15cdf | 69 | } |