岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

main.cpp@6:43708adf2e5d, 2019-02-10 (annotated)

Committer:
yuto17320508
Date:
Sun Feb 10 09:14:15 2019 +0000
Revision:
6:43708adf2e5d
Parent:
5:556d5a5e9d24
Child:
7:72c80a7c20d6
maybe can walk;

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