serbo4soku

Dependencies:   mbed nhk19mr2_can_info splitData SerialHalfDuplex_HM

Files at this revision

API Documentation at this revision

Comitter:
shimizuta
Date:
Mon Feb 11 04:07:51 2019 +0000
Parent:
9:905f93247688
Child:
11:e81425872740
Commit message:
printf debug finish

Changed in this revision

MoveServoOfLeg/MoveServoOfLeg.cpp Show annotated file Show diff for this revision Revisions of this file
MoveServoOfLeg/MoveServoOfLeg.h Show annotated file Show diff for this revision Revisions of this file
ToePosi/ToePosi.cpp Show annotated file Show diff for this revision Revisions of this file
ToePosi/ToePosi.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
--- a/MoveServoOfLeg/MoveServoOfLeg.cpp	Mon Feb 11 01:56:49 2019 +0000
+++ b/MoveServoOfLeg/MoveServoOfLeg.cpp	Mon Feb 11 04:07:51 2019 +0000
@@ -49,6 +49,3 @@
     return rad_[servo_num];
 }
 
-
-
-
--- a/MoveServoOfLeg/MoveServoOfLeg.h	Mon Feb 11 01:56:49 2019 +0000
+++ b/MoveServoOfLeg/MoveServoOfLeg.h	Mon Feb 11 04:07:51 2019 +0000
@@ -10,11 +10,10 @@
     float leglength1_;
     float leglength2_;
     KondoServo servo_;
-
   public:
-MoveServoOfLeg(PinName pin_serial_tx, PinName pin_serial_rx,
-                               float between_servo_half_m,
-                               float leglength1, float leglength2);
+    MoveServoOfLeg(PinName pin_serial_tx, PinName pin_serial_rx,
+                   float between_servo_half_m,
+                   float leglength1, float leglength2);
     void MoveServo(int servo_num);
     void CalServoRad(float x_m, float y_m);
     void SetRad(float rad, int servo_num);
--- a/ToePosi/ToePosi.cpp	Mon Feb 11 01:56:49 2019 +0000
+++ b/ToePosi/ToePosi.cpp	Mon Feb 11 04:07:51 2019 +0000
@@ -11,19 +11,18 @@
 float ToePosi::GetY_m()
 {
     float phase = GetPhase();
-    float y_m;
-    if (phase < M_PI) //最初の半周期は空中
-        y_m = sin(phase) * height_m_;
-    else //後の半周期は着地。直線移動
-        y_m = 0;
+    float y_m = offsety_m_;
+    if (phase < M_PI) //空中にいるとき(最初の半周期)のみy座標は変化
+        y_m += sin(phase) * height_m_;
     return y_m;
 }
 
-ToePosi::ToePosi(float stride_m, float height_m)
+ToePosi::ToePosi(float stride_m, float height_m, float offsety_m)
 {
     phase_ = 0;
     stride_m_ = stride_m;
     height_m_ = height_m;
+    offsety_m_ = offsety_m;
 }
 float ToePosi::GetPhase()
 {
@@ -36,8 +35,5 @@
         phase -= 2 * M_PI;
     while (phase < 0)
         phase += 2 * M_PI;
-
     phase_ = phase;
 }
