岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

OneLeg/OneLeg.cpp@43:2ed84f3558c1, 2019-03-06 (annotated)

Committer:
shimizuta
Date:
Wed Mar 06 12:13:46 2019 +0000
Revision:
43:2ed84f3558c1
Parent:
27:79b4b932a6dd 
moved 3/06

Who changed what in which revision?

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