test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Diff: main.cpp
- Revision:
- 6:43708adf2e5d
- Parent:
- 5:556d5a5e9d24
- Child:
- 7:72c80a7c20d6
diff -r 556d5a5e9d24 -r 43708adf2e5d main.cpp --- a/main.cpp Sun Feb 10 06:07:02 2019 +0000 +++ b/main.cpp Sun Feb 10 09:14:15 2019 +0000 @@ -4,37 +4,39 @@ #include "mbed.h" #include "KondoServo.h" -#define Mastar ; +#define Mastar; //#define Slave; -//定義 -namespace Quadruped +//各値 +namespace Parameters { 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 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 Stride_length_m = 0.05f;//歩幅 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); +} +//定義 +namespace Quadruped//4脚ロボット +{ +using namespace Parameters; +void SetOffsetY(float offsetY){OffsetY=offsetY;} +void SetStrideMotion(float stride_time_s, float stride_length_m){Stride_time_s=stride_time_s;Stride_length_m=stride_length_m;}///////////////////////////////////設定 +void SetRisiongMotion(float rising_time_s, float width_m){Rising_time_s=rising_time_s;Width_m =width_m;}////////////////////////////////////////設定 +//ROSから送られてきたLRFのデータをもとに現在すべき動作を定義する。 +int WalkMode = 0; +void SetWalkMode(int mode){WalkMode = mode;} //4つの脚のtargetを保存する Legクラスで後で使えるようにしておく -float targetX[4]; -float targetY[4]; +float TargetX[4]; +float TargetY[4]; //脚の定義 位置を入力するとその方向にサーボが動く class Leg; //脚ごとのモーション これを組み合わせる @@ -48,8 +50,9 @@ namespace CanConnector { -//class Mastar; -//class Slave; +//Maserとslaveの定義 同じ関数名で動くようにしたい +class MastarConnector; +class SlaveConnector; } //実装 @@ -76,8 +79,9 @@ 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); //引数に上げる高さ +void Stride(float time_s,float &targetX, float &targetY); +void Rising(float time_s,float &targetX, float &targetY); +void Stop(float &targetX, float &targetY); } // namespace Motion namespace WalkingPattern @@ -92,7 +96,6 @@ Leg::Leg(PinName pin_serial_tx, PinName pin_serial_rx) : servo_(pin_serial_tx, pin_serial_rx) { - rad_[0] = 0; rad_[1] = Pi; }; @@ -131,50 +134,52 @@ } -void Motion::Stride(float time_s,float stride_length_m,float &targetX, float &targetY) +void Motion::Stride(float time_s,float &targetX, float &targetY) { - + targetX = -((Stride_length_m*time_s/Stride_time_s) - (Stride_length_m/2.0f)); + targetY = 0.0f; } -void Motion::Rising(float time_s,float width_m,float &targetX, float &targetY) +void Motion::Rising(float time_s,float &targetX, float &targetY) { - + //targetX = ((Stride_length_m*time_s/Rising_time_s) - (Stride_length_m/2.0f)); + //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) + 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)//4脚それぞれのtargetPoseを設定する { - 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]); + float thisLegTime = time_s + offsetTime[i];//脚に合わせたそれぞれの時間 + if(thisLegTime <= Stride_time_s) Motion::Stride(thisLegTime, TargetX[i],TargetY[i]); + else if(thisLegTime <= walkPeriod) Motion::Rising(thisLegTime-Stride_time_s,TargetX[i],TargetY[i]); + else Motion::Stride(thisLegTime-walkPeriod,TargetX[i],TargetY[i]); } time_s += tickerTime*2.0f; - if(time_s >= walkPeriod) time_s == 0.0f; + if(time_s >= walkPeriod) time_s = 0.0f; } void WalkingPattern::TurnLeft() { - static float time_s = 0.0f; + //static float time_s = 0.0f; } void WalkingPattern::TurnRight() { - static float time_s = 0.0f; + //static float time_s = 0.0f; } void WalkingPattern::Climb() { - static float time_s = 0.0f; + //static float time_s = 0.0f; } void WalkingPattern::Overcoming() { - static float time_s = 0.0f; + //static float time_s = 0.0f; } @@ -201,6 +206,35 @@ } } // namespace Quadruped +namespace CanConnector +{ + class MastarConnector + { + public: + MastarConnector(PinName rd, PinName td); + CAN can_; + + }; + MastarConnector::MastarConnector(PinName rd, PinName td): can_(rd, td) + { + + } + + + class SlaveConnector + { + public: + SlaveConnector(PinName rd, PinName td); + CAN can_; + + }; + SlaveConnector::SlaveConnector(PinName rd, PinName td): can_(rd, td) + { + + } + +} + //////////////////////////////////////////////////////////////以上が4脚ロボットに関する記述 using namespace Quadruped; Leg leg1(p9, p10); @@ -209,27 +243,10 @@ 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); @@ -238,9 +255,10 @@ 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); + Quadruped::CalcWalk(); + //このマイコンで使う脚だけ計算 + leg1.CalServoRad(TargetX[0], TargetY[0] + OffsetY); + leg2.CalServoRad(TargetX[1], TargetY[1] + OffsetY); servo_num = 0; } time_s += tickerTime; @@ -248,10 +266,8 @@ } 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); + Quadruped::CalcWalk(); + leg1.CalServoRad(TargetX[0], TargetY[0] + OffsetY); + leg2.CalServoRad(TargetX[1], TargetY[1] + OffsetY); fliper.attach(&cb_timer, tickerTime); }