File content as of revision 2:a92568bdeb5c:

#include "mbed.h"
#include "KondoServo.h"
////////////////Legクラス。下にmain文がある。
const float Pi = 3.141592;
const float kRadToDegree = 180.0 / Pi;
//const float kDegreeToRad = Pi / 180.0;
const float LegLength1 = 0.1f;
const float LegLength2 = 0.2f;

class Leg
{
    float rad_[2];
    int id_[2];
    //パラメータ。実際の機体に合わせていじる
    static const float dist_between_servo_half_m_ = 0.06f * 0.5;
    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];
}

//////////////////////////////////////////////////////////////Legクラスの定義終了

Leg leg(p9, p10);
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.5f * LegLength2 * cos(theta);  //0.5
        tarY = 0.15f * LegLength2 * sin(theta); //0.15
    }
    else
    {
        tarX = 0.5f * LegLength2 * cos(theta);
        tarY = 0.0f * LegLength2 * 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;
    //出力。片サーボずつ
    leg.MoveServo(servo_num);
    ++servo_num;
    if (servo_num > 1) //両サーボ出力したら
    {
        //目標角の更新
        F_t(time_s, target_x_m, target_y_m);
        leg.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);
    leg.CalServoRad(target_x_m, target_y_m + offsetY);
    fliper.attach(&cb_timer, tickerTime);
}