test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
main.cpp
- Committer:
- yuto17320508
- Date:
- 2019-02-09
- Revision:
- 4:fffdb273836e
- Parent:
- 3:bcae0bb64b81
- Child:
- 5:556d5a5e9d24
File content as of revision 4:fffdb273836e:
//mbed間の同期を、master-slaveで行いたい。 //masetrで4つの脚の動きをすべて決定, //その動きをslaveに送る。 #include "mbed.h" #include "KondoServo.h" #define Mastar; //#define Slave; const float Pi = 3.141592; const float kRadToDegree = 180.0 / Pi; //定義 namespace Quadruped { class Leg; namespace Motion{} namespace WalkingPattern{} } namespace CanConnector { //class Mastar; //class Slave; } //実装 namespace Quadruped { //脚の定義 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); }; 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]; } namespace Motion { void Slide(Leg leg,float stride_m);//引数に歩幅 void Rising(Leg leg,float width_m);//引数に上げる高さ } void Motion::Slide(Leg leg,float stride_m) { static float time_s = 0.0f; } void Motion::Rising(Leg leg,float width_m) { static float time_s = 0.0f; } namespace WalkingPattern { using namespace Motion; void Straight(); void TurnRight(); void TurnLeft(); void Climb(); void Overcoming();//段差、紐乗り越え動作 } } //////////////////////////////////////////////////////////////以上が4脚ロボットに関する記述 using namespace Quadruped; Leg leg1(p9, p10); Leg leg2(p28,p27); CAN can1(p30,p29); Ticker fliper; const float tickerTime = 0.006f; //Ticker間隔 const float omega = 3.1415f * 2.5f; //角速度 const float offsetDegree = 0.0f; const float offsetY = 0.15f; void F_t(float t, float &tarX, float &tarY) { float theta = -omega * t + offsetDegree; if (sin(theta) <= 0.0f) { tarX = 0.1f * cos(theta); //0.1 tarY = 0.03f * sin(theta); //0.03 } else { tarX = 0.1f * cos(theta); tarY = 0.0f * sin(theta); } } void cb_timer() { static int servo_num = 0; static float time_s = 0.0f; float target_x_m = 0; float target_y_m = 0; //出力。片サーボずつ leg1.MoveServo(servo_num); leg2.MoveServo(servo_num); ++servo_num; if (servo_num > 1) //両サーボ出力したら { //目標角の更新 F_t(time_s, target_x_m, target_y_m); 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_y_m = 0; F_t(0, target_x_m, target_y_m); leg1.CalServoRad(target_x_m, target_y_m + offsetY); leg2.CalServoRad(target_x_m, target_y_m + offsetY); fliper.attach(&cb_timer, tickerTime); }