岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

main.cpp@9:905f93247688, 2019-02-11 (annotated)

Committer:
shimizuta
Date:
Mon Feb 11 01:56:49 2019 +0000
Revision:
9:905f93247688
Parent:
8:21b932c4e6c5
Child:
10:7a340c52e270
made program readable

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