Diff: main.cpp

Revision:

18:0033ef1814ba

Parent:

17:ca18c5980a34

Child:

19:1adc7302cfd9

--- a/main.cpp	Wed Feb 13 07:49:24 2019 +0000
+++ b/main.cpp	Thu Feb 14 09:04:25 2019 +0000
@@ -2,18 +2,19 @@
 #include "mbed.h"
 #include "pinnames.h"
 #include "KondoServo.h"
+//#define DEBUG_ON//デバッグ用。使わないときはコメントアウト
 #include "debug.h"
 #include "pi.h"
 #include "can.h"
 #include "OneLeg.h" ///足先の座標を保存するクラス。x,yやサーボの角度の保存、サーボの駆動も行う。他の足を考慮した処理は別のクラスに任せる。
 #include "Walk.h"   //歩き方に関するファイル
-//#define USE_CAN//can通信するならdefine.しないなら切らないとエラー出る
+#define USE_CAN//can通信するならdefine.しないなら切らないとエラー出る
 
 ////////////調整すべきパラメータ.全てここに集めた。
 const float kCycleTime_s = 0.03f;              //計算周期
 const float kBetweenServoHalf_m = 0.03f * 0.5; //サーボ間の距離の半分
 const float kLegLength1 = 0.1f;
-const float kLegLength2 = 0.2f;
+const float kLegLength2 = 0.2452f+0.0025f;//0.22529+0.0025
 //サーボの設定
 const int kServoSpan_ms = 10; //サーボの送信間隔
 const double kServoValToDegree = 270.0 / (11500 - 3500);
@@ -23,18 +24,18 @@
                                   -1,
                               },
                               {
+                                  -1,
                                   1,
-                                  -1,
                               }}; 
 //欲しい座標系0度でのサーボのICSマネージャーの値
 const double kOriginDegree[2][2] = {
     {
-        (7374 - 3500) * kServoValToDegree,
-        (7458 - 3500) * kServoValToDegree + 180,
+        (7199 - 3500) * kServoValToDegree,
+        (6600 - 3500) * kServoValToDegree + 180,
     },
     {
-        (6629 - 3500) * kServoValToDegree ,
-        (7092 - 3500) * kServoValToDegree+180,
+        (7523 - 3500) * kServoValToDegree ,
+        (6657 - 3500) * kServoValToDegree-180,//180度の時6657シータ負の方向に動かすと値は減る
     },
 };
 ///////////////
@@ -43,6 +44,7 @@
     KondoServo(pin_serial_servo_tx[0], pin_serial_servo_rx[0]),
     KondoServo(pin_serial_servo_tx[1], pin_serial_servo_rx[1]),
 };
+//足の作成、サイズデータのみの足の骨組
 OneLeg leg[4] = {
     OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2),
     OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2),
