岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

main.cpp@4:fffdb273836e, 2019-02-09 (annotated)

Committer:
yuto17320508
Date:
Sat Feb 09 14:18:15 2019 +0000
Revision:
4:fffdb273836e
Parent:
3:bcae0bb64b81
Child:
5:556d5a5e9d24
make namespace;

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 3:bcae0bb64b81 7 #define Mastar;
yuto17320508 3:bcae0bb64b81 8 //#define Slave;
yuto17320508 4:fffdb273836e 9
shimizuta 2:a92568bdeb5c 10 const float Pi = 3.141592;
shimizuta 2:a92568bdeb5c 11 const float kRadToDegree = 180.0 / Pi;
yuto17320508 1:acaa3d7ede4c 12
yuto17320508 4:fffdb273836e 13 //定義　
yuto17320508 3:bcae0bb64b81 14 namespace Quadruped
shimizuta 2:a92568bdeb5c 15 {
yuto17320508 4:fffdb273836e 16 class Leg;
yuto17320508 4:fffdb273836e 17 namespace Motion{}
yuto17320508 4:fffdb273836e 18 namespace WalkingPattern{}
yuto17320508 4:fffdb273836e 19 }
yuto17320508 4:fffdb273836e 20
yuto17320508 4:fffdb273836e 21 namespace CanConnector
yuto17320508 4:fffdb273836e 22 {
yuto17320508 4:fffdb273836e 23 //class Mastar;
yuto17320508 4:fffdb273836e 24 //class Slave;
yuto17320508 4:fffdb273836e 25 }
yuto17320508 4:fffdb273836e 26
yuto17320508 4:fffdb273836e 27 //実装
yuto17320508 4:fffdb273836e 28 namespace Quadruped
yuto17320508 4:fffdb273836e 29 {
yuto17320508 4:fffdb273836e 30 //脚の定義
yuto17320508 3:bcae0bb64b81 31 class Leg
yuto17320508 3:bcae0bb64b81 32 {
yuto17320508 3:bcae0bb64b81 33 float rad_[2];
yuto17320508 3:bcae0bb64b81 34 int id_[2];
yuto17320508 3:bcae0bb64b81 35 //パラメータ。実際の機体に合わせていじる
yuto17320508 3:bcae0bb64b81 36 static const float dist_between_servo_half_m_ = 0.06f * 0.5;
yuto17320508 3:bcae0bb64b81 37 static const float LegLength1 = 0.1f;
yuto17320508 3:bcae0bb64b81 38 static const float LegLength2 = 0.2f;
yuto17320508 3:bcae0bb64b81 39 KondoServo servo_;
yuto17320508 3:bcae0bb64b81 40
yuto17320508 3:bcae0bb64b81 41 public:
yuto17320508 3:bcae0bb64b81 42 Leg(PinName pin_serial_tx, PinName pin_serial_rx);
yuto17320508 3:bcae0bb64b81 43 void MoveServo(int servo_num);
yuto17320508 3:bcae0bb64b81 44 void CalServoRad(float x_m, float y_m);
yuto17320508 3:bcae0bb64b81 45 void SetRad(float rad, int servo_num);
yuto17320508 3:bcae0bb64b81 46 float GetRad(int servo_num);
yuto17320508 3:bcae0bb64b81 47 };
yuto17320508 3:bcae0bb64b81 48 Leg::Leg(PinName pin_serial_tx, PinName pin_serial_rx) : servo_(pin_serial_tx, pin_serial_rx)
yuto17320508 3:bcae0bb64b81 49 {
yuto17320508 3:bcae0bb64b81 50 rad_[0] = 0;
yuto17320508 3:bcae0bb64b81 51 rad_[1] = Pi;
yuto17320508 3:bcae0bb64b81 52 };
yuto17320508 3:bcae0bb64b81 53 void Leg::MoveServo(int servo_num)
yuto17320508 3:bcae0bb64b81 54 {
yuto17320508 3:bcae0bb64b81 55 float degree = GetRad(servo_num) * kRadToDegree;
yuto17320508 3:bcae0bb64b81 56 //servo1は反転させる
yuto17320508 3:bcae0bb64b81 57 if (servo_num == 0)
yuto17320508 3:bcae0bb64b81 58 degree += 90;
yuto17320508 3:bcae0bb64b81 59 else
yuto17320508 3:bcae0bb64b81 60 degree = 270 - degree;
yuto17320508 3:bcae0bb64b81 61 servo_.set_degree(servo_num, degree);
yuto17320508 3:bcae0bb64b81 62 }
yuto17320508 3:bcae0bb64b81 63 void Leg::CalServoRad(float x_m, float y_m)
yuto17320508 3:bcae0bb64b81 64 {
yuto17320508 3:bcae0bb64b81 65 //処理を軽くするために共通部分は先に計算
yuto17320508 3:bcae0bb64b81 66 float temp_x[] = {x_m + dist_between_servo_half_m_,
yuto17320508 3:bcae0bb64b81 67 x_m - dist_between_servo_half_m_};
yuto17320508 3:bcae0bb64b81 68 float temp_y2 = y_m * y_m;
yuto17320508 3:bcae0bb64b81 69 float temp_L = LegLength1 * LegLength1 - LegLength2 * LegLength2;
yuto17320508 3:bcae0bb64b81 70
yuto17320508 3:bcae0bb64b81 71 float r1 = sqrt((temp_x[1]) * (temp_x[1]) + temp_y2);
yuto17320508 3:bcae0bb64b81 72 float r2 = sqrt((temp_x[0]) * (temp_x[0]) + temp_y2);
yuto17320508 3:bcae0bb64b81 73 float targetTheta[] = {atan2(y_m, temp_x[1]) - acos((temp_L + r1 * r1) / (2.0f * r1 * LegLength1)),
yuto17320508 3:bcae0bb64b81 74 atan2(y_m, temp_x[0]) + acos((temp_L + r2 * r2) / (2.0f * r2 * LegLength1))};
yuto17320508 3:bcae0bb64b81 75 for (size_t i = 0; i < 2; i++)
yuto17320508 3:bcae0bb64b81 76 SetRad(targetTheta[i], i);
yuto17320508 3:bcae0bb64b81 77 }
yuto17320508 3:bcae0bb64b81 78 void Leg::SetRad(float rad, int servo_num)
yuto17320508 3:bcae0bb64b81 79 {
yuto17320508 3:bcae0bb64b81 80 rad_[servo_num] = rad;
yuto17320508 3:bcae0bb64b81 81 }
yuto17320508 3:bcae0bb64b81 82 float Leg::GetRad(int servo_num)
yuto17320508 3:bcae0bb64b81 83 {
yuto17320508 3:bcae0bb64b81 84 return rad_[servo_num];
yuto17320508 4:fffdb273836e 85 }
yuto17320508 4:fffdb273836e 86
yuto17320508 4:fffdb273836e 87 namespace Motion
yuto17320508 4:fffdb273836e 88 {
yuto17320508 4:fffdb273836e 89 void Slide(Leg leg,float stride_m);//引数に歩幅
yuto17320508 4:fffdb273836e 90 void Rising(Leg leg,float width_m);//引数に上げる高さ
yuto17320508 4:fffdb273836e 91 }
yuto17320508 4:fffdb273836e 92 void Motion::Slide(Leg leg,float stride_m)
yuto17320508 4:fffdb273836e 93 {
yuto17320508 4:fffdb273836e 94 static float time_s = 0.0f;
yuto17320508 4:fffdb273836e 95
yuto17320508 4:fffdb273836e 96 }
yuto17320508 4:fffdb273836e 97 void Motion::Rising(Leg leg,float width_m)
yuto17320508 4:fffdb273836e 98 {
yuto17320508 4:fffdb273836e 99 static float time_s = 0.0f;
yuto17320508 4:fffdb273836e 100
yuto17320508 4:fffdb273836e 101 }
yuto17320508 4:fffdb273836e 102
yuto17320508 4:fffdb273836e 103 namespace WalkingPattern
yuto17320508 4:fffdb273836e 104 {
yuto17320508 4:fffdb273836e 105 using namespace Motion;
yuto17320508 4:fffdb273836e 106 void Straight();
yuto17320508 4:fffdb273836e 107 void TurnRight();
yuto17320508 4:fffdb273836e 108 void TurnLeft();
yuto17320508 4:fffdb273836e 109 void Climb();
yuto17320508 4:fffdb273836e 110 void Overcoming();//段差、紐乗り越え動作
yuto17320508 4:fffdb273836e 111 }
shimizuta 2:a92568bdeb5c 112 }
shimizuta 2:a92568bdeb5c 113
yuto17320508 3:bcae0bb64b81 114
shimizuta 2:a92568bdeb5c 115
yuto17320508 3:bcae0bb64b81 116 //////////////////////////////////////////////////////////////以上が4脚ロボットに関する記述
yuto17320508 3:bcae0bb64b81 117 using namespace Quadruped;
yuto17320508 3:bcae0bb64b81 118 Leg leg1(p9, p10);
yuto17320508 3:bcae0bb64b81 119 Leg leg2(p28,p27);
yuto17320508 3:bcae0bb64b81 120 CAN can1(p30,p29);
shimizuta 2:a92568bdeb5c 121 Ticker fliper;
shimizuta 2:a92568bdeb5c 122
shimizuta 2:a92568bdeb5c 123 const float tickerTime = 0.006f; //Ticker間隔
shimizuta 2:a92568bdeb5c 124 const float omega = 3.1415f * 2.5f; //角速度
yuto17320508 0:f000d896d188 125 const float offsetDegree = 0.0f;
yuto17320508 0:f000d896d188 126 const float offsetY = 0.15f;
yuto17320508 0:f000d896d188 127
yuto17320508 0:f000d896d188 128 void F_t(float t, float &tarX, float &tarY)
yuto17320508 0:f000d896d188 129 {
yuto17320508 0:f000d896d188 130 float theta = -omega * t + offsetDegree;
yuto17320508 0:f000d896d188 131 if (sin(theta) <= 0.0f)
yuto17320508 0:f000d896d188 132 {
yuto17320508 3:bcae0bb64b81 133 tarX = 0.1f * cos(theta); //0.1
yuto17320508 3:bcae0bb64b81 134 tarY = 0.03f * sin(theta); //0.03
yuto17320508 0:f000d896d188 135 }
yuto17320508 0:f000d896d188 136 else
yuto17320508 0:f000d896d188 137 {
yuto17320508 3:bcae0bb64b81 138 tarX = 0.1f * cos(theta);
yuto17320508 3:bcae0bb64b81 139 tarY = 0.0f * sin(theta);
yuto17320508 0:f000d896d188 140 }
yuto17320508 0:f000d896d188 141 }
yuto17320508 0:f000d896d188 142 void cb_timer()
yuto17320508 0:f000d896d188 143 {
shimizuta 2:a92568bdeb5c 144 static int servo_num = 0;
shimizuta 2:a92568bdeb5c 145 static float time_s = 0.0f;
shimizuta 2:a92568bdeb5c 146 float target_x_m = 0;
shimizuta 2:a92568bdeb5c 147 float target_y_m = 0;
shimizuta 2:a92568bdeb5c 148 //出力。片サーボずつ
yuto17320508 3:bcae0bb64b81 149 leg1.MoveServo(servo_num);
yuto17320508 3:bcae0bb64b81 150 leg2.MoveServo(servo_num);
shimizuta 2:a92568bdeb5c 151 ++servo_num;
shimizuta 2:a92568bdeb5c 152 if (servo_num > 1) //両サーボ出力したら
yuto17320508 0:f000d896d188 153 {
shimizuta 2:a92568bdeb5c 154 //目標角の更新
shimizuta 2:a92568bdeb5c 155 F_t(time_s, target_x_m, target_y_m);
yuto17320508 3:bcae0bb64b81 156 leg1.CalServoRad(target_x_m, target_y_m + offsetY);
yuto17320508 3:bcae0bb64b81 157 leg2.CalServoRad(target_x_m, target_y_m + offsetY);
shimizuta 2:a92568bdeb5c 158 servo_num = 0;
yuto17320508 0:f000d896d188 159 }
shimizuta 2:a92568bdeb5c 160 time_s += tickerTime;
yuto17320508 0:f000d896d188 161 }
shimizuta 2:a92568bdeb5c 162 int main()
yuto17320508 0:f000d896d188 163 {
yuto17320508 3:bcae0bb64b81 164 float target_x_m = 0;
shimizuta 2:a92568bdeb5c 165 float target_y_m = 0;
shimizuta 2:a92568bdeb5c 166 F_t(0, target_x_m, target_y_m);
yuto17320508 3:bcae0bb64b81 167 leg1.CalServoRad(target_x_m, target_y_m + offsetY);
yuto17320508 3:bcae0bb64b81 168 leg2.CalServoRad(target_x_m, target_y_m + offsetY);
yuto17320508 0:f000d896d188 169 fliper.attach(&cb_timer, tickerTime);
yuto17320508 0:f000d896d188 170 }