File content as of revision 5:556d5a5e9d24:

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

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

//定義　
namespace Quadruped
{
const float Pi = 3.141592;
const float kRadToDegree = 180.0 / Pi;
const float tickerTime = 0.006f;    //Ticker間隔

const float omega = 3.1415f * 2.5f; //角速度
const float offsetDegree = 0.0f;
const float offsetY = 0.15f;

float Stride_time_s = 1.0f;//床についている時間
float Stride_length_m = 0.1f;//歩幅
void SetStrideMotion(float stride_time_s, float stride_length_m);
float Rising_time_s = 0.2f;//脚を上げている時間
float Width_m = 0.03f;//脚を上げる高さ
void SetRisiongMotion(float rising_time_s, float width_m);
//ROSから送られてきたLRFのデータをもとに現在すべき動作を定義する。
int WalkMode = 0;
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;
void SetWalkMode(int 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
{
//class Mastar;
//class Slave;
}

//実装
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,float stride_length_m,float &targetX, float &targetY); //引数に歩幅
void Rising(float time_s,float width_m,float &targetX, float &targetY); //引数に上げる高さ
} // 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,float stride_length_m,float &targetX, float &targetY)
{
    
}

void Motion::Rising(float time_s,float width_m,float &targetX, float &targetY)
{
    
}

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)
    {
        float thisLegTime = time_s + offsetTime[i];
        if(thisLegTime <= Stride_time_s) Motion::Stride(thisLegTime, Stride_length_m, targetX[i],targetY[i]);
        else if(thisLegTime <= walkPeriod) Motion::Rising(thisLegTime-Stride_time_s, Width_m,targetX[i],targetY[i]);
        else Motion::Stride(thisLegTime-walkPeriod, Stride_length_m,targetX[i],targetY[i]);
    }
    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

//////////////////////////////////////////////////////////////以上が4脚ロボットに関する記述
using namespace Quadruped;
Leg leg1(p9, p10);
Leg leg2(p28, p27);
CAN can1(p30, p29);
Ticker fliper;



void F_t(float t, float &tarX, float &tarY)
{
    float theta = -omega * t + offsetDegree;
    if (sin(theta) <= 0.0f)
    {
        tarX = 0.1f * cos(theta);  //0.1
        tarY = 0.03f * sin(theta); //0.03
    }
    else
    {
        tarX = 0.1f * cos(theta);
        tarY = 0.0f * sin(theta);
    }
}
void cb_timer()
{
    static int servo_num = 0;
    static float time_s = 0.0f;
    float target_x_m = 0;
    float target_y_m = 0;
    //出力。片サーボずつ
    leg1.MoveServo(servo_num);
    leg2.MoveServo(servo_num);
    ++servo_num;
    
    if (servo_num > 1) //両サーボ出力したら
    {
        //目標角の更新
        F_t(time_s, target_x_m, target_y_m);
        leg1.CalServoRad(target_x_m, target_y_m + offsetY);
        leg2.CalServoRad(target_x_m, target_y_m + offsetY);
        servo_num = 0;
    }
    time_s += tickerTime;
    
}
int main()
{
    float target_x_m = 0;
    float target_y_m = 0;
    F_t(0, target_x_m, target_y_m);
    leg1.CalServoRad(target_x_m, target_y_m + offsetY);
    leg2.CalServoRad(target_x_m, target_y_m + offsetY);
    fliper.attach(&cb_timer, tickerTime);
}