File content as of revision 21:af9a33d69745:

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

//////////////////////////////変更できるパラメータ
const int kServoSign[] = {-1, -1, -1, 1};//サーボの正負と座標系の正負の補正
const double kOriginTheta[] = {
    180,
    0,
    135,
    205.875,
};//初期状態の角度
const int kServoSpan_ms = 3; //指示の前後に必要なwait
const double kOpenVal = 203.61375;//ハンドを開いたときのサーボへの指示
const double kCloseVal = 136;//ハンドを閉じたときのサーボへの指示
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,
                     };
void ServoRad(int id, double rad_relative)
{
    double degree = kServoSign[id] * rad_relative * kRadToDegree + kOriginTheta[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, 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 == 1) {
        double rad = GetNextRadRelative(3);
        ServoRad(3, rad);
    }
    ++count;
    if(count == 2) count = 0;
}
//一般性は皆無
void ServoMoveOnArm()
{
    for(int i = 0; i < 2; i++) {
        ServoMoveOnArmInTurns();
        wait_ms(kServoSpan_ms);
    }
}
void Open()
{
    servo[2].set_degree(kTipServoID, kOpenVal);
}
void Close()
{
    servo[2].set_degree(kTipServoID, kCloseVal);
}