test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
main.cpp
- Committer:
- yuto17320508
- Date:
- 2019-02-15
- Revision:
- 20:70cc6083e9c7
- Parent:
- 19:1adc7302cfd9
- Child:
- 21:61971fc18b90
- Child:
- 25:5d0977d2d79d
File content as of revision 20:70cc6083e9c7:
//NHK2019MR2 馬型機構プログラム. #include "mbed.h" #include "pinnames.h" #include "KondoServo.h" //#define DEBUG_ON//デバッグ用。使わないときはコメントアウト #include "debug.h" #include "pi.h" #include "can.h" #include "OneLeg.h" ///足先の座標を保存するクラス。x,yやサーボの角度の保存、サーボの駆動も行う。他の足を考慮した処理は別のクラスに任せる。 #include "Walk.h" //歩き方に関するファイル #define USE_CAN //can通信するならdefine.しないなら切らないとエラー出る ////////////調整すべきパラメータ.全てここに集めた。 const float kCycleTime_s = 0.03; //計算周期 const float kBetweenServoHalf_m = 0.03 * 0.5; //サーボ間の距離の半分 float kLegLength1[2] = {0.1, 0.1}; float kLegLength2[2] = {0.2452 + 0.0025, 0.22529 + 0.0025}; //サーボの設定 const int kServoSpan_ms = 10; //サーボの送信間隔 const double kServoValToDegree = 270.0 / (11500 - 3500); //サーボの正負と座標系の正負の補正.足で一セット。 const int kServoSign[2][2] = {{ 1, -1, }, { -1, 1, }}; //欲しい座標系0度でのサーボのICSマネージャーの値 const double kOriginDegree[2][2] = { { (7257 - 3500) * kServoValToDegree, (6590 - 3500) * kServoValToDegree + 180, }, { (6470 - 3500) * kServoValToDegree, (7419 - 3500) * kServoValToDegree - 180, //180度の時6657シータ負の方向に動かすと値は減るので、0度の位置は-180すれば出る }, }; /////////////// Timer timer; KondoServo servo[2] = { KondoServo(pin_serial_servo_tx[0], pin_serial_servo_rx[0]), KondoServo(pin_serial_servo_tx[1], pin_serial_servo_rx[1]), }; //足の作成、サイズデータのみの足の骨組 OneLeg leg[4] = { OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2), OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2), OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2), OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2), }; DigitalOut led[4] = {DigitalOut(LED1), DigitalOut(LED2), DigitalOut(LED3), DigitalOut(LED4)}; const float kRadToDegree = 180.0 / M_PI; void Move(Walk WalkWay, OneLeg (&leg)[4], float dist_m); void MoveServo(OneLeg leg, int legnum, int servo_id); int IsArrived(int count, int finish) { if (count > finish) return 1; else return 0; } int main() { printf("When you push any key, this robot starts.\r\n"); while (pc.readable() == 0) //キーボード押したらスタート ; printf("stand up for 1 s\r\n"); //stand時の足の軌道設定. Orbit stand_orbit[4]; float stand_offset_y_m = 0.2; stand_orbit[0].SetStandParam(stand_offset_y_m); for (int i = 0; i < 4; i++) //全部足を同じにしてる stand_orbit[i] = stand_orbit[0]; //4足の位相ずれOffsetTime_sをまとめる float stand_offset_time_s[4] = { 0, stand_orbit[0].GetOneWalkTime() * 0.5, stand_orbit[0].GetOneWalkTime() * 0, stand_orbit[0].GetOneWalkTime() * 0.5, }; //4つの足のorbit, 位相を代入してstraightという歩行パターンを作成している //このインスタンスはlegそれぞれの計算を行う役割を担う //orbitはここでしか使わないあくまでパラメータ設定用クラス Walk stand(stand_orbit, stand_offset_time_s, kCycleTime_s); float dist_m = 10; //収束判定用。現在は回すループの数 //動作開始 Move(stand, leg, dist_m); wait(1); DEBUG("move start\r\n"); //取り敢えず歩行したパラメータ //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; //曲げてみる float stridetime_s = 2, risetime_s = 0.5, stride_r_m = 0.1, stride_l_m = 0.05, height_m = 0.03, offset_x_m = -0.05, offset_y_m = stand_offset_y_m; //軌道の作成 //代入用の軌道SetStraightParam関数を用いると真っ直ぐ進む前提となる Orbit orbit[4]; orbit[0].SetStraightParam(stridetime_s, risetime_s, stride_r_m, height_m, offset_x_m, offset_y_m); orbit[1].SetStraightParam(stridetime_s, risetime_s, stride_r_m, height_m, 0, offset_y_m); orbit[2].SetStraightParam(stridetime_s, risetime_s, stride_l_m, height_m, offset_x_m, offset_y_m); orbit[3].SetStraightParam(stridetime_s, risetime_s, stride_l_m, height_m, 0, offset_y_m); //4足の軌道と位相ずれOffsetTime_sをまとめる float offset_time_s[4] = { 0, orbit[0].GetOneWalkTime() * 0.5, orbit[0].GetOneWalkTime() * 0.5, orbit[0].GetOneWalkTime() * 0, }; //4つの足のorbit, 位相を代入して歩行パターンを作成している //このインスタンスはlegそれぞれの計算を行う役割を担う //orbitはここでしか使わないあくまで鍵のような扱い Walk walk(orbit, offset_time_s, kCycleTime_s); dist_m = 500000; //収束判定用。取り敢えず今はループ数。 //動く Move(walk, leg, dist_m); DEBUG("program end\r\n"); } //到達判定が来ない間同じ歩行方法でループ void Move(Walk WalkWay, OneLeg (&leg)[4], float dist_m) { timer.reset(); timer.start(); int is_arrived = 0; int count = 0; while (is_arrived == 0) { float time_s = timer.read(); //注:未実装。到着したかの判定.LRFからのデータが必要.今は取り敢えずループ回数で判断 is_arrived = IsArrived(count, (int)dist_m); ++count; //4本の足それぞれの足先サーボ角度更新 WalkWay.Cal4LegsPosi(leg); #ifdef USE_CAN //slave_mbed分の足の目標位置を送信 SendRad(leg[2], leg[3]); #endif //自身が動かす足のサーボを動かす MoveServo(leg[0], 0, 0); MoveServo(leg[1], 1, 0); wait_ms(kServoSpan_ms); MoveServo(leg[0], 0, 1); MoveServo(leg[1], 1, 1); DEBUG("%f, %f, %f, %f\r\n", leg[2].GetRad(0), leg[2].GetRad(1), leg[3].GetRad(0), leg[3].GetRad(1)); //計算周期がWalkWay.cycletime_s_になるようwait float rest_time_s = WalkWay.cycletime_s_ - (timer.read() - time_s); if (rest_time_s > 0) wait(rest_time_s); else { //計算周期が達成できないときはDEBUGで知らせるだけ。動きはする。 DEBUG("error: rest_time_s = %f in Move()\r\n", rest_time_s); led[0] = 1; } } } void MoveServo(OneLeg leg, int serial_num, int servo_id) { float degree = leg.GetRad(servo_id) * kRadToDegree; //サーボの座標系に変更 float servo_degree = kServoSign[serial_num][servo_id] * degree + kOriginDegree[serial_num][servo_id]; DEBUG("servo_degree[%d][%d],%f\r\n", serial_num, servo_id, servo_degree); servo[serial_num].set_degree(servo_id, servo_degree); }