@@ -55,33 +57,74 @@
 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){
+    if(count > finish)
+        return 1;
+    else
+        return 0;
+}
+
 int main()
 {
-    DEBUG("When you push any key, this robot starts.\r\n");
+    printf("When you push any key, this robot starts.\r\n");
     while (pc.readable() == 0) //キーボード押したらスタート
         ;
-    DEBUG("move start\r\n");
+    printf("stand up. 1 sec stand\r\n");
+    
+    //stand時の足の軌道設定
+    Orbit stand_orbits[4];
+    Orbit temp_orbit(ELLIPSE);
+    float stand_stridetime_s = 1, stand_risetime_s = 0, stand_stride_m = 0, stand_height_m = 0.05f, stand_ground_m = 0.25f;
+    temp_orbit.SetStraightParam(stand_stridetime_s, stand_risetime_s, stand_stride_m, stand_height_m, stand_ground_m);
+    for (int i = 0; i < 4; i++)
+        stand_orbits[i] = temp_orbit;
+    
+    //4足の位相ずれOffsetTime_sをまとめる
+    float stand_offset_time_s[4] = {
+        0,
+        stand_orbits[0].GetOneWalkTime() * 0.5,
+        stand_orbits[0].GetOneWalkTime() * 0,
+        stand_orbits[0].GetOneWalkTime() * 0.5,};
+    //4つの足のorbit, 位相を代入してstraightという歩行パターンを作成している
+    //このインスタンスはlegそれぞれの計算を行う役割を担う
+    //orbitはここでしか使わないあくまで鍵のような扱い
+    Walk stand(stand_orbits, stand_offset_time_s, kCycleTime_s);
+    //収束判定用関数。現在は回すループの数
+    float dist_m = 10;
+    //Walkの指示通りに動作
+    Move(stand, leg, dist_m);
+    
+    wait(1);
+     DEBUG("move start\r\n");
     //各足の軌道の設定.今回は全部同じにすることで直線を描く。
-    float stridetime_s = 1, risetime_s = 0.5, stride_m = 0.2f, height_m = 0.05f, ground_m = 0.2f;
+    //直進したパラメータstridetime_s = 0.5, risetime_s = 0.3, stride_m = 0.12f, height_m = 0.03f, ground_m = 0.16f;
+    float stridetime_s = 3, risetime_s = 0.3, stride_m = 0.10f, height_m = 0.1f, ground_m = stand_ground_m;
+    //軌道の作成
+    //代入用の軌道SetStraightParam関数を用いると真っ直ぐ進む前提となる
+    temp_orbit.SetStraightParam(stridetime_s, risetime_s, stride_m, height_m, ground_m);
     Orbit straightOrbit[4];
-    Orbit default_orbit(ELLIPSE);
-    default_orbit.SetStraightParam(stridetime_s, risetime_s, stride_m, height_m, ground_m);
-    for (int i = 0; i < 4; i++)
-        straightOrbit[i] = default_orbit;
+    straightOrbit[1] = temp_orbit;
+    straightOrbit[3]= temp_orbit;
+    //stride_m = 0.075f;
+    float offset_x_m = 0.1;
+    temp_orbit.SetStraightParam(stridetime_s, risetime_s, stride_m, height_m, ground_m, offset_x_m);
+    straightOrbit[0] = temp_orbit;
+    straightOrbit[2]= temp_orbit;
+    
     //4足の軌道と位相ずれOffsetTime_sをまとめる
     float offset_time_s[4] = {
         0,
+        straightOrbit[0].GetOneWalkTime() * 0.25,
         straightOrbit[0].GetOneWalkTime() * 0.5,
-        0,
-        straightOrbit[0].GetOneWalkTime() * 0.5,
+        straightOrbit[0].GetOneWalkTime() * 0.75,
     };
     //4つの足のorbit, 位相を代入してstraightという歩行パターンを作成している
     //このインスタンスはlegそれぞれの計算を行う役割を担う
     //orbitはここでしか使わないあくまで鍵のような扱い
-    Walk straight(straightOrbit, offset_time_s, kCycleTime_s);
+    Walk straight2(straightOrbit, offset_time_s, kCycleTime_s);
     //実際にWalkの指示通りに動かす
-    float dist_m = 0.5f;
-    Move(straight, leg, dist_m);
+    dist_m = 500;
+    Move(straight2, leg, dist_m);
     DEBUG("program end\r\n");
 }
 
@@ -91,11 +134,13 @@
     timer.reset();
     timer.start();
     int is_arrived = 0;
+    int count = 0;
     while (is_arrived == 0)
     {
         float time_s = timer.read();
         //注:未実装。到着したかの判定.LRFからのデータが必要？
-        //is_arrived = IsArrived();
+        is_arrived = IsArrived(count, (int)dist_m);
+        ++count;
         //4本の足それぞれの足先サーボ角度更新
         WalkWay.Cal4LegsPosi(leg);
         #ifdef USE_CAN
@@ -125,6 +170,6 @@
     float degree = leg.GetRad(servo_id) * kRadToDegree;
     //サーボの座標系に変更
     float servo_degree = kServoSign[serial_num][servo_id] * degree + kOriginDegree[serial_num][servo_id];
-    //DEBUG("servo_degree[%d][%d],%f\r\n",serial_num,servo_id,servo_degree);
+    DEBUG("servo_degree[%d][%d],%f\r\n",serial_num,servo_id,servo_degree);
     servo[serial_num].set_degree(servo_id, servo_degree);
 }