test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
main.cpp@2:a92568bdeb5c, 2019-02-08 (annotated)
- Committer:
- shimizuta
- Date:
- Fri Feb 08 12:14:19 2019 +0000
- Revision:
- 2:a92568bdeb5c
- Parent:
- 1:acaa3d7ede4c
- Child:
- 3:bcae0bb64b81
made class Leg
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
yuto17320508 | 0:f000d896d188 | 1 | #include "mbed.h" |
yuto17320508 | 0:f000d896d188 | 2 | #include "KondoServo.h" |
shimizuta | 2:a92568bdeb5c | 3 | ////////////////Legクラス。下にmain文がある。 |
shimizuta | 2:a92568bdeb5c | 4 | const float Pi = 3.141592; |
shimizuta | 2:a92568bdeb5c | 5 | const float kRadToDegree = 180.0 / Pi; |
shimizuta | 2:a92568bdeb5c | 6 | //const float kDegreeToRad = Pi / 180.0; |
shimizuta | 2:a92568bdeb5c | 7 | const float LegLength1 = 0.1f; |
shimizuta | 2:a92568bdeb5c | 8 | const float LegLength2 = 0.2f; |
yuto17320508 | 1:acaa3d7ede4c | 9 | |
yuto17320508 | 1:acaa3d7ede4c | 10 | class Leg |
yuto17320508 | 1:acaa3d7ede4c | 11 | { |
shimizuta | 2:a92568bdeb5c | 12 | float rad_[2]; |
shimizuta | 2:a92568bdeb5c | 13 | int id_[2]; |
shimizuta | 2:a92568bdeb5c | 14 | //パラメータ。実際の機体に合わせていじる |
shimizuta | 2:a92568bdeb5c | 15 | static const float dist_between_servo_half_m_ = 0.06f * 0.5; |
shimizuta | 2:a92568bdeb5c | 16 | KondoServo servo_; |
shimizuta | 2:a92568bdeb5c | 17 | |
shimizuta | 2:a92568bdeb5c | 18 | public: |
shimizuta | 2:a92568bdeb5c | 19 | Leg(PinName pin_serial_tx, PinName pin_serial_rx); |
shimizuta | 2:a92568bdeb5c | 20 | void MoveServo(int servo_num); |
shimizuta | 2:a92568bdeb5c | 21 | void CalServoRad(float x_m, float y_m); |
shimizuta | 2:a92568bdeb5c | 22 | void SetRad(float rad, int servo_num); |
shimizuta | 2:a92568bdeb5c | 23 | float GetRad(int servo_num); |
shimizuta | 2:a92568bdeb5c | 24 | }; |
shimizuta | 2:a92568bdeb5c | 25 | |
shimizuta | 2:a92568bdeb5c | 26 | Leg::Leg(PinName pin_serial_tx, PinName pin_serial_rx) : servo_(pin_serial_tx, pin_serial_rx) |
shimizuta | 2:a92568bdeb5c | 27 | { |
shimizuta | 2:a92568bdeb5c | 28 | rad_[0] = 0; |
shimizuta | 2:a92568bdeb5c | 29 | rad_[1] = Pi; |
shimizuta | 2:a92568bdeb5c | 30 | }; |
shimizuta | 2:a92568bdeb5c | 31 | void Leg::MoveServo(int servo_num) |
shimizuta | 2:a92568bdeb5c | 32 | { |
shimizuta | 2:a92568bdeb5c | 33 | float degree = GetRad(servo_num) * kRadToDegree; |
shimizuta | 2:a92568bdeb5c | 34 | //servo1は反転させる |
shimizuta | 2:a92568bdeb5c | 35 | if (servo_num == 0) |
shimizuta | 2:a92568bdeb5c | 36 | degree += 90; |
shimizuta | 2:a92568bdeb5c | 37 | else |
shimizuta | 2:a92568bdeb5c | 38 | degree = 270 - degree; |
shimizuta | 2:a92568bdeb5c | 39 | servo_.set_degree(servo_num, degree); |
shimizuta | 2:a92568bdeb5c | 40 | } |
shimizuta | 2:a92568bdeb5c | 41 | void Leg::CalServoRad(float x_m, float y_m) |
shimizuta | 2:a92568bdeb5c | 42 | { |
shimizuta | 2:a92568bdeb5c | 43 | //処理を軽くするために共通部分は先に計算 |
shimizuta | 2:a92568bdeb5c | 44 | float temp_x[] = {x_m + dist_between_servo_half_m_, |
shimizuta | 2:a92568bdeb5c | 45 | x_m - dist_between_servo_half_m_}; |
shimizuta | 2:a92568bdeb5c | 46 | float temp_y2 = y_m * y_m; |
shimizuta | 2:a92568bdeb5c | 47 | float temp_L = LegLength1 * LegLength1 - LegLength2 * LegLength2; |
shimizuta | 2:a92568bdeb5c | 48 | |
shimizuta | 2:a92568bdeb5c | 49 | float r1 = sqrt((temp_x[1]) * (temp_x[1]) + temp_y2); |
shimizuta | 2:a92568bdeb5c | 50 | float r2 = sqrt((temp_x[0]) * (temp_x[0]) + temp_y2); |
shimizuta | 2:a92568bdeb5c | 51 | float targetTheta[] = {atan2(y_m, temp_x[1]) - acos((temp_L + r1 * r1) / (2.0f * r1 * LegLength1)), |
shimizuta | 2:a92568bdeb5c | 52 | atan2(y_m, temp_x[0]) + acos((temp_L + r2 * r2) / (2.0f * r2 * LegLength1))}; |
shimizuta | 2:a92568bdeb5c | 53 | for (size_t i = 0; i < 2; i++) |
shimizuta | 2:a92568bdeb5c | 54 | SetRad(targetTheta[i], i); |
shimizuta | 2:a92568bdeb5c | 55 | } |
shimizuta | 2:a92568bdeb5c | 56 | void Leg::SetRad(float rad, int servo_num) |
shimizuta | 2:a92568bdeb5c | 57 | { |
shimizuta | 2:a92568bdeb5c | 58 | rad_[servo_num] = rad; |
shimizuta | 2:a92568bdeb5c | 59 | } |
shimizuta | 2:a92568bdeb5c | 60 | float Leg::GetRad(int servo_num) |
shimizuta | 2:a92568bdeb5c | 61 | { |
shimizuta | 2:a92568bdeb5c | 62 | return rad_[servo_num]; |
shimizuta | 2:a92568bdeb5c | 63 | } |
shimizuta | 2:a92568bdeb5c | 64 | |
shimizuta | 2:a92568bdeb5c | 65 | //////////////////////////////////////////////////////////////Legクラスの定義終了 |
shimizuta | 2:a92568bdeb5c | 66 | |
shimizuta | 2:a92568bdeb5c | 67 | Leg leg(p9, p10); |
shimizuta | 2:a92568bdeb5c | 68 | Ticker fliper; |
shimizuta | 2:a92568bdeb5c | 69 | |
shimizuta | 2:a92568bdeb5c | 70 | const float tickerTime = 0.006f; //Ticker間隔 |
shimizuta | 2:a92568bdeb5c | 71 | const float omega = 3.1415f * 2.5f; //角速度 |
yuto17320508 | 0:f000d896d188 | 72 | const float offsetDegree = 0.0f; |
yuto17320508 | 0:f000d896d188 | 73 | const float offsetY = 0.15f; |
yuto17320508 | 0:f000d896d188 | 74 | |
yuto17320508 | 0:f000d896d188 | 75 | void F_t(float t, float &tarX, float &tarY) |
yuto17320508 | 0:f000d896d188 | 76 | { |
yuto17320508 | 0:f000d896d188 | 77 | float theta = -omega * t + offsetDegree; |
yuto17320508 | 0:f000d896d188 | 78 | if (sin(theta) <= 0.0f) |
yuto17320508 | 0:f000d896d188 | 79 | { |
shimizuta | 2:a92568bdeb5c | 80 | tarX = 0.5f * LegLength2 * cos(theta); //0.5 |
shimizuta | 2:a92568bdeb5c | 81 | tarY = 0.15f * LegLength2 * sin(theta); //0.15 |
yuto17320508 | 0:f000d896d188 | 82 | } |
yuto17320508 | 0:f000d896d188 | 83 | else |
yuto17320508 | 0:f000d896d188 | 84 | { |
yuto17320508 | 0:f000d896d188 | 85 | tarX = 0.5f * LegLength2 * cos(theta); |
yuto17320508 | 0:f000d896d188 | 86 | tarY = 0.0f * LegLength2 * sin(theta); |
yuto17320508 | 0:f000d896d188 | 87 | } |
yuto17320508 | 0:f000d896d188 | 88 | } |
yuto17320508 | 0:f000d896d188 | 89 | void cb_timer() |
yuto17320508 | 0:f000d896d188 | 90 | { |
shimizuta | 2:a92568bdeb5c | 91 | static int servo_num = 0; |
shimizuta | 2:a92568bdeb5c | 92 | static float time_s = 0.0f; |
shimizuta | 2:a92568bdeb5c | 93 | float target_x_m = 0; |
shimizuta | 2:a92568bdeb5c | 94 | float target_y_m = 0; |
shimizuta | 2:a92568bdeb5c | 95 | //出力。片サーボずつ |
shimizuta | 2:a92568bdeb5c | 96 | leg.MoveServo(servo_num); |
shimizuta | 2:a92568bdeb5c | 97 | ++servo_num; |
shimizuta | 2:a92568bdeb5c | 98 | if (servo_num > 1) //両サーボ出力したら |
yuto17320508 | 0:f000d896d188 | 99 | { |
shimizuta | 2:a92568bdeb5c | 100 | //目標角の更新 |
shimizuta | 2:a92568bdeb5c | 101 | F_t(time_s, target_x_m, target_y_m); |
shimizuta | 2:a92568bdeb5c | 102 | leg.CalServoRad(target_x_m, target_y_m + offsetY); |
shimizuta | 2:a92568bdeb5c | 103 | servo_num = 0; |
yuto17320508 | 0:f000d896d188 | 104 | } |
shimizuta | 2:a92568bdeb5c | 105 | time_s += tickerTime; |
yuto17320508 | 0:f000d896d188 | 106 | } |
shimizuta | 2:a92568bdeb5c | 107 | int main() |
yuto17320508 | 0:f000d896d188 | 108 | { |
shimizuta | 2:a92568bdeb5c | 109 | float target_x_m = 0; |
shimizuta | 2:a92568bdeb5c | 110 | float target_y_m = 0; |
shimizuta | 2:a92568bdeb5c | 111 | F_t(0, target_x_m, target_y_m); |
shimizuta | 2:a92568bdeb5c | 112 | leg.CalServoRad(target_x_m, target_y_m + offsetY); |
yuto17320508 | 0:f000d896d188 | 113 | fliper.attach(&cb_timer, tickerTime); |
yuto17320508 | 0:f000d896d188 | 114 | } |