岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

main.cpp@11:e81425872740, 2019-02-11 (annotated)

Committer:
shimizuta
Date:
Mon Feb 11 12:56:00 2019 +0000
Revision:
11:e81425872740
Parent:
10:7a340c52e270
Child:
12:2ac37fe6c3bb
ver2

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 11:e81425872740 9 const float kCycleTime_s = 0.01f; //計算周期
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 11:e81425872740 13 const float kServoSpan_ms = 5; //サーボの送信間隔
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 9:905f93247688 32 int main()
yuto17320508 5:556d5a5e9d24 33 {
shimizuta 11:e81425872740 34 printf("When you push any key, this robot starts.\r\n");
shimizuta 11:e81425872740 35 while (pc.readable() == 0) //キーボード押したらスタート
shimizuta 10:7a340c52e270 36 ;
shimizuta 10:7a340c52e270 37 printf("move start\r\n");
shimizuta 11:e81425872740 38 //各足の軌道の設定.今回は全部同じにすることで直線を描く。
shimizuta 11:e81425872740 39 float stridetime_s = 1, risetime_s = 0.3, stride_m = 0.2f, height_m = 0.03f, ground_m = 0.14f;
shimizuta 11:e81425872740 40 Orbit orbit[4] = {
shimizuta 11:e81425872740 41 Orbit(stridetime_s, risetime_s, stride_m, height_m, ground_m),
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 };
shimizuta 11:e81425872740 46 //4足の軌道と位相ずれOffsetTime_sをまとめる
shimizuta 11:e81425872740 47 float offset_time_s[4] = {
shimizuta 11:e81425872740 48 0,
shimizuta 11:e81425872740 49 orbit[0].GetOneWalkTime() * 0.5,
shimizuta 11:e81425872740 50 0,
shimizuta 11:e81425872740 51 orbit[0].GetOneWalkTime() * 0.5,
shimizuta 11:e81425872740 52 };
shimizuta 11:e81425872740 53 Walk straight(orbit, offset_time_s, kCycleTime_s);
shimizuta 11:e81425872740 54 //実際にWalkの指示通りに動かす
shimizuta 11:e81425872740 55 float dist_m = 0.5f;
shimizuta 11:e81425872740 56 Move(straight, leg, dist_m);
shimizuta 11:e81425872740 57
shimizuta 10:7a340c52e270 58 printf("program end\r\n");
shimizuta 2:a92568bdeb5c 59 }
shimizuta 2:a92568bdeb5c 60
shimizuta 11:e81425872740 61 void Move(Walk WalkWay, OneLeg (&leg)[4], float dist_m)
yuto17320508 5:556d5a5e9d24 62 {
shimizuta 9:905f93247688 63 timer.reset();
shimizuta 9:905f93247688 64 timer.start();
shimizuta 9:905f93247688 65 int is_arrived = 0;
shimizuta 9:905f93247688 66 while (is_arrived == 0)
yuto17320508 8:21b932c4e6c5 67 {
shimizuta 9:905f93247688 68 float time_s = timer.read();
shimizuta 9:905f93247688 69 //注:未実装。到着したかの判定.LRFからのデータが必要？
shimizuta 10:7a340c52e270 70 // is_arrived = IsArrived();
shimizuta 9:905f93247688 71
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 printf("%f,%f\r\n", leg[0].GetRad(0), leg[1].GetRad(0));
shimizuta 11:e81425872740 83 //計算周期がWalkWay.cycletime_s_になるようwait
shimizuta 11:e81425872740 84 float rest_time_s = WalkWay.cycletime_s_ - (timer.read() - time_s);
shimizuta 9:905f93247688 85 if (rest_time_s > 0)
shimizuta 9:905f93247688 86 wait(rest_time_s);
shimizuta 10:7a340c52e270 87 else //計算周期が達成できないときはprintfで知らせるだけ。動きはする。
shimizuta 9:905f93247688 88 printf("error: rest_time_s = %f in Move()\r\n", rest_time_s);
shimizuta 9:905f93247688 89 }
yuto17320508 5:556d5a5e9d24 90 }
yuto17320508 5:556d5a5e9d24 91
shimizuta 11:e81425872740 92 void MoveServo(OneLeg leg, int legnum, int servo_id)
yuto17320508 6:43708adf2e5d 93 {
shimizuta 11:e81425872740 94 float degree = leg.GetRad(servo_id) * kRadToDegree;
shimizuta 11:e81425872740 95 //servo1は反転させる
shimizuta 11:e81425872740 96 if (servo_id == 0)
shimizuta 11:e81425872740 97 degree += 90;
shimizuta 11:e81425872740 98 else
shimizuta 11:e81425872740 99 degree = 270 - degree;
shimizuta 11:e81425872740 100 servo[legnum].set_degree(servo_id, degree);
shimizuta 11:e81425872740 101 }