Diff: OneLeg/OneLeg.cpp

Revision:

19:1adc7302cfd9

Parent:

14:d7cb429946f4

Child:

27:79b4b932a6dd

diff -r 0033ef1814ba -r 1adc7302cfd9 OneLeg/OneLeg.cpp
--- a/OneLeg/OneLeg.cpp	Thu Feb 14 09:04:25 2019 +0000
+++ b/OneLeg/OneLeg.cpp	Fri Feb 15 03:46:52 2019 +0000
@@ -10,43 +10,44 @@
 {
     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)
+OneLeg::OneLeg(float between_servo_half_m, float leglength1[2], float leglength2[2])
 {
     rad_[0] = 0;
     rad_[1] = M_PI;
     between_servo_half_m_ = between_servo_half_m;
-    leglength1_ = leglength1;
-    leglength2_ = leglength2;
+    for (int i = 0; i < 2; i++)
+    {
+        leglength1_[i] = leglength1[i];
+        leglength2_[i] = leglength2[i];
+    }
 }
-void OneLeg::CalServoRad(float x_m, float y_m)
+void OneLeg::SetXY_m(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 temp_L[2];
+    for (int i = 0; i < 2; i++)
+        temp_L[i]= leglength1_[i] * leglength1_[i] - leglength2_[i] * leglength2_[i];
 
     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);
+    float targetTheta[] = {atan2(y_m, temp_x[1]) - acos((temp_L[0] + r1 * r1) / (2.0f * r1 * leglength1_[0])),
+                           atan2(y_m, temp_x[0]) + acos((temp_L[1] + r2 * r2) / (2.0f * r2 * leglength1_[1]))};
+    if (isnan(targetTheta[0]) || isnan(targetTheta[1])) //解が出ないときは値を更新しない
+    {
+        printf("error:(x,y) = (%f,%f) is out of range ", x_m, y_m);
+    }
+    else //解が出るならrad, x, y を更新する
+    {
+        for (size_t i = 0; i < 2; i++)
+            SetRad(targetTheta[i], i);
+        x_m_ = x_m, y_m_ = y_m;
+    }
 }
 void OneLeg::SetRad(float rad, int servo_num)
 {