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?
User | Revision | Line number | New 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 | } |