岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

main.cpp@20:70cc6083e9c7, 2019-02-15 (annotated)

Committer:
yuto17320508
Date:
Fri Feb 15 06:46:39 2019 +0000
Revision:
20:70cc6083e9c7
Parent:
19:1adc7302cfd9
Child:
21:61971fc18b90
Child:
25:5d0977d2d79d
servo degree change;

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" //歩き方に関するファイル
shimizuta 19:1adc7302cfd9 11 #define USE_CAN //can通信するならdefine.しないなら切らないとエラー出る
yuto17320508 4:fffdb273836e 12
shimizuta 9:905f93247688 13 ////////////調整すべきパラメータ.全てここに集めた。
shimizuta 19:1adc7302cfd9 14 const float kCycleTime_s = 0.03; //計算周期
shimizuta 19:1adc7302cfd9 15 const float kBetweenServoHalf_m = 0.03 * 0.5; //サーボ間の距離の半分
shimizuta 19:1adc7302cfd9 16 float kLegLength1[2] = {0.1, 0.1};
shimizuta 19:1adc7302cfd9 17 float kLegLength2[2] = {0.2452 + 0.0025, 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 19:1adc7302cfd9 29 }};
shimizuta 14:d7cb429946f4 30 //欲しい座標系0度でのサーボのICSマネージャーの値
shimizuta 14:d7cb429946f4 31 const double kOriginDegree[2][2] = {
shimizuta 14:d7cb429946f4 32 {
yuto17320508 20:70cc6083e9c7 33 (7257 - 3500) * kServoValToDegree,
yuto17320508 20:70cc6083e9c7 34 (6590 - 3500) * kServoValToDegree + 180,
shimizuta 14:d7cb429946f4 35 },
shimizuta 14:d7cb429946f4 36 {
yuto17320508 20:70cc6083e9c7 37 (6470 - 3500) * kServoValToDegree,
yuto17320508 20:70cc6083e9c7 38 (7419 - 3500) * kServoValToDegree - 180, //180度の時6657シータ負の方向に動かすと値は減るので、0度の位置は-180すれば出る
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 19:1adc7302cfd9 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 19:1adc7302cfd9 59 int IsArrived(int count, int finish)
shimizuta 19:1adc7302cfd9 60 {
shimizuta 19:1adc7302cfd9 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 }
shimizuta 9:905f93247688 66 int main()
yuto17320508 5:556d5a5e9d24 67 {
yuto17320508 18:0033ef1814ba 68 printf("When you push any key, this robot starts.\r\n");
shimizuta 11:e81425872740 69 while (pc.readable() == 0) //キーボード押したらスタート
shimizuta 14:d7cb429946f4 70 ;
shimizuta 19:1adc7302cfd9 71 printf("stand up for 1 s\r\n");
shimizuta 19:1adc7302cfd9 72
shimizuta 19:1adc7302cfd9 73 //stand時の足の軌道設定.
shimizuta 19:1adc7302cfd9 74 Orbit stand_orbit[4];
shimizuta 19:1adc7302cfd9 75 float stand_offset_y_m = 0.2;
shimizuta 19:1adc7302cfd9 76 stand_orbit[0].SetStandParam(stand_offset_y_m);
shimizuta 19:1adc7302cfd9 77 for (int i = 0; i < 4; i++) //全部足を同じにしてる
shimizuta 19:1adc7302cfd9 78 stand_orbit[i] = stand_orbit[0];
yuto17320508 18:0033ef1814ba 79 //4足の位相ずれOffsetTime_sをまとめる
yuto17320508 18:0033ef1814ba 80 float stand_offset_time_s[4] = {
yuto17320508 18:0033ef1814ba 81 0,
shimizuta 19:1adc7302cfd9 82 stand_orbit[0].GetOneWalkTime() * 0.5,
shimizuta 19:1adc7302cfd9 83 stand_orbit[0].GetOneWalkTime() * 0,
shimizuta 19:1adc7302cfd9 84 stand_orbit[0].GetOneWalkTime() * 0.5,
shimizuta 19:1adc7302cfd9 85 };
yuto17320508 18:0033ef1814ba 86 //4つの足のorbit, 位相を代入してstraightという歩行パターンを作成している
yuto17320508 18:0033ef1814ba 87 //このインスタンスはlegそれぞれの計算を行う役割を担う
shimizuta 19:1adc7302cfd9 88 //orbitはここでしか使わないあくまでパラメータ設定用クラス
shimizuta 19:1adc7302cfd9 89 Walk stand(stand_orbit, stand_offset_time_s, kCycleTime_s);
shimizuta 19:1adc7302cfd9 90 float dist_m = 10; //収束判定用。現在は回すループの数
shimizuta 19:1adc7302cfd9 91 //動作開始
yuto17320508 18:0033ef1814ba 92 Move(stand, leg, dist_m);
yuto17320508 18:0033ef1814ba 93 wait(1);
shimizuta 19:1adc7302cfd9 94
shimizuta 19:1adc7302cfd9 95 DEBUG("move start\r\n");
shimizuta 19:1adc7302cfd9 96 //取り敢えず歩行したパラメータ
shimizuta 19:1adc7302cfd9 97 //stridetime_s = 0.5, risetime_s = 0.3,stride_m= 0.12f, height_m = 0.03f, offset_x_m = 0, offset_y_m = 0.16f;
shimizuta 19:1adc7302cfd9 98 //曲げてみる
shimizuta 19:1adc7302cfd9 99 float stridetime_s = 2, risetime_s = 0.5,
shimizuta 19:1adc7302cfd9 100 stride_r_m = 0.1, stride_l_m = 0.05, height_m = 0.03,
shimizuta 19:1adc7302cfd9 101 offset_x_m = -0.05, offset_y_m = stand_offset_y_m;
yuto17320508 18:0033ef1814ba 102 //軌道の作成
yuto17320508 18:0033ef1814ba 103 //代入用の軌道SetStraightParam関数を用いると真っ直ぐ進む前提となる
shimizuta 19:1adc7302cfd9 104 Orbit orbit[4];
shimizuta 19:1adc7302cfd9 105 orbit[0].SetStraightParam(stridetime_s, risetime_s, stride_r_m, height_m, offset_x_m, offset_y_m);
shimizuta 19:1adc7302cfd9 106 orbit[1].SetStraightParam(stridetime_s, risetime_s, stride_r_m, height_m, 0, offset_y_m);
shimizuta 19:1adc7302cfd9 107 orbit[2].SetStraightParam(stridetime_s, risetime_s, stride_l_m, height_m, offset_x_m, offset_y_m);
shimizuta 19:1adc7302cfd9 108 orbit[3].SetStraightParam(stridetime_s, risetime_s, stride_l_m, height_m, 0, offset_y_m);
shimizuta 11:e81425872740 109 //4足の軌道と位相ずれOffsetTime_sをまとめる
shimizuta 11:e81425872740 110 float offset_time_s[4] = {
shimizuta 11:e81425872740 111 0,
shimizuta 19:1adc7302cfd9 112 orbit[0].GetOneWalkTime() * 0.5,
shimizuta 19:1adc7302cfd9 113 orbit[0].GetOneWalkTime() * 0.5,
shimizuta 19:1adc7302cfd9 114 orbit[0].GetOneWalkTime() * 0,
shimizuta 11:e81425872740 115 };
shimizuta 19:1adc7302cfd9 116 //4つの足のorbit, 位相を代入して歩行パターンを作成している
yuto17320508 13:e7ecdb20665a 117 //このインスタンスはlegそれぞれの計算を行う役割を担う
yuto17320508 13:e7ecdb20665a 118 //orbitはここでしか使わないあくまで鍵のような扱い
shimizuta 19:1adc7302cfd9 119 Walk walk(orbit, offset_time_s, kCycleTime_s);
shimizuta 19:1adc7302cfd9 120 dist_m = 500000; //収束判定用。取り敢えず今はループ数。
shimizuta 19:1adc7302cfd9 121 //動く
shimizuta 19:1adc7302cfd9 122 Move(walk, leg, dist_m);
shimizuta 14:d7cb429946f4 123 DEBUG("program end\r\n");
shimizuta 2:a92568bdeb5c 124 }
shimizuta 2:a92568bdeb5c 125
yuto17320508 13:e7ecdb20665a 126 //到達判定が来ない間同じ歩行方法でループ
shimizuta 11:e81425872740 127 void Move(Walk WalkWay, OneLeg (&leg)[4], float dist_m)
yuto17320508 5:556d5a5e9d24 128 {
shimizuta 9:905f93247688 129 timer.reset();
shimizuta 9:905f93247688 130 timer.start();
shimizuta 9:905f93247688 131 int is_arrived = 0;
yuto17320508 18:0033ef1814ba 132 int count = 0;
shimizuta 9:905f93247688 133 while (is_arrived == 0)
yuto17320508 8:21b932c4e6c5 134 {
shimizuta 9:905f93247688 135 float time_s = timer.read();
shimizuta 19:1adc7302cfd9 136 //注:未実装。到着したかの判定.LRFからのデータが必要.今は取り敢えずループ回数で判断
yuto17320508 18:0033ef1814ba 137 is_arrived = IsArrived(count, (int)dist_m);
yuto17320508 18:0033ef1814ba 138 ++count;
shimizuta 11:e81425872740 139 //4本の足それぞれの足先サーボ角度更新
shimizuta 11:e81425872740 140 WalkWay.Cal4LegsPosi(leg);
shimizuta 19:1adc7302cfd9 141 #ifdef USE_CAN
shimizuta 16:0069a56f11a3 142 //slave_mbed分の足の目標位置を送信
shimizuta 19:1adc7302cfd9 143 SendRad(leg[2], leg[3]);
shimizuta 19:1adc7302cfd9 144 #endif
shimizuta 9:905f93247688 145 //自身が動かす足のサーボを動かす
shimizuta 16:0069a56f11a3 146 MoveServo(leg[0], 0, 0);
shimizuta 11:e81425872740 147 MoveServo(leg[1], 1, 0);
shimizuta 9:905f93247688 148 wait_ms(kServoSpan_ms);
shimizuta 16:0069a56f11a3 149 MoveServo(leg[0], 0, 1);
shimizuta 11:e81425872740 150 MoveServo(leg[1], 1, 1);
shimizuta 19:1adc7302cfd9 151 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 152 //計算周期がWalkWay.cycletime_s_になるようwait
shimizuta 11:e81425872740 153 float rest_time_s = WalkWay.cycletime_s_ - (timer.read() - time_s);
shimizuta 9:905f93247688 154 if (rest_time_s > 0)
shimizuta 9:905f93247688 155 wait(rest_time_s);
shimizuta 19:1adc7302cfd9 156 else
shimizuta 19:1adc7302cfd9 157 { //計算周期が達成できないときはDEBUGで知らせるだけ。動きはする。
shimizuta 19:1adc7302cfd9 158 DEBUG("error: rest_time_s = %f in Move()\r\n", rest_time_s);
shimizuta 14:d7cb429946f4 159 led[0] = 1;
shimizuta 14:d7cb429946f4 160 }
shimizuta 9:905f93247688 161 }
yuto17320508 5:556d5a5e9d24 162 }
shimizuta 14:d7cb429946f4 163 void MoveServo(OneLeg leg, int serial_num, int servo_id)
yuto17320508 6:43708adf2e5d 164 {
shimizuta 11:e81425872740 165 float degree = leg.GetRad(servo_id) * kRadToDegree;
shimizuta 14:d7cb429946f4 166 //サーボの座標系に変更
shimizuta 14:d7cb429946f4 167 float servo_degree = kServoSign[serial_num][servo_id] * degree + kOriginDegree[serial_num][servo_id];
shimizuta 19:1adc7302cfd9 168 DEBUG("servo_degree[%d][%d],%f\r\n", serial_num, servo_id, servo_degree);
shimizuta 14:d7cb429946f4 169 servo[serial_num].set_degree(servo_id, servo_degree);
shimizuta 12:2ac37fe6c3bb 170 }