岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

OneLeg/OneLeg.cpp@11:e81425872740, 2019-02-11 (annotated)

Committer:
shimizuta
Date:
Mon Feb 11 12:56:00 2019 +0000
Revision:
11:e81425872740
Child:
14:d7cb429946f4
ver2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimizuta 11:e81425872740 1 #include "OneLeg.h"
shimizuta 11:e81425872740 2 #include "mbed.h"
shimizuta 11:e81425872740 3 const float M_PI = 3.141592;
shimizuta 11:e81425872740 4 //注：未完成。本来ならradから計算したい。今のままだとradが更新されてもx,yが更新されない
shimizuta 11:e81425872740 5 float OneLeg::GetX_m()
shimizuta 11:e81425872740 6 {
shimizuta 11:e81425872740 7 return x_m_;
shimizuta 11:e81425872740 8 }
shimizuta 11:e81425872740 9 float OneLeg::GetY_m()
shimizuta 11:e81425872740 10 {
shimizuta 11:e81425872740 11 return y_m_;
shimizuta 11:e81425872740 12 }
shimizuta 11:e81425872740 13 void OneLeg::SetXY_m(float x_m, float y_m)
shimizuta 11:e81425872740 14 {
shimizuta 11:e81425872740 15 CalServoRad(x_m, y_m);
shimizuta 11:e81425872740 16 if (isnan(GetRad(0)) || isnan(GetRad(1)))
shimizuta 11:e81425872740 17 printf("error:(x,y) = (%f,%f) is out of range ", x_m, y_m);
shimizuta 11:e81425872740 18 else
shimizuta 11:e81425872740 19 {
shimizuta 11:e81425872740 20 x_m_ = x_m;
shimizuta 11:e81425872740 21 y_m_ = y_m;
shimizuta 11:e81425872740 22 }
shimizuta 11:e81425872740 23 }
shimizuta 11:e81425872740 24
shimizuta 11:e81425872740 25 OneLeg::OneLeg(float between_servo_half_m,
shimizuta 11:e81425872740 26 float leglength1, float leglength2)
shimizuta 11:e81425872740 27 {
shimizuta 11:e81425872740 28 rad_[0] = 0;
shimizuta 11:e81425872740 29 rad_[1] = M_PI;
shimizuta 11:e81425872740 30 between_servo_half_m_ = between_servo_half_m;
shimizuta 11:e81425872740 31 leglength1_ = leglength1;
shimizuta 11:e81425872740 32 leglength2_ = leglength2;
shimizuta 11:e81425872740 33 }
shimizuta 11:e81425872740 34 void OneLeg::CalServoRad(float x_m, float y_m)
shimizuta 11:e81425872740 35 {
shimizuta 11:e81425872740 36 //処理を軽くするために共通部分は先に計算
shimizuta 11:e81425872740 37 float temp_x[] = {
shimizuta 11:e81425872740 38 x_m + between_servo_half_m_,
shimizuta 11:e81425872740 39 x_m - between_servo_half_m_,
shimizuta 11:e81425872740 40 };
shimizuta 11:e81425872740 41 float temp_y2 = y_m * y_m;
shimizuta 11:e81425872740 42 float temp_L = leglength1_ * leglength1_ - leglength2_ * leglength2_;
shimizuta 11:e81425872740 43
shimizuta 11:e81425872740 44 float r1 = sqrt((temp_x[1]) * (temp_x[1]) + temp_y2);
shimizuta 11:e81425872740 45 float r2 = sqrt((temp_x[0]) * (temp_x[0]) + temp_y2);
shimizuta 11:e81425872740 46 float targetTheta[] = {atan2(y_m, temp_x[1]) - acos((temp_L + r1 * r1) / (2.0f * r1 * leglength1_)),
shimizuta 11:e81425872740 47 atan2(y_m, temp_x[0]) + acos((temp_L + r2 * r2) / (2.0f * r2 * leglength1_))};
shimizuta 11:e81425872740 48 for (size_t i = 0; i < 2; i++)
shimizuta 11:e81425872740 49 SetRad(targetTheta[i], i);
shimizuta 11:e81425872740 50 }
shimizuta 11:e81425872740 51 void OneLeg::SetRad(float rad, int servo_num)
shimizuta 11:e81425872740 52 {
shimizuta 11:e81425872740 53 rad_[servo_num] = rad;
shimizuta 11:e81425872740 54 }
shimizuta 11:e81425872740 55 float OneLeg::GetRad(int servo_num)
shimizuta 11:e81425872740 56 {
shimizuta 11:e81425872740 57 return rad_[servo_num];
shimizuta 11:e81425872740 58 }