File content as of revision 16:b2358fd35999:

#include "servo.h"
#include <mbed.h>
#include "debug.h"//DEBUG("",変数);でデバッグ。
#include "pinnames.h"
#include "coordinate.h"
#include "KondoServo.h"

KondoServo servo(pin_serial_servo_tx, pin_serial_servo_rx);//サーボID:0,1,2,3,4
const double kOriginTheta[] = {
    163.9575,
    236.79,
    151.1325,
    205.875,
};//初期状態の角度


const int kServoSign[] = {-1, -1, -1, 1};//サーボの正負と座標系の正負の補正
const int kServoSpan_ms = 3; //指示の前後に必要なwait

void Open();
void Move(double x, double y, double theta);
void ServoRad(int i, double rad){
    servo.set_degree(i, kServoSign[i] * rad *kRadToDegree + kOriginTheta[i]);
}
void ServoMoveOnArm()
{
    for(int i = 0; i < kServoNum; i++) {
        double rad = GetNextRadRelative(i);
        ServoRad(i, rad);
        SetNowRadRelative(i,rad);
        wait_ms(kServoSpan_ms);
    }

}

int HandSet(WorkState &params)
{
    switch(params.areaname) {
        case COMMONAREA:
        case WORKAREA:
            Open();
            DEBUG("open\r\n");
            break;
        case BOX:
            Close();
            DEBUG("close\r\n");
            break;
    }
    return 1;
}

void Open()
{
    servo.set_degree(4,203.61375);
    DEBUG("open real\r\n");
    //wait(1);
}
void Close()
{
    servo.set_degree(4,124.63875);
    DEBUG("close real\r\n");
    //wait(1);
}