岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

main.cpp@7:72c80a7c20d6, 2019-02-10 (annotated)

Committer:
yuto17320508
Date:
Sun Feb 10 10:37:02 2019 +0000
Revision:
7:72c80a7c20d6
Parent:
6:43708adf2e5d
Child:
8:21b932c4e6c5
too heavy program

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 7:72c80a7c20d6 17 const float tickerTime = 0.02f; //Ticker間隔
yuto17320508 6:43708adf2e5d 18 float OffsetY = 0.15f;
yuto17320508 7:72c80a7c20d6 19 float Stride_time_s = 1.0f*10.0f;//床についている時間 1.0f
yuto17320508 6:43708adf2e5d 20 float Stride_length_m = 0.05f;//歩幅
yuto17320508 7:72c80a7c20d6 21 float Rising_time_s = 0.2f*10.0f;//脚を上げている時間 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 5:556d5a5e9d24 93 void Straight();
yuto17320508 5:556d5a5e9d24 94 void TurnLeft();
yuto17320508 5:556d5a5e9d24 95 void TurnRight();
yuto17320508 5:556d5a5e9d24 96 void Climb();
yuto17320508 5:556d5a5e9d24 97 void Overcoming(); //段差、紐乗り越え動作
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 5:556d5a5e9d24 158 void WalkingPattern::Straight()
yuto17320508 5:556d5a5e9d24 159 {
yuto17320508 6:43708adf2e5d 160 static float time_s = 0.0f;//共通時間
yuto17320508 6:43708adf2e5d 161 for(int i=0;i<4;++i)//4脚それぞれのtargetPoseを設定する
yuto17320508 5:556d5a5e9d24 162 {
yuto17320508 6:43708adf2e5d 163 float thisLegTime = time_s + offsetTime[i];//脚に合わせたそれぞれの時間
yuto17320508 7:72c80a7c20d6 164 if(thisLegTime <= Stride_time_s) Motion::Stride(thisLegTime, i);
yuto17320508 7:72c80a7c20d6 165 else if(thisLegTime <= walkPeriod) Motion::Rising(thisLegTime-Stride_time_s,i);
yuto17320508 7:72c80a7c20d6 166 else Motion::Stride(thisLegTime-walkPeriod,i);
yuto17320508 5:556d5a5e9d24 167 }
yuto17320508 7:72c80a7c20d6 168 #ifdef DebugMode
yuto17320508 7:72c80a7c20d6 169 printf("time_s: %.3f targetX0: %.3f targetY0: %.3f\r\n", time_s, TargetX[0],TargetY[0]);
yuto17320508 7:72c80a7c20d6 170 #endif
yuto17320508 5:556d5a5e9d24 171 time_s += tickerTime*2.0f;
yuto17320508 6:43708adf2e5d 172 if(time_s >= walkPeriod) time_s = 0.0f;
yuto17320508 5:556d5a5e9d24 173
yuto17320508 5:556d5a5e9d24 174 }
yuto17320508 5:556d5a5e9d24 175 void WalkingPattern::TurnLeft()
yuto17320508 5:556d5a5e9d24 176 {
yuto17320508 6:43708adf2e5d 177 //static float time_s = 0.0f;
yuto17320508 5:556d5a5e9d24 178
yuto17320508 5:556d5a5e9d24 179 }
yuto17320508 5:556d5a5e9d24 180 void WalkingPattern::TurnRight()
yuto17320508 5:556d5a5e9d24 181 {
yuto17320508 6:43708adf2e5d 182 //static float time_s = 0.0f;
yuto17320508 5:556d5a5e9d24 183
yuto17320508 5:556d5a5e9d24 184 }
yuto17320508 5:556d5a5e9d24 185 void WalkingPattern::Climb()
yuto17320508 5:556d5a5e9d24 186 {
yuto17320508 6:43708adf2e5d 187 //static float time_s = 0.0f;
yuto17320508 5:556d5a5e9d24 188
yuto17320508 5:556d5a5e9d24 189 }
yuto17320508 5:556d5a5e9d24 190 void WalkingPattern::Overcoming()
yuto17320508 5:556d5a5e9d24 191 {
yuto17320508 6:43708adf2e5d 192 //static float time_s = 0.0f;
yuto17320508 5:556d5a5e9d24 193
yuto17320508 5:556d5a5e9d24 194 }
yuto17320508 5:556d5a5e9d24 195
yuto17320508 5:556d5a5e9d24 196 void CalcWalk()
yuto17320508 5:556d5a5e9d24 197 {
yuto17320508 5:556d5a5e9d24 198 switch(WalkMode)
yuto17320508 5:556d5a5e9d24 199 {
yuto17320508 5:556d5a5e9d24 200 case straight:
yuto17320508 5:556d5a5e9d24 201 WalkingPattern::Straight();
yuto17320508 5:556d5a5e9d24 202 break;
yuto17320508 5:556d5a5e9d24 203 case turnleft:
yuto17320508 5:556d5a5e9d24 204 WalkingPattern::TurnLeft();
yuto17320508 5:556d5a5e9d24 205 break;
yuto17320508 5:556d5a5e9d24 206 case turnright:
yuto17320508 5:556d5a5e9d24 207 WalkingPattern::TurnRight();
yuto17320508 5:556d5a5e9d24 208 break;
yuto17320508 5:556d5a5e9d24 209 case climb:
yuto17320508 5:556d5a5e9d24 210 WalkingPattern::Climb();
yuto17320508 5:556d5a5e9d24 211 break;
yuto17320508 5:556d5a5e9d24 212 case overcoming:
yuto17320508 5:556d5a5e9d24 213 WalkingPattern::Overcoming();
yuto17320508 5:556d5a5e9d24 214 break;
yuto17320508 5:556d5a5e9d24 215 }
yuto17320508 5:556d5a5e9d24 216 }
yuto17320508 5:556d5a5e9d24 217 } // namespace Quadruped
shimizuta 2:a92568bdeb5c 218
yuto17320508 6:43708adf2e5d 219 namespace CanConnector
yuto17320508 6:43708adf2e5d 220 {
yuto17320508 6:43708adf2e5d 221 class MastarConnector
yuto17320508 6:43708adf2e5d 222 {
yuto17320508 6:43708adf2e5d 223 public:
yuto17320508 6:43708adf2e5d 224 MastarConnector(PinName rd, PinName td);
yuto17320508 6:43708adf2e5d 225 CAN can_;
yuto17320508 7:72c80a7c20d6 226 void Connect();
yuto17320508 6:43708adf2e5d 227
yuto17320508 6:43708adf2e5d 228 };
yuto17320508 7:72c80a7c20d6 229 MastarConnector::MastarConnector(PinName rd, PinName td): can_(rd, td){}
yuto17320508 6:43708adf2e5d 230
yuto17320508 6:43708adf2e5d 231
yuto17320508 6:43708adf2e5d 232 class SlaveConnector
yuto17320508 6:43708adf2e5d 233 {
yuto17320508 6:43708adf2e5d 234 public:
yuto17320508 6:43708adf2e5d 235 SlaveConnector(PinName rd, PinName td);
yuto17320508 6:43708adf2e5d 236 CAN can_;
yuto17320508 7:72c80a7c20d6 237 void Connect();
yuto17320508 6:43708adf2e5d 238
yuto17320508 6:43708adf2e5d 239 };
yuto17320508 7:72c80a7c20d6 240 SlaveConnector::SlaveConnector(PinName rd, PinName td): can_(rd, td){}
yuto17320508 6:43708adf2e5d 241
yuto17320508 6:43708adf2e5d 242 }
yuto17320508 6:43708adf2e5d 243
yuto17320508 3:bcae0bb64b81 244 //////////////////////////////////////////////////////////////以上が4脚ロボットに関する記述
yuto17320508 3:bcae0bb64b81 245 using namespace Quadruped;
yuto17320508 3:bcae0bb64b81 246 Leg leg1(p9, p10);
yuto17320508 5:556d5a5e9d24 247 Leg leg2(p28, p27);
yuto17320508 7:72c80a7c20d6 248 //CAN can1(p30, p29);
shimizuta 2:a92568bdeb5c 249 Ticker fliper;
yuto17320508 7:72c80a7c20d6 250 #ifdef Mastar
yuto17320508 7:72c80a7c20d6 251 CanConnector::MastarConnector connector(p30,p29);
yuto17320508 7:72c80a7c20d6 252 #elif Slave
yuto17320508 7:72c80a7c20d6 253 CanConnector::SlaveConnector connector(p30,p29);
yuto17320508 7:72c80a7c20d6 254 #endif
yuto17320508 5:556d5a5e9d24 255
yuto17320508 0:f000d896d188 256 void cb_timer()
yuto17320508 0:f000d896d188 257 {
shimizuta 2:a92568bdeb5c 258 static int servo_num = 0;
shimizuta 2:a92568bdeb5c 259 static float time_s = 0.0f;
shimizuta 2:a92568bdeb5c 260 //出力。片サーボずつ
yuto17320508 3:bcae0bb64b81 261 leg1.MoveServo(servo_num);
yuto17320508 3:bcae0bb64b81 262 leg2.MoveServo(servo_num);
shimizuta 2:a92568bdeb5c 263 ++servo_num;
yuto17320508 5:556d5a5e9d24 264
shimizuta 2:a92568bdeb5c 265 if (servo_num > 1) //両サーボ出力したら
yuto17320508 0:f000d896d188 266 {
shimizuta 2:a92568bdeb5c 267 //目標角の更新
yuto17320508 6:43708adf2e5d 268 Quadruped::CalcWalk();
yuto17320508 6:43708adf2e5d 269 //このマイコンで使う脚だけ計算
yuto17320508 6:43708adf2e5d 270 leg1.CalServoRad(TargetX[0], TargetY[0] + OffsetY);
yuto17320508 6:43708adf2e5d 271 leg2.CalServoRad(TargetX[1], TargetY[1] + OffsetY);
yuto17320508 7:72c80a7c20d6 272
shimizuta 2:a92568bdeb5c 273 servo_num = 0;
yuto17320508 0:f000d896d188 274 }
shimizuta 2:a92568bdeb5c 275 time_s += tickerTime;
yuto17320508 5:556d5a5e9d24 276
yuto17320508 0:f000d896d188 277 }
shimizuta 2:a92568bdeb5c 278 int main()
yuto17320508 0:f000d896d188 279 {
yuto17320508 6:43708adf2e5d 280 Quadruped::CalcWalk();
yuto17320508 6:43708adf2e5d 281 leg1.CalServoRad(TargetX[0], TargetY[0] + OffsetY);
yuto17320508 6:43708adf2e5d 282 leg2.CalServoRad(TargetX[1], TargetY[1] + OffsetY);
yuto17320508 0:f000d896d188 283 fliper.attach(&cb_timer, tickerTime);
yuto17320508 0:f000d896d188 284 }