岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

main.cpp@10:7a340c52e270, 2019-02-11 (annotated)

Committer:
shimizuta
Date:
Mon Feb 11 04:07:51 2019 +0000
Revision:
10:7a340c52e270
Parent:
9:905f93247688
Child:
11:e81425872740
printf debug finish

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimizuta 9:905f93247688 1 //NHK2019MR2 馬型機構プログラム.main.cppでは4足の協調に関わるプログラムを主に書く。
yuto17320508 0:f000d896d188 2 #include "mbed.h"
shimizuta 9:905f93247688 3 #include "pinnames.h"
shimizuta 9:905f93247688 4 #include "ToePosi.h" //個々の足先の位置を決めるプログラム
shimizuta 9:905f93247688 5 #include "MoveServoOfLeg.h" //サーボを実際に動かすプログラム
shimizuta 10:7a340c52e270 6 #define DEBUG
yuto17320508 4:fffdb273836e 7
shimizuta 9:905f93247688 8 ////////////調整すべきパラメータ.全てここに集めた。
shimizuta 9:905f93247688 9 const float kCycleTime_s = 0.02f; //計算周期
shimizuta 9:905f93247688 10 const float kStride_m = 0.05f; //歩幅
shimizuta 10:7a340c52e270 11 const float kHeight_m = 0.03f; //足の上げ幅
shimizuta 10:7a340c52e270 12 const float kOffsetY_m = 0.02f; //足のデフォルト高さ
shimizuta 9:905f93247688 13 const float kBetweenServoHalf_m = 0.06f * 0.5; //サーボ間の距離の半分
shimizuta 9:905f93247688 14 const float kLegLength1 = 0.1f;
shimizuta 9:905f93247688 15 const float kLegLength2 = 0.2f;
shimizuta 10:7a340c52e270 16 const float kServoSpan_ms = 0; //サーボの送信間隔
shimizuta 9:905f93247688 17 const float kVelocity_m_s = 0.01f; //進む速度(適当)
shimizuta 9:905f93247688 18 const float kDist_m = 10; //歩行中に進む距離(適当)
shimizuta 9:905f93247688 19 ///////////////
shimizuta 9:905f93247688 20 Timer timer;
shimizuta 9:905f93247688 21 MoveServoOfLeg moveleg[2] = {
shimizuta 9:905f93247688 22 MoveServoOfLeg(pin_serial_servo_tx[0], pin_serial_servo_rx[0],
shimizuta 9:905f93247688 23 kBetweenServoHalf_m, kLegLength1, kLegLength2),
shimizuta 9:905f93247688 24 MoveServoOfLeg(pin_serial_servo_tx[1], pin_serial_servo_rx[1],
shimizuta 9:905f93247688 25 kBetweenServoHalf_m, kLegLength1, kLegLength2),
shimizuta 9:905f93247688 26 };
shimizuta 9:905f93247688 27 DigitalOut led(LED1);
shimizuta 10:7a340c52e270 28 Serial pc(USBTX, USBRX);
yuto17320508 4:fffdb273836e 29
shimizuta 9:905f93247688 30 const float M_PI = 3.141592;
shimizuta 9:905f93247688 31 //歩き方のパターン
shimizuta 9:905f93247688 32 enum WalkingPattern
yuto17320508 5:556d5a5e9d24 33 {
shimizuta 9:905f93247688 34 STRAIGHT_TROT,
yuto17320508 5:556d5a5e9d24 35 };
shimizuta 9:905f93247688 36 //歩き方:patternでdist_mだけ進む。
shimizuta 9:905f93247688 37 void Move(WalkingPattern pattern, ToePosi (&now)[4], float maxvelocity_m_s, float dist_m);
shimizuta 10:7a340c52e270 38 //直進時の歩き方
shimizuta 9:905f93247688 39 void CalPhaseStraight(ToePosi now_base, ToePosi (&target)[4], float ave_v_m_s);
yuto17320508 5:556d5a5e9d24 40
shimizuta 9:905f93247688 41 int main()
yuto17320508 5:556d5a5e9d24 42 {
shimizuta 10:7a340c52e270 43 printf("program start\r\n");
shimizuta 9:905f93247688 44 //現在のそれぞれの足の位置を保存するもの
shimizuta 9:905f93247688 45 ToePosi now[4] = {
shimizuta 10:7a340c52e270 46 ToePosi(kStride_m, kHeight_m, kOffsetY_m),
shimizuta 10:7a340c52e270 47 ToePosi(kStride_m, kHeight_m, kOffsetY_m),
shimizuta 10:7a340c52e270 48 ToePosi(kStride_m, kHeight_m, kOffsetY_m),
shimizuta 10:7a340c52e270 49 ToePosi(kStride_m, kHeight_m, kOffsetY_m),
shimizuta 9:905f93247688 50 };
shimizuta 9:905f93247688 51 //直進
shimizuta 10:7a340c52e270 52 printf("please put some key\r\n");
shimizuta 10:7a340c52e270 53 while (pc.readable() == 0)
shimizuta 10:7a340c52e270 54 ;
shimizuta 10:7a340c52e270 55 printf("move start\r\n");
shimizuta 9:905f93247688 56 Move(STRAIGHT_TROT, now, kVelocity_m_s, kDist_m);
shimizuta 10:7a340c52e270 57 printf("program end\r\n");
shimizuta 2:a92568bdeb5c 58 }
shimizuta 2:a92568bdeb5c 59
shimizuta 9:905f93247688 60 void Move(WalkingPattern pattern, ToePosi (&now)[4], float maxvelocity_m_s, float dist_m)
yuto17320508 5:556d5a5e9d24 61 {
shimizuta 9:905f93247688 62 //patternを見て、使う関数を決定.関数ポインタを利用
shimizuta 9:905f93247688 63 void (*Cal4LegsPhase)(ToePosi, ToePosi(&target)[4], float);
shimizuta 9:905f93247688 64 switch (pattern)
yuto17320508 5:556d5a5e9d24 65 {
shimizuta 9:905f93247688 66 case STRAIGHT_TROT:
shimizuta 9:905f93247688 67 Cal4LegsPhase = CalPhaseStraight;
shimizuta 9:905f93247688 68 //収束判定関数もここでpatternによって変える必要あるかも。
shimizuta 9:905f93247688 69 break;
shimizuta 9:905f93247688 70 default:
shimizuta 9:905f93247688 71 printf("error:there is no func of the pattern");
yuto17320508 5:556d5a5e9d24 72 }
shimizuta 9:905f93247688 73
shimizuta 9:905f93247688 74 ToePosi target[4] = {now[0], now[1], now[2], now[3]}; //目標位置を取り敢えず現在位置に。
shimizuta 9:905f93247688 75 timer.reset();
shimizuta 9:905f93247688 76 timer.start();
shimizuta 9:905f93247688 77 int is_arrived = 0;
shimizuta 9:905f93247688 78 while (is_arrived == 0)
yuto17320508 8:21b932c4e6c5 79 {
shimizuta 9:905f93247688 80 float time_s = timer.read();
shimizuta 9:905f93247688 81 //注:未実装。到着したかの判定.LRFからのデータが必要？
shimizuta 10:7a340c52e270 82 // is_arrived = IsArrived();
shimizuta 9:905f93247688 83
shimizuta 9:905f93247688 84 //4本の足それぞれの位相更新.二次曲線のどこにいるべきかが決まる
shimizuta 9:905f93247688 85 Cal4LegsPhase(now[0], target, maxvelocity_m_s);
shimizuta 9:905f93247688 86 //注:未実装。slave_mbed分の足の目標位置を送信
yuto17320508 5:556d5a5e9d24 87
shimizuta 9:905f93247688 88 //自身が動かす足のサーボ角度更新
shimizuta 9:905f93247688 89 moveleg[0].CalServoRad(target[0].GetX_m(), target[0].GetY_m());
shimizuta 9:905f93247688 90 moveleg[1].CalServoRad(target[1].GetX_m(), target[1].GetY_m());
shimizuta 9:905f93247688 91 //自身が動かす足のサーボを動かす
shimizuta 9:905f93247688 92 moveleg[0].MoveServo(0);
shimizuta 9:905f93247688 93 moveleg[1].MoveServo(0);
shimizuta 9:905f93247688 94 wait_ms(kServoSpan_ms);
shimizuta 9:905f93247688 95 moveleg[0].MoveServo(1);
shimizuta 9:905f93247688 96 moveleg[1].MoveServo(1);
shimizuta 9:905f93247688 97 //現在位置更新
shimizuta 10:7a340c52e270 98 for (int i = 0; i < 4; i++)
shimizuta 9:905f93247688 99 now[i] = target[i];
shimizuta 9:905f93247688 100 //計算周期をkCycleTime_sにする
shimizuta 9:905f93247688 101 float rest_time_s = kCycleTime_s - (timer.read() - time_s);
shimizuta 9:905f93247688 102 if (rest_time_s > 0)
shimizuta 9:905f93247688 103 wait(rest_time_s);
shimizuta 10:7a340c52e270 104 else //計算周期が達成できないときはprintfで知らせるだけ。動きはする。
shimizuta 9:905f93247688 105 printf("error: rest_time_s = %f in Move()\r\n", rest_time_s);
shimizuta 9:905f93247688 106 }
yuto17320508 5:556d5a5e9d24 107 }
yuto17320508 5:556d5a5e9d24 108
shimizuta 9:905f93247688 109 void CalPhaseStraight(ToePosi now_base, ToePosi (&target)[4], float ave_v_m_s)
yuto17320508 6:43708adf2e5d 110 {
shimizuta 9:905f93247688 111 //指定速度で進むための周期 = 平均速度＊時間/歩幅 * pi(半周期で歩幅分進む)
shimizuta 9:905f93247688 112 float phase_step = ave_v_m_s * kCycleTime_s / kStride_m * M_PI;
shimizuta 9:905f93247688 113 //対角線上の足の位相は同じで、反対側は半周期先。
shimizuta 9:905f93247688 114 float target_phase0 = now_base.GetPhase() + phase_step;
shimizuta 9:905f93247688 115 float target_phase1 = target_phase0 + M_PI;
shimizuta 9:905f93247688 116 target[0].SetPhase(target_phase0);
shimizuta 9:905f93247688 117 target[1].SetPhase(target_phase1);
shimizuta 9:905f93247688 118 target[2].SetPhase(target_phase0);
shimizuta 9:905f93247688 119 target[3].SetPhase(target_phase1);
yuto17320508 6:43708adf2e5d 120 }