Walk.cpp

00001 #include "Walk.h"
00002 #include <stdio.h>
00003 Walk::Walk(OneLeg legs[4])
00004 {
00005     for (int i = 0; i < 4; i++)
00006     {
00007         offset_multi[i] = 0;
00008         leg[i] = legs[i];
00009     }
00010 }
00011 float Walk::calctime_s_;
00012 int Walk::Cal4LegsPosi(OneLeg legs[4]) //失敗したら1を返す。成功なら0
00013 {
00014     int is_out = 0;
00015     for (int i = 0; i < 4; i++)
00016     {
00017         float one_walk_time = orbit[i].GetOneWalkTime();
00018         phasetime_s_[i] += calctime_s_;
00019         while (phasetime_s_[i] > one_walk_time)
00020             phasetime_s_[i] -= one_walk_time;
00021         if (orbit[i].GetOrbit(legs[i], phasetime_s_[i]) == 1)
00022         {
00023             printf("error:leg %d in Cal4LegsPosi\r\n", i);
00024             is_out = 1;
00025         }
00026     }
00027     return is_out;
00028 }
00029 void Walk::SetOffsetTime(float offset_multi0, float offset_multi1, float offset_multi2, float offset_multi3)
00030 {
00031     offset_multi[0] = offset_multi0;
00032     offset_multi[1] = offset_multi1;
00033     offset_multi[2] = offset_multi2;
00034     offset_multi[3] = offset_multi3;
00035 }
00036 //軌道がリンク定義外になっていないかチェック。reutn 0:ok 1:out
00037 int Walk::CheckOrbit()
00038 {
00039     for (int i = 0; i < 4; i++)
00040     {
00041         //軌道が値域の外に出ないか計算で確かめる
00042         float one_walk_time = orbit[i].GetOneWalkTime();
00043         float step = calctime_s_ * 0.1;
00044         for (float j = 0; j < one_walk_time; j += step)
00045         {
00046             if (orbit[i].GetOrbit(leg[i], j) == 1)
00047             {
00048                 printf("error:leg %d, time %f\r\n", i, j);
00049                 return 1; //解が出ないときは1を返す
00050             }
00051         }
00052     }
00053     ResetPhase();
00054     return 0;
00055 }
00056 void Walk::Copy(Walk &walk)
00057 {
00058     for (int i = 0; i < 4; i++)
00059     {
00060         orbit[i].Copy(walk.orbit[i]);
00061         phasetime_s_[i] = walk.phasetime_s_[i];
00062         offset_multi[i] = walk.offset_multi[i];
00063         leg[i] = walk.leg[i];
00064     }
00065 }
00066 void Walk::ResetPhase(){
00067     for (int i = 0; i < 4; i++)
00068         phasetime_s_[i] = orbit[i].GetOneWalkTime() * offset_multi[i];
00069 }