serbo4soku

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

Committer:
shimizuta
Date:
Mon Feb 11 04:07:51 2019 +0000
Revision:
10:7a340c52e270
Parent:
9:905f93247688
printf debug finish

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimizuta 9:905f93247688 1 #include "MoveServoOfLeg.h"
shimizuta 9:905f93247688 2 #include "mbed.h"
shimizuta 9:905f93247688 3 const float M_PI = 3.141592;
shimizuta 9:905f93247688 4 const float kRadToDegree = 180.0 / M_PI;
shimizuta 9:905f93247688 5 MoveServoOfLeg::MoveServoOfLeg(PinName pin_serial_tx, PinName pin_serial_rx,
shimizuta 9:905f93247688 6 float between_servo_half_m,
shimizuta 9:905f93247688 7 float leglength1, float leglength2)
shimizuta 9:905f93247688 8 : servo_(pin_serial_tx, pin_serial_rx)
shimizuta 9:905f93247688 9 {
shimizuta 9:905f93247688 10 rad_[0] = 0;
shimizuta 9:905f93247688 11 rad_[1] = M_PI;
shimizuta 9:905f93247688 12 between_servo_half_m_ = between_servo_half_m;
shimizuta 9:905f93247688 13 leglength1_ = leglength1;
shimizuta 9:905f93247688 14 leglength2_ = leglength2;
shimizuta 9:905f93247688 15 };
shimizuta 9:905f93247688 16 void MoveServoOfLeg::MoveServo(int servo_num)
shimizuta 9:905f93247688 17 {
shimizuta 9:905f93247688 18 float degree = GetRad(servo_num) * kRadToDegree;
shimizuta 9:905f93247688 19 //servo1は反転させる
shimizuta 9:905f93247688 20 if (servo_num == 0)
shimizuta 9:905f93247688 21 degree += 90;
shimizuta 9:905f93247688 22 else
shimizuta 9:905f93247688 23 degree = 270 - degree;
shimizuta 9:905f93247688 24 servo_.set_degree(servo_num, degree);
shimizuta 9:905f93247688 25 }
shimizuta 9:905f93247688 26 void MoveServoOfLeg::CalServoRad(float x_m, float y_m)
shimizuta 9:905f93247688 27 {
shimizuta 9:905f93247688 28 //処理を軽くするために共通部分は先に計算
shimizuta 9:905f93247688 29 float temp_x[] = {
shimizuta 9:905f93247688 30 x_m + between_servo_half_m_,
shimizuta 9:905f93247688 31 x_m - between_servo_half_m_,
shimizuta 9:905f93247688 32 };
shimizuta 9:905f93247688 33 float temp_y2 = y_m * y_m;
shimizuta 9:905f93247688 34 float temp_L = leglength1_ * leglength1_ - leglength2_ * leglength2_;
shimizuta 9:905f93247688 35
shimizuta 9:905f93247688 36 float r1 = sqrt((temp_x[1]) * (temp_x[1]) + temp_y2);
shimizuta 9:905f93247688 37 float r2 = sqrt((temp_x[0]) * (temp_x[0]) + temp_y2);
shimizuta 9:905f93247688 38 float targetTheta[] = {atan2(y_m, temp_x[1]) - acos((temp_L + r1 * r1) / (2.0f * r1 * leglength1_)),
shimizuta 9:905f93247688 39 atan2(y_m, temp_x[0]) + acos((temp_L + r2 * r2) / (2.0f * r2 * leglength1_))};
shimizuta 9:905f93247688 40 for (size_t i = 0; i < 2; i++)
shimizuta 9:905f93247688 41 SetRad(targetTheta[i], i);
shimizuta 9:905f93247688 42 }
shimizuta 9:905f93247688 43 void MoveServoOfLeg::SetRad(float rad, int servo_num)
shimizuta 9:905f93247688 44 {
shimizuta 9:905f93247688 45 rad_[servo_num] = rad;
shimizuta 9:905f93247688 46 }
shimizuta 9:905f93247688 47 float MoveServoOfLeg::GetRad(int servo_num)
shimizuta 9:905f93247688 48 {
shimizuta 9:905f93247688 49 return rad_[servo_num];
shimizuta 9:905f93247688 50 }
shimizuta 9:905f93247688 51