岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

OneLeg/OneLeg.cpp@19:1adc7302cfd9, 2019-02-15 (annotated)

Committer:
shimizuta
Date:
Fri Feb 15 03:46:52 2019 +0000
Revision:
19:1adc7302cfd9
Parent:
14:d7cb429946f4
Child:
27:79b4b932a6dd
some change

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 14:d7cb429946f4 3 #include "pi.h"
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 19:1adc7302cfd9 13 OneLeg::OneLeg(float between_servo_half_m, float leglength1[2], float leglength2[2])
shimizuta 11:e81425872740 14 {
shimizuta 11:e81425872740 15 rad_[0] = 0;
shimizuta 11:e81425872740 16 rad_[1] = M_PI;
shimizuta 11:e81425872740 17 between_servo_half_m_ = between_servo_half_m;
shimizuta 19:1adc7302cfd9 18 for (int i = 0; i < 2; i++)
shimizuta 19:1adc7302cfd9 19 {
shimizuta 19:1adc7302cfd9 20 leglength1_[i] = leglength1[i];
shimizuta 19:1adc7302cfd9 21 leglength2_[i] = leglength2[i];
shimizuta 19:1adc7302cfd9 22 }
shimizuta 11:e81425872740 23 }
shimizuta 19:1adc7302cfd9 24 void OneLeg::SetXY_m(float x_m, float y_m)
shimizuta 11:e81425872740 25 {
shimizuta 19:1adc7302cfd9 26 //対応する角度も同時に計算。
shimizuta 11:e81425872740 27 //処理を軽くするために共通部分は先に計算
shimizuta 11:e81425872740 28 float temp_x[] = {
shimizuta 11:e81425872740 29 x_m + between_servo_half_m_,
shimizuta 11:e81425872740 30 x_m - between_servo_half_m_,
shimizuta 11:e81425872740 31 };
shimizuta 11:e81425872740 32 float temp_y2 = y_m * y_m;
shimizuta 19:1adc7302cfd9 33 float temp_L[2];
shimizuta 19:1adc7302cfd9 34 for (int i = 0; i < 2; i++)
shimizuta 19:1adc7302cfd9 35 temp_L[i]= leglength1_[i] * leglength1_[i] - leglength2_[i] * leglength2_[i];
shimizuta 11:e81425872740 36
shimizuta 11:e81425872740 37 float r1 = sqrt((temp_x[1]) * (temp_x[1]) + temp_y2);
shimizuta 11:e81425872740 38 float r2 = sqrt((temp_x[0]) * (temp_x[0]) + temp_y2);
shimizuta 19:1adc7302cfd9 39 float targetTheta[] = {atan2(y_m, temp_x[1]) - acos((temp_L[0] + r1 * r1) / (2.0f * r1 * leglength1_[0])),
shimizuta 19:1adc7302cfd9 40 atan2(y_m, temp_x[0]) + acos((temp_L[1] + r2 * r2) / (2.0f * r2 * leglength1_[1]))};
shimizuta 19:1adc7302cfd9 41 if (isnan(targetTheta[0]) || isnan(targetTheta[1])) //解が出ないときは値を更新しない
shimizuta 19:1adc7302cfd9 42 {
shimizuta 19:1adc7302cfd9 43 printf("error:(x,y) = (%f,%f) is out of range ", x_m, y_m);
shimizuta 19:1adc7302cfd9 44 }
shimizuta 19:1adc7302cfd9 45 else //解が出るならrad, x, y を更新する
shimizuta 19:1adc7302cfd9 46 {
shimizuta 19:1adc7302cfd9 47 for (size_t i = 0; i < 2; i++)
shimizuta 19:1adc7302cfd9 48 SetRad(targetTheta[i], i);
shimizuta 19:1adc7302cfd9 49 x_m_ = x_m, y_m_ = y_m;
shimizuta 19:1adc7302cfd9 50 }
shimizuta 11:e81425872740 51 }
shimizuta 11:e81425872740 52 void OneLeg::SetRad(float rad, int servo_num)
shimizuta 11:e81425872740 53 {
shimizuta 11:e81425872740 54 rad_[servo_num] = rad;
shimizuta 11:e81425872740 55 }
shimizuta 11:e81425872740 56 float OneLeg::GetRad(int servo_num)
shimizuta 11:e81425872740 57 {
shimizuta 11:e81425872740 58 return rad_[servo_num];
shimizuta 11:e81425872740 59 }