Diff: main.cpp

Revision:

29:7d8b8011a88d

Parent:

28:8e1cbeffe6c2

Child:

31:86eb746eaed4

--- a/main.cpp	Wed Feb 27 01:25:01 2019 +0000
+++ b/main.cpp	Wed Feb 27 12:16:18 2019 +0000
@@ -11,7 +11,7 @@
 #include "pi.h"
 #include "can.h"
 #define USE_CAN //can通信するならdefine.しないなら切らないとエラー出る
-#define USE_ROS
+//#define USE_ROS
 #include <ros.h>
 #include <geometry_msgs/Vector3.h>
 #endif
@@ -24,15 +24,15 @@
 const int kServoSpan_ms = 6;                  //サーボの送信間隔
 const float kBetweenServoHalf_m = 0.03 * 0.5; //サーボ間の距離の半分
 float kLegLength1[2] = {0.1, 0.1};
-float kLegLength2[2] = {0.2452 + 0.0025, 0.22529 + 0.0025};
+float kLegLength2[2] = {0.2452 + 0.0025, 0.23};
 //サーボの正負と座標系の正負の補正.足で一セット。
 const int kServoSign[2][2] = {{1, -1}, {-1, 1}};
 //欲しい座標系0度でのサーボのICSマネージャーの値
 const double kServoValToDegree = 270.0 / (11500 - 3500); //ICSの値を度に変換
 const double kOriginDegree[2][2] = {
     {
-        (8338 - 3500) * kServoValToDegree,
-        (5887 - 3500) * kServoValToDegree + 180,
+        (8375 - 3500) * kServoValToDegree,
+        (5523 - 3500) * kServoValToDegree + 180,
     },
     {
         (6470 - 3500) * kServoValToDegree,
@@ -71,65 +71,64 @@
 void WaitStdin(char startchar);
 int FileOpen();
 int IsArrived(int count, int finish);
+//段差があるときの機体角度を計算
+//param back_height_on_step:段差の高さ。後ろにあるとする。前にあるときはマイナスを入れる
+float GetSteepBodyRad(float back_height_on_step);
 
 ////////調整するべきパラメータ
 enum WalkWay
 {
     STANDUP,
     STRAIGHT,
+    END,
     TURNLEFT,
     TURNRIGHT,
-    END,
+    UP,            //足を伸ばして立つ
+    OVERCOME,      //前足を乗せる
+    SANDDUNE,      //前足だけがsandduneに乗った状態で進む
+    OVERCOME_BACK, //後ろ足を乗せる
+    STRAIGHT_W,    //両足がsandduneに乗った状態で進む
+    STRAIGHT_W2,   //後足がsandduneに乗った状態で進む
+    ROPE_F,        //ropeをまたぐ（前足)
+    ROPE_B,        //ropeをまたぐ（後ろ足)
 };
 void ParamsSetup(Walk walks[END], OneLeg leg[4]) //各パラメータの設定。減らしていく必要あり
 {
+
     Walk::calctime_s_ = 0.03; //計算周期
     //足の作成、サイズデータのみの足の骨組.4足同じにしている
     for (int i = 0; i < 4; i++)
         leg[i] = OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2);
-    //軌道のデフォルト値
-    const float kStrideTime_s = 0.3, kRiseTime_s = 0.1,
-                kStride_m = 0.15, kHeight_m = 0.03, kGround_m = 0.2,
-                kEllipseCenterX_m = 0, kEllipseCenterY_m = kGround_m;
-    //STANDUP時のパラメータ。
-    walks[STANDUP].SetAllLegStandParam(0, 0.2, 0.5); //一括で設定できる
-
-    //STRAIGHT1のparamater
-    walks[STRAIGHT].SetOffset(0, 0.5, 0.5, 0); //位相ずれの程度(値域[0..1])
-    walks[STRAIGHT].SetAllLegEllipseParam      // 4足一括で設定
-        (kStrideTime_s, kRiseTime_s, kStride_m, kHeight_m, kGround_m, kEllipseCenterX_m, kEllipseCenterY_m);
+    walks[STANDUP].SetAllLegStandParam(0, 0.2, 0.5); //x,y,time_s.一括で設定できる
 
-    //TURNLEFT1のparam
-    float stridetime_s = 0.2, risetime_s = 0.1, stride_short_m = 0.2, stride_long_m = 0.1;
-    walks[TURNLEFT].SetAllLegEllipseParam // 4足を取り敢えず一括で設定してから
-        (stridetime_s, risetime_s, stride_short_m, kHeight_m, kGround_m, 0, kEllipseCenterY_m);
-    walks[TURNLEFT].ChangeOneParam(LEFT_F, STRIDE_M, stride_long_m); //一部のパラメータを変更
-    walks[TURNLEFT].ChangeOneParam(LEFT_B, STRIDE_M, stride_long_m);
-    walks[TURNLEFT].SetOffset(0, 0.5, 0.5, 0); //位相ずれの程度(値域[0..1])
-
-    //TURNRIGHT1のparam
-    walks[TURNRIGHT].SetAllLegEllipseParam // 4足を取り敢えず一括で設定してから
-        (stridetime_s, risetime_s, stride_short_m, kHeight_m, kGround_m, 0, kEllipseCenterY_m);
-    walks[TURNRIGHT].ChangeOneParam(RIGHT_F, STRIDE_M, stride_long_m); //一部のパラメータを変更
-    walks[TURNRIGHT].ChangeOneParam(RIGHT_B, STRIDE_M, stride_long_m);
-    walks[TURNRIGHT].SetOffset(0, 0.5, 0.5, 0); //位相ずれの程度(値域[0..1])
+    //STRAIGT 論文通りのとき、start_y_mは <=0.32
+    float start_x_m = 0.06, start_y_m = 0.2, stride_m = 0.12, height_m = 0.03, buffer_height_m = 0.01,
+          stridetime_s = 0.55, toptime_s = 0.30, buffer_time_s = 0.15;
+    walks[STRAIGHT].SetAllLegTriangleParam(start_x_m, start_y_m, stride_m, height_m, buffer_height_m,
+                                           stridetime_s, toptime_s, buffer_time_s);
+    walks[STRAIGHT].SetOffsetTime(0, 0.5, 0.5, 0);
 }
 Walk walks[END]; //歩行法
 int main()
 {
+#ifdef VSCODE
     if (FileOpen()) //csv fileに書き込み
         return 1;   //異常終了したら強制終了
+#endif
     DEBUG("param set start");
     ParamsSetup(walks, leg);      //各動きの際の足の軌道を設定。パラメータはこの関数内に打ち込む
     for (int i = 0; i < END; i++) //軌道のチェック
     {
         if (walks[i].CheckOrbit(leg[0]) == 1) //軌道が定義外なら
-            return 1;                         //強制終了.errorは内部の関数からprintfで知らせる
+        {
+            printf("error: move pattern %d", i);
+            return 1; //強制終了.errorは内部の関数からprintfで知らせる
+        }
     }
-    DEBUG("Stand up?\r\n");
+    printf("Stand up?\r\n");
     WaitStdin('y'); // ボタンを押したら立つ
     MoveOneCycle(walks[STANDUP], leg);
-    DEBUG("Move?\r\n");
+    printf("Move?\r\n");
     WaitStdin('y'); // ボタンを押したらスタート
 #ifdef USE_ROS
     nh_mbed.getHardware()->setBaud(115200);
@@ -146,8 +145,10 @@
     }
     MoveOneCycle(walks[STANDUP], leg); //最後はLRFを保護するためSTANDUPの状態で終わる
     printf("program end\r\n");
+#ifdef VSCODE
     fclose(fp);
 #endif
+#endif
 }
 //一サイクル分進む
 void MoveOneCycle(Walk walkway, OneLeg leg[4])
