File content as of revision 11:e81425872740:

//NHK2019MR2 馬型機構プログラム.
#include "mbed.h"
#include "pinnames.h"
#include "KondoServo.h"
#include "OneLeg.h" ///足先の座標を保存するクラス。x,yやサーボの角度の保存、サーボの駆動も行う。他の足を考慮した処理は別のクラスに任せる。
#include "Walk.h"   //歩き方に関するファイル

////////////調整すべきパラメータ.全てここに集めた。
const float kCycleTime_s = 0.01f;              //計算周期
const float kBetweenServoHalf_m = 0.06f * 0.5; //サーボ間の距離の半分
const float kLegLength1 = 0.1f;
const float kLegLength2 = 0.2f;
const float kServoSpan_ms = 5; //サーボの送信間隔
///////////////
Timer timer;
KondoServo servo[2] = {
    KondoServo(pin_serial_servo_tx[0], pin_serial_servo_rx[0]),
    KondoServo(pin_serial_servo_tx[1], pin_serial_servo_rx[1]),
};
OneLeg leg[4] = {
    OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2),
    OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2),
    OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2),
    OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2),
};
DigitalOut led(LED1);
Serial pc(USBTX, USBRX);
const float M_PI = 3.141592;
const float kRadToDegree = 180.0 / M_PI;
void Move(Walk WalkWay, OneLeg (&leg)[4], float dist_m);
void MoveServo(OneLeg leg, int legnum, int servo_id);
int main()
{
    printf("When you push any key, this robot starts.\r\n");
    while (pc.readable() == 0) //キーボード押したらスタート
        ;
    printf("move start\r\n");
    //各足の軌道の設定.今回は全部同じにすることで直線を描く。
    float stridetime_s = 1, risetime_s = 0.3, stride_m = 0.2f, height_m = 0.03f, ground_m = 0.14f;
    Orbit orbit[4] = {
        Orbit(stridetime_s, risetime_s, stride_m, height_m, ground_m),
        Orbit(stridetime_s, risetime_s, stride_m, height_m, ground_m),
        Orbit(stridetime_s, risetime_s, stride_m, height_m, ground_m),
        Orbit(stridetime_s, risetime_s, stride_m, height_m, ground_m),
    };
    //4足の軌道と位相ずれOffsetTime_sをまとめる
    float offset_time_s[4] = {
        0,
        orbit[0].GetOneWalkTime() * 0.5,
        0,
        orbit[0].GetOneWalkTime() * 0.5,
    };
    Walk straight(orbit, offset_time_s, kCycleTime_s);
    //実際にWalkの指示通りに動かす
    float dist_m = 0.5f;
    Move(straight, leg, dist_m);

    printf("program end\r\n");
}

void Move(Walk WalkWay, OneLeg (&leg)[4], float dist_m)
{
    timer.reset();
    timer.start();
    int is_arrived = 0;
    while (is_arrived == 0)
    {
        float time_s = timer.read();
        //注:未実装。到着したかの判定.LRFからのデータが必要？
        // is_arrived = IsArrived();

        //4本の足それぞれの足先サーボ角度更新
        WalkWay.Cal4LegsPosi(leg);
        //注:未実装。slave_mbed分の足の目標位置を送信

        //自身が動かす足のサーボを動かす
        MoveServo(leg[0], 0, 0);
        MoveServo(leg[1], 1, 0);
        wait_ms(kServoSpan_ms);
        MoveServo(leg[0], 0, 1);
        MoveServo(leg[1], 1, 1);
        printf("%f,%f\r\n", leg[0].GetRad(0), leg[1].GetRad(0));
        //計算周期がWalkWay.cycletime_s_になるようwait
        float rest_time_s = WalkWay.cycletime_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 MoveServo(OneLeg leg, int legnum, int servo_id)
{
    float degree = leg.GetRad(servo_id) * kRadToDegree;
    //servo1は反転させる
    if (servo_id == 0)
        degree += 90;
    else
        degree = 270 - degree;
    servo[legnum].set_degree(servo_id, degree);
}