test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
main.cpp@13:e7ecdb20665a, 2019-02-12 (annotated)
- Committer:
- yuto17320508
- Date:
- Tue Feb 12 07:51:07 2019 +0000
- Revision:
- 13:e7ecdb20665a
- Parent:
- 12:2ac37fe6c3bb
- Child:
- 14:d7cb429946f4
coment
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shimizuta | 11:e81425872740 | 1 | //NHK2019MR2 馬型機構プログラム. |
yuto17320508 | 0:f000d896d188 | 2 | #include "mbed.h" |
shimizuta | 9:905f93247688 | 3 | #include "pinnames.h" |
shimizuta | 11:e81425872740 | 4 | #include "KondoServo.h" |
shimizuta | 11:e81425872740 | 5 | #include "OneLeg.h" ///足先の座標を保存するクラス。x,yやサーボの角度の保存、サーボの駆動も行う。他の足を考慮した処理は別のクラスに任せる。 |
shimizuta | 11:e81425872740 | 6 | #include "Walk.h" //歩き方に関するファイル |
yuto17320508 | 4:fffdb273836e | 7 | |
shimizuta | 9:905f93247688 | 8 | ////////////調整すべきパラメータ.全てここに集めた。 |
shimizuta | 12:2ac37fe6c3bb | 9 | const float kCycleTime_s = 0.02f; //計算周期 |
shimizuta | 9:905f93247688 | 10 | const float kBetweenServoHalf_m = 0.06f * 0.5; //サーボ間の距離の半分 |
shimizuta | 9:905f93247688 | 11 | const float kLegLength1 = 0.1f; |
shimizuta | 9:905f93247688 | 12 | const float kLegLength2 = 0.2f; |
yuto17320508 | 13:e7ecdb20665a | 13 | const float kServoSpan_ms = 7.0f; //サーボの送信間隔 |
shimizuta | 9:905f93247688 | 14 | /////////////// |
shimizuta | 9:905f93247688 | 15 | Timer timer; |
shimizuta | 11:e81425872740 | 16 | KondoServo servo[2] = { |
shimizuta | 11:e81425872740 | 17 | KondoServo(pin_serial_servo_tx[0], pin_serial_servo_rx[0]), |
shimizuta | 11:e81425872740 | 18 | KondoServo(pin_serial_servo_tx[1], pin_serial_servo_rx[1]), |
shimizuta | 11:e81425872740 | 19 | }; |
shimizuta | 11:e81425872740 | 20 | OneLeg leg[4] = { |
shimizuta | 11:e81425872740 | 21 | OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2), |
shimizuta | 11:e81425872740 | 22 | OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2), |
shimizuta | 11:e81425872740 | 23 | OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2), |
shimizuta | 11:e81425872740 | 24 | OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2), |
shimizuta | 9:905f93247688 | 25 | }; |
shimizuta | 9:905f93247688 | 26 | DigitalOut led(LED1); |
shimizuta | 10:7a340c52e270 | 27 | Serial pc(USBTX, USBRX); |
shimizuta | 9:905f93247688 | 28 | const float M_PI = 3.141592; |
shimizuta | 11:e81425872740 | 29 | const float kRadToDegree = 180.0 / M_PI; |
shimizuta | 11:e81425872740 | 30 | void Move(Walk WalkWay, OneLeg (&leg)[4], float dist_m); |
shimizuta | 11:e81425872740 | 31 | void MoveServo(OneLeg leg, int legnum, int servo_id); |
shimizuta | 12:2ac37fe6c3bb | 32 | |
shimizuta | 9:905f93247688 | 33 | int main() |
yuto17320508 | 5:556d5a5e9d24 | 34 | { |
yuto17320508 | 13:e7ecdb20665a | 35 | /*printf("When you push any key, this robot starts.\r\n"); |
shimizuta | 11:e81425872740 | 36 | while (pc.readable() == 0) //キーボード押したらスタート |
yuto17320508 | 13:e7ecdb20665a | 37 | ;*/ |
shimizuta | 10:7a340c52e270 | 38 | printf("move start\r\n"); |
shimizuta | 11:e81425872740 | 39 | //各足の軌道の設定.今回は全部同じにすることで直線を描く。 |
shimizuta | 11:e81425872740 | 40 | float stridetime_s = 1, risetime_s = 0.3, stride_m = 0.2f, height_m = 0.03f, ground_m = 0.14f; |
yuto17320508 | 13:e7ecdb20665a | 41 | Orbit straightOrbit[4] = { |
yuto17320508 | 13:e7ecdb20665a | 42 | |
shimizuta | 11:e81425872740 | 43 | Orbit(stridetime_s, risetime_s, stride_m, height_m, ground_m), |
shimizuta | 11:e81425872740 | 44 | Orbit(stridetime_s, risetime_s, stride_m, height_m, ground_m), |
shimizuta | 11:e81425872740 | 45 | Orbit(stridetime_s, risetime_s, stride_m, height_m, ground_m), |
shimizuta | 11:e81425872740 | 46 | Orbit(stridetime_s, risetime_s, stride_m, height_m, ground_m), |
shimizuta | 11:e81425872740 | 47 | }; |
shimizuta | 11:e81425872740 | 48 | //4足の軌道と位相ずれOffsetTime_sをまとめる |
shimizuta | 11:e81425872740 | 49 | float offset_time_s[4] = { |
shimizuta | 11:e81425872740 | 50 | 0, |
yuto17320508 | 13:e7ecdb20665a | 51 | straightOrbit[0].GetOneWalkTime() * 0.5, |
shimizuta | 11:e81425872740 | 52 | 0, |
yuto17320508 | 13:e7ecdb20665a | 53 | straightOrbit[0].GetOneWalkTime() * 0.5, |
shimizuta | 11:e81425872740 | 54 | }; |
yuto17320508 | 13:e7ecdb20665a | 55 | //4つの足のorbit, 位相を代入してstraightという歩行パターンを作成している |
yuto17320508 | 13:e7ecdb20665a | 56 | //このインスタンスはlegそれぞれの計算を行う役割を担う |
yuto17320508 | 13:e7ecdb20665a | 57 | //orbitはここでしか使わないあくまで鍵のような扱い |
yuto17320508 | 13:e7ecdb20665a | 58 | Walk straight(straightOrbit, offset_time_s, kCycleTime_s); |
shimizuta | 11:e81425872740 | 59 | //実際にWalkの指示通りに動かす |
shimizuta | 11:e81425872740 | 60 | float dist_m = 0.5f; |
shimizuta | 11:e81425872740 | 61 | Move(straight, leg, dist_m); |
shimizuta | 11:e81425872740 | 62 | |
shimizuta | 10:7a340c52e270 | 63 | printf("program end\r\n"); |
shimizuta | 2:a92568bdeb5c | 64 | } |
shimizuta | 2:a92568bdeb5c | 65 | |
yuto17320508 | 13:e7ecdb20665a | 66 | //到達判定が来ない間同じ歩行方法でループ |
shimizuta | 11:e81425872740 | 67 | void Move(Walk WalkWay, OneLeg (&leg)[4], float dist_m) |
yuto17320508 | 5:556d5a5e9d24 | 68 | { |
shimizuta | 9:905f93247688 | 69 | timer.reset(); |
shimizuta | 9:905f93247688 | 70 | timer.start(); |
shimizuta | 9:905f93247688 | 71 | int is_arrived = 0; |
shimizuta | 9:905f93247688 | 72 | while (is_arrived == 0) |
yuto17320508 | 8:21b932c4e6c5 | 73 | { |
shimizuta | 9:905f93247688 | 74 | float time_s = timer.read(); |
shimizuta | 9:905f93247688 | 75 | //注:未実装。到着したかの判定.LRFからのデータが必要? |
shimizuta | 12:2ac37fe6c3bb | 76 | //is_arrived = IsArrived(); |
shimizuta | 11:e81425872740 | 77 | //4本の足それぞれの足先サーボ角度更新 |
shimizuta | 11:e81425872740 | 78 | WalkWay.Cal4LegsPosi(leg); |
shimizuta | 9:905f93247688 | 79 | //注:未実装。slave_mbed分の足の目標位置を送信 |
yuto17320508 | 5:556d5a5e9d24 | 80 | |
shimizuta | 9:905f93247688 | 81 | //自身が動かす足のサーボを動かす |
shimizuta | 11:e81425872740 | 82 | MoveServo(leg[0], 0, 0); |
shimizuta | 11:e81425872740 | 83 | MoveServo(leg[1], 1, 0); |
shimizuta | 9:905f93247688 | 84 | wait_ms(kServoSpan_ms); |
shimizuta | 11:e81425872740 | 85 | MoveServo(leg[0], 0, 1); |
shimizuta | 11:e81425872740 | 86 | MoveServo(leg[1], 1, 1); |
shimizuta | 11:e81425872740 | 87 | //計算周期がWalkWay.cycletime_s_になるようwait |
shimizuta | 11:e81425872740 | 88 | float rest_time_s = WalkWay.cycletime_s_ - (timer.read() - time_s); |
shimizuta | 9:905f93247688 | 89 | if (rest_time_s > 0) |
shimizuta | 9:905f93247688 | 90 | wait(rest_time_s); |
shimizuta | 10:7a340c52e270 | 91 | else //計算周期が達成できないときはprintfで知らせるだけ。動きはする。 |
shimizuta | 9:905f93247688 | 92 | printf("error: rest_time_s = %f in Move()\r\n", rest_time_s); |
shimizuta | 9:905f93247688 | 93 | } |
yuto17320508 | 5:556d5a5e9d24 | 94 | } |
yuto17320508 | 5:556d5a5e9d24 | 95 | |
shimizuta | 11:e81425872740 | 96 | void MoveServo(OneLeg leg, int legnum, int servo_id) |
yuto17320508 | 6:43708adf2e5d | 97 | { |
shimizuta | 11:e81425872740 | 98 | float degree = leg.GetRad(servo_id) * kRadToDegree; |
shimizuta | 11:e81425872740 | 99 | //servo1は反転させる |
shimizuta | 11:e81425872740 | 100 | if (servo_id == 0) |
shimizuta | 11:e81425872740 | 101 | degree += 90; |
shimizuta | 11:e81425872740 | 102 | else |
shimizuta | 11:e81425872740 | 103 | degree = 270 - degree; |
shimizuta | 11:e81425872740 | 104 | servo[legnum].set_degree(servo_id, degree); |
shimizuta | 12:2ac37fe6c3bb | 105 | } |