test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Walk/Walk.cpp
- Committer:
- shimizuta
- Date:
- 2019-03-11
- Revision:
- 50:36741e8ab197
- Parent:
- 44:4aac39b8670b
File content as of revision 50:36741e8ab197:
#include "Walk.h" #include <stdio.h> Walk::Walk(OneLeg legs[4]) { for (int i = 0; i < 4; i++) { offset_multi[i] = 0; leg[i] = legs[i]; } } float Walk::calctime_s_; int Walk::Cal4LegsPosi(OneLeg legs[4]) //失敗したら1を返す。成功なら0 { int is_out = 0; for (int i = 0; i < 4; i++) { float one_walk_time = orbit[i].GetOneWalkTime(); phasetime_s_[i] += calctime_s_; while (phasetime_s_[i] > one_walk_time) phasetime_s_[i] -= one_walk_time; if (orbit[i].GetOrbit(legs[i], phasetime_s_[i]) == 1) { printf("error:leg %d in Cal4LegsPosi\r\n", i); is_out = 1; } } return is_out; } void Walk::SetOffsetTime(float offset_multi0, float offset_multi1, float offset_multi2, float offset_multi3) { offset_multi[0] = offset_multi0; offset_multi[1] = offset_multi1; offset_multi[2] = offset_multi2; offset_multi[3] = offset_multi3; } //軌道がリンク定義外になっていないかチェック。reutn 0:ok 1:out int Walk::CheckOrbit() { for (int i = 0; i < 4; i++) { //軌道が値域の外に出ないか計算で確かめる float one_walk_time = orbit[i].GetOneWalkTime(); float step = calctime_s_ * 0.1; for (float j = 0; j < one_walk_time; j += step) { if (orbit[i].GetOrbit(leg[i], j) == 1) { printf("error:leg %d, time %f\r\n", i, j); return 1; //解が出ないときは1を返す } } } ResetPhase(); return 0; } void Walk::Copy(Walk &walk) { for (int i = 0; i < 4; i++) { orbit[i].Copy(walk.orbit[i]); phasetime_s_[i] = walk.phasetime_s_[i]; offset_multi[i] = walk.offset_multi[i]; leg[i] = walk.leg[i]; } } void Walk::ResetPhase(){ for (int i = 0; i < 4; i++) phasetime_s_[i] = orbit[i].GetOneWalkTime() * offset_multi[i]; }