岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

Walk/Walk.cpp@35:b4e1b8f25cd7, 2019-03-04 (annotated)

Committer:
shimizuta
Date:
Mon Mar 04 09:54:47 2019 +0000
Revision:
35:b4e1b8f25cd7
Parent:
34:89d701e15cdf
Child:
43:2ed84f3558c1
new MR2;

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 35:b4e1b8f25cd7 5 for (int i = 0; i < 4; i++){
shimizuta 35:b4e1b8f25cd7 6 offset_multi[i] = 0;
shimizuta 35:b4e1b8f25cd7 7 leg[i] = legs[i];
shimizuta 27:79b4b932a6dd 8 }
shimizuta 27:79b4b932a6dd 9 }
shimizuta 27:79b4b932a6dd 10 float Walk::calctime_s_;
shimizuta 35:b4e1b8f25cd7 11 int Walk::Cal4LegsPosi(OneLeg legs[4]) //失敗したら1を返す。成功なら0
shimizuta 11:e81425872740 12 {
shimizuta 29:7d8b8011a88d 13 int is_out = 0;
shimizuta 11:e81425872740 14 for (int i = 0; i < 4; i++)
shimizuta 11:e81425872740 15 {
shimizuta 35:b4e1b8f25cd7 16 float one_walk_time = orbit[i].GetOneWalkTime();
shimizuta 27:79b4b932a6dd 17 phasetime_s_[i] += calctime_s_;
shimizuta 27:79b4b932a6dd 18 while (phasetime_s_[i] > one_walk_time)
shimizuta 27:79b4b932a6dd 19 phasetime_s_[i] -= one_walk_time;
shimizuta 35:b4e1b8f25cd7 20 if (orbit[i].GetOrbit(legs[i], phasetime_s_[i]) == 1)
shimizuta 29:7d8b8011a88d 21 {
shimizuta 29:7d8b8011a88d 22 printf("error:leg %d in Cal4LegsPosi\r\n", i);
shimizuta 29:7d8b8011a88d 23 is_out = 1;
shimizuta 29:7d8b8011a88d 24 }
shimizuta 11:e81425872740 25 }
shimizuta 29:7d8b8011a88d 26 return is_out;
shimizuta 11:e81425872740 27 }
shimizuta 29:7d8b8011a88d 28 void Walk::SetOffsetTime(float offset_multi0, float offset_multi1, float offset_multi2, float offset_multi3)
shimizuta 11:e81425872740 29 {
shimizuta 35:b4e1b8f25cd7 30 offset_multi[0] = offset_multi0;
shimizuta 35:b4e1b8f25cd7 31 offset_multi[1] = offset_multi1;
shimizuta 35:b4e1b8f25cd7 32 offset_multi[2] = offset_multi2;
shimizuta 35:b4e1b8f25cd7 33 offset_multi[3] = offset_multi3;
shimizuta 27:79b4b932a6dd 34 }
shimizuta 27:79b4b932a6dd 35 //軌道がリンク定義外になっていないかチェック。reutn 0:ok 1:out
shimizuta 35:b4e1b8f25cd7 36 int Walk::CheckOrbit()
shimizuta 27:79b4b932a6dd 37 {
shimizuta 11:e81425872740 38 for (int i = 0; i < 4; i++)
shimizuta 11:e81425872740 39 {
shimizuta 27:79b4b932a6dd 40 //軌道が値域の外に出ないか計算で確かめる
shimizuta 35:b4e1b8f25cd7 41 float one_walk_time = orbit[i].GetOneWalkTime();
shimizuta 34:89d701e15cdf 42 float step = calctime_s_ * 0.1;
shimizuta 27:79b4b932a6dd 43 for (float j = 0; j < one_walk_time; j += step)
shimizuta 27:79b4b932a6dd 44 {
shimizuta 35:b4e1b8f25cd7 45 if (orbit[i].GetOrbit(leg[i], j) == 1)
shimizuta 29:7d8b8011a88d 46 {
shimizuta 29:7d8b8011a88d 47 printf("error:leg %d, time %f", i, j);
shimizuta 27:79b4b932a6dd 48 return 1; //解が出ないときは1を返す
shimizuta 29:7d8b8011a88d 49 }
shimizuta 27:79b4b932a6dd 50 }
shimizuta 35:b4e1b8f25cd7 51 phasetime_s_[i] = one_walk_time * offset_multi[i];
shimizuta 11:e81425872740 52 }
shimizuta 27:79b4b932a6dd 53 return 0;
shimizuta 11:e81425872740 54 }
shimizuta 35:b4e1b8f25cd7 55 void Walk::Copy(Walk &walk)
shimizuta 32:dc684a0b8448 56 {
shimizuta 32:dc684a0b8448 57 for (int i = 0; i < 4; i++)
shimizuta 35:b4e1b8f25cd7 58 {
shimizuta 35:b4e1b8f25cd7 59 orbit[i].Copy(walk.orbit[i]);
shimizuta 35:b4e1b8f25cd7 60 phasetime_s_[i] = walk.phasetime_s_[i];
shimizuta 35:b4e1b8f25cd7 61 offset_multi[i] = walk.offset_multi[i];
shimizuta 35:b4e1b8f25cd7 62 leg[i] = walk.leg[i];
shimizuta 35:b4e1b8f25cd7 63 }
shimizuta 34:89d701e15cdf 64 }