岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

main.cpp@3:bcae0bb64b81, 2019-02-08 (annotated)

Committer:
yuto17320508
Date:
Fri Feb 08 16:59:48 2019 +0000
Revision:
3:bcae0bb64b81
Parent:
2:a92568bdeb5c
Child:
4:fffdb273836e
a bit fix

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yuto17320508 3:bcae0bb64b81 1 //mbed間の同期を、master-slaveで行いたい。
yuto17320508 3:bcae0bb64b81 2
yuto17320508 0:f000d896d188 3 #include "mbed.h"
yuto17320508 0:f000d896d188 4 #include "KondoServo.h"
yuto17320508 3:bcae0bb64b81 5 #define Mastar;
yuto17320508 3:bcae0bb64b81 6 //#define Slave;
shimizuta 2:a92568bdeb5c 7 ////////////////Legクラス。下にmain文がある。
shimizuta 2:a92568bdeb5c 8 const float Pi = 3.141592;
shimizuta 2:a92568bdeb5c 9 const float kRadToDegree = 180.0 / Pi;
yuto17320508 1:acaa3d7ede4c 10
yuto17320508 3:bcae0bb64b81 11 namespace Quadruped
shimizuta 2:a92568bdeb5c 12 {
yuto17320508 3:bcae0bb64b81 13 class Leg
yuto17320508 3:bcae0bb64b81 14 {
yuto17320508 3:bcae0bb64b81 15 float rad_[2];
yuto17320508 3:bcae0bb64b81 16 int id_[2];
yuto17320508 3:bcae0bb64b81 17 //パラメータ。実際の機体に合わせていじる
yuto17320508 3:bcae0bb64b81 18 static const float dist_between_servo_half_m_ = 0.06f * 0.5;
yuto17320508 3:bcae0bb64b81 19 static const float LegLength1 = 0.1f;
yuto17320508 3:bcae0bb64b81 20 static const float LegLength2 = 0.2f;
yuto17320508 3:bcae0bb64b81 21 KondoServo servo_;
yuto17320508 3:bcae0bb64b81 22
yuto17320508 3:bcae0bb64b81 23 public:
yuto17320508 3:bcae0bb64b81 24 Leg(PinName pin_serial_tx, PinName pin_serial_rx);
yuto17320508 3:bcae0bb64b81 25 void MoveServo(int servo_num);
yuto17320508 3:bcae0bb64b81 26 void CalServoRad(float x_m, float y_m);
yuto17320508 3:bcae0bb64b81 27 void SetRad(float rad, int servo_num);
yuto17320508 3:bcae0bb64b81 28 float GetRad(int servo_num);
yuto17320508 3:bcae0bb64b81 29 };
shimizuta 2:a92568bdeb5c 30
yuto17320508 3:bcae0bb64b81 31 Leg::Leg(PinName pin_serial_tx, PinName pin_serial_rx) : servo_(pin_serial_tx, pin_serial_rx)
yuto17320508 3:bcae0bb64b81 32 {
yuto17320508 3:bcae0bb64b81 33 rad_[0] = 0;
yuto17320508 3:bcae0bb64b81 34 rad_[1] = Pi;
yuto17320508 3:bcae0bb64b81 35 };
yuto17320508 3:bcae0bb64b81 36 void Leg::MoveServo(int servo_num)
yuto17320508 3:bcae0bb64b81 37 {
yuto17320508 3:bcae0bb64b81 38 float degree = GetRad(servo_num) * kRadToDegree;
yuto17320508 3:bcae0bb64b81 39 //servo1は反転させる
yuto17320508 3:bcae0bb64b81 40 if (servo_num == 0)
yuto17320508 3:bcae0bb64b81 41 degree += 90;
yuto17320508 3:bcae0bb64b81 42 else
yuto17320508 3:bcae0bb64b81 43 degree = 270 - degree;
yuto17320508 3:bcae0bb64b81 44 servo_.set_degree(servo_num, degree);
yuto17320508 3:bcae0bb64b81 45 }
yuto17320508 3:bcae0bb64b81 46 void Leg::CalServoRad(float x_m, float y_m)
yuto17320508 3:bcae0bb64b81 47 {
yuto17320508 3:bcae0bb64b81 48 //処理を軽くするために共通部分は先に計算
yuto17320508 3:bcae0bb64b81 49 float temp_x[] = {x_m + dist_between_servo_half_m_,
yuto17320508 3:bcae0bb64b81 50 x_m - dist_between_servo_half_m_};
yuto17320508 3:bcae0bb64b81 51 float temp_y2 = y_m * y_m;
yuto17320508 3:bcae0bb64b81 52 float temp_L = LegLength1 * LegLength1 - LegLength2 * LegLength2;
yuto17320508 3:bcae0bb64b81 53
yuto17320508 3:bcae0bb64b81 54 float r1 = sqrt((temp_x[1]) * (temp_x[1]) + temp_y2);
yuto17320508 3:bcae0bb64b81 55 float r2 = sqrt((temp_x[0]) * (temp_x[0]) + temp_y2);
yuto17320508 3:bcae0bb64b81 56 float targetTheta[] = {atan2(y_m, temp_x[1]) - acos((temp_L + r1 * r1) / (2.0f * r1 * LegLength1)),
yuto17320508 3:bcae0bb64b81 57 atan2(y_m, temp_x[0]) + acos((temp_L + r2 * r2) / (2.0f * r2 * LegLength1))};
yuto17320508 3:bcae0bb64b81 58 for (size_t i = 0; i < 2; i++)
yuto17320508 3:bcae0bb64b81 59 SetRad(targetTheta[i], i);
yuto17320508 3:bcae0bb64b81 60 }
yuto17320508 3:bcae0bb64b81 61 void Leg::SetRad(float rad, int servo_num)
yuto17320508 3:bcae0bb64b81 62 {
yuto17320508 3:bcae0bb64b81 63 rad_[servo_num] = rad;
yuto17320508 3:bcae0bb64b81 64 }
yuto17320508 3:bcae0bb64b81 65 float Leg::GetRad(int servo_num)
yuto17320508 3:bcae0bb64b81 66 {
yuto17320508 3:bcae0bb64b81 67 return rad_[servo_num];
yuto17320508 3:bcae0bb64b81 68 }
shimizuta 2:a92568bdeb5c 69 }
shimizuta 2:a92568bdeb5c 70
yuto17320508 3:bcae0bb64b81 71
shimizuta 2:a92568bdeb5c 72
yuto17320508 3:bcae0bb64b81 73 //////////////////////////////////////////////////////////////以上が4脚ロボットに関する記述
yuto17320508 3:bcae0bb64b81 74 using namespace Quadruped;
yuto17320508 3:bcae0bb64b81 75 Leg leg1(p9, p10);
yuto17320508 3:bcae0bb64b81 76 Leg leg2(p28,p27);
yuto17320508 3:bcae0bb64b81 77 CAN can1(p30,p29);
shimizuta 2:a92568bdeb5c 78 Ticker fliper;
shimizuta 2:a92568bdeb5c 79
shimizuta 2:a92568bdeb5c 80 const float tickerTime = 0.006f; //Ticker間隔
shimizuta 2:a92568bdeb5c 81 const float omega = 3.1415f * 2.5f; //角速度
yuto17320508 0:f000d896d188 82 const float offsetDegree = 0.0f;
yuto17320508 0:f000d896d188 83 const float offsetY = 0.15f;
yuto17320508 0:f000d896d188 84
yuto17320508 0:f000d896d188 85 void F_t(float t, float &tarX, float &tarY)
yuto17320508 0:f000d896d188 86 {
yuto17320508 0:f000d896d188 87 float theta = -omega * t + offsetDegree;
yuto17320508 0:f000d896d188 88 if (sin(theta) <= 0.0f)
yuto17320508 0:f000d896d188 89 {
yuto17320508 3:bcae0bb64b81 90 tarX = 0.1f * cos(theta); //0.1
yuto17320508 3:bcae0bb64b81 91 tarY = 0.03f * sin(theta); //0.03
yuto17320508 0:f000d896d188 92 }
yuto17320508 0:f000d896d188 93 else
yuto17320508 0:f000d896d188 94 {
yuto17320508 3:bcae0bb64b81 95 tarX = 0.1f * cos(theta);
yuto17320508 3:bcae0bb64b81 96 tarY = 0.0f * sin(theta);
yuto17320508 0:f000d896d188 97 }
yuto17320508 0:f000d896d188 98 }
yuto17320508 0:f000d896d188 99 void cb_timer()
yuto17320508 0:f000d896d188 100 {
shimizuta 2:a92568bdeb5c 101 static int servo_num = 0;
shimizuta 2:a92568bdeb5c 102 static float time_s = 0.0f;
shimizuta 2:a92568bdeb5c 103 float target_x_m = 0;
shimizuta 2:a92568bdeb5c 104 float target_y_m = 0;
shimizuta 2:a92568bdeb5c 105 //出力。片サーボずつ
yuto17320508 3:bcae0bb64b81 106 leg1.MoveServo(servo_num);
yuto17320508 3:bcae0bb64b81 107 leg2.MoveServo(servo_num);
shimizuta 2:a92568bdeb5c 108 ++servo_num;
shimizuta 2:a92568bdeb5c 109 if (servo_num > 1) //両サーボ出力したら
yuto17320508 0:f000d896d188 110 {
shimizuta 2:a92568bdeb5c 111 //目標角の更新
shimizuta 2:a92568bdeb5c 112 F_t(time_s, target_x_m, target_y_m);
yuto17320508 3:bcae0bb64b81 113 leg1.CalServoRad(target_x_m, target_y_m + offsetY);
yuto17320508 3:bcae0bb64b81 114 leg2.CalServoRad(target_x_m, target_y_m + offsetY);
shimizuta 2:a92568bdeb5c 115 servo_num = 0;
yuto17320508 0:f000d896d188 116 }
shimizuta 2:a92568bdeb5c 117 time_s += tickerTime;
yuto17320508 0:f000d896d188 118 }
shimizuta 2:a92568bdeb5c 119 int main()
yuto17320508 0:f000d896d188 120 {
yuto17320508 3:bcae0bb64b81 121 float target_x_m = 0;
shimizuta 2:a92568bdeb5c 122 float target_y_m = 0;
shimizuta 2:a92568bdeb5c 123 F_t(0, target_x_m, target_y_m);
yuto17320508 3:bcae0bb64b81 124 leg1.CalServoRad(target_x_m, target_y_m + offsetY);
yuto17320508 3:bcae0bb64b81 125 leg2.CalServoRad(target_x_m, target_y_m + offsetY);
yuto17320508 0:f000d896d188 126 fliper.attach(&cb_timer, tickerTime);
yuto17320508 0:f000d896d188 127 }