test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
main.cpp
- Committer:
- yuto17320508
- Date:
- 2019-02-10
- Revision:
- 5:556d5a5e9d24
- Parent:
- 4:fffdb273836e
- Child:
- 6:43708adf2e5d
File content as of revision 5:556d5a5e9d24:
//mbed間の同期を、master-slaveで行いたい。 //masetrで4つの脚の動きをすべて決定, //その動きをslaveに送る。 #include "mbed.h" #include "KondoServo.h" #define Mastar ; //#define Slave; //定義 namespace Quadruped { const float Pi = 3.141592; const float kRadToDegree = 180.0 / Pi; const float tickerTime = 0.006f; //Ticker間隔 const float omega = 3.1415f * 2.5f; //角速度 const float offsetDegree = 0.0f; const float offsetY = 0.15f; float Stride_time_s = 1.0f;//床についている時間 float Stride_length_m = 0.1f;//歩幅 void SetStrideMotion(float stride_time_s, float stride_length_m); float Rising_time_s = 0.2f;//脚を上げている時間 float Width_m = 0.03f;//脚を上げる高さ void SetRisiongMotion(float rising_time_s, float width_m); //ROSから送られてきたLRFのデータをもとに現在すべき動作を定義する。 int WalkMode = 0; static const int straight = 0; static const int turnleft = 1; static const int turnright = 2; static const int climb = 3; static const int overcoming = 4; void SetWalkMode(int mode); //4つの脚のtargetを保存する Legクラスで後で使えるようにしておく float targetX[4]; float targetY[4]; //脚の定義 位置を入力するとその方向にサーボが動く class Leg; //脚ごとのモーション これを組み合わせる namespace Motion{} //モーションを組み合わせて歩行パターンを形成する //前進、旋回など namespace WalkingPattern{} //パターン関数を呼び出す 一つの関数でいいようにswitchする void CalcWalk(int legNum, Leg leg); } // namespace Quadruped 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); }; namespace Motion { void Stride(float time_s,float stride_length_m,float &targetX, float &targetY); //引数に歩幅 void Rising(float time_s,float width_m,float &targetX, float &targetY); //引数に上げる高さ } // namespace Motion namespace WalkingPattern { using namespace Motion; void Straight(); void TurnLeft(); void TurnRight(); void Climb(); void Overcoming(); //段差、紐乗り越え動作 } // namespace WalkingPattern 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]; } void Motion::Stride(float time_s,float stride_length_m,float &targetX, float &targetY) { } void Motion::Rising(float time_s,float width_m,float &targetX, float &targetY) { } void WalkingPattern::Straight() { static float time_s = 0.0f; float walkPeriod = Stride_time_s+Rising_time_s; float offsetTime[4] = {0.0f, walkPeriod/2.0f, 0.0f, walkPeriod/2.0f}; for(int i=0;i<4;++i) { float thisLegTime = time_s + offsetTime[i]; if(thisLegTime <= Stride_time_s) Motion::Stride(thisLegTime, Stride_length_m, targetX[i],targetY[i]); else if(thisLegTime <= walkPeriod) Motion::Rising(thisLegTime-Stride_time_s, Width_m,targetX[i],targetY[i]); else Motion::Stride(thisLegTime-walkPeriod, Stride_length_m,targetX[i],targetY[i]); } time_s += tickerTime*2.0f; if(time_s >= walkPeriod) time_s == 0.0f; } void WalkingPattern::TurnLeft() { static float time_s = 0.0f; } void WalkingPattern::TurnRight() { static float time_s = 0.0f; } void WalkingPattern::Climb() { static float time_s = 0.0f; } void WalkingPattern::Overcoming() { static float time_s = 0.0f; } void CalcWalk() { switch(WalkMode) { case straight: WalkingPattern::Straight(); break; case turnleft: WalkingPattern::TurnLeft(); break; case turnright: WalkingPattern::TurnRight(); break; case climb: WalkingPattern::Climb(); break; case overcoming: WalkingPattern::Overcoming(); break; } } } // namespace Quadruped //////////////////////////////////////////////////////////////以上が4脚ロボットに関する記述 using namespace Quadruped; Leg leg1(p9, p10); Leg leg2(p28, p27); CAN can1(p30, p29); Ticker fliper; 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); }