test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Diff: main.cpp
- Revision:
- 3:bcae0bb64b81
- Parent:
- 2:a92568bdeb5c
- Child:
- 4:fffdb273836e
--- a/main.cpp Fri Feb 08 12:14:19 2019 +0000 +++ b/main.cpp Fri Feb 08 16:59:48 2019 +0000 @@ -1,70 +1,80 @@ +//mbed間の同期を、master-slaveで行いたい。 + #include "mbed.h" #include "KondoServo.h" +#define Mastar; +//#define Slave; ////////////////Legクラス。下にmain文がある。 const float Pi = 3.141592; const float kRadToDegree = 180.0 / Pi; -//const float kDegreeToRad = Pi / 180.0; -const float LegLength1 = 0.1f; -const float LegLength2 = 0.2f; -class Leg -{ - float rad_[2]; - int id_[2]; - //パラメータ。実際の機体に合わせていじる - static const float dist_between_servo_half_m_ = 0.06f * 0.5; - KondoServo servo_; - - public: - Leg(PinName pin_serial_tx, PinName pin_serial_rx); - void MoveServo(int servo_num); - void CalServoRad(float x_m, float y_m); - void SetRad(float rad, int servo_num); - float GetRad(int servo_num); -}; - -Leg::Leg(PinName pin_serial_tx, PinName pin_serial_rx) : servo_(pin_serial_tx, pin_serial_rx) -{ - rad_[0] = 0; - rad_[1] = Pi; -}; -void Leg::MoveServo(int servo_num) +namespace Quadruped { - float degree = GetRad(servo_num) * kRadToDegree; - //servo1は反転させる - if (servo_num == 0) - degree += 90; - else - degree = 270 - degree; - servo_.set_degree(servo_num, degree); -} -void Leg::CalServoRad(float x_m, float y_m) -{ - //処理を軽くするために共通部分は先に計算 - float temp_x[] = {x_m + dist_between_servo_half_m_, - x_m - dist_between_servo_half_m_}; - float temp_y2 = y_m * y_m; - float temp_L = LegLength1 * LegLength1 - LegLength2 * LegLength2; + class Leg + { + float rad_[2]; + int id_[2]; + //パラメータ。実際の機体に合わせていじる + static const float dist_between_servo_half_m_ = 0.06f * 0.5; + static const float LegLength1 = 0.1f; + static const float LegLength2 = 0.2f; + KondoServo servo_; + + public: + Leg(PinName pin_serial_tx, PinName pin_serial_rx); + void MoveServo(int servo_num); + void CalServoRad(float x_m, float y_m); + void SetRad(float rad, int servo_num); + float GetRad(int servo_num); + }; - float r1 = sqrt((temp_x[1]) * (temp_x[1]) + temp_y2); - float r2 = sqrt((temp_x[0]) * (temp_x[0]) + temp_y2); - float targetTheta[] = {atan2(y_m, temp_x[1]) - acos((temp_L + r1 * r1) / (2.0f * r1 * LegLength1)), - atan2(y_m, temp_x[0]) + acos((temp_L + r2 * r2) / (2.0f * r2 * LegLength1))}; - for (size_t i = 0; i < 2; i++) - SetRad(targetTheta[i], i); -} -void Leg::SetRad(float rad, int servo_num) -{ - rad_[servo_num] = rad; -} -float Leg::GetRad(int servo_num) -{ - return rad_[servo_num]; + Leg::Leg(PinName pin_serial_tx, PinName pin_serial_rx) : servo_(pin_serial_tx, pin_serial_rx) + { + rad_[0] = 0; + rad_[1] = Pi; + }; + void Leg::MoveServo(int servo_num) + { + float degree = GetRad(servo_num) * kRadToDegree; + //servo1は反転させる + if (servo_num == 0) + degree += 90; + else + degree = 270 - degree; + servo_.set_degree(servo_num, degree); + } + void Leg::CalServoRad(float x_m, float y_m) + { + //処理を軽くするために共通部分は先に計算 + float temp_x[] = {x_m + dist_between_servo_half_m_, + x_m - dist_between_servo_half_m_}; + float temp_y2 = y_m * y_m; + float temp_L = LegLength1 * LegLength1 - LegLength2 * LegLength2; + + float r1 = sqrt((temp_x[1]) * (temp_x[1]) + temp_y2); + float r2 = sqrt((temp_x[0]) * (temp_x[0]) + temp_y2); + float targetTheta[] = {atan2(y_m, temp_x[1]) - acos((temp_L + r1 * r1) / (2.0f * r1 * LegLength1)), + atan2(y_m, temp_x[0]) + acos((temp_L + r2 * r2) / (2.0f * r2 * LegLength1))}; + for (size_t i = 0; i < 2; i++) + SetRad(targetTheta[i], i); + } + void Leg::SetRad(float rad, int servo_num) + { + rad_[servo_num] = rad; + } + float Leg::GetRad(int servo_num) + { + return rad_[servo_num]; + } } -//////////////////////////////////////////////////////////////Legクラスの定義終了 + -Leg leg(p9, p10); +//////////////////////////////////////////////////////////////以上が4脚ロボットに関する記述 +using namespace Quadruped; +Leg leg1(p9, p10); +Leg leg2(p28,p27); +CAN can1(p30,p29); Ticker fliper; const float tickerTime = 0.006f; //Ticker間隔 @@ -77,13 +87,13 @@ float theta = -omega * t + offsetDegree; if (sin(theta) <= 0.0f) { - tarX = 0.5f * LegLength2 * cos(theta); //0.5 - tarY = 0.15f * LegLength2 * sin(theta); //0.15 + tarX = 0.1f * cos(theta); //0.1 + tarY = 0.03f * sin(theta); //0.03 } else { - tarX = 0.5f * LegLength2 * cos(theta); - tarY = 0.0f * LegLength2 * sin(theta); + tarX = 0.1f * cos(theta); + tarY = 0.0f * sin(theta); } } void cb_timer() @@ -93,22 +103,25 @@ float target_x_m = 0; float target_y_m = 0; //出力。片サーボずつ - leg.MoveServo(servo_num); + leg1.MoveServo(servo_num); + leg2.MoveServo(servo_num); ++servo_num; if (servo_num > 1) //両サーボ出力したら { //目標角の更新 F_t(time_s, target_x_m, target_y_m); - leg.CalServoRad(target_x_m, target_y_m + offsetY); + leg1.CalServoRad(target_x_m, target_y_m + offsetY); + leg2.CalServoRad(target_x_m, target_y_m + offsetY); servo_num = 0; } time_s += tickerTime; } int main() { - float target_x_m = 0; + float target_x_m = 0; float target_y_m = 0; F_t(0, target_x_m, target_y_m); - leg.CalServoRad(target_x_m, target_y_m + offsetY); + leg1.CalServoRad(target_x_m, target_y_m + offsetY); + leg2.CalServoRad(target_x_m, target_y_m + offsetY); fliper.attach(&cb_timer, tickerTime); }