Diff: main.cpp

Revision:

25:5d0977d2d79d

Parent:

20:70cc6083e9c7

diff -r 70cc6083e9c7 -r 5d0977d2d79d main.cpp
--- a/main.cpp	Fri Feb 15 06:46:39 2019 +0000
+++ b/main.cpp	Wed Feb 20 01:00:23 2019 +0000
@@ -20,13 +20,15 @@
 const double kServoValToDegree = 270.0 / (11500 - 3500);
 //サーボの正負と座標系の正負の補正.足で一セット。
 const int kServoSign[2][2] = {{
-                                  1,
-                                  -1,
-                              },
-                              {
-                                  -1,
-                                  1,
-                              }};
+        1,
+        -1,
+        
+    },
+    {
+        -1,
+        1,
+    }
+};
 //欲しい座標系0度でのサーボのICSマネージャーの値
 const double kOriginDegree[2][2] = {
     {
@@ -65,14 +67,17 @@
 }
 int main()
 {
-    printf("When you push any key, this robot starts.\r\n");
+    printf("------------------------New Session Start------------------------.\r\n");
+    printf("Ready to stand up. Press any key.\r\n");
     while (pc.readable() == 0) //キーボード押したらスタート
         ;
-    printf("stand up for 1 s\r\n");
-
     //stand時の足の軌道設定.
     Orbit stand_orbit[4];
-    float stand_offset_y_m = 0.2;
+    
+    //機体高さ
+        float stand_offset_y_m = 0.18;
+    
+    
     stand_orbit[0].SetStandParam(stand_offset_y_m);
     for (int i = 0; i < 4; i++) //全部足を同じにしてる
         stand_orbit[i] = stand_orbit[0];
@@ -89,38 +94,49 @@
     Walk stand(stand_orbit, stand_offset_time_s, kCycleTime_s);
     float dist_m = 10; //収束判定用。現在は回すループの数
     //動作開始
+
     Move(stand, leg, dist_m);
+
     wait(1);
+    printf("Ready to walk. Press [a] to start.\r\n");
+    while(1) {
+        char c = pc.getc();
+        if(c == 'a') {printf("Waliking Session Start.\r\n");break;}
+}
+
+
 
-    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;
-    //曲げてみる
-    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;
-    //軌道の作成
-    //代入用の軌道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);
-    //4足の軌道と位相ずれOffsetTime_sをまとめる
-    float offset_time_s[4] = {
-        0,
-        orbit[0].GetOneWalkTime() * 0.5,
-        orbit[0].GetOneWalkTime() * 0.5,
-        orbit[0].GetOneWalkTime() * 0,
-    };
-    //4つの足のorbit, 位相を代入して歩行パターンを作成している
-    //このインスタンスはlegそれぞれの計算を行う役割を担う
-    //orbitはここでしか使わないあくまで鍵のような扱い
-    Walk walk(orbit, offset_time_s, kCycleTime_s);
-    dist_m = 500000; //収束判定用。取り敢えず今はループ数。
-    //動く
-    Move(walk, leg, dist_m);
-    DEBUG("program end\r\n");
+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;
+//曲げてみる
+float stridetime_s = 0.3, risetime_s = 0.15,
+      stride_r_m = 0.1, stride_l_m = 0.1, height_m = 0.03,
+      offset_x_m = 0
+      , offset_y_m = stand_offset_y_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);
+//4足の軌道と位相ずれOffsetTime_sをまとめる
+float offset_time_s[4] = {
+    0,
+    orbit[0].GetOneWalkTime() * 0.5,
+    orbit[0].GetOneWalkTime() * 0.5,
+    orbit[0].GetOneWalkTime() * 0,
+};
+//4つの足のorbit, 位相を代入して歩行パターンを作成している
+//このインスタンスはlegそれぞれの計算を行う役割を担う
+//orbitはここでしか使わないあくまで鍵のような扱い
+Walk walk(orbit, offset_time_s, kCycleTime_s);
+dist_m = 500000; //収束判定用。取り敢えず今はループ数。
+//動く
+
+Move(walk, leg, dist_m);
+DEBUG("program end\r\n");
 }
 
 //到達判定が来ない間同じ歩行方法でループ
@@ -130,8 +146,7 @@
     timer.start();
     int is_arrived = 0;
     int count = 0;
-    while (is_arrived == 0)
-    {
+    while (is_arrived == 0) {
         float time_s = timer.read();
         //注:未実装。到着したかの判定.LRFからのデータが必要.今は取り敢えずループ回数で判断
         is_arrived = IsArrived(count, (int)dist_m);
@@ -153,8 +168,8 @@
         float rest_time_s = WalkWay.cycletime_s_ - (timer.read() - time_s);
         if (rest_time_s > 0)
             wait(rest_time_s);
-        else
-        { //計算周期が達成できないときはDEBUGで知らせるだけ。動きはする。
+        else {
+            //計算周期が達成できないときはDEBUGで知らせるだけ。動きはする。
             DEBUG("error: rest_time_s = %f in Move()\r\n", rest_time_s);
             led[0] = 1;
         }