test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Diff: main.cpp
- Revision:
- 7:72c80a7c20d6
- Parent:
- 6:43708adf2e5d
- Child:
- 8:21b932c4e6c5
diff -r 43708adf2e5d -r 72c80a7c20d6 main.cpp --- a/main.cpp Sun Feb 10 09:14:15 2019 +0000 +++ b/main.cpp Sun Feb 10 10:37:02 2019 +0000 @@ -6,17 +6,19 @@ #include "KondoServo.h" #define Mastar; //#define Slave; +#define DebugMode; //各値 namespace Parameters { -const float Pi = 3.141592; + //パラメータ +const float Pi = 3.14; const float kRadToDegree = 180.0 / Pi; -const float tickerTime = 0.006f; //Ticker間隔 +const float tickerTime = 0.02f; //Ticker間隔 float OffsetY = 0.15f; -float Stride_time_s = 1.0f;//床についている時間 +float Stride_time_s = 1.0f*10.0f;//床についている時間 1.0f float Stride_length_m = 0.05f;//歩幅 -float Rising_time_s = 0.2f;//脚を上げている時間 +float Rising_time_s = 0.2f*10.0f;//脚を上げている時間 0.2f float Width_m = 0.03f;//脚を上げる高さ static const int straight = 0; static const int turnleft = 1; @@ -28,6 +30,7 @@ namespace Quadruped//4脚ロボット { using namespace Parameters; +//USERが直接制御に使う関数 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;}////////////////////////////////////////設定 @@ -51,8 +54,8 @@ namespace CanConnector { //Maserとslaveの定義 同じ関数名で動くようにしたい -class MastarConnector; -class SlaveConnector; +class MastarConnector;//cansendするマイコンの場合 +class SlaveConnector;//canreadするマイコンの場合 } //実装 @@ -79,9 +82,9 @@ namespace Motion { -void Stride(float time_s,float &targetX, float &targetY); -void Rising(float time_s,float &targetX, float &targetY); -void Stop(float &targetX, float &targetY); +void Stride(float time_s,int legnum); +void Rising(float time_s,int legnum); +void Stop(int legnum); } // namespace Motion namespace WalkingPattern @@ -134,30 +137,37 @@ } -void Motion::Stride(float time_s,float &targetX, float &targetY) +void Motion::Stride(float time_s,int legnum) { - targetX = -((Stride_length_m*time_s/Stride_time_s) - (Stride_length_m/2.0f)); - targetY = 0.0f; + TargetX[legnum] = (Stride_length_m*time_s/Stride_time_s) - (Stride_length_m/2.0f); + TargetY[legnum] = 0.0f; + } -void Motion::Rising(float time_s,float &targetX, float &targetY) +void Motion::Rising(float time_s,int legnum) { - //targetX = ((Stride_length_m*time_s/Rising_time_s) - (Stride_length_m/2.0f)); - //targetY = + TargetX[legnum] = (Stride_length_m/2.0f)*cos(-Pi*time_s/Rising_time_s); + TargetY[legnum] = Width_m*sin(-Pi*time_s/Rising_time_s); + } +float walkPeriod = Stride_time_s+Rising_time_s;//周期 +float offsetTime[4] = {0.0f, walkPeriod/2.0f, 0.0f, walkPeriod/2.0f};//位相をずれすための時間の初期誤差 + + 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)//4脚それぞれのtargetPoseを設定する { 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]); + if(thisLegTime <= Stride_time_s) Motion::Stride(thisLegTime, i); + else if(thisLegTime <= walkPeriod) Motion::Rising(thisLegTime-Stride_time_s,i); + else Motion::Stride(thisLegTime-walkPeriod,i); } + #ifdef DebugMode + printf("time_s: %.3f targetX0: %.3f targetY0: %.3f\r\n", time_s, TargetX[0],TargetY[0]); + #endif time_s += tickerTime*2.0f; if(time_s >= walkPeriod) time_s = 0.0f; @@ -213,12 +223,10 @@ public: MastarConnector(PinName rd, PinName td); CAN can_; + void Connect(); }; - MastarConnector::MastarConnector(PinName rd, PinName td): can_(rd, td) - { - - } + MastarConnector::MastarConnector(PinName rd, PinName td): can_(rd, td){} class SlaveConnector @@ -226,12 +234,10 @@ public: SlaveConnector(PinName rd, PinName td); CAN can_; + void Connect(); }; - SlaveConnector::SlaveConnector(PinName rd, PinName td): can_(rd, td) - { - - } + SlaveConnector::SlaveConnector(PinName rd, PinName td): can_(rd, td){} } @@ -239,9 +245,13 @@ using namespace Quadruped; Leg leg1(p9, p10); Leg leg2(p28, p27); -CAN can1(p30, p29); +//CAN can1(p30, p29); Ticker fliper; - +#ifdef Mastar +CanConnector::MastarConnector connector(p30,p29); +#elif Slave +CanConnector::SlaveConnector connector(p30,p29); +#endif void cb_timer() { @@ -259,6 +269,7 @@ //このマイコンで使う脚だけ計算 leg1.CalServoRad(TargetX[0], TargetY[0] + OffsetY); leg2.CalServoRad(TargetX[1], TargetY[1] + OffsetY); + servo_num = 0; } time_s += tickerTime;