Diff: main.cpp

Revision:

21:61971fc18b90

Parent:

20:70cc6083e9c7

Child:

27:79b4b932a6dd

--- a/main.cpp	Fri Feb 15 06:46:39 2019 +0000
+++ b/main.cpp	Sat Feb 16 05:00:19 2019 +0000
@@ -54,7 +54,7 @@
 DigitalOut led[4] = {DigitalOut(LED1), DigitalOut(LED2), DigitalOut(LED3), DigitalOut(LED4)};
 
 const float kRadToDegree = 180.0 / M_PI;
-void Move(Walk WalkWay, OneLeg (&leg)[4], float dist_m);
+void Move(Walk walkway, OneLeg (&leg)[4], float dist_m);
 void MoveServo(OneLeg leg, int legnum, int servo_id);
 int IsArrived(int count, int finish)
 {
@@ -72,8 +72,8 @@
 
     //stand時の足の軌道設定.
     Orbit stand_orbit[4];
-    float stand_offset_y_m = 0.2;
-    stand_orbit[0].SetStandParam(stand_offset_y_m);
+    float stand_ground_m = 0.2;
+    stand_orbit[0].SetStandParam(stand_ground_m);
     for (int i = 0; i < 4; i++) //全部足を同じにしてる
         stand_orbit[i] = stand_orbit[0];
     //4足の位相ずれOffsetTime_sをまとめる
@@ -94,18 +94,18 @@
 
     DEBUG("move start\r\n");
     //取り敢えず歩行したパラメータ
-    //stridetime_s = 0.5, risetime_s = 0.3,stride_m= 0.12f, height_m = 0.03f, offset_x_m  = 0, offset_y_m = 0.16f;
+    //stridetime_s = 0.5, risetime_s = 0.3,stride_m= 0.12f, height_m = 0.03f, ellipse_center_x_m  = 0, ellipse_center_y_m = 0.16f;
     //曲げてみる
     float stridetime_s = 2, risetime_s = 0.5,
-          stride_r_m = 0.1, stride_l_m = 0.05, height_m = 0.03,
-          offset_x_m = -0.05, offset_y_m = stand_offset_y_m;
+          stride_r_m = 0.1, stride_l_m = 0.05, height_m = 0.03, ground_m = stand_ground_m,
+          ellipse_center_x_m = -0.05, ellipse_center_y_m = ground_m;
     //軌道の作成
     //代入用の軌道SetStraightParam関数を用いると真っ直ぐ進む前提となる
     Orbit orbit[4];
-    orbit[0].SetStraightParam(stridetime_s, risetime_s, stride_r_m, height_m, offset_x_m, offset_y_m);
-    orbit[1].SetStraightParam(stridetime_s, risetime_s, stride_r_m, height_m, 0, offset_y_m);
-    orbit[2].SetStraightParam(stridetime_s, risetime_s, stride_l_m, height_m, offset_x_m, offset_y_m);
-    orbit[3].SetStraightParam(stridetime_s, risetime_s, stride_l_m, height_m, 0, offset_y_m);
+    orbit[0].SetStraightParam(stridetime_s, risetime_s, stride_r_m, height_m, ground_m, ellipse_center_x_m, ellipse_center_y_m);
+    orbit[1].SetStraightParam(stridetime_s, risetime_s, stride_r_m, height_m, ground_m, 0, ellipse_center_y_m);
+    orbit[2].SetStraightParam(stridetime_s, risetime_s, stride_l_m, height_m, ground_m, ellipse_center_x_m, ellipse_center_y_m);
+    orbit[3].SetStraightParam(stridetime_s, risetime_s, stride_l_m, height_m, ground_m, 0, ellipse_center_y_m);
     //4足の軌道と位相ずれOffsetTime_sをまとめる
     float offset_time_s[4] = {
         0,
@@ -123,8 +123,8 @@
     DEBUG("program end\r\n");
 }
 
-//到達判定が来ない間同じ歩行方法でループ
-void Move(Walk WalkWay, OneLeg (&leg)[4], float dist_m)
+//到達判定が来ない間,同じ歩行方法でループ.関数に入る前の足の位置を一切考慮しない点に注意
+void Move(Walk walkway, OneLeg (&leg)[4], float dist_m)
 {
     timer.reset();
     timer.start();
@@ -137,7 +137,7 @@
         is_arrived = IsArrived(count, (int)dist_m);
         ++count;
         //4本の足それぞれの足先サーボ角度更新
-        WalkWay.Cal4LegsPosi(leg);
+        walkway.Cal4LegsPosi(leg);
 #ifdef USE_CAN
         //slave_mbed分の足の目標位置を送信
         SendRad(leg[2], leg[3]);
@@ -149,8 +149,8 @@
         MoveServo(leg[0], 0, 1);
         MoveServo(leg[1], 1, 1);
         DEBUG("%f, %f, %f, %f\r\n", leg[2].GetRad(0), leg[2].GetRad(1), leg[3].GetRad(0), leg[3].GetRad(1));
-        //計算周期がWalkWay.cycletime_s_になるようwait
-        float rest_time_s = WalkWay.cycletime_s_ - (timer.read() - time_s);
+        //計算周期がwalkway.cycletime_s_になるようwait
+        float rest_time_s = walkway.cycletime_s_ - (timer.read() - time_s);
         if (rest_time_s > 0)
             wait(rest_time_s);
         else