岡本 陸 / Mbed 2 deprecated 0311

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?

UserRevisionLine numberNew 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 }