test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
main.cpp
- Committer:
- shimizuta
- Date:
- 2019-02-11
- Revision:
- 10:7a340c52e270
- Parent:
- 9:905f93247688
- Child:
- 11:e81425872740
File content as of revision 10:7a340c52e270:
//NHK2019MR2 馬型機構プログラム.main.cppでは4足の協調に関わるプログラムを主に書く。 #include "mbed.h" #include "pinnames.h" #include "ToePosi.h" //個々の足先の位置を決めるプログラム #include "MoveServoOfLeg.h" //サーボを実際に動かすプログラム #define DEBUG ////////////調整すべきパラメータ.全てここに集めた。 const float kCycleTime_s = 0.02f; //計算周期 const float kStride_m = 0.05f; //歩幅 const float kHeight_m = 0.03f; //足の上げ幅 const float kOffsetY_m = 0.02f; //足のデフォルト高さ const float kBetweenServoHalf_m = 0.06f * 0.5; //サーボ間の距離の半分 const float kLegLength1 = 0.1f; const float kLegLength2 = 0.2f; const float kServoSpan_ms = 0; //サーボの送信間隔 const float kVelocity_m_s = 0.01f; //進む速度(適当) const float kDist_m = 10; //歩行中に進む距離(適当) /////////////// Timer timer; MoveServoOfLeg moveleg[2] = { MoveServoOfLeg(pin_serial_servo_tx[0], pin_serial_servo_rx[0], kBetweenServoHalf_m, kLegLength1, kLegLength2), MoveServoOfLeg(pin_serial_servo_tx[1], pin_serial_servo_rx[1], kBetweenServoHalf_m, kLegLength1, kLegLength2), }; DigitalOut led(LED1); Serial pc(USBTX, USBRX); const float M_PI = 3.141592; //歩き方のパターン enum WalkingPattern { STRAIGHT_TROT, }; //歩き方:patternでdist_mだけ進む。 void Move(WalkingPattern pattern, ToePosi (&now)[4], float maxvelocity_m_s, float dist_m); //直進時の歩き方 void CalPhaseStraight(ToePosi now_base, ToePosi (&target)[4], float ave_v_m_s); int main() { printf("program start\r\n"); //現在のそれぞれの足の位置を保存するもの ToePosi now[4] = { ToePosi(kStride_m, kHeight_m, kOffsetY_m), ToePosi(kStride_m, kHeight_m, kOffsetY_m), ToePosi(kStride_m, kHeight_m, kOffsetY_m), ToePosi(kStride_m, kHeight_m, kOffsetY_m), }; //直進 printf("please put some key\r\n"); while (pc.readable() == 0) ; printf("move start\r\n"); Move(STRAIGHT_TROT, now, kVelocity_m_s, kDist_m); printf("program end\r\n"); } void Move(WalkingPattern pattern, ToePosi (&now)[4], float maxvelocity_m_s, float dist_m) { //patternを見て、使う関数を決定.関数ポインタを利用 void (*Cal4LegsPhase)(ToePosi, ToePosi(&target)[4], float); switch (pattern) { case STRAIGHT_TROT: Cal4LegsPhase = CalPhaseStraight; //収束判定関数もここでpatternによって変える必要あるかも。 break; default: printf("error:there is no func of the pattern"); } ToePosi target[4] = {now[0], now[1], now[2], now[3]}; //目標位置を取り敢えず現在位置に。 timer.reset(); timer.start(); int is_arrived = 0; while (is_arrived == 0) { float time_s = timer.read(); //注:未実装。到着したかの判定.LRFからのデータが必要? // is_arrived = IsArrived(); //4本の足それぞれの位相更新.二次曲線のどこにいるべきかが決まる Cal4LegsPhase(now[0], target, maxvelocity_m_s); //注:未実装。slave_mbed分の足の目標位置を送信 //自身が動かす足のサーボ角度更新 moveleg[0].CalServoRad(target[0].GetX_m(), target[0].GetY_m()); moveleg[1].CalServoRad(target[1].GetX_m(), target[1].GetY_m()); //自身が動かす足のサーボを動かす moveleg[0].MoveServo(0); moveleg[1].MoveServo(0); wait_ms(kServoSpan_ms); moveleg[0].MoveServo(1); moveleg[1].MoveServo(1); //現在位置更新 for (int i = 0; i < 4; i++) now[i] = target[i]; //計算周期をkCycleTime_sにする float rest_time_s = kCycleTime_s - (timer.read() - time_s); if (rest_time_s > 0) wait(rest_time_s); else //計算周期が達成できないときはprintfで知らせるだけ。動きはする。 printf("error: rest_time_s = %f in Move()\r\n", rest_time_s); } } void CalPhaseStraight(ToePosi now_base, ToePosi (&target)[4], float ave_v_m_s) { //指定速度で進むための周期 = 平均速度*時間/歩幅 * pi(半周期で歩幅分進む) float phase_step = ave_v_m_s * kCycleTime_s / kStride_m * M_PI; //対角線上の足の位相は同じで、反対側は半周期先。 float target_phase0 = now_base.GetPhase() + phase_step; float target_phase1 = target_phase0 + M_PI; target[0].SetPhase(target_phase0); target[1].SetPhase(target_phase1); target[2].SetPhase(target_phase0); target[3].SetPhase(target_phase1); }