File content as of revision 3:05b1dcb3634e:

#include "servo.h"
#include <mbed.h>
#include "debug.h"//DEBUG("",変数);でデバッグ。
#include "pinnames.h"
#include "KondoServo.h"
double L = 500; //関節の長さ
double l = 50;  //ハンドまでの長さ

KondoServo servo(pin_serial_servo_tx, pin_serial_servo_rx);//サーボID:0,1,2,3,4
const double kRadToDegree = 180/3.14159265359;
const double origin[3] = {180,180,180}; //初期状態の角度
const int kServoSpan_ms = 5; //指示の前後に必要なwait

void Open();
void Move(double x, double y, double theta);

void ServoMoveOnArm(int id, double theta)
{
    servo.set_degree(id, (-1)*theta+origin[i]);
}

void Move(double x, double y, double theta)
{
    x = x - kArmLength[2] * cos(theta);
    y = y - kArmLength[2] * sin(theta);
    double theta_2 = acos((pow(x,2)+pow(y,2)-2*pow(L,2))/(2*pow(L,2)));
    double theta_1 = -acos((L+L*cos(theta_2))/sqrt(pow(x,2)+pow(y,2)))+atan(y/x);
    double theta_3 = theta * kRadToDegree+180 - (theta_1 + theta_2);
    double angle[3] = {theta_1,theta_2,theta_3};
    for(int i = 0; i < 3; i++){
        ServoMoveOnArm(i, angle[i]);
        wait_ms(kServoSpan_ms);
    }
}

int HandSet(WorkState &params)
{
    switch(params.areaname) {
        case COMMONAREA:
        case WORKAREA:
            Open();
            break;
        case BOX:
            Open();
            break;
    }
    return 1;
}

void Open()
{
    servo.set_degree(4,182.7225);
}

void Close()
{
    servo.set_degree(4,135.43875);
}

void ArmHorizon()
{
    servo.set_degree(3,172.125);
    SetArmState(HORIZON);
}
void ArmVertical()
{
    servo.set_degree(3,262.125);
}

void ArmVerticalForward()
{
    ArmVertical();
    SetArmState(VERTICAL_FORWARD);
}
void ArmVerticalBack()
{
    ArmVertical();
    SetArmState(VERTICAL_BACK);
}