OneLeg.cpp

00001 #define _USE_MATH_DEFINES
00002 #include "math.h"
00003 #include "OneLeg.h"
00004 #include "pi.h"
00005 #include "stdio.h"
00006 //注：未完成。本来ならradから計算したい。今のままだとradが更新されてもx,yが更新されない
00007 float OneLeg::GetX_m()
00008 {
00009     return x_m_;
00010 }
00011 float OneLeg::GetY_m()
00012 {
00013     return y_m_;
00014 }
00015 OneLeg::OneLeg(float between_servo_half_m, float leglength1[2], float leglength2[2])
00016 {
00017     rad_[0] = 0;
00018     rad_[1] = M_PI;
00019     between_servo_half_m_ = between_servo_half_m;
00020     for (int i = 0; i < 2; i++)
00021     {
00022         leglength1_[i] = leglength1[i];
00023         leglength2_[i] = leglength2[i];
00024     }
00025 }
00026 int OneLeg::SetXY_m(float x_m, float y_m)
00027 {
00028     int is_error = 0;
00029     //対応する角度も同時に計算。
00030     //処理を軽くするために共通部分は先に計算
00031     float temp_x[] = {
00032         x_m + between_servo_half_m_,
00033         x_m - between_servo_half_m_,
00034     };
00035     float temp_y2 = y_m * y_m;
00036     float temp_L[2];
00037     for (int i = 0; i < 2; i++)
00038         temp_L[i] = leglength1_[i] * leglength1_[i] - leglength2_[i] * leglength2_[i];
00039 
00040     float r1 = sqrt((temp_x[1]) * (temp_x[1]) + temp_y2);
00041     float r2 = sqrt((temp_x[0]) * (temp_x[0]) + temp_y2);
00042     float targetTheta[] = {atan2(y_m, temp_x[1]) - acos((temp_L[0] + r1 * r1) / (2.0f * r1 * leglength1_[0])),
00043                            atan2(y_m, temp_x[0]) + acos((temp_L[1] + r2 * r2) / (2.0f * r2 * leglength1_[1]))};
00044     if (isnan(targetTheta[0]) || isnan(targetTheta[1])) //解が出ないときは値を更新しない
00045     {
00046         printf("error:(x,y) = (%f,%f) is out of range\r\n", x_m, y_m);
00047         is_error = 1;
00048     }
00049     else //解が出るならrad, x, y を更新する
00050     {
00051         for (size_t i = 0; i < 2; i++)
00052             SetRad(targetTheta[i], i);
00053         x_m_ = x_m, y_m_ = y_m;
00054     }
00055     return is_error;
00056 }
00057 void OneLeg::SetRad(float rad, int servo_num)
00058 {
00059     rad_[servo_num] = rad;
00060 }
00061 float OneLeg::GetRad(int servo_num)
00062 {
00063     return rad_[servo_num];
00064 }