岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

main.cpp@8:21b932c4e6c5, 2019-02-10 (annotated)

Committer:
yuto17320508
Date:
Sun Feb 10 12:38:16 2019 +0000
Revision:
8:21b932c4e6c5
Parent:
7:72c80a7c20d6
Child:
9:905f93247688
devide calculate process

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yuto17320508 3:bcae0bb64b81 1 //mbed間の同期を、master-slaveで行いたい。
yuto17320508 4:fffdb273836e 2 //masetrで4つの脚の動きをすべて決定,
yuto17320508 4:fffdb273836e 3 //その動きをslaveに送る。
yuto17320508 3:bcae0bb64b81 4
yuto17320508 0:f000d896d188 5 #include "mbed.h"
yuto17320508 0:f000d896d188 6 #include "KondoServo.h"
yuto17320508 6:43708adf2e5d 7 #define Mastar;
yuto17320508 3:bcae0bb64b81 8 //#define Slave;
yuto17320508 7:72c80a7c20d6 9 #define DebugMode;
yuto17320508 4:fffdb273836e 10
yuto17320508 6:43708adf2e5d 11 //各値
yuto17320508 6:43708adf2e5d 12 namespace Parameters
shimizuta 2:a92568bdeb5c 13 {
yuto17320508 7:72c80a7c20d6 14 //パラメータ
yuto17320508 7:72c80a7c20d6 15 const float Pi = 3.14;
yuto17320508 5:556d5a5e9d24 16 const float kRadToDegree = 180.0 / Pi;
yuto17320508 8:21b932c4e6c5 17 const float tickerTime = 0.007f;//Ticker間隔
yuto17320508 6:43708adf2e5d 18 float OffsetY = 0.15f;
yuto17320508 8:21b932c4e6c5 19 float Stride_time_s = 0.8f;//床についている時間 1.0f
yuto17320508 6:43708adf2e5d 20 float Stride_length_m = 0.05f;//歩幅
yuto17320508 8:21b932c4e6c5 21 float Rising_time_s = 0.2f;//脚を上げている時間 0.2f
yuto17320508 5:556d5a5e9d24 22 float Width_m = 0.03f;//脚を上げる高さ
yuto17320508 5:556d5a5e9d24 23 static const int straight = 0;
yuto17320508 5:556d5a5e9d24 24 static const int turnleft = 1;
yuto17320508 5:556d5a5e9d24 25 static const int turnright = 2;
yuto17320508 5:556d5a5e9d24 26 static const int climb = 3;
yuto17320508 5:556d5a5e9d24 27 static const int overcoming = 4;
yuto17320508 6:43708adf2e5d 28 }
yuto17320508 6:43708adf2e5d 29 //定義　
yuto17320508 6:43708adf2e5d 30 namespace Quadruped//4脚ロボット
yuto17320508 6:43708adf2e5d 31 {
yuto17320508 6:43708adf2e5d 32 using namespace Parameters;
yuto17320508 7:72c80a7c20d6 33 //USERが直接制御に使う関数
yuto17320508 6:43708adf2e5d 34 void SetOffsetY(float offsetY){OffsetY=offsetY;}
yuto17320508 6:43708adf2e5d 35 void SetStrideMotion(float stride_time_s, float stride_length_m){Stride_time_s=stride_time_s;Stride_length_m=stride_length_m;}///////////////////////////////////設定
yuto17320508 6:43708adf2e5d 36 void SetRisiongMotion(float rising_time_s, float width_m){Rising_time_s=rising_time_s;Width_m =width_m;}////////////////////////////////////////設定
yuto17320508 6:43708adf2e5d 37 //ROSから送られてきたLRFのデータをもとに現在すべき動作を定義する。
yuto17320508 6:43708adf2e5d 38 int WalkMode = 0;
yuto17320508 6:43708adf2e5d 39 void SetWalkMode(int mode){WalkMode = mode;}
yuto17320508 5:556d5a5e9d24 40 //4つの脚のtargetを保存する　Legクラスで後で使えるようにしておく
yuto17320508 6:43708adf2e5d 41 float TargetX[4];
yuto17320508 6:43708adf2e5d 42 float TargetY[4];
yuto17320508 5:556d5a5e9d24 43 //脚の定義　位置を入力するとその方向にサーボが動く
yuto17320508 5:556d5a5e9d24 44 class Leg;
yuto17320508 5:556d5a5e9d24 45 //脚ごとのモーション　これを組み合わせる
yuto17320508 5:556d5a5e9d24 46 namespace Motion{}
yuto17320508 5:556d5a5e9d24 47 //モーションを組み合わせて歩行パターンを形成する
yuto17320508 5:556d5a5e9d24 48 //前進、旋回など
yuto17320508 5:556d5a5e9d24 49 namespace WalkingPattern{}
yuto17320508 5:556d5a5e9d24 50 //パターン関数を呼び出す　一つの関数でいいようにswitchする
yuto17320508 5:556d5a5e9d24 51 void CalcWalk(int legNum, Leg leg);
yuto17320508 5:556d5a5e9d24 52 } // namespace Quadruped
yuto17320508 4:fffdb273836e 53
yuto17320508 4:fffdb273836e 54 namespace CanConnector
yuto17320508 4:fffdb273836e 55 {
yuto17320508 6:43708adf2e5d 56 //Maserとslaveの定義　同じ関数名で動くようにしたい
yuto17320508 7:72c80a7c20d6 57 class MastarConnector;//cansendするマイコンの場合
yuto17320508 7:72c80a7c20d6 58 class SlaveConnector;//canreadするマイコンの場合
yuto17320508 4:fffdb273836e 59 }
yuto17320508 4:fffdb273836e 60
yuto17320508 4:fffdb273836e 61 //実装
yuto17320508 4:fffdb273836e 62 namespace Quadruped
yuto17320508 4:fffdb273836e 63 {
yuto17320508 5:556d5a5e9d24 64 //脚の定義
yuto17320508 5:556d5a5e9d24 65 class Leg
yuto17320508 5:556d5a5e9d24 66 {
yuto17320508 5:556d5a5e9d24 67 float rad_[2];
yuto17320508 5:556d5a5e9d24 68 int id_[2];
yuto17320508 5:556d5a5e9d24 69 //パラメータ。実際の機体に合わせていじる
yuto17320508 5:556d5a5e9d24 70 static const float dist_between_servo_half_m_ = 0.06f * 0.5;
yuto17320508 5:556d5a5e9d24 71 static const float LegLength1 = 0.1f;
yuto17320508 5:556d5a5e9d24 72 static const float LegLength2 = 0.2f;
yuto17320508 5:556d5a5e9d24 73 KondoServo servo_;
yuto17320508 3:bcae0bb64b81 74
yuto17320508 5:556d5a5e9d24 75 public:
yuto17320508 5:556d5a5e9d24 76 Leg(PinName pin_serial_tx, PinName pin_serial_rx);
yuto17320508 5:556d5a5e9d24 77 void MoveServo(int servo_num);
yuto17320508 5:556d5a5e9d24 78 void CalServoRad(float x_m, float y_m);
yuto17320508 5:556d5a5e9d24 79 void SetRad(float rad, int servo_num);
yuto17320508 5:556d5a5e9d24 80 float GetRad(int servo_num);
yuto17320508 5:556d5a5e9d24 81 };
yuto17320508 5:556d5a5e9d24 82
yuto17320508 5:556d5a5e9d24 83 namespace Motion
yuto17320508 5:556d5a5e9d24 84 {
yuto17320508 7:72c80a7c20d6 85 void Stride(float time_s,int legnum);
yuto17320508 7:72c80a7c20d6 86 void Rising(float time_s,int legnum);
yuto17320508 7:72c80a7c20d6 87 void Stop(int legnum);
yuto17320508 5:556d5a5e9d24 88 } // namespace Motion
yuto17320508 5:556d5a5e9d24 89
yuto17320508 5:556d5a5e9d24 90 namespace WalkingPattern
yuto17320508 5:556d5a5e9d24 91 {
yuto17320508 5:556d5a5e9d24 92 using namespace Motion;
yuto17320508 8:21b932c4e6c5 93 void Straight(int servo_num);
yuto17320508 8:21b932c4e6c5 94 void TurnLeft(int servo_num);
yuto17320508 8:21b932c4e6c5 95 void TurnRight(int servo_num);
yuto17320508 8:21b932c4e6c5 96 void Climb(int servo_num);
yuto17320508 8:21b932c4e6c5 97 void Overcoming(int servo_num); //段差、紐乗り越え動作
yuto17320508 5:556d5a5e9d24 98 } // namespace WalkingPattern
yuto17320508 3:bcae0bb64b81 99
yuto17320508 5:556d5a5e9d24 100 Leg::Leg(PinName pin_serial_tx, PinName pin_serial_rx) : servo_(pin_serial_tx, pin_serial_rx)
yuto17320508 5:556d5a5e9d24 101 {
yuto17320508 5:556d5a5e9d24 102 rad_[0] = 0;
yuto17320508 5:556d5a5e9d24 103 rad_[1] = Pi;
yuto17320508 5:556d5a5e9d24 104 };
yuto17320508 5:556d5a5e9d24 105 void Leg::MoveServo(int servo_num)
yuto17320508 5:556d5a5e9d24 106 {
yuto17320508 5:556d5a5e9d24 107 float degree = GetRad(servo_num) * kRadToDegree;
yuto17320508 5:556d5a5e9d24 108 //servo1は反転させる
yuto17320508 5:556d5a5e9d24 109 if (servo_num == 0)
yuto17320508 5:556d5a5e9d24 110 degree += 90;
yuto17320508 5:556d5a5e9d24 111 else
yuto17320508 5:556d5a5e9d24 112 degree = 270 - degree;
yuto17320508 5:556d5a5e9d24 113 servo_.set_degree(servo_num, degree);
yuto17320508 5:556d5a5e9d24 114 }
yuto17320508 5:556d5a5e9d24 115 void Leg::CalServoRad(float x_m, float y_m)
yuto17320508 5:556d5a5e9d24 116 {
yuto17320508 5:556d5a5e9d24 117 //処理を軽くするために共通部分は先に計算
yuto17320508 5:556d5a5e9d24 118 float temp_x[] = {x_m + dist_between_servo_half_m_,
yuto17320508 5:556d5a5e9d24 119 x_m - dist_between_servo_half_m_};
yuto17320508 5:556d5a5e9d24 120 float temp_y2 = y_m * y_m;
yuto17320508 5:556d5a5e9d24 121 float temp_L = LegLength1 * LegLength1 - LegLength2 * LegLength2;
yuto17320508 4:fffdb273836e 122
yuto17320508 5:556d5a5e9d24 123 float r1 = sqrt((temp_x[1]) * (temp_x[1]) + temp_y2);
yuto17320508 5:556d5a5e9d24 124 float r2 = sqrt((temp_x[0]) * (temp_x[0]) + temp_y2);
yuto17320508 5:556d5a5e9d24 125 float targetTheta[] = {atan2(y_m, temp_x[1]) - acos((temp_L + r1 * r1) / (2.0f * r1 * LegLength1)),
yuto17320508 5:556d5a5e9d24 126 atan2(y_m, temp_x[0]) + acos((temp_L + r2 * r2) / (2.0f * r2 * LegLength1))};
yuto17320508 5:556d5a5e9d24 127 for (size_t i = 0; i < 2; i++)
yuto17320508 5:556d5a5e9d24 128 SetRad(targetTheta[i], i);
yuto17320508 5:556d5a5e9d24 129 }
yuto17320508 5:556d5a5e9d24 130 void Leg::SetRad(float rad, int servo_num)
yuto17320508 5:556d5a5e9d24 131 {
yuto17320508 5:556d5a5e9d24 132 rad_[servo_num] = rad;
yuto17320508 5:556d5a5e9d24 133 }
yuto17320508 5:556d5a5e9d24 134 float Leg::GetRad(int servo_num)
yuto17320508 5:556d5a5e9d24 135 {
yuto17320508 5:556d5a5e9d24 136 return rad_[servo_num];
shimizuta 2:a92568bdeb5c 137 }
shimizuta 2:a92568bdeb5c 138
yuto17320508 3:bcae0bb64b81 139
yuto17320508 7:72c80a7c20d6 140 void Motion::Stride(float time_s,int legnum)
yuto17320508 5:556d5a5e9d24 141 {
yuto17320508 7:72c80a7c20d6 142 TargetX[legnum] = (Stride_length_m*time_s/Stride_time_s) - (Stride_length_m/2.0f);
yuto17320508 7:72c80a7c20d6 143 TargetY[legnum] = 0.0f;
yuto17320508 7:72c80a7c20d6 144
yuto17320508 5:556d5a5e9d24 145 }
yuto17320508 5:556d5a5e9d24 146
yuto17320508 7:72c80a7c20d6 147 void Motion::Rising(float time_s,int legnum)
yuto17320508 5:556d5a5e9d24 148 {
yuto17320508 7:72c80a7c20d6 149 TargetX[legnum] = (Stride_length_m/2.0f)*cos(-Pi*time_s/Rising_time_s);
yuto17320508 7:72c80a7c20d6 150 TargetY[legnum] = Width_m*sin(-Pi*time_s/Rising_time_s);
yuto17320508 7:72c80a7c20d6 151
yuto17320508 5:556d5a5e9d24 152 }
yuto17320508 5:556d5a5e9d24 153
yuto17320508 7:72c80a7c20d6 154 float walkPeriod = Stride_time_s+Rising_time_s;//周期
yuto17320508 7:72c80a7c20d6 155 float offsetTime[4] = {0.0f, walkPeriod/2.0f, 0.0f, walkPeriod/2.0f};//位相をずれすための時間の初期誤差
yuto17320508 7:72c80a7c20d6 156
yuto17320508 7:72c80a7c20d6 157
yuto17320508 8:21b932c4e6c5 158 void WalkingPattern::Straight(int servo_num)
yuto17320508 5:556d5a5e9d24 159 {
yuto17320508 8:21b932c4e6c5 160 int servo_state = 2*servo_num;
yuto17320508 6:43708adf2e5d 161 static float time_s = 0.0f;//共通時間
yuto17320508 8:21b932c4e6c5 162 for(int i=0+servo_state;i<2+servo_state;++i)//2脚ずつそれぞれのtargetPoseを設定する
yuto17320508 5:556d5a5e9d24 163 {
yuto17320508 6:43708adf2e5d 164 float thisLegTime = time_s + offsetTime[i];//脚に合わせたそれぞれの時間
yuto17320508 7:72c80a7c20d6 165 if(thisLegTime <= Stride_time_s) Motion::Stride(thisLegTime, i);
yuto17320508 7:72c80a7c20d6 166 else if(thisLegTime <= walkPeriod) Motion::Rising(thisLegTime-Stride_time_s,i);
yuto17320508 7:72c80a7c20d6 167 else Motion::Stride(thisLegTime-walkPeriod,i);
yuto17320508 5:556d5a5e9d24 168 }
yuto17320508 7:72c80a7c20d6 169 #ifdef DebugMode
yuto17320508 8:21b932c4e6c5 170 if(servo_num == 0)
yuto17320508 8:21b932c4e6c5 171 printf("time_s: %.3f targetX0: %.3f targetY0: %.3f\r\n", time_s, TargetX[0], TargetY[0]);
yuto17320508 7:72c80a7c20d6 172 #endif
yuto17320508 8:21b932c4e6c5 173 time_s += tickerTime;
yuto17320508 8:21b932c4e6c5 174 if(time_s >= walkPeriod)
yuto17320508 8:21b932c4e6c5 175 {
yuto17320508 8:21b932c4e6c5 176 time_s = 0.0f;
yuto17320508 8:21b932c4e6c5 177 }
yuto17320508 5:556d5a5e9d24 178 }
yuto17320508 8:21b932c4e6c5 179 void WalkingPattern::TurnLeft(int servo_num)
yuto17320508 5:556d5a5e9d24 180 {
yuto17320508 6:43708adf2e5d 181 //static float time_s = 0.0f;
yuto17320508 5:556d5a5e9d24 182
yuto17320508 5:556d5a5e9d24 183 }
yuto17320508 8:21b932c4e6c5 184 void WalkingPattern::TurnRight(int servo_num)
yuto17320508 5:556d5a5e9d24 185 {
yuto17320508 6:43708adf2e5d 186 //static float time_s = 0.0f;
yuto17320508 5:556d5a5e9d24 187
yuto17320508 5:556d5a5e9d24 188 }
yuto17320508 8:21b932c4e6c5 189 void WalkingPattern::Climb(int servo_num)
yuto17320508 5:556d5a5e9d24 190 {
yuto17320508 6:43708adf2e5d 191 //static float time_s = 0.0f;
yuto17320508 5:556d5a5e9d24 192
yuto17320508 5:556d5a5e9d24 193 }
yuto17320508 8:21b932c4e6c5 194 void WalkingPattern::Overcoming(int servo_num)
yuto17320508 5:556d5a5e9d24 195 {
yuto17320508 6:43708adf2e5d 196 //static float time_s = 0.0f;
yuto17320508 5:556d5a5e9d24 197
yuto17320508 5:556d5a5e9d24 198 }
yuto17320508 5:556d5a5e9d24 199
yuto17320508 8:21b932c4e6c5 200 void CalcWalk(int servo_num)
yuto17320508 5:556d5a5e9d24 201 {
yuto17320508 5:556d5a5e9d24 202 switch(WalkMode)
yuto17320508 5:556d5a5e9d24 203 {
yuto17320508 5:556d5a5e9d24 204 case straight:
yuto17320508 8:21b932c4e6c5 205 WalkingPattern::Straight(servo_num);
yuto17320508 5:556d5a5e9d24 206 break;
yuto17320508 5:556d5a5e9d24 207 case turnleft:
yuto17320508 8:21b932c4e6c5 208 WalkingPattern::TurnLeft(servo_num);
yuto17320508 5:556d5a5e9d24 209 break;
yuto17320508 5:556d5a5e9d24 210 case turnright:
yuto17320508 8:21b932c4e6c5 211 WalkingPattern::TurnRight(servo_num);
yuto17320508 5:556d5a5e9d24 212 break;
yuto17320508 5:556d5a5e9d24 213 case climb:
yuto17320508 8:21b932c4e6c5 214 WalkingPattern::Climb(servo_num);
yuto17320508 5:556d5a5e9d24 215 break;
yuto17320508 5:556d5a5e9d24 216 case overcoming:
yuto17320508 8:21b932c4e6c5 217 WalkingPattern::Overcoming(servo_num);
yuto17320508 5:556d5a5e9d24 218 break;
yuto17320508 5:556d5a5e9d24 219 }
yuto17320508 5:556d5a5e9d24 220 }
yuto17320508 5:556d5a5e9d24 221 } // namespace Quadruped
shimizuta 2:a92568bdeb5c 222
yuto17320508 6:43708adf2e5d 223 namespace CanConnector
yuto17320508 6:43708adf2e5d 224 {
yuto17320508 6:43708adf2e5d 225 class MastarConnector
yuto17320508 6:43708adf2e5d 226 {
yuto17320508 6:43708adf2e5d 227 public:
yuto17320508 6:43708adf2e5d 228 MastarConnector(PinName rd, PinName td);
yuto17320508 6:43708adf2e5d 229 CAN can_;
yuto17320508 7:72c80a7c20d6 230 void Connect();
yuto17320508 6:43708adf2e5d 231
yuto17320508 6:43708adf2e5d 232 };
yuto17320508 7:72c80a7c20d6 233 MastarConnector::MastarConnector(PinName rd, PinName td): can_(rd, td){}
yuto17320508 6:43708adf2e5d 234
yuto17320508 6:43708adf2e5d 235
yuto17320508 6:43708adf2e5d 236 class SlaveConnector
yuto17320508 6:43708adf2e5d 237 {
yuto17320508 6:43708adf2e5d 238 public:
yuto17320508 6:43708adf2e5d 239 SlaveConnector(PinName rd, PinName td);
yuto17320508 6:43708adf2e5d 240 CAN can_;
yuto17320508 7:72c80a7c20d6 241 void Connect();
yuto17320508 6:43708adf2e5d 242
yuto17320508 6:43708adf2e5d 243 };
yuto17320508 7:72c80a7c20d6 244 SlaveConnector::SlaveConnector(PinName rd, PinName td): can_(rd, td){}
yuto17320508 6:43708adf2e5d 245
yuto17320508 6:43708adf2e5d 246 }
yuto17320508 6:43708adf2e5d 247
yuto17320508 3:bcae0bb64b81 248 //////////////////////////////////////////////////////////////以上が4脚ロボットに関する記述
yuto17320508 3:bcae0bb64b81 249 using namespace Quadruped;
yuto17320508 3:bcae0bb64b81 250 Leg leg1(p9, p10);
yuto17320508 5:556d5a5e9d24 251 Leg leg2(p28, p27);
yuto17320508 7:72c80a7c20d6 252 //CAN can1(p30, p29);
shimizuta 2:a92568bdeb5c 253 Ticker fliper;
yuto17320508 7:72c80a7c20d6 254 #ifdef Mastar
yuto17320508 7:72c80a7c20d6 255 CanConnector::MastarConnector connector(p30,p29);
yuto17320508 7:72c80a7c20d6 256 #elif Slave
yuto17320508 7:72c80a7c20d6 257 CanConnector::SlaveConnector connector(p30,p29);
yuto17320508 7:72c80a7c20d6 258 #endif
yuto17320508 5:556d5a5e9d24 259
yuto17320508 0:f000d896d188 260 void cb_timer()
yuto17320508 0:f000d896d188 261 {
shimizuta 2:a92568bdeb5c 262 static int servo_num = 0;
shimizuta 2:a92568bdeb5c 263 //出力。片サーボずつ
yuto17320508 3:bcae0bb64b81 264 leg1.MoveServo(servo_num);
yuto17320508 3:bcae0bb64b81 265 leg2.MoveServo(servo_num);
yuto17320508 8:21b932c4e6c5 266 Quadruped::CalcWalk(servo_num);
shimizuta 2:a92568bdeb5c 267 ++servo_num;
yuto17320508 8:21b932c4e6c5 268 if(servo_num > 1)servo_num = 0;
yuto17320508 5:556d5a5e9d24 269
yuto17320508 8:21b932c4e6c5 270 /*if (servo_num > 1) //両サーボ出力したら
yuto17320508 0:f000d896d188 271 {
shimizuta 2:a92568bdeb5c 272 //目標角の更新
yuto17320508 8:21b932c4e6c5 273 Quadruped::CalcWalk(servo_num);
yuto17320508 6:43708adf2e5d 274 //このマイコンで使う脚だけ計算
yuto17320508 6:43708adf2e5d 275 leg1.CalServoRad(TargetX[0], TargetY[0] + OffsetY);
yuto17320508 6:43708adf2e5d 276 leg2.CalServoRad(TargetX[1], TargetY[1] + OffsetY);
yuto17320508 7:72c80a7c20d6 277
shimizuta 2:a92568bdeb5c 278 servo_num = 0;
yuto17320508 8:21b932c4e6c5 279 }*/
yuto17320508 5:556d5a5e9d24 280
yuto17320508 0:f000d896d188 281 }
shimizuta 2:a92568bdeb5c 282 int main()
yuto17320508 0:f000d896d188 283 {
yuto17320508 8:21b932c4e6c5 284 Quadruped::CalcWalk(0);
yuto17320508 8:21b932c4e6c5 285 Quadruped::CalcWalk(1);
yuto17320508 6:43708adf2e5d 286 leg1.CalServoRad(TargetX[0], TargetY[0] + OffsetY);
yuto17320508 6:43708adf2e5d 287 leg2.CalServoRad(TargetX[1], TargetY[1] + OffsetY);
yuto17320508 0:f000d896d188 288 fliper.attach(&cb_timer, tickerTime);
yuto17320508 0:f000d896d188 289 }