File content as of revision 21:61971fc18b90:

//NHK2019MR2 馬型機構プログラム.
#include "mbed.h"
#include "pinnames.h"
#include "KondoServo.h"
//#define DEBUG_ON//デバッグ用。使わないときはコメントアウト
#include "debug.h"
#include "pi.h"
#include "can.h"
#include "OneLeg.h" ///足先の座標を保存するクラス。x,yやサーボの角度の保存、サーボの駆動も行う。他の足を考慮した処理は別のクラスに任せる。
#include "Walk.h"   //歩き方に関するファイル
#define USE_CAN     //can通信するならdefine.しないなら切らないとエラー出る

////////////調整すべきパラメータ.全てここに集めた。
const float kCycleTime_s = 0.03;              //計算周期
const float kBetweenServoHalf_m = 0.03 * 0.5; //サーボ間の距離の半分
float kLegLength1[2] = {0.1, 0.1};
float kLegLength2[2] = {0.2452 + 0.0025, 0.22529 + 0.0025};
//サーボの設定
const int kServoSpan_ms = 10; //サーボの送信間隔
const double kServoValToDegree = 270.0 / (11500 - 3500);
//サーボの正負と座標系の正負の補正.足で一セット。
const int kServoSign[2][2] = {{
                                  1,
                                  -1,
                              },
                              {
                                  -1,
                                  1,
                              }};
//欲しい座標系0度でのサーボのICSマネージャーの値
const double kOriginDegree[2][2] = {
    {
        (7257 - 3500) * kServoValToDegree,
        (6590 - 3500) * kServoValToDegree + 180,
    },
    {
        (6470 - 3500) * kServoValToDegree,
        (7419 - 3500) * kServoValToDegree - 180, //180度の時6657シータ負の方向に動かすと値は減るので、0度の位置は-180すれば出る
    },
};
///////////////
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[4] = {DigitalOut(LED1), DigitalOut(LED2), DigitalOut(LED3), DigitalOut(LED4)};

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 IsArrived(int count, int finish)
{
    if (count > finish)
        return 1;
    else
        return 0;
}
int main()
{
    printf("When you push any key, this robot starts.\r\n");
    while (pc.readable() == 0) //キーボード押したらスタート
        ;
    printf("stand up for 1 s\r\n");

    //stand時の足の軌道設定.
    Orbit stand_orbit[4];
    float stand_ground_m = 0.2;
    stand_orbit[0].SetStandParam(stand_ground_m);
    for (int i = 0; i < 4; i++) //全部足を同じにしてる
        stand_orbit[i] = stand_orbit[0];
    //4足の位相ずれOffsetTime_sをまとめる
    float stand_offset_time_s[4] = {
        0,
        stand_orbit[0].GetOneWalkTime() * 0.5,
        stand_orbit[0].GetOneWalkTime() * 0,
        stand_orbit[0].GetOneWalkTime() * 0.5,
    };
    //4つの足のorbit, 位相を代入してstraightという歩行パターンを作成している
    //このインスタンスはlegそれぞれの計算を行う役割を担う
    //orbitはここでしか使わないあくまでパラメータ設定用クラス
    Walk stand(stand_orbit, stand_offset_time_s, kCycleTime_s);
    float dist_m = 10; //収束判定用。現在は回すループの数
    //動作開始
    Move(stand, leg, dist_m);
    wait(1);

    DEBUG("move start\r\n");
    //取り敢えず歩行したパラメータ
    //stridetime_s = 0.5, risetime_s = 0.3,stride_m= 0.12f, height_m = 0.03f, ellipse_center_x_m  = 0, ellipse_center_y_m = 0.16f;
    //曲げてみる
    float stridetime_s = 2, risetime_s = 0.5,
          stride_r_m = 0.1, stride_l_m = 0.05, height_m = 0.03, ground_m = stand_ground_m,
          ellipse_center_x_m = -0.05, ellipse_center_y_m = ground_m;
    //軌道の作成
    //代入用の軌道SetStraightParam関数を用いると真っ直ぐ進む前提となる
    Orbit orbit[4];
    orbit[0].SetStraightParam(stridetime_s, risetime_s, stride_r_m, height_m, ground_m, ellipse_center_x_m, ellipse_center_y_m);
    orbit[1].SetStraightParam(stridetime_s, risetime_s, stride_r_m, height_m, ground_m, 0, ellipse_center_y_m);
    orbit[2].SetStraightParam(stridetime_s, risetime_s, stride_l_m, height_m, ground_m, ellipse_center_x_m, ellipse_center_y_m);
    orbit[3].SetStraightParam(stridetime_s, risetime_s, stride_l_m, height_m, ground_m, 0, ellipse_center_y_m);
    //4足の軌道と位相ずれOffsetTime_sをまとめる
    float offset_time_s[4] = {
        0,
        orbit[0].GetOneWalkTime() * 0.5,
        orbit[0].GetOneWalkTime() * 0.5,
        orbit[0].GetOneWalkTime() * 0,
    };
    //4つの足のorbit, 位相を代入して歩行パターンを作成している
    //このインスタンスはlegそれぞれの計算を行う役割を担う
    //orbitはここでしか使わないあくまで鍵のような扱い
    Walk walk(orbit, offset_time_s, kCycleTime_s);
    dist_m = 500000; //収束判定用。取り敢えず今はループ数。
    //動く
    Move(walk, leg, dist_m);
    DEBUG("program end\r\n");
}

//到達判定が来ない間,同じ歩行方法でループ.関数に入る前の足の位置を一切考慮しない点に注意
void Move(Walk walkway, OneLeg (&leg)[4], float dist_m)
{
    timer.reset();
    timer.start();
    int is_arrived = 0;
    int count = 0;
    while (is_arrived == 0)
    {
        float time_s = timer.read();
        //注:未実装。到着したかの判定.LRFからのデータが必要.今は取り敢えずループ回数で判断
        is_arrived = IsArrived(count, (int)dist_m);
        ++count;
        //4本の足それぞれの足先サーボ角度更新
        walkway.Cal4LegsPosi(leg);
#ifdef USE_CAN
        //slave_mbed分の足の目標位置を送信
        SendRad(leg[2], leg[3]);
#endif
        //自身が動かす足のサーボを動かす
        MoveServo(leg[0], 0, 0);
        MoveServo(leg[1], 1, 0);
        wait_ms(kServoSpan_ms);
        MoveServo(leg[0], 0, 1);
        MoveServo(leg[1], 1, 1);
        DEBUG("%f, %f, %f, %f\r\n", leg[2].GetRad(0), leg[2].GetRad(1), leg[3].GetRad(0), leg[3].GetRad(1));
        //計算周期が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
        { //計算周期が達成できないときはDEBUGで知らせるだけ。動きはする。
            DEBUG("error: rest_time_s = %f in Move()\r\n", rest_time_s);
            led[0] = 1;
        }
    }
}
void MoveServo(OneLeg leg, int serial_num, int servo_id)
{
    float degree = leg.GetRad(servo_id) * kRadToDegree;
    //サーボの座標系に変更
    float servo_degree = kServoSign[serial_num][servo_id] * degree + kOriginDegree[serial_num][servo_id];
    DEBUG("servo_degree[%d][%d],%f\r\n", serial_num, servo_id, servo_degree);
    servo[serial_num].set_degree(servo_id, servo_degree);
}