岡本 陸 / Mbed 2 deprecated 0311

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?

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 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 }