@@ -162,7 +163,9 @@
 #ifndef VSCODE
         float time_s = timer.read();
 #endif
-        walkway.Cal4LegsPosi(leg); //4本の足それぞれの足先サーボ角度更新
+        //4本の足それぞれの足先サーボ角度更新
+        if (walkway.Cal4LegsPosi(leg) == 1)
+            printf("error: time = %f\r\n", i * walkway.calctime_s_);
 #ifdef USE_CAN
         SendRad(leg[2], leg[3]); //slave_mbed分の足の目標位置を送信
 #endif
@@ -174,12 +177,13 @@
 #endif
         MoveServo(leg[0], 0, 1);
         MoveServo(leg[1], 1, 1);
+#ifdef VSCODE
         //ファイルに書き込み。time[s],x[0],y[0],x[1],y[1],x[2],y[2],x[3],y[3]の順
         fprintf(fp, "%f", i * walkway.calctime_s_);
         for (int i = 0; i < 4; i++)
             fprintf(fp, ",%f,%f", leg[i].GetX_m(), leg[i].GetY_m());
         fprintf(fp, "\r\n");
-#ifndef VSCODE
+#else
         //計算周期がwalkway.calctime_s_になるようwait
         float rest_time_s = walkway.calctime_s_ - (timer.read() - time_s);
         if (rest_time_s > 0)
@@ -237,3 +241,10 @@
         MoveOneCycle(walks[STRAIGHT], leg);
 }
 #endif
+//param back_height_on_step:段差の高さ。後ろにあるとする。前にあるときはマイナスを入れる
+float GetSteepBodyRad(float back_height_on_step)
+{
+    float offset_hight = back_height_on_step + leg[LEFT_B].GetY_m() - leg[LEFT_B].GetY_m();
+    float theta = atan2(offset_hight, (float)(sqrt(0.09 - offset_hight * offset_hight)));
+    return theta;
+}
\ No newline at end of file