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);
}