serbo4soku

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

Revision:
9:905f93247688
Child:
10:7a340c52e270
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MoveServoOfLeg/MoveServoOfLeg.cpp	Mon Feb 11 01:56:49 2019 +0000
@@ -0,0 +1,54 @@
+#include "MoveServoOfLeg.h"
+#include "mbed.h"
+const float M_PI = 3.141592;
+const float kRadToDegree = 180.0 / M_PI;
+MoveServoOfLeg::MoveServoOfLeg(PinName pin_serial_tx, PinName pin_serial_rx,
+                               float between_servo_half_m,
+                               float leglength1, float leglength2)
+    : servo_(pin_serial_tx, pin_serial_rx)
+{
+    rad_[0] = 0;
+    rad_[1] = M_PI;
+    between_servo_half_m_ = between_servo_half_m;
+    leglength1_ = leglength1;
+    leglength2_ = leglength2;
+};
+void MoveServoOfLeg::MoveServo(int servo_num)
+{
+    float degree = GetRad(servo_num) * kRadToDegree;
+    //servo1は反転させる
+    if (servo_num == 0)
+        degree += 90;
+    else
+        degree = 270 - degree;
+    servo_.set_degree(servo_num, degree);
+}
+void MoveServoOfLeg::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 MoveServoOfLeg::SetRad(float rad, int servo_num)
+{
+    rad_[servo_num] = rad;
+}
+float MoveServoOfLeg::GetRad(int servo_num)
+{
+    return rad_[servo_num];
+}
+
+
+
+