test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Diff: main.cpp
- Revision:
- 9:905f93247688
- Parent:
- 8:21b932c4e6c5
- Child:
- 10:7a340c52e270
diff -r 21b932c4e6c5 -r 905f93247688 main.cpp --- a/main.cpp Sun Feb 10 12:38:16 2019 +0000 +++ b/main.cpp Mon Feb 11 01:56:49 2019 +0000 @@ -1,289 +1,115 @@ -//mbed間の同期を、master-slaveで行いたい。 -//masetrで4つの脚の動きをすべて決定, -//その動きをslaveに送る。 - +//NHK2019MR2 馬型機構プログラム.main.cppでは4足の協調に関わるプログラムを主に書く。 #include "mbed.h" -#include "KondoServo.h" -#define Mastar; -//#define Slave; -#define DebugMode; +#include "pinnames.h" +#include "ToePosi.h" //個々の足先の位置を決めるプログラム +#include "MoveServoOfLeg.h" //サーボを実際に動かすプログラム -//各値 -namespace Parameters -{ - //パラメータ -const float Pi = 3.14; -const float kRadToDegree = 180.0 / Pi; -const float tickerTime = 0.007f;//Ticker間隔 -float OffsetY = 0.15f; -float Stride_time_s = 0.8f;//床についている時間 1.0f -float Stride_length_m = 0.05f;//歩幅 -float Rising_time_s = 0.2f;//脚を上げている時間 0.2f -float Width_m = 0.03f;//脚を上げる高さ -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; -} -//定義 -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;}////////////////////////////////////////設定 -//ROSから送られてきたLRFのデータをもとに現在すべき動作を定義する。 -int WalkMode = 0; -void SetWalkMode(int mode){WalkMode = 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 +////////////調整すべきパラメータ.全てここに集めた。 +const float kCycleTime_s = 0.02f; //計算周期 +const float kStride_m = 0.05f; //歩幅 +const float kHeight_m = 0.03f; //足を上げる高さ +const float kBetweenServoHalf_m = 0.06f * 0.5; //サーボ間の距離の半分 +const float kLegLength1 = 0.1f; +const float kLegLength2 = 0.2f; +const float kServoSpan_ms = 50; //サーボの送信感覚 +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); -namespace CanConnector -{ -//Maserとslaveの定義 同じ関数名で動くようにしたい -class MastarConnector;//cansendするマイコンの場合 -class SlaveConnector;//canreadするマイコンの場合 -} - -//実装 -namespace Quadruped -{ -//脚の定義 -class Leg +const float M_PI = 3.141592; +//歩き方のパターン +enum WalkingPattern { - 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); + 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); -namespace Motion -{ -void Stride(float time_s,int legnum); -void Rising(float time_s,int legnum); -void Stop(int legnum); -} // namespace Motion - -namespace WalkingPattern -{ -using namespace Motion; -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) +int main() { - 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]; + //現在のそれぞれの足の位置を保存するもの + ToePosi now[4] = { + ToePosi(kStride_m, kHeight_m), + ToePosi(kStride_m, kHeight_m), + ToePosi(kStride_m, kHeight_m), + ToePosi(kStride_m, kHeight_m), + }; + //直進 + Move(STRAIGHT_TROT, now, kVelocity_m_s, kDist_m); } - -void Motion::Stride(float time_s,int legnum) -{ - 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,int legnum) +void Move(WalkingPattern pattern, ToePosi (&now)[4], float maxvelocity_m_s, float dist_m) { - 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(int servo_num) -{ - int servo_state = 2*servo_num; - static float time_s = 0.0f;//共通時間 - for(int i=0+servo_state;i<2+servo_state;++i)//2脚ずつそれぞれのtargetPoseを設定する + //patternを見て、使う関数を決定.関数ポインタを利用 + void (*Cal4LegsPhase)(ToePosi, ToePosi(&target)[4], float); + switch (pattern) { - float thisLegTime = time_s + offsetTime[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); + case STRAIGHT_TROT: + Cal4LegsPhase = CalPhaseStraight; + //収束判定関数もここでpatternによって変える必要あるかも。 + break; + default: + printf("error:there is no func of the pattern"); } - #ifdef DebugMode - if(servo_num == 0) - printf("time_s: %.3f targetX0: %.3f targetY0: %.3f\r\n", time_s, TargetX[0], TargetY[0]); - #endif - time_s += tickerTime; - if(time_s >= walkPeriod) + + ToePosi target[4] = {now[0], now[1], now[2], now[3]}; //目標位置を取り敢えず現在位置に。 + timer.reset(); + timer.start(); + int is_arrived = 0; + while (is_arrived == 0) { - time_s = 0.0f; - } -} -void WalkingPattern::TurnLeft(int servo_num) -{ - //static float time_s = 0.0f; + float time_s = timer.read(); + //注:未実装。到着したかの判定.LRFからのデータが必要? + //is_arrived = IsArrived(); + + //4本の足それぞれの位相更新.二次曲線のどこにいるべきかが決まる + Cal4LegsPhase(now[0], target, maxvelocity_m_s); + //注:未実装。slave_mbed分の足の目標位置を送信 -} -void WalkingPattern::TurnRight(int servo_num) -{ - //static float time_s = 0.0f; - -} -void WalkingPattern::Climb(int servo_num) -{ - //static float time_s = 0.0f; - -} -void WalkingPattern::Overcoming(int servo_num) -{ - //static float time_s = 0.0f; - + //自身が動かす足のサーボ角度更新 + 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 //計算周期が達成できないときは + { + led = 1; //LED1を光らせて知らせる。 + printf("error: rest_time_s = %f in Move()\r\n", rest_time_s); + } + } } -void CalcWalk(int servo_num) -{ - switch(WalkMode) - { - case straight: - WalkingPattern::Straight(servo_num); - break; - case turnleft: - WalkingPattern::TurnLeft(servo_num); - break; - case turnright: - WalkingPattern::TurnRight(servo_num); - break; - case climb: - WalkingPattern::Climb(servo_num); - break; - case overcoming: - WalkingPattern::Overcoming(servo_num); - break; - } -} -} // namespace Quadruped - -namespace CanConnector +void CalPhaseStraight(ToePosi now_base, ToePosi (&target)[4], float ave_v_m_s) { - class MastarConnector - { - public: - MastarConnector(PinName rd, PinName td); - CAN can_; - void Connect(); - - }; - MastarConnector::MastarConnector(PinName rd, PinName td): can_(rd, td){} - - - class SlaveConnector - { - public: - SlaveConnector(PinName rd, PinName td); - CAN can_; - void Connect(); - - }; - SlaveConnector::SlaveConnector(PinName rd, PinName td): can_(rd, td){} - + //指定速度で進むための周期 = 平均速度*時間/歩幅 * 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); } - -//////////////////////////////////////////////////////////////以上が4脚ロボットに関する記述 -using namespace Quadruped; -Leg leg1(p9, p10); -Leg leg2(p28, p27); -//CAN can1(p30, p29); -Ticker fliper; -#ifdef Mastar -CanConnector::MastarConnector connector(p30,p29); -#elif Slave -CanConnector::SlaveConnector connector(p30,p29); -#endif - -void cb_timer() -{ - static int servo_num = 0; - //出力。片サーボずつ - 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) //両サーボ出力したら - { - //目標角の更新 - Quadruped::CalcWalk(servo_num); - //このマイコンで使う脚だけ計算 - leg1.CalServoRad(TargetX[0], TargetY[0] + OffsetY); - leg2.CalServoRad(TargetX[1], TargetY[1] + OffsetY); - - servo_num = 0; - }*/ - -} -int main() -{ - 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); -}