File content as of revision 25:55f7409c3502:

#include "servo.h"
#include <mbed.h>
#include "debug.h"//DEBUG("",変数);でデバッグ。
#include "pinnames.h"
#include "coordinate.h"
#include "KondoServo.h"
const double kServoValToDegree =  270.0 / (11500 - 3500);
//////////////////////////////変更できるパラメータ
const int kServoSign[] = {-1, -1, -1, -1};//サーボの正負と座標系の正負の補正
const double kOriginDegree[] = {
    (8850 - 3500)  * kServoValToDegree,
    (11400 - 3500)  * kServoValToDegree - 90,
    (7500 - 3500) * kServoValToDegree,
    (8000 - 3500) * kServoValToDegree,
};//初期状態の角度
const int kServoSpan_ms = 3; //指示の前後に必要なwait
const double kOpenVal = (7400 - 3500) * kServoValToDegree;//ハンドを開いたときのサーボへの指示
const double kCloseVal = (5300 - 3500) * kServoValToDegree;//ハンドを閉じたときのサーボへの指示
const int kTipServoID = 4;
//////////////////////////////
KondoServo servo[] = {KondoServo(pin_serial_servo_tx[0], pin_serial_servo_rx[0]),//サーボID:2,3,4
                      KondoServo(pin_serial_servo_tx[1], pin_serial_servo_rx[1]),//サーボID:0,
                      KondoServo(pin_serial_servo_tx[2], pin_serial_servo_rx[2]),//サーボID:1,
                     };
double old_rad[SERVONUM] = {};

void ServoRad(int id, double rad_relative)
{
    double degree = kServoSign[id] * rad_relative * kRadToDegree + kOriginDegree[id];
    switch(id) {
        case 0:
            servo[1].set_degree(id, degree);
            break;
        case 1:
            servo[2].set_degree(id, degree);
            break;
        case 2:
        case 3:
        case 4:
            servo[0].set_degree(id, degree);
            break;
    }
    SetNowRadRelative(id, old_rad[id]);//現在地変更はワンテンポ遅らせる(サーボの応答が遅いため)
    old_rad[id] = rad_relative;
}
//一般性は皆無
void ServoMoveOnArmInTurns()
{
    static int count = 0;
    if(count == 0) {
        for(int i = 0; i < 3; i++) {
            double rad = GetNextRadRelative(i);
            ServoRad(i, rad);
        }
    } else if(count == 2) {
        double rad = GetNextRadRelative(3);
        ServoRad(3, rad);
    }
    ++count;
    if(count == 3) count = 0;
}
//一般性は皆無
void ServoMoveOnArm()
{
    for(int i = 0; i < 2; i++) {
        ServoMoveOnArmInTurns();
        wait_ms(kServoSpan_ms);
    }
}
void Open()
{
    servo[0].set_degree(kTipServoID, kOpenVal);
    wait_ms(kServoSpan_ms);
}
void Close()
{
    servo[0].set_degree(kTipServoID, kCloseVal);
    wait_ms(kServoSpan_ms);
}