File content as of revision 7:72c80a7c20d6:

//mbed間の同期を、master-slaveで行いたい。
//masetrで4つの脚の動きをすべて決定,
//その動きをslaveに送る。

#include "mbed.h"
#include "KondoServo.h"
#define Mastar;
//#define Slave;
#define DebugMode;

//各値
namespace Parameters
{
    //パラメータ
const float Pi = 3.14;
const float kRadToDegree = 180.0 / Pi;
const float tickerTime = 0.02f;    //Ticker間隔
float OffsetY = 0.15f;
float Stride_time_s = 1.0f*10.0f;//床についている時間 1.0f
float Stride_length_m = 0.05f;//歩幅
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;
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

namespace CanConnector
{
//Maserとslaveの定義　同じ関数名で動くようにしたい
class MastarConnector;//cansendするマイコンの場合
class SlaveConnector;//canreadするマイコンの場合
}

//実装
namespace Quadruped
{
//脚の定義
class Leg
{
    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);
};

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();
void TurnLeft();
void TurnRight();
void Climb();
void Overcoming(); //段差、紐乗り越え動作
} // namespace WalkingPattern

Leg::Leg(PinName pin_serial_tx, PinName pin_serial_rx) : servo_(pin_serial_tx, pin_serial_rx)
{
    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];
}


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)
{
    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;//共通時間
    for(int i=0;i<4;++i)//4脚それぞれのtargetPoseを設定する
    {
        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);
    }
    #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;
    
}
void WalkingPattern::TurnLeft()
{
    //static float time_s = 0.0f;

}
void WalkingPattern::TurnRight()
{
    //static float time_s = 0.0f;

}
void WalkingPattern::Climb()
{
    //static float time_s = 0.0f;
    
}
void WalkingPattern::Overcoming()
{
    //static float time_s = 0.0f;
    
}

void CalcWalk()
{
    switch(WalkMode)
    {
        case straight:
            WalkingPattern::Straight();
        break;
        case turnleft:
            WalkingPattern::TurnLeft();
        break;
        case turnright:
            WalkingPattern::TurnRight(); 
        break;
        case climb:
            WalkingPattern::Climb();
        break;
        case overcoming:
            WalkingPattern::Overcoming();
        break;
    }
}
} // namespace Quadruped

namespace CanConnector
{
    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){}

}

//////////////////////////////////////////////////////////////以上が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;
    static float time_s = 0.0f;
    //出力。片サーボずつ
    leg1.MoveServo(servo_num);
    leg2.MoveServo(servo_num);
    ++servo_num;
    
    if (servo_num > 1) //両サーボ出力したら
    {
        //目標角の更新
        Quadruped::CalcWalk();
        //このマイコンで使う脚だけ計算
        leg1.CalServoRad(TargetX[0], TargetY[0] + OffsetY);
        leg2.CalServoRad(TargetX[1], TargetY[1] + OffsetY);
        
        servo_num = 0;
    }
    time_s += tickerTime;
    
}
int main()
{
    Quadruped::CalcWalk();
    leg1.CalServoRad(TargetX[0], TargetY[0] + OffsetY);
    leg2.CalServoRad(TargetX[1], TargetY[1] + OffsetY);
    fliper.attach(&cb_timer, tickerTime);
}