岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

main.cpp@18:0033ef1814ba, 2019-02-14 (annotated)

Committer:
yuto17320508
Date:
Thu Feb 14 09:04:25 2019 +0000
Revision:
18:0033ef1814ba
Parent:
17:ca18c5980a34
Child:
19:1adc7302cfd9
move

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"
yuto17320508 18:0033ef1814ba 5 //#define DEBUG_ON//デバッグ用。使わないときはコメントアウト
shimizuta 14:d7cb429946f4 6 #include "debug.h"
shimizuta 14:d7cb429946f4 7 #include "pi.h"
shimizuta 16:0069a56f11a3 8 #include "can.h"
shimizuta 11:e81425872740 9 #include "OneLeg.h" ///足先の座標を保存するクラス。x,yやサーボの角度の保存、サーボの駆動も行う。他の足を考慮した処理は別のクラスに任せる。
shimizuta 11:e81425872740 10 #include "Walk.h" //歩き方に関するファイル
yuto17320508 18:0033ef1814ba 11 #define USE_CAN//can通信するならdefine.しないなら切らないとエラー出る
yuto17320508 4:fffdb273836e 12
shimizuta 9:905f93247688 13 ////////////調整すべきパラメータ.全てここに集めた。
shimizuta 14:d7cb429946f4 14 const float kCycleTime_s = 0.03f; //計算周期
shimizuta 14:d7cb429946f4 15 const float kBetweenServoHalf_m = 0.03f * 0.5; //サーボ間の距離の半分
shimizuta 9:905f93247688 16 const float kLegLength1 = 0.1f;
yuto17320508 18:0033ef1814ba 17 const float kLegLength2 = 0.2452f+0.0025f;//0.22529+0.0025
shimizuta 16:0069a56f11a3 18 //サーボの設定
shimizuta 14:d7cb429946f4 19 const int kServoSpan_ms = 10; //サーボの送信間隔
shimizuta 14:d7cb429946f4 20 const double kServoValToDegree = 270.0 / (11500 - 3500);
shimizuta 14:d7cb429946f4 21 //サーボの正負と座標系の正負の補正.足で一セット。
shimizuta 14:d7cb429946f4 22 const int kServoSign[2][2] = {{
shimizuta 14:d7cb429946f4 23 1,
shimizuta 14:d7cb429946f4 24 -1,
shimizuta 14:d7cb429946f4 25 },
shimizuta 14:d7cb429946f4 26 {
yuto17320508 18:0033ef1814ba 27 -1,
shimizuta 14:d7cb429946f4 28 1,
shimizuta 14:d7cb429946f4 29 }};
shimizuta 14:d7cb429946f4 30 //欲しい座標系0度でのサーボのICSマネージャーの値
shimizuta 14:d7cb429946f4 31 const double kOriginDegree[2][2] = {
shimizuta 14:d7cb429946f4 32 {
yuto17320508 18:0033ef1814ba 33 (7199 - 3500) * kServoValToDegree,
yuto17320508 18:0033ef1814ba 34 (6600 - 3500) * kServoValToDegree + 180,
shimizuta 14:d7cb429946f4 35 },
shimizuta 14:d7cb429946f4 36 {
yuto17320508 18:0033ef1814ba 37 (7523 - 3500) * kServoValToDegree ,
yuto17320508 18:0033ef1814ba 38 (6657 - 3500) * kServoValToDegree-180,//180度の時6657シータ負の方向に動かすと値は減る
shimizuta 14:d7cb429946f4 39 },
shimizuta 14:d7cb429946f4 40 };
shimizuta 9:905f93247688 41 ///////////////
shimizuta 9:905f93247688 42 Timer timer;
shimizuta 11:e81425872740 43 KondoServo servo[2] = {
shimizuta 11:e81425872740 44 KondoServo(pin_serial_servo_tx[0], pin_serial_servo_rx[0]),
shimizuta 11:e81425872740 45 KondoServo(pin_serial_servo_tx[1], pin_serial_servo_rx[1]),
shimizuta 11:e81425872740 46 };
yuto17320508 18:0033ef1814ba 47 //足の作成、サイズデータのみの足の骨組
shimizuta 11:e81425872740 48 OneLeg leg[4] = {
shimizuta 11:e81425872740 49 OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2),
shimizuta 11:e81425872740 50 OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2),
shimizuta 11:e81425872740 51 OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2),
shimizuta 11:e81425872740 52 OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2),
shimizuta 9:905f93247688 53 };
shimizuta 14:d7cb429946f4 54 DigitalOut led[4] = {DigitalOut(LED1),DigitalOut(LED2),DigitalOut(LED3),DigitalOut(LED4)};
shimizuta 14:d7cb429946f4 55
shimizuta 11:e81425872740 56 const float kRadToDegree = 180.0 / M_PI;
shimizuta 11:e81425872740 57 void Move(Walk WalkWay, OneLeg (&leg)[4], float dist_m);
shimizuta 11:e81425872740 58 void MoveServo(OneLeg leg, int legnum, int servo_id);
shimizuta 12:2ac37fe6c3bb 59
yuto17320508 18:0033ef1814ba 60 int IsArrived(int count, int finish){
yuto17320508 18:0033ef1814ba 61 if(count > finish)
yuto17320508 18:0033ef1814ba 62 return 1;
yuto17320508 18:0033ef1814ba 63 else
yuto17320508 18:0033ef1814ba 64 return 0;
yuto17320508 18:0033ef1814ba 65 }
yuto17320508 18:0033ef1814ba 66
shimizuta 9:905f93247688 67 int main()
yuto17320508 5:556d5a5e9d24 68 {
yuto17320508 18:0033ef1814ba 69 printf("When you push any key, this robot starts.\r\n");
shimizuta 11:e81425872740 70 while (pc.readable() == 0) //キーボード押したらスタート
shimizuta 14:d7cb429946f4 71 ;
yuto17320508 18:0033ef1814ba 72 printf("stand up. 1 sec stand\r\n");
yuto17320508 18:0033ef1814ba 73
yuto17320508 18:0033ef1814ba 74 //stand時の足の軌道設定
yuto17320508 18:0033ef1814ba 75 Orbit stand_orbits[4];
yuto17320508 18:0033ef1814ba 76 Orbit temp_orbit(ELLIPSE);
yuto17320508 18:0033ef1814ba 77 float stand_stridetime_s = 1, stand_risetime_s = 0, stand_stride_m = 0, stand_height_m = 0.05f, stand_ground_m = 0.25f;
yuto17320508 18:0033ef1814ba 78 temp_orbit.SetStraightParam(stand_stridetime_s, stand_risetime_s, stand_stride_m, stand_height_m, stand_ground_m);
yuto17320508 18:0033ef1814ba 79 for (int i = 0; i < 4; i++)
yuto17320508 18:0033ef1814ba 80 stand_orbits[i] = temp_orbit;
yuto17320508 18:0033ef1814ba 81
yuto17320508 18:0033ef1814ba 82 //4足の位相ずれOffsetTime_sをまとめる
yuto17320508 18:0033ef1814ba 83 float stand_offset_time_s[4] = {
yuto17320508 18:0033ef1814ba 84 0,
yuto17320508 18:0033ef1814ba 85 stand_orbits[0].GetOneWalkTime() * 0.5,
yuto17320508 18:0033ef1814ba 86 stand_orbits[0].GetOneWalkTime() * 0,
yuto17320508 18:0033ef1814ba 87 stand_orbits[0].GetOneWalkTime() * 0.5,};
yuto17320508 18:0033ef1814ba 88 //4つの足のorbit, 位相を代入してstraightという歩行パターンを作成している
yuto17320508 18:0033ef1814ba 89 //このインスタンスはlegそれぞれの計算を行う役割を担う
yuto17320508 18:0033ef1814ba 90 //orbitはここでしか使わないあくまで鍵のような扱い
yuto17320508 18:0033ef1814ba 91 Walk stand(stand_orbits, stand_offset_time_s, kCycleTime_s);
yuto17320508 18:0033ef1814ba 92 //収束判定用関数。現在は回すループの数
yuto17320508 18:0033ef1814ba 93 float dist_m = 10;
yuto17320508 18:0033ef1814ba 94 //Walkの指示通りに動作
yuto17320508 18:0033ef1814ba 95 Move(stand, leg, dist_m);
yuto17320508 18:0033ef1814ba 96
yuto17320508 18:0033ef1814ba 97 wait(1);
yuto17320508 18:0033ef1814ba 98 DEBUG("move start\r\n");
shimizuta 11:e81425872740 99 //各足の軌道の設定.今回は全部同じにすることで直線を描く。
yuto17320508 18:0033ef1814ba 100 //直進したパラメータstridetime_s = 0.5, risetime_s = 0.3, stride_m = 0.12f, height_m = 0.03f, ground_m = 0.16f;
yuto17320508 18:0033ef1814ba 101 float stridetime_s = 3, risetime_s = 0.3, stride_m = 0.10f, height_m = 0.1f, ground_m = stand_ground_m;
yuto17320508 18:0033ef1814ba 102 //軌道の作成
yuto17320508 18:0033ef1814ba 103 //代入用の軌道SetStraightParam関数を用いると真っ直ぐ進む前提となる
yuto17320508 18:0033ef1814ba 104 temp_orbit.SetStraightParam(stridetime_s, risetime_s, stride_m, height_m, ground_m);
shimizuta 14:d7cb429946f4 105 Orbit straightOrbit[4];
yuto17320508 18:0033ef1814ba 106 straightOrbit[1] = temp_orbit;
yuto17320508 18:0033ef1814ba 107 straightOrbit[3]= temp_orbit;
yuto17320508 18:0033ef1814ba 108 //stride_m = 0.075f;
yuto17320508 18:0033ef1814ba 109 float offset_x_m = 0.1;
yuto17320508 18:0033ef1814ba 110 temp_orbit.SetStraightParam(stridetime_s, risetime_s, stride_m, height_m, ground_m, offset_x_m);
yuto17320508 18:0033ef1814ba 111 straightOrbit[0] = temp_orbit;
yuto17320508 18:0033ef1814ba 112 straightOrbit[2]= temp_orbit;
yuto17320508 18:0033ef1814ba 113
shimizuta 11:e81425872740 114 //4足の軌道と位相ずれOffsetTime_sをまとめる
shimizuta 11:e81425872740 115 float offset_time_s[4] = {
shimizuta 11:e81425872740 116 0,
yuto17320508 18:0033ef1814ba 117 straightOrbit[0].GetOneWalkTime() * 0.25,
yuto17320508 13:e7ecdb20665a 118 straightOrbit[0].GetOneWalkTime() * 0.5,
yuto17320508 18:0033ef1814ba 119 straightOrbit[0].GetOneWalkTime() * 0.75,
shimizuta 11:e81425872740 120 };
yuto17320508 13:e7ecdb20665a 121 //4つの足のorbit, 位相を代入してstraightという歩行パターンを作成している
yuto17320508 13:e7ecdb20665a 122 //このインスタンスはlegそれぞれの計算を行う役割を担う
yuto17320508 13:e7ecdb20665a 123 //orbitはここでしか使わないあくまで鍵のような扱い
yuto17320508 18:0033ef1814ba 124 Walk straight2(straightOrbit, offset_time_s, kCycleTime_s);
shimizuta 11:e81425872740 125 //実際にWalkの指示通りに動かす
yuto17320508 18:0033ef1814ba 126 dist_m = 500;
yuto17320508 18:0033ef1814ba 127 Move(straight2, leg, dist_m);
shimizuta 14:d7cb429946f4 128 DEBUG("program end\r\n");
shimizuta 2:a92568bdeb5c 129 }
shimizuta 2:a92568bdeb5c 130
yuto17320508 13:e7ecdb20665a 131 //到達判定が来ない間同じ歩行方法でループ
shimizuta 11:e81425872740 132 void Move(Walk WalkWay, OneLeg (&leg)[4], float dist_m)
yuto17320508 5:556d5a5e9d24 133 {
shimizuta 9:905f93247688 134 timer.reset();
shimizuta 9:905f93247688 135 timer.start();
shimizuta 9:905f93247688 136 int is_arrived = 0;
yuto17320508 18:0033ef1814ba 137 int count = 0;
shimizuta 9:905f93247688 138 while (is_arrived == 0)
yuto17320508 8:21b932c4e6c5 139 {
shimizuta 9:905f93247688 140 float time_s = timer.read();
shimizuta 9:905f93247688 141 //注:未実装。到着したかの判定.LRFからのデータが必要？
yuto17320508 18:0033ef1814ba 142 is_arrived = IsArrived(count, (int)dist_m);
yuto17320508 18:0033ef1814ba 143 ++count;
shimizuta 11:e81425872740 144 //4本の足それぞれの足先サーボ角度更新
shimizuta 11:e81425872740 145 WalkWay.Cal4LegsPosi(leg);
shimizuta 16:0069a56f11a3 146 #ifdef USE_CAN
shimizuta 16:0069a56f11a3 147 //slave_mbed分の足の目標位置を送信
shimizuta 16:0069a56f11a3 148 SendRad(leg[2],leg[3]);
shimizuta 16:0069a56f11a3 149 #endif
shimizuta 9:905f93247688 150 //自身が動かす足のサーボを動かす
shimizuta 16:0069a56f11a3 151 MoveServo(leg[0], 0, 0);
shimizuta 11:e81425872740 152 MoveServo(leg[1], 1, 0);
shimizuta 9:905f93247688 153 wait_ms(kServoSpan_ms);
shimizuta 16:0069a56f11a3 154 MoveServo(leg[0], 0, 1);
shimizuta 11:e81425872740 155 MoveServo(leg[1], 1, 1);
shimizuta 16:0069a56f11a3 156 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 157 //計算周期がWalkWay.cycletime_s_になるようwait
shimizuta 11:e81425872740 158 float rest_time_s = WalkWay.cycletime_s_ - (timer.read() - time_s);
shimizuta 9:905f93247688 159 if (rest_time_s > 0)
shimizuta 9:905f93247688 160 wait(rest_time_s);
shimizuta 14:d7cb429946f4 161 else {//計算周期が達成できないときはDEBUGで知らせるだけ。動きはする。
shimizuta 9:905f93247688 162 printf("error: rest_time_s = %f in Move()\r\n", rest_time_s);
shimizuta 14:d7cb429946f4 163 led[0] = 1;
shimizuta 14:d7cb429946f4 164 }
shimizuta 9:905f93247688 165 }
yuto17320508 5:556d5a5e9d24 166 }
yuto17320508 5:556d5a5e9d24 167
shimizuta 14:d7cb429946f4 168 void MoveServo(OneLeg leg, int serial_num, int servo_id)
yuto17320508 6:43708adf2e5d 169 {
shimizuta 11:e81425872740 170 float degree = leg.GetRad(servo_id) * kRadToDegree;
shimizuta 14:d7cb429946f4 171 //サーボの座標系に変更
shimizuta 14:d7cb429946f4 172 float servo_degree = kServoSign[serial_num][servo_id] * degree + kOriginDegree[serial_num][servo_id];
yuto17320508 18:0033ef1814ba 173 DEBUG("servo_degree[%d][%d],%f\r\n",serial_num,servo_id,servo_degree);
shimizuta 14:d7cb429946f4 174 servo[serial_num].set_degree(servo_id, servo_degree);
shimizuta 12:2ac37fe6c3bb 175 }