Diff: OneLeg/OneLeg.cpp

Revision:

27:79b4b932a6dd

Parent:

19:1adc7302cfd9

Child:

43:2ed84f3558c1

--- a/OneLeg/OneLeg.cpp	Fri Feb 22 05:20:28 2019 +0000
+++ b/OneLeg/OneLeg.cpp	Wed Feb 27 01:24:00 2019 +0000
@@ -1,6 +1,8 @@
+#define _USE_MATH_DEFINES
+#include "math.h"
 #include "OneLeg.h"
-#include "mbed.h"
 #include "pi.h"
+#include "stdio.h"
 //注：未完成。本来ならradから計算したい。今のままだとradが更新されてもx,yが更新されない
 float OneLeg::GetX_m()
 {
@@ -21,8 +23,9 @@
         leglength2_[i] = leglength2[i];
     }
 }
-void OneLeg::SetXY_m(float x_m, float y_m)
+int OneLeg::SetXY_m(float x_m, float y_m)
 {
+    int is_error = 0;
     //対応する角度も同時に計算。
     //処理を軽くするために共通部分は先に計算
     float temp_x[] = {
@@ -32,7 +35,7 @@
     float temp_y2 = y_m * y_m;
     float temp_L[2];
     for (int i = 0; i < 2; i++)
-        temp_L[i]= leglength1_[i] * leglength1_[i] - leglength2_[i] * leglength2_[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);
@@ -41,6 +44,7 @@
     if (isnan(targetTheta[0]) || isnan(targetTheta[1])) //解が出ないときは値を更新しない
     {
         printf("error:(x,y) = (%f,%f) is out of range ", x_m, y_m);
+        is_error = 1;
     }
     else //解が出るならrad, x, y を更新する
     {
@@ -48,6 +52,7 @@
             SetRad(targetTheta[i], i);
         x_m_ = x_m, y_m_ = y_m;
     }
+    return is_error;
 }
 void OneLeg::SetRad(float rad, int servo_num)
 {