test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Diff: OneLeg/OneLeg.cpp
- Revision:
- 11:e81425872740
- Child:
- 14:d7cb429946f4
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/OneLeg/OneLeg.cpp Mon Feb 11 12:56:00 2019 +0000 @@ -0,0 +1,58 @@ +#include "OneLeg.h" +#include "mbed.h" +const float M_PI = 3.141592; +//注:未完成。本来ならradから計算したい。今のままだとradが更新されてもx,yが更新されない +float OneLeg::GetX_m() +{ + return x_m_; +} +float OneLeg::GetY_m() +{ + return y_m_; +} +void OneLeg::SetXY_m(float x_m, float y_m) +{ + CalServoRad(x_m, y_m); + if (isnan(GetRad(0)) || isnan(GetRad(1))) + printf("error:(x,y) = (%f,%f) is out of range ", x_m, y_m); + else + { + x_m_ = x_m; + y_m_ = y_m; + } +} + +OneLeg::OneLeg(float between_servo_half_m, + float leglength1, float leglength2) +{ + rad_[0] = 0; + rad_[1] = M_PI; + between_servo_half_m_ = between_servo_half_m; + leglength1_ = leglength1; + leglength2_ = leglength2; +} +void OneLeg::CalServoRad(float x_m, float y_m) +{ + //処理を軽くするために共通部分は先に計算 + float temp_x[] = { + x_m + between_servo_half_m_, + x_m - between_servo_half_m_, + }; + float temp_y2 = y_m * y_m; + float temp_L = leglength1_ * leglength1_ - leglength2_ * leglength2_; + + float r1 = sqrt((temp_x[1]) * (temp_x[1]) + temp_y2); + float r2 = sqrt((temp_x[0]) * (temp_x[0]) + temp_y2); + float targetTheta[] = {atan2(y_m, temp_x[1]) - acos((temp_L + r1 * r1) / (2.0f * r1 * leglength1_)), + atan2(y_m, temp_x[0]) + acos((temp_L + r2 * r2) / (2.0f * r2 * leglength1_))}; + for (size_t i = 0; i < 2; i++) + SetRad(targetTheta[i], i); +} +void OneLeg::SetRad(float rad, int servo_num) +{ + rad_[servo_num] = rad; +} +float OneLeg::GetRad(int servo_num) +{ + return rad_[servo_num]; +}