岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

main.cpp@5:556d5a5e9d24, 2019-02-10 (annotated)

Committer:
yuto17320508
Date:
Sun Feb 10 06:07:02 2019 +0000
Revision:
5:556d5a5e9d24
Parent:
4:fffdb273836e
Child:
6:43708adf2e5d
make born

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