test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
main.cpp@25:5d0977d2d79d, 2019-02-20 (annotated)
- Committer:
- kageyuta
- Date:
- Wed Feb 20 01:00:23 2019 +0000
- Revision:
- 25:5d0977d2d79d
- Parent:
- 20:70cc6083e9c7
hokou
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" |
yuto17320508 | 18:0033ef1814ba | 5 | //#define DEBUG_ON//デバッグ用。使わないときはコメントアウト |
shimizuta | 14:d7cb429946f4 | 6 | #include "debug.h" |
shimizuta | 14:d7cb429946f4 | 7 | #include "pi.h" |
shimizuta | 16:0069a56f11a3 | 8 | #include "can.h" |
shimizuta | 11:e81425872740 | 9 | #include "OneLeg.h" ///足先の座標を保存するクラス。x,yやサーボの角度の保存、サーボの駆動も行う。他の足を考慮した処理は別のクラスに任せる。 |
shimizuta | 11:e81425872740 | 10 | #include "Walk.h" //歩き方に関するファイル |
shimizuta | 19:1adc7302cfd9 | 11 | #define USE_CAN //can通信するならdefine.しないなら切らないとエラー出る |
yuto17320508 | 4:fffdb273836e | 12 | |
shimizuta | 9:905f93247688 | 13 | ////////////調整すべきパラメータ.全てここに集めた。 |
shimizuta | 19:1adc7302cfd9 | 14 | const float kCycleTime_s = 0.03; //計算周期 |
shimizuta | 19:1adc7302cfd9 | 15 | const float kBetweenServoHalf_m = 0.03 * 0.5; //サーボ間の距離の半分 |
shimizuta | 19:1adc7302cfd9 | 16 | float kLegLength1[2] = {0.1, 0.1}; |
shimizuta | 19:1adc7302cfd9 | 17 | float kLegLength2[2] = {0.2452 + 0.0025, 0.22529 + 0.0025}; |
shimizuta | 16:0069a56f11a3 | 18 | //サーボの設定 |
shimizuta | 14:d7cb429946f4 | 19 | const int kServoSpan_ms = 10; //サーボの送信間隔 |
shimizuta | 14:d7cb429946f4 | 20 | const double kServoValToDegree = 270.0 / (11500 - 3500); |
shimizuta | 14:d7cb429946f4 | 21 | //サーボの正負と座標系の正負の補正.足で一セット。 |
shimizuta | 14:d7cb429946f4 | 22 | const int kServoSign[2][2] = {{ |
kageyuta | 25:5d0977d2d79d | 23 | 1, |
kageyuta | 25:5d0977d2d79d | 24 | -1, |
kageyuta | 25:5d0977d2d79d | 25 | |
kageyuta | 25:5d0977d2d79d | 26 | }, |
kageyuta | 25:5d0977d2d79d | 27 | { |
kageyuta | 25:5d0977d2d79d | 28 | -1, |
kageyuta | 25:5d0977d2d79d | 29 | 1, |
kageyuta | 25:5d0977d2d79d | 30 | } |
kageyuta | 25:5d0977d2d79d | 31 | }; |
shimizuta | 14:d7cb429946f4 | 32 | //欲しい座標系0度でのサーボのICSマネージャーの値 |
shimizuta | 14:d7cb429946f4 | 33 | const double kOriginDegree[2][2] = { |
shimizuta | 14:d7cb429946f4 | 34 | { |
yuto17320508 | 20:70cc6083e9c7 | 35 | (7257 - 3500) * kServoValToDegree, |
yuto17320508 | 20:70cc6083e9c7 | 36 | (6590 - 3500) * kServoValToDegree + 180, |
shimizuta | 14:d7cb429946f4 | 37 | }, |
shimizuta | 14:d7cb429946f4 | 38 | { |
yuto17320508 | 20:70cc6083e9c7 | 39 | (6470 - 3500) * kServoValToDegree, |
yuto17320508 | 20:70cc6083e9c7 | 40 | (7419 - 3500) * kServoValToDegree - 180, //180度の時6657シータ負の方向に動かすと値は減るので、0度の位置は-180すれば出る |
shimizuta | 14:d7cb429946f4 | 41 | }, |
shimizuta | 14:d7cb429946f4 | 42 | }; |
shimizuta | 9:905f93247688 | 43 | /////////////// |
shimizuta | 9:905f93247688 | 44 | Timer timer; |
shimizuta | 11:e81425872740 | 45 | KondoServo servo[2] = { |
shimizuta | 11:e81425872740 | 46 | KondoServo(pin_serial_servo_tx[0], pin_serial_servo_rx[0]), |
shimizuta | 11:e81425872740 | 47 | KondoServo(pin_serial_servo_tx[1], pin_serial_servo_rx[1]), |
shimizuta | 11:e81425872740 | 48 | }; |
yuto17320508 | 18:0033ef1814ba | 49 | //足の作成、サイズデータのみの足の骨組 |
shimizuta | 11:e81425872740 | 50 | OneLeg leg[4] = { |
shimizuta | 11:e81425872740 | 51 | OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2), |
shimizuta | 11:e81425872740 | 52 | OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2), |
shimizuta | 11:e81425872740 | 53 | OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2), |
shimizuta | 11:e81425872740 | 54 | OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2), |
shimizuta | 9:905f93247688 | 55 | }; |
shimizuta | 19:1adc7302cfd9 | 56 | DigitalOut led[4] = {DigitalOut(LED1), DigitalOut(LED2), DigitalOut(LED3), DigitalOut(LED4)}; |
shimizuta | 14:d7cb429946f4 | 57 | |
shimizuta | 11:e81425872740 | 58 | const float kRadToDegree = 180.0 / M_PI; |
shimizuta | 11:e81425872740 | 59 | void Move(Walk WalkWay, OneLeg (&leg)[4], float dist_m); |
shimizuta | 11:e81425872740 | 60 | void MoveServo(OneLeg leg, int legnum, int servo_id); |
shimizuta | 19:1adc7302cfd9 | 61 | int IsArrived(int count, int finish) |
shimizuta | 19:1adc7302cfd9 | 62 | { |
shimizuta | 19:1adc7302cfd9 | 63 | if (count > finish) |
yuto17320508 | 18:0033ef1814ba | 64 | return 1; |
yuto17320508 | 18:0033ef1814ba | 65 | else |
yuto17320508 | 18:0033ef1814ba | 66 | return 0; |
yuto17320508 | 18:0033ef1814ba | 67 | } |
shimizuta | 9:905f93247688 | 68 | int main() |
yuto17320508 | 5:556d5a5e9d24 | 69 | { |
kageyuta | 25:5d0977d2d79d | 70 | printf("------------------------New Session Start------------------------.\r\n"); |
kageyuta | 25:5d0977d2d79d | 71 | printf("Ready to stand up. Press any key.\r\n"); |
shimizuta | 11:e81425872740 | 72 | while (pc.readable() == 0) //キーボード押したらスタート |
shimizuta | 14:d7cb429946f4 | 73 | ; |
shimizuta | 19:1adc7302cfd9 | 74 | //stand時の足の軌道設定. |
shimizuta | 19:1adc7302cfd9 | 75 | Orbit stand_orbit[4]; |
kageyuta | 25:5d0977d2d79d | 76 | |
kageyuta | 25:5d0977d2d79d | 77 | //機体高さ |
kageyuta | 25:5d0977d2d79d | 78 | float stand_offset_y_m = 0.18; |
kageyuta | 25:5d0977d2d79d | 79 | |
kageyuta | 25:5d0977d2d79d | 80 | |
shimizuta | 19:1adc7302cfd9 | 81 | stand_orbit[0].SetStandParam(stand_offset_y_m); |
shimizuta | 19:1adc7302cfd9 | 82 | for (int i = 0; i < 4; i++) //全部足を同じにしてる |
shimizuta | 19:1adc7302cfd9 | 83 | stand_orbit[i] = stand_orbit[0]; |
yuto17320508 | 18:0033ef1814ba | 84 | //4足の位相ずれOffsetTime_sをまとめる |
yuto17320508 | 18:0033ef1814ba | 85 | float stand_offset_time_s[4] = { |
yuto17320508 | 18:0033ef1814ba | 86 | 0, |
shimizuta | 19:1adc7302cfd9 | 87 | stand_orbit[0].GetOneWalkTime() * 0.5, |
shimizuta | 19:1adc7302cfd9 | 88 | stand_orbit[0].GetOneWalkTime() * 0, |
shimizuta | 19:1adc7302cfd9 | 89 | stand_orbit[0].GetOneWalkTime() * 0.5, |
shimizuta | 19:1adc7302cfd9 | 90 | }; |
yuto17320508 | 18:0033ef1814ba | 91 | //4つの足のorbit, 位相を代入してstraightという歩行パターンを作成している |
yuto17320508 | 18:0033ef1814ba | 92 | //このインスタンスはlegそれぞれの計算を行う役割を担う |
shimizuta | 19:1adc7302cfd9 | 93 | //orbitはここでしか使わないあくまでパラメータ設定用クラス |
shimizuta | 19:1adc7302cfd9 | 94 | Walk stand(stand_orbit, stand_offset_time_s, kCycleTime_s); |
shimizuta | 19:1adc7302cfd9 | 95 | float dist_m = 10; //収束判定用。現在は回すループの数 |
shimizuta | 19:1adc7302cfd9 | 96 | //動作開始 |
kageyuta | 25:5d0977d2d79d | 97 | |
yuto17320508 | 18:0033ef1814ba | 98 | Move(stand, leg, dist_m); |
kageyuta | 25:5d0977d2d79d | 99 | |
yuto17320508 | 18:0033ef1814ba | 100 | wait(1); |
kageyuta | 25:5d0977d2d79d | 101 | printf("Ready to walk. Press [a] to start.\r\n"); |
kageyuta | 25:5d0977d2d79d | 102 | while(1) { |
kageyuta | 25:5d0977d2d79d | 103 | char c = pc.getc(); |
kageyuta | 25:5d0977d2d79d | 104 | if(c == 'a') {printf("Waliking Session Start.\r\n");break;} |
kageyuta | 25:5d0977d2d79d | 105 | } |
kageyuta | 25:5d0977d2d79d | 106 | |
kageyuta | 25:5d0977d2d79d | 107 | |
shimizuta | 19:1adc7302cfd9 | 108 | |
kageyuta | 25:5d0977d2d79d | 109 | DEBUG("move start\r\n"); |
kageyuta | 25:5d0977d2d79d | 110 | //取り敢えず歩行したパラメータ |
kageyuta | 25:5d0977d2d79d | 111 | //stridetime_s = 0.5, risetime_s = 0.3,stride_m= 0.12f, height_m = 0.03f, offset_x_m = 0, offset_y_m = 0.16f; |
kageyuta | 25:5d0977d2d79d | 112 | //曲げてみる |
kageyuta | 25:5d0977d2d79d | 113 | float stridetime_s = 0.3, risetime_s = 0.15, |
kageyuta | 25:5d0977d2d79d | 114 | stride_r_m = 0.1, stride_l_m = 0.1, height_m = 0.03, |
kageyuta | 25:5d0977d2d79d | 115 | offset_x_m = 0 |
kageyuta | 25:5d0977d2d79d | 116 | , offset_y_m = stand_offset_y_m; |
kageyuta | 25:5d0977d2d79d | 117 | //軌道の作成 |
kageyuta | 25:5d0977d2d79d | 118 | //代入用の軌道SetStraightParam関数を用いると真っ直ぐ進む前提となる |
kageyuta | 25:5d0977d2d79d | 119 | Orbit orbit[4]; |
kageyuta | 25:5d0977d2d79d | 120 | orbit[0].SetStraightParam(stridetime_s, risetime_s, stride_r_m, height_m, offset_x_m, offset_y_m); |
kageyuta | 25:5d0977d2d79d | 121 | orbit[1].SetStraightParam(stridetime_s, risetime_s, stride_r_m, height_m, 0, offset_y_m); |
kageyuta | 25:5d0977d2d79d | 122 | orbit[2].SetStraightParam(stridetime_s, risetime_s, stride_l_m, height_m, offset_x_m, offset_y_m); |
kageyuta | 25:5d0977d2d79d | 123 | orbit[3].SetStraightParam(stridetime_s, risetime_s, stride_l_m, height_m, 0, offset_y_m); |
kageyuta | 25:5d0977d2d79d | 124 | //4足の軌道と位相ずれOffsetTime_sをまとめる |
kageyuta | 25:5d0977d2d79d | 125 | float offset_time_s[4] = { |
kageyuta | 25:5d0977d2d79d | 126 | 0, |
kageyuta | 25:5d0977d2d79d | 127 | orbit[0].GetOneWalkTime() * 0.5, |
kageyuta | 25:5d0977d2d79d | 128 | orbit[0].GetOneWalkTime() * 0.5, |
kageyuta | 25:5d0977d2d79d | 129 | orbit[0].GetOneWalkTime() * 0, |
kageyuta | 25:5d0977d2d79d | 130 | }; |
kageyuta | 25:5d0977d2d79d | 131 | //4つの足のorbit, 位相を代入して歩行パターンを作成している |
kageyuta | 25:5d0977d2d79d | 132 | //このインスタンスはlegそれぞれの計算を行う役割を担う |
kageyuta | 25:5d0977d2d79d | 133 | //orbitはここでしか使わないあくまで鍵のような扱い |
kageyuta | 25:5d0977d2d79d | 134 | Walk walk(orbit, offset_time_s, kCycleTime_s); |
kageyuta | 25:5d0977d2d79d | 135 | dist_m = 500000; //収束判定用。取り敢えず今はループ数。 |
kageyuta | 25:5d0977d2d79d | 136 | //動く |
kageyuta | 25:5d0977d2d79d | 137 | |
kageyuta | 25:5d0977d2d79d | 138 | Move(walk, leg, dist_m); |
kageyuta | 25:5d0977d2d79d | 139 | DEBUG("program end\r\n"); |
shimizuta | 2:a92568bdeb5c | 140 | } |
shimizuta | 2:a92568bdeb5c | 141 | |
yuto17320508 | 13:e7ecdb20665a | 142 | //到達判定が来ない間同じ歩行方法でループ |
shimizuta | 11:e81425872740 | 143 | void Move(Walk WalkWay, OneLeg (&leg)[4], float dist_m) |
yuto17320508 | 5:556d5a5e9d24 | 144 | { |
shimizuta | 9:905f93247688 | 145 | timer.reset(); |
shimizuta | 9:905f93247688 | 146 | timer.start(); |
shimizuta | 9:905f93247688 | 147 | int is_arrived = 0; |
yuto17320508 | 18:0033ef1814ba | 148 | int count = 0; |
kageyuta | 25:5d0977d2d79d | 149 | while (is_arrived == 0) { |
shimizuta | 9:905f93247688 | 150 | float time_s = timer.read(); |
shimizuta | 19:1adc7302cfd9 | 151 | //注:未実装。到着したかの判定.LRFからのデータが必要.今は取り敢えずループ回数で判断 |
yuto17320508 | 18:0033ef1814ba | 152 | is_arrived = IsArrived(count, (int)dist_m); |
yuto17320508 | 18:0033ef1814ba | 153 | ++count; |
shimizuta | 11:e81425872740 | 154 | //4本の足それぞれの足先サーボ角度更新 |
shimizuta | 11:e81425872740 | 155 | WalkWay.Cal4LegsPosi(leg); |
shimizuta | 19:1adc7302cfd9 | 156 | #ifdef USE_CAN |
shimizuta | 16:0069a56f11a3 | 157 | //slave_mbed分の足の目標位置を送信 |
shimizuta | 19:1adc7302cfd9 | 158 | SendRad(leg[2], leg[3]); |
shimizuta | 19:1adc7302cfd9 | 159 | #endif |
shimizuta | 9:905f93247688 | 160 | //自身が動かす足のサーボを動かす |
shimizuta | 16:0069a56f11a3 | 161 | MoveServo(leg[0], 0, 0); |
shimizuta | 11:e81425872740 | 162 | MoveServo(leg[1], 1, 0); |
shimizuta | 9:905f93247688 | 163 | wait_ms(kServoSpan_ms); |
shimizuta | 16:0069a56f11a3 | 164 | MoveServo(leg[0], 0, 1); |
shimizuta | 11:e81425872740 | 165 | MoveServo(leg[1], 1, 1); |
shimizuta | 19:1adc7302cfd9 | 166 | DEBUG("%f, %f, %f, %f\r\n", leg[2].GetRad(0), leg[2].GetRad(1), leg[3].GetRad(0), leg[3].GetRad(1)); |
shimizuta | 11:e81425872740 | 167 | //計算周期がWalkWay.cycletime_s_になるようwait |
shimizuta | 11:e81425872740 | 168 | float rest_time_s = WalkWay.cycletime_s_ - (timer.read() - time_s); |
shimizuta | 9:905f93247688 | 169 | if (rest_time_s > 0) |
shimizuta | 9:905f93247688 | 170 | wait(rest_time_s); |
kageyuta | 25:5d0977d2d79d | 171 | else { |
kageyuta | 25:5d0977d2d79d | 172 | //計算周期が達成できないときはDEBUGで知らせるだけ。動きはする。 |
shimizuta | 19:1adc7302cfd9 | 173 | DEBUG("error: rest_time_s = %f in Move()\r\n", rest_time_s); |
shimizuta | 14:d7cb429946f4 | 174 | led[0] = 1; |
shimizuta | 14:d7cb429946f4 | 175 | } |
shimizuta | 9:905f93247688 | 176 | } |
yuto17320508 | 5:556d5a5e9d24 | 177 | } |
shimizuta | 14:d7cb429946f4 | 178 | void MoveServo(OneLeg leg, int serial_num, int servo_id) |
yuto17320508 | 6:43708adf2e5d | 179 | { |
shimizuta | 11:e81425872740 | 180 | float degree = leg.GetRad(servo_id) * kRadToDegree; |
shimizuta | 14:d7cb429946f4 | 181 | //サーボの座標系に変更 |
shimizuta | 14:d7cb429946f4 | 182 | float servo_degree = kServoSign[serial_num][servo_id] * degree + kOriginDegree[serial_num][servo_id]; |
shimizuta | 19:1adc7302cfd9 | 183 | DEBUG("servo_degree[%d][%d],%f\r\n", serial_num, servo_id, servo_degree); |
shimizuta | 14:d7cb429946f4 | 184 | servo[serial_num].set_degree(servo_id, servo_degree); |
shimizuta | 12:2ac37fe6c3bb | 185 | } |