test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Diff: main.cpp
- Revision:
- 10:7a340c52e270
- Parent:
- 9:905f93247688
- Child:
- 11:e81425872740
--- 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); - } } }