File content as of revision 0:f000d896d188:

#include "mbed.h"
#include "KondoServo.h"

KondoServo servo(p9,p10);
Ticker fliper;

const float LegLength1 = 0.1f;
const float LegLength2 = 0.2f;
const float dist = 0.03f;
const float omega = 3.1415f * 2.5f;//角速度
const float tickerTime = 0.006f;//Ticker間隔
const float offsetDegree = 0.0f;
const float offsetY = 0.15f;

float TargetTheta1 = 0.00f;
float TargetTheta2 = 3.14f;

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 PoseToAngles(float targetX, float targetY)
{
    float alpha = atan2(targetY , targetX - dist );
    float beta = atan2(targetY , targetX + dist );
    //printf("alpha %.3f  beta %.3f\r\n",alpha,beta);
    float r1 = sqrt((targetX - dist) * (targetX - dist) + targetY * targetY);
    float r2 = sqrt((targetX + dist) * (targetX + dist) + targetY * targetY);
    //printf("r1 %.3f  r2 %.3f\r\n",r1,r2);
    float X1 = (LegLength1 * LegLength1 - LegLength2 * LegLength2 + r1 * r1) / (2.0f * r1 * LegLength1);
    float X2 = (LegLength1 * LegLength1 - LegLength2 * LegLength2 + r2 * r2) / (2.0f * r2 * LegLength1);
    //printf("X1 %.3f  X2 %.3f\r\n",X1,X2);
    float theta1pre1 = atan2(sqrt(1.0f - X1 * X1), X1);
    float theta1pre2 = atan2(-sqrt(1.0f - X1 * X1), X1);
    float theta2pre1 = atan2(sqrt(1.0f - X2 * X2), X2);
    float theta2pre2 = atan2(-sqrt(1.0f - X2 * X2), X2);

    if (theta1pre1 >= 0 && theta1pre2 <= 0)
    {
        TargetTheta1 = alpha - theta1pre1;
    }
    else if(theta1pre1 <=0 && theta1pre2 >= 0)
    {
        TargetTheta1 = alpha - theta1pre2;
    }
    if (theta2pre1 >= 0 && theta2pre2 <= 0)
    {
        TargetTheta2 = beta - theta2pre2;
    }
    else if (theta2pre1 <= 0 && theta2pre2 >= 0)
    {
        TargetTheta2 = beta - theta2pre1;
    }
}

double Theta1;
double Theta2;
int flagMotor;

void MoveLeg(float theta1, float theta2)
{
     Theta1 = ((double)theta1*180.0/3.14) + 90.0;
     Theta2 = 270.0 - (double)theta2*180.0/3.14;
     
     /*printf("target theta1:%.3f  ",Theta1);
     printf("target theta2:%.3f  \r\n",Theta2);*/
}

void InputPose(float X, float Y)
{
    PoseToAngles(X,Y);
    MoveLeg(TargetTheta1,TargetTheta2);
}

float t = 0.0f;

void cb_timer()
{
    float tX,tY;
    if(flagMotor == 0)
    {
        servo.set_degree(1, Theta1);
        F_t(t, tX, tY);
        InputPose(tX,tY+offsetY);
        t = t+ 2.0f*tickerTime;
        flagMotor = 1;
    }
    else if(flagMotor == 1)
    {
        servo.set_degree(0, Theta2);
        flagMotor = 0;
    }
}

int main() 
{
    fliper.attach(&cb_timer, tickerTime);
}