岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

main.cpp@16:0069a56f11a3, 2019-02-13 (annotated)

Committer:
shimizuta
Date:
Wed Feb 13 06:43:05 2019 +0000
Revision:
16:0069a56f11a3
Parent:
14:d7cb429946f4
Child:
17:ca18c5980a34
can is ok

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