岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

main.cpp@14:d7cb429946f4, 2019-02-12 (annotated)

Committer:
shimizuta
Date:
Tue Feb 12 12:50:45 2019 +0000
Revision:
14:d7cb429946f4
Parent:
13:e7ecdb20665a
Child:
16:0069a56f11a3
sorry, there're compile errors

Who changed what in which revision?

UserRevisionLine numberNew 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 14:d7cb429946f4 5 #include "debug.h"
shimizuta 14:d7cb429946f4 6 #include "pi.h"
shimizuta 11:e81425872740 7 #include "OneLeg.h" ///足先の座標を保存するクラス。x,yやサーボの角度の保存、サーボの駆動も行う。他の足を考慮した処理は別のクラスに任せる。
shimizuta 11:e81425872740 8 #include "Walk.h" //歩き方に関するファイル
yuto17320508 4:fffdb273836e 9
shimizuta 9:905f93247688 10 ////////////調整すべきパラメータ.全てここに集めた。
shimizuta 14:d7cb429946f4 11 const float kCycleTime_s = 0.03f; //計算周期
shimizuta 14:d7cb429946f4 12 const float kBetweenServoHalf_m = 0.03f * 0.5; //サーボ間の距離の半分
shimizuta 9:905f93247688 13 const float kLegLength1 = 0.1f;
shimizuta 9:905f93247688 14 const float kLegLength2 = 0.2f;
shimizuta 14:d7cb429946f4 15 const int kServoSpan_ms = 10; //サーボの送信間隔
shimizuta 14:d7cb429946f4 16
shimizuta 14:d7cb429946f4 17 //サーボの設定
shimizuta 14:d7cb429946f4 18 const double kServoValToDegree = 270.0 / (11500 - 3500);
shimizuta 14:d7cb429946f4 19 //サーボの正負と座標系の正負の補正.足で一セット。
shimizuta 14:d7cb429946f4 20 const int kServoSign[2][2] = {{
shimizuta 14:d7cb429946f4 21 1,
shimizuta 14:d7cb429946f4 22 -1,
shimizuta 14:d7cb429946f4 23 },
shimizuta 14:d7cb429946f4 24 {
shimizuta 14:d7cb429946f4 25 1,
shimizuta 14:d7cb429946f4 26 -1,
shimizuta 14:d7cb429946f4 27 }};
shimizuta 14:d7cb429946f4 28 //欲しい座標系0度でのサーボのICSマネージャーの値
shimizuta 14:d7cb429946f4 29 const double kOriginDegree[2][2] = {
shimizuta 14:d7cb429946f4 30 {
shimizuta 14:d7cb429946f4 31 (4096 - 3500) * kServoValToDegree + 90,
shimizuta 14:d7cb429946f4 32 (4724 - 3500) * kServoValToDegree + 270,
shimizuta 14:d7cb429946f4 33 },
shimizuta 14:d7cb429946f4 34 {
shimizuta 14:d7cb429946f4 35 (7384 - 3500) * kServoValToDegree ,
shimizuta 14:d7cb429946f4 36 (7523 - 3500) * kServoValToDegree+180,
shimizuta 14:d7cb429946f4 37 },
shimizuta 14:d7cb429946f4 38 };
shimizuta 9:905f93247688 39 ///////////////
shimizuta 9:905f93247688 40 Timer timer;
shimizuta 11:e81425872740 41 KondoServo servo[2] = {
shimizuta 11:e81425872740 42 KondoServo(pin_serial_servo_tx[0], pin_serial_servo_rx[0]),
shimizuta 11:e81425872740 43 KondoServo(pin_serial_servo_tx[1], pin_serial_servo_rx[1]),
shimizuta 11:e81425872740 44 };
shimizuta 11:e81425872740 45 OneLeg leg[4] = {
shimizuta 11:e81425872740 46 OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2),
shimizuta 11:e81425872740 47 OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2),
shimizuta 11:e81425872740 48 OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2),
shimizuta 11:e81425872740 49 OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2),
shimizuta 9:905f93247688 50 };
shimizuta 14:d7cb429946f4 51 DigitalOut led[4] = {DigitalOut(LED1),DigitalOut(LED2),DigitalOut(LED3),DigitalOut(LED4)};
shimizuta 14:d7cb429946f4 52
shimizuta 11:e81425872740 53 const float kRadToDegree = 180.0 / M_PI;
shimizuta 11:e81425872740 54 void Move(Walk WalkWay, OneLeg (&leg)[4], float dist_m);
shimizuta 11:e81425872740 55 void MoveServo(OneLeg leg, int legnum, int servo_id);
shimizuta 12:2ac37fe6c3bb 56
shimizuta 9:905f93247688 57 int main()
yuto17320508 5:556d5a5e9d24 58 {
shimizuta 14:d7cb429946f4 59 DEBUG("When you push any key, this robot starts.\r\n");
shimizuta 11:e81425872740 60 while (pc.readable() == 0) //キーボード押したらスタート
shimizuta 14:d7cb429946f4 61 ;
shimizuta 14:d7cb429946f4 62 DEBUG("move start\r\n");
shimizuta 11:e81425872740 63 //各足の軌道の設定.今回は全部同じにすることで直線を描く。
shimizuta 14:d7cb429946f4 64 float stridetime_s = 0.5, risetime_s = 0.15, stride_m = 0.2f, height_m = 0.05f, ground_m = 0.2f;
shimizuta 14:d7cb429946f4 65 Orbit straightOrbit[4];
shimizuta 14:d7cb429946f4 66 Orbit default_orbit(ELLIPSE);
shimizuta 14:d7cb429946f4 67 default_orbit.SetStraightParam(stridetime_s, risetime_s, stride_m, height_m, ground_m);
shimizuta 14:d7cb429946f4 68 for (int i = 0; i < 4; i++)
shimizuta 14:d7cb429946f4 69 straightOrbit[i] = default_orbit;
shimizuta 11:e81425872740 70 //4足の軌道と位相ずれOffsetTime_sをまとめる
shimizuta 11:e81425872740 71 float offset_time_s[4] = {
shimizuta 11:e81425872740 72 0,
yuto17320508 13:e7ecdb20665a 73 straightOrbit[0].GetOneWalkTime() * 0.5,
shimizuta 11:e81425872740 74 0,
yuto17320508 13:e7ecdb20665a 75 straightOrbit[0].GetOneWalkTime() * 0.5,
shimizuta 11:e81425872740 76 };
yuto17320508 13:e7ecdb20665a 77 //4つの足のorbit, 位相を代入してstraightという歩行パターンを作成している
yuto17320508 13:e7ecdb20665a 78 //このインスタンスはlegそれぞれの計算を行う役割を担う
yuto17320508 13:e7ecdb20665a 79 //orbitはここでしか使わないあくまで鍵のような扱い
yuto17320508 13:e7ecdb20665a 80 Walk straight(straightOrbit, offset_time_s, kCycleTime_s);
shimizuta 11:e81425872740 81 //実際にWalkの指示通りに動かす
shimizuta 11:e81425872740 82 float dist_m = 0.5f;
shimizuta 11:e81425872740 83 Move(straight, leg, dist_m);
shimizuta 14:d7cb429946f4 84 DEBUG("program end\r\n");
shimizuta 2:a92568bdeb5c 85 }
shimizuta 2:a92568bdeb5c 86
yuto17320508 13:e7ecdb20665a 87 //到達判定が来ない間同じ歩行方法でループ
shimizuta 11:e81425872740 88 void Move(Walk WalkWay, OneLeg (&leg)[4], float dist_m)
yuto17320508 5:556d5a5e9d24 89 {
shimizuta 9:905f93247688 90 timer.reset();
shimizuta 9:905f93247688 91 timer.start();
shimizuta 9:905f93247688 92 int is_arrived = 0;
shimizuta 9:905f93247688 93 while (is_arrived == 0)
yuto17320508 8:21b932c4e6c5 94 {
shimizuta 9:905f93247688 95 float time_s = timer.read();
shimizuta 9:905f93247688 96 //注:未実装。到着したかの判定.LRFからのデータが必要？
shimizuta 14:d7cb429946f4 97 //is_arrived = IsArrived();
shimizuta 11:e81425872740 98 //4本の足それぞれの足先サーボ角度更新
shimizuta 11:e81425872740 99 WalkWay.Cal4LegsPosi(leg);
shimizuta 9:905f93247688 100 //注:未実装。slave_mbed分の足の目標位置を送信
yuto17320508 5:556d5a5e9d24 101
shimizuta 9:905f93247688 102 //自身が動かす足のサーボを動かす
shimizuta 14:d7cb429946f4 103 //MoveServo(leg[0], 0, 0);
shimizuta 11:e81425872740 104 MoveServo(leg[1], 1, 0);
shimizuta 9:905f93247688 105 wait_ms(kServoSpan_ms);
shimizuta 14:d7cb429946f4 106 //MoveServo(leg[0], 0, 1);
shimizuta 11:e81425872740 107 MoveServo(leg[1], 1, 1);
shimizuta 14:d7cb429946f4 108 DEBUG("x,y %f,%f\r\n",leg[0].GetX_m(),leg[0].GetY_m());
shimizuta 11:e81425872740 109 //計算周期がWalkWay.cycletime_s_になるようwait
shimizuta 11:e81425872740 110 float rest_time_s = WalkWay.cycletime_s_ - (timer.read() - time_s);
shimizuta 9:905f93247688 111 if (rest_time_s > 0)
shimizuta 9:905f93247688 112 wait(rest_time_s);
shimizuta 14:d7cb429946f4 113 else {//計算周期が達成できないときはDEBUGで知らせるだけ。動きはする。
shimizuta 9:905f93247688 114 printf("error: rest_time_s = %f in Move()\r\n", rest_time_s);
shimizuta 14:d7cb429946f4 115 led[0] = 1;
shimizuta 14:d7cb429946f4 116 }
shimizuta 9:905f93247688 117 }
yuto17320508 5:556d5a5e9d24 118 }
yuto17320508 5:556d5a5e9d24 119
shimizuta 14:d7cb429946f4 120 void MoveServo(OneLeg leg, int serial_num, int servo_id)
yuto17320508 6:43708adf2e5d 121 {
shimizuta 11:e81425872740 122 float degree = leg.GetRad(servo_id) * kRadToDegree;
shimizuta 14:d7cb429946f4 123
shimizuta 14:d7cb429946f4 124
shimizuta 14:d7cb429946f4 125 //サーボの座標系に変更
shimizuta 14:d7cb429946f4 126 float servo_degree = kServoSign[serial_num][servo_id] * degree + kOriginDegree[serial_num][servo_id];
shimizuta 14:d7cb429946f4 127 DEBUG("servo_degree[%d][%d],%f\r\n",serial_num,servo_id,servo_degree);
shimizuta 14:d7cb429946f4 128 servo[serial_num].set_degree(servo_id, servo_degree);
shimizuta 12:2ac37fe6c3bb 129 }