File content as of revision 3:bcae0bb64b81:

//mbed間の同期を、master-slaveで行いたい。

#include "mbed.h"
#include "KondoServo.h"
#define Mastar;
//#define Slave;
////////////////Legクラス。下にmain文がある。
const float Pi = 3.141592;
const float kRadToDegree = 180.0 / Pi;

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

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



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

const float tickerTime = 0.006f;    //Ticker間隔
const float omega = 3.1415f * 2.5f; //角速度
const float offsetDegree = 0.0f;
const float offsetY = 0.15f;

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