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?
User | Revision | Line number | New 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 | } |