File content as of revision 44:d02b2165c4ed:

#include "servo.h"
#include <mbed.h>
#include "debug.h"//DEBUG("",変数);でデバッグ。
#include "pinnames.h"
#include "coordinate.h"
#include "KondoServo.h"
#include "servo.h"
const double kServoValToDegree =  270.0 / (11500 - 3500);
//////////////////////////////変更できるパラメータ
const int kServoSign[] = {-1, -1, -1, -1};//サーボの正負と座標系の正負の補正
const double kOriginDegree[SERVONUM - 1] = {
#ifdef RIGHT
    (8850 - 3500)  * kServoValToDegree ,
    (8635 - 3500)  * kServoValToDegree,
    (7533 - 3500) * kServoValToDegree,
    (8000 - 3500) * kServoValToDegree,
#elif defined LEFT
    (7550 - 3500)  * kServoValToDegree ,
    (8635 - 3500)  * kServoValToDegree,
    (7462 - 3500) * kServoValToDegree,
    (8000 - 3500) * kServoValToDegree,
#endif
};//初期状態の角度

const int kServoSpan_ms = 5; //指示の前後に必要なwait
#ifdef RIGHT
const double kOpenVal = (7500 - 3500) * kServoValToDegree;//ハンドを開いたときのサーボへの指示
const double kCloseVal = (5000 - 3500) * kServoValToDegree;//ハンドを閉じたときのサーボへの指示
#elif defined LEFT
const double kOpenVal = (5400 - 3500) * kServoValToDegree;//ハンドを開いたときのサーボへの指示
const double kCloseVal = (7600 - 3500) * kServoValToDegree;//ハンドを閉じたときのサーボへの指示
#endif
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] = {};
double OriginDegree[SERVONUM - 1] = {kOriginDegree[0], kOriginDegree[1], kOriginDegree[2], kOriginDegree[3]};
void ServoRad(int id, double rad_relative)
{
    double degree = kServoSign[id] * rad_relative * kRadToDegree + OriginDegree[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 < 4; i++) {
        ServoMoveOnArmInTurns();
        wait_ms(kServoSpan_ms);
    }
}
void Open()
{
    servo[0].set_degree(kTipServoID, kOpenVal);
}
void Close()
{
        servo[0].set_degree(kTipServoID, kCloseVal);
}