test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Diff: main.cpp
- Revision:
- 8:21b932c4e6c5
- Parent:
- 7:72c80a7c20d6
- Child:
- 9:905f93247688
--- a/main.cpp Sun Feb 10 10:37:02 2019 +0000 +++ b/main.cpp Sun Feb 10 12:38:16 2019 +0000 @@ -14,11 +14,11 @@ //パラメータ const float Pi = 3.14; const float kRadToDegree = 180.0 / Pi; -const float tickerTime = 0.02f; //Ticker間隔 +const float tickerTime = 0.007f;//Ticker間隔 float OffsetY = 0.15f; -float Stride_time_s = 1.0f*10.0f;//床についている時間 1.0f +float Stride_time_s = 0.8f;//床についている時間 1.0f float Stride_length_m = 0.05f;//歩幅 -float Rising_time_s = 0.2f*10.0f;//脚を上げている時間 0.2f +float Rising_time_s = 0.2f;//脚を上げている時間 0.2f float Width_m = 0.03f;//脚を上げる高さ static const int straight = 0; static const int turnleft = 1; @@ -90,11 +90,11 @@ namespace WalkingPattern { using namespace Motion; -void Straight(); -void TurnLeft(); -void TurnRight(); -void Climb(); -void Overcoming(); //段差、紐乗り越え動作 +void Straight(int servo_num); +void TurnLeft(int servo_num); +void TurnRight(int servo_num); +void Climb(int servo_num); +void Overcoming(int servo_num); //段差、紐乗り越え動作 } // namespace WalkingPattern Leg::Leg(PinName pin_serial_tx, PinName pin_serial_rx) : servo_(pin_serial_tx, pin_serial_rx) @@ -155,10 +155,11 @@ float offsetTime[4] = {0.0f, walkPeriod/2.0f, 0.0f, walkPeriod/2.0f};//位相をずれすための時間の初期誤差 -void WalkingPattern::Straight() +void WalkingPattern::Straight(int servo_num) { + int servo_state = 2*servo_num; static float time_s = 0.0f;//共通時間 - for(int i=0;i<4;++i)//4脚それぞれのtargetPoseを設定する + for(int i=0+servo_state;i<2+servo_state;++i)//2脚ずつそれぞれのtargetPoseを設定する { float thisLegTime = time_s + offsetTime[i];//脚に合わせたそれぞれの時間 if(thisLegTime <= Stride_time_s) Motion::Stride(thisLegTime, i); @@ -166,51 +167,54 @@ else Motion::Stride(thisLegTime-walkPeriod,i); } #ifdef DebugMode - printf("time_s: %.3f targetX0: %.3f targetY0: %.3f\r\n", time_s, TargetX[0],TargetY[0]); + if(servo_num == 0) + 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; - + time_s += tickerTime; + if(time_s >= walkPeriod) + { + time_s = 0.0f; + } } -void WalkingPattern::TurnLeft() +void WalkingPattern::TurnLeft(int servo_num) { //static float time_s = 0.0f; } -void WalkingPattern::TurnRight() +void WalkingPattern::TurnRight(int servo_num) { //static float time_s = 0.0f; } -void WalkingPattern::Climb() +void WalkingPattern::Climb(int servo_num) { //static float time_s = 0.0f; } -void WalkingPattern::Overcoming() +void WalkingPattern::Overcoming(int servo_num) { //static float time_s = 0.0f; } -void CalcWalk() +void CalcWalk(int servo_num) { switch(WalkMode) { case straight: - WalkingPattern::Straight(); + WalkingPattern::Straight(servo_num); break; case turnleft: - WalkingPattern::TurnLeft(); + WalkingPattern::TurnLeft(servo_num); break; case turnright: - WalkingPattern::TurnRight(); + WalkingPattern::TurnRight(servo_num); break; case climb: - WalkingPattern::Climb(); + WalkingPattern::Climb(servo_num); break; case overcoming: - WalkingPattern::Overcoming(); + WalkingPattern::Overcoming(servo_num); break; } } @@ -256,28 +260,29 @@ void cb_timer() { static int servo_num = 0; - static float time_s = 0.0f; //出力。片サーボずつ leg1.MoveServo(servo_num); leg2.MoveServo(servo_num); + Quadruped::CalcWalk(servo_num); ++servo_num; + if(servo_num > 1)servo_num = 0; - if (servo_num > 1) //両サーボ出力したら + /*if (servo_num > 1) //両サーボ出力したら { //目標角の更新 - Quadruped::CalcWalk(); + Quadruped::CalcWalk(servo_num); //このマイコンで使う脚だけ計算 leg1.CalServoRad(TargetX[0], TargetY[0] + OffsetY); leg2.CalServoRad(TargetX[1], TargetY[1] + OffsetY); servo_num = 0; - } - time_s += tickerTime; + }*/ } int main() { - Quadruped::CalcWalk(); + Quadruped::CalcWalk(0); + Quadruped::CalcWalk(1); leg1.CalServoRad(TargetX[0], TargetY[0] + OffsetY); leg2.CalServoRad(TargetX[1], TargetY[1] + OffsetY); fliper.attach(&cb_timer, tickerTime);