岡本 陸 / Mbed 2 deprecated 0311

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?

UserRevisionLine numberNew 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 }