-
-
--- a/ToePosi/ToePosi.h	Mon Feb 11 01:56:49 2019 +0000
+++ b/ToePosi/ToePosi.h	Mon Feb 11 04:07:51 2019 +0000
@@ -3,16 +3,16 @@
 //足先の位置に関するもの全般
 class ToePosi
 {
-    float phase_;//足先の位相
+    float phase_; //足先の位相
     float stride_m_;
     float height_m_;
+    float offsety_m_;
   public:
-    ToePosi(float stride_m, float height_m);
+    ToePosi(float stride_m, float height_m, float offsety_m);
     //現在phaseでの足先の位置を計算して返す.
     //足の軌道の式の実装部分でもある。要調整
     float GetX_m();
     float GetY_m();
-
     //phase:足先の位相。値域は[0,2CalPhase].
     //0:一番右下で着地している状態とする。0~Piなら空中、Pi~2Piなら着地中で,右下に戻ってくる
     //GetPhase(),SetPhase()で呼び出しや代入を行う。値域を確実に0~2Piにするため。
--- a/main.cpp	Mon Feb 11 01:56:49 2019 +0000
+++ b/main.cpp	Mon Feb 11 04:07:51 2019 +0000
@@ -3,15 +3,17 @@
 #include "pinnames.h"
 #include "ToePosi.h"        //個々の足先の位置を決めるプログラム
 #include "MoveServoOfLeg.h" //サーボを実際に動かすプログラム
+#define DEBUG
 
 ////////////調整すべきパラメータ.全てここに集めた。
 const float kCycleTime_s = 0.02f;              //計算周期
 const float kStride_m = 0.05f;                 //歩幅
-const float kHeight_m = 0.03f;                 //足を上げる高さ
+const float kHeight_m = 0.03f;                 //足の上げ幅
+const float kOffsetY_m = 0.02f;                //足のデフォルト高さ
 const float kBetweenServoHalf_m = 0.06f * 0.5; //サーボ間の距離の半分
 const float kLegLength1 = 0.1f;
 const float kLegLength2 = 0.2f;
-const float kServoSpan_ms = 50;    //サーボの送信感覚
+const float kServoSpan_ms = 0;     //サーボの送信間隔
 const float kVelocity_m_s = 0.01f; //進む速度(適当)
 const float kDist_m = 10;          //歩行中に進む距離(適当)
 ///////////////
@@ -23,6 +25,7 @@
                    kBetweenServoHalf_m, kLegLength1, kLegLength2),
 };
 DigitalOut led(LED1);
+Serial pc(USBTX, USBRX);
 
 const float M_PI = 3.141592;
 //歩き方のパターン
@@ -32,19 +35,26 @@
 };
 //歩き方:patternでdist_mだけ進む。
 void Move(WalkingPattern pattern, ToePosi (&now)[4], float maxvelocity_m_s, float dist_m);
+//直進時の歩き方
 void CalPhaseStraight(ToePosi now_base, ToePosi (&target)[4], float ave_v_m_s);
 
 int main()
 {
+    printf("program start\r\n");
     //現在のそれぞれの足の位置を保存するもの
     ToePosi now[4] = {
-        ToePosi(kStride_m, kHeight_m),
-        ToePosi(kStride_m, kHeight_m),
-        ToePosi(kStride_m, kHeight_m),
-        ToePosi(kStride_m, kHeight_m),
+        ToePosi(kStride_m, kHeight_m, kOffsetY_m),
+        ToePosi(kStride_m, kHeight_m, kOffsetY_m),
+        ToePosi(kStride_m, kHeight_m, kOffsetY_m),
+        ToePosi(kStride_m, kHeight_m, kOffsetY_m),
     };
     //直進
+    printf("please put some key\r\n");
+    while (pc.readable() == 0)
+        ;
+    printf("move start\r\n");
     Move(STRAIGHT_TROT, now, kVelocity_m_s, kDist_m);
+    printf("program end\r\n");
 }
 
 void Move(WalkingPattern pattern, ToePosi (&now)[4], float maxvelocity_m_s, float dist_m)
@@ -69,7 +79,7 @@
     {
         float time_s = timer.read();
         //注:未実装。到着したかの判定.LRFからのデータが必要?
-        //is_arrived = IsArrived();
+        // is_arrived = IsArrived();
 
         //4本の足それぞれの位相更新.二次曲線のどこにいるべきかが決まる
         Cal4LegsPhase(now[0], target, maxvelocity_m_s);
@@ -84,20 +94,15 @@
         wait_ms(kServoSpan_ms);
         moveleg[0].MoveServo(1);
         moveleg[1].MoveServo(1);
-        
         //現在位置更新
-        for(int i = 0; i < 4;i++)
+        for (int i = 0; i < 4; i++)
             now[i] = target[i];
-        
         //計算周期をkCycleTime_sにする
         float rest_time_s = kCycleTime_s - (timer.read() - time_s);
         if (rest_time_s > 0)
             wait(rest_time_s);
-        else //計算周期が達成できないときは
-        {
-            led = 1; //LED1を光らせて知らせる。
+        else //計算周期が達成できないときはprintfで知らせるだけ。動きはする。
             printf("error: rest_time_s = %f in Move()\r\n", rest_time_s);
-        }
     }
 }