岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

main.cpp@12:2ac37fe6c3bb, 2019-02-11 (annotated)

Committer:
shimizuta
Date:
Mon Feb 11 13:32:46 2019 +0000
Revision:
12:2ac37fe6c3bb
Parent:
11:e81425872740
Child:
13:e7ecdb20665a
printf debug finish

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;
shimizuta 12:2ac37fe6c3bb 13 const float kServoSpan_ms = 0; //サーボの送信間隔
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 {
shimizuta 11:e81425872740 35 printf("When you push any key, this robot starts.\r\n");
shimizuta 11:e81425872740 36 while (pc.readable() == 0) //キーボード押したらスタート
shimizuta 10:7a340c52e270 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;
shimizuta 11:e81425872740 41 Orbit orbit[4] = {
shimizuta 11:e81425872740 42 Orbit(stridetime_s, risetime_s, stride_m, height_m, ground_m),
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 };
shimizuta 11:e81425872740 47 //4足の軌道と位相ずれOffsetTime_sをまとめる
shimizuta 11:e81425872740 48 float offset_time_s[4] = {
shimizuta 11:e81425872740 49 0,
shimizuta 11:e81425872740 50 orbit[0].GetOneWalkTime() * 0.5,
shimizuta 11:e81425872740 51 0,
shimizuta 11:e81425872740 52 orbit[0].GetOneWalkTime() * 0.5,
shimizuta 11:e81425872740 53 };
shimizuta 11:e81425872740 54 Walk straight(orbit, offset_time_s, kCycleTime_s);
shimizuta 11:e81425872740 55 //実際にWalkの指示通りに動かす
shimizuta 11:e81425872740 56 float dist_m = 0.5f;
shimizuta 11:e81425872740 57 Move(straight, leg, dist_m);
shimizuta 11:e81425872740 58
shimizuta 10:7a340c52e270 59 printf("program end\r\n");
shimizuta 2:a92568bdeb5c 60 }
shimizuta 2:a92568bdeb5c 61
shimizuta 11:e81425872740 62 void Move(Walk WalkWay, OneLeg (&leg)[4], float dist_m)
yuto17320508 5:556d5a5e9d24 63 {
shimizuta 9:905f93247688 64 timer.reset();
shimizuta 9:905f93247688 65 timer.start();
shimizuta 9:905f93247688 66 int is_arrived = 0;
shimizuta 9:905f93247688 67 while (is_arrived == 0)
yuto17320508 8:21b932c4e6c5 68 {
shimizuta 9:905f93247688 69 float time_s = timer.read();
shimizuta 9:905f93247688 70 //注:未実装。到着したかの判定.LRFからのデータが必要？
shimizuta 12:2ac37fe6c3bb 71 //is_arrived = IsArrived();
shimizuta 11:e81425872740 72 //4本の足それぞれの足先サーボ角度更新
shimizuta 11:e81425872740 73 WalkWay.Cal4LegsPosi(leg);
shimizuta 9:905f93247688 74 //注:未実装。slave_mbed分の足の目標位置を送信
yuto17320508 5:556d5a5e9d24 75
shimizuta 9:905f93247688 76 //自身が動かす足のサーボを動かす
shimizuta 11:e81425872740 77 MoveServo(leg[0], 0, 0);
shimizuta 11:e81425872740 78 MoveServo(leg[1], 1, 0);
shimizuta 9:905f93247688 79 wait_ms(kServoSpan_ms);
shimizuta 11:e81425872740 80 MoveServo(leg[0], 0, 1);
shimizuta 11:e81425872740 81 MoveServo(leg[1], 1, 1);
shimizuta 11:e81425872740 82 //計算周期がWalkWay.cycletime_s_になるようwait
shimizuta 11:e81425872740 83 float rest_time_s = WalkWay.cycletime_s_ - (timer.read() - time_s);
shimizuta 9:905f93247688 84 if (rest_time_s > 0)
shimizuta 9:905f93247688 85 wait(rest_time_s);
shimizuta 10:7a340c52e270 86 else //計算周期が達成できないときはprintfで知らせるだけ。動きはする。
shimizuta 9:905f93247688 87 printf("error: rest_time_s = %f in Move()\r\n", rest_time_s);
shimizuta 9:905f93247688 88 }
yuto17320508 5:556d5a5e9d24 89 }
yuto17320508 5:556d5a5e9d24 90
shimizuta 11:e81425872740 91 void MoveServo(OneLeg leg, int legnum, int servo_id)
yuto17320508 6:43708adf2e5d 92 {
shimizuta 11:e81425872740 93 float degree = leg.GetRad(servo_id) * kRadToDegree;
shimizuta 11:e81425872740 94 //servo1は反転させる
shimizuta 11:e81425872740 95 if (servo_id == 0)
shimizuta 11:e81425872740 96 degree += 90;
shimizuta 11:e81425872740 97 else
shimizuta 11:e81425872740 98 degree = 270 - degree;
shimizuta 11:e81425872740 99 servo[legnum].set_degree(servo_id, degree);
shimizuta 12:2ac37fe6c3bb 100 }