File content as of revision 4:8289dcb78a23:

#include "mbed.h"
#include "RS405CB.h"

RS405CB::RS405CB(PinName tx,PinName rx,PinName permit):Serial(tx,rx),_permit(permit)
{
    baud(115200);
    format(8,Serial::None,1);
}

/* -----------------------------------------------------------------------------
Arguments:
    ID   ... トルクを設定したいサーボのID
    data ... トルクの状態の設定
             0x00 OFF
             0x01 ON
             0x02 BREKE
Description:
    トルク設定。入力されたIDのサーボモータのトルクを制御する
----------------------------------------------------------------------------- */
void RS405CB::Torque(unsigned char ID, unsigned char data)
{
    unsigned char TxData[9]= {0xFA, 0xAF, ID, 0x00, 0x24, 0x01, 0x01, data, 0};
    unsigned char CheckSum = 0;
    for(int i = 2; i <= 7; i++) CheckSum = CheckSum ^ TxData[i];
    TxData[8] = CheckSum;
    _permit = 1;
    for (int i=0; i<=8; i++) putc((char)TxData[i]);
    wait_us(750);
    _permit = 0;
}

/* -----------------------------------------------------------------------------
Arguments:
    ID   ... トルクをオンに設定したいサーボのID
    
Description:
    トルクをオンにする
----------------------------------------------------------------------------- */
void RS405CB::TorqueOn(unsigned char ID)
{
    unsigned char TxData[9]= {0xFA, 0xAF, ID, 0x00, 0x24, 0x01, 0x01, 0x01, 0};
    unsigned char CheckSum = 0;
    for(int i = 2; i <= 7; i++) CheckSum = CheckSum ^ TxData[i];
    TxData[8] = CheckSum;
    _permit = 1;
    for (int i=0; i<=8; i++) putc(TxData[i]);
    wait_us(750);
    _permit = 0;
}

/* -----------------------------------------------------------------------------
Arguments:
    ID   ... トルクをオンに設定したいサーボのIDの配列
    
Description:
    トルクをオンにする
----------------------------------------------------------------------------- */
void RS405CB::TorqueOn(unsigned char* ID)
{
    const unsigned char size = sizeof(ID);
    unsigned char TxData[8 + 2*size];
    TxData[0] = 0xFA; //header
    TxData[1] = 0xAF; //header
    TxData[2] = 0x00; // ID -> 0x00
    TxData[3] = 0x00; // Flags -> 0x00
    TxData[4] = 0x24; // Addr -> Torque(0x24)
    TxData[5] = 0x02; // Length -> ID(1Byte) + Torque(1Byte)
    TxData[6] =  (unsigned char)size;  // Count -> Number of Servos
    
    for(int i=0; i<size; i++)
    {
        TxData[7 + 2*i] = ID[i];
        TxData[8 + 2*i] = 0x01;
    }
    unsigned char CheckSum = 0;
    for(int i=2; i<=6+2*size; i++)
        CheckSum = CheckSum ^ TxData[i];
    TxData[7+2*size] = CheckSum;
    _permit = 1;
    for (int i=0; i<=7+2*size; i++) putc(TxData[i]);
    wait_us(750);
    _permit = 0;
}

/* -----------------------------------------------------------------------------
Arguments:
    ID   ... 動かしたいサーボのID
    data ... 目標角．degreeの10倍の値を指定する．（ex 90° -> data = 900）

Description:
    
----------------------------------------------------------------------------- */
void RS405CB::GoalPosition(unsigned char ID, int data)
{
    unsigned char TxData[10];
    unsigned char CheckSum = 0;

    TxData[0] = 0xFA;
    TxData[1] = 0xAF;
    TxData[2] = ID;
    TxData[3] = 0x00;
    TxData[4] = 0x1E;
    TxData[5] = 0x02;
    TxData[6] = 0x01;
    TxData[7] = (unsigned char)0x00FF & data;
    TxData[8] = (unsigned char)0x00FF & (data >> 8);

    for(int i=2; i<=8; i++)
        CheckSum = CheckSum ^ TxData[i];
        
    TxData[9] = CheckSum;

    _permit = 1;
    for(int i=0; i<=9; i++) {
        putc(TxData[i]);
    }
    wait_us(810);
    _permit = 0;
}

/* -----------------------------------------------------------------------------
Arguments:
    ID   ... 動かしたいサーボのIDの配列
    data ... 目標角の配列．degreeの10倍の値を指定する．（ex 90° -> data = 900）
    speed... 移動時間の配列．10ms単位で指定(ex. 5000ms -> 500)

Description:
    GoalPositionのロングパケットバージョン
    
----------------------------------------------------------------------------- */
void RS405CB::GoalPosition(unsigned char* ID, int* data)
{
    const unsigned char size = sizeof(ID);
    unsigned char TxData[8 + 3 * size];
    unsigned char CheckSum = 0;

    TxData[0] = 0xFA; //header
    TxData[1] = 0xAF; //header
    TxData[2] = 0x00; // ID -> 0x00
    TxData[3] = 0x00; // Flags -> 0x00
    TxData[4] = 0x1E; // Addr -> GoalPosition(0x1E)
    TxData[5] = 0x03; // Length -> ID(1Byte) + GoalPosition_L,H(2Byte)
    TxData[6] =  (unsigned char)size;  // Count -> Number of Servos
    
    for(int i=0; i<size; i++)
    {
        TxData[7  + i*3] = ID[i];
        TxData[8  + i*3] = (unsigned char)0x00FF & data[i];
        TxData[9  + i*3] = (unsigned char)0x00FF & (data[i] >> 8);
    }

    for(int i=2; i<=6+3*size; i++)
        CheckSum = CheckSum ^ TxData[i];
        
    TxData[7 + 3*size] = CheckSum;

    _permit = 1;
    
    for(int i=0; i<=7+3*size; i++)
        putc(TxData[i]);
        
    wait_us(810);
    _permit = 0;
}


/* -----------------------------------------------------------------------------
Arguments:
    ID   ... 
    speed... 移動時間の指定．10ms単位で指定(ex. 5000ms -> 500)

Description:
    サーボの移動時間を設定する
----------------------------------------------------------------------------- */
void RS405CB::SetSpeed(unsigned char ID, int speed)
{
    unsigned char TxData[10];
    unsigned char CheckSum = 0;

    TxData[0] = 0xFA;
    TxData[1] = 0xAF;
    TxData[2] = ID;
    TxData[3] = 0x00;
    TxData[4] = 0x1E + 2;
    TxData[5] = 0x02;
    TxData[6] = 0x01;
    TxData[7] = (unsigned char)0x00FF & speed;
    TxData[8] = (unsigned char)0x00FF & (speed >> 8);

    for(int i=2; i<=8; i++)
    {
        CheckSum = CheckSum ^ TxData[i];
    }
    TxData[9] = CheckSum;

    _permit = 1;
    for(int i=0; i<=9; i++) {
        putc(TxData[i]);
    }
    wait_us(810);
    _permit = 0;
}


/* -----------------------------------------------------------------------------
Arguments:
    ID   ... 
    speed... 移動時間の配列．10ms単位で指定(ex. 5000ms -> 500)

Description:
    
    
----------------------------------------------------------------------------- */
void RS405CB::SetSpeed(unsigned char* ID, int* speed);
{
    const unsigned char size = sizeof(ID);
    unsigned char TxData[8 + 3 * size];
    unsigned char CheckSum = 0;

    TxData[0] = 0xFA; //header
    TxData[1] = 0xAF; //header
    TxData[2] = 0x00; // ID -> 0x00
    TxData[3] = 0x00; // Flags -> 0x00
    TxData[4] = 0x1E+2; // Addr -> GoalTime
    TxData[5] = 0x03; // Length -> ID(1Byte) + GoalTime_L,H(2Byte)
    TxData[6] =  (unsigned char)size;  // Count -> Number of Servos
    
    for(int i=0; i<size; i++)
    {
        TxData[7  + i*3] = ID[i];
        TxData[8  + i*3] = (unsigned char)0x00FF & speed[i];
        TxData[9  + i*3] = (unsigned char)0x00FF & (speed[i] >> 8);
    }

    for(int i=2; i<=6+3*size; i++)
        CheckSum = CheckSum ^ TxData[i];
        
    TxData[7 + 3*size] = CheckSum;

    _permit = 1;
    
    for(int i=0; i<=7+3*size; i++)
        putc(TxData[i]);
        
    wait_us(810);
    _permit = 0;
    
}


/* -----------------------------------------------------------------------------
Arguments:
    oldID ... IDを変更したいサーボの現在のID
    newID ... 新しいID．最大は255

Description:
    
----------------------------------------------------------------------------- */
void RS405CB::Change_ID(unsigned char oldID, unsigned char newID){
    unsigned char TxData[9];
    unsigned char CheckSum = 0;

    TxData[0] = 0xFA;
    TxData[1] = 0xAF;
    TxData[2] = oldID;
    TxData[3] = 0x00;
    TxData[4] = 0x04;
    TxData[5] = 0x01;
    TxData[6] = 0x01;
    TxData[7] = newID;
    
    for(int i=2; i<=7; i++)
    {
        CheckSum = CheckSum ^ TxData[i];
    }
    TxData[8] = CheckSum;

    _permit = 1;
    for(int i=0; i<=8; i++) {
        putc(TxData[i]);
    }
    wait_us(810);
    _permit = 0;

    _ROM_Update(newID);
}

/* -----------------------------------------------------------------------------
Arguments:
    ID ... 角度を読みたいサーボのID 

Return:
    get_deg(int) ... サーボの角度

Description:
    
----------------------------------------------------------------------------- */
int RS405CB::Read_Deg(unsigned char ID)
{
    unsigned char CheckSum=0;
    int get_deg = 0;
    unsigned char RxData[8];
    unsigned char TxData[8];
    
    TxData[0] = 0xFA;
    TxData[1] = 0xAF;
    TxData[2] = ID;
    TxData[3] = 0x0F;
    TxData[4] = 0x2A;
    TxData[5] = 0x02;
    TxData[6] = 0x00;

    for(int j=2; j<=6; j++) {
        CheckSum = CheckSum ^ TxData[j];
    }
    TxData[7]=CheckSum;

    _permit = 1;
    for (int j=0; j<=7; j++)
        putc(TxData[j]);
    wait_us(670);
    _permit = 0;

    CheckSum = 0;
    while(getc() != 0xFD);
    while(getc() != 0xDF);

    for(int j=0; j<=6; j++){
        RxData[j] = getc();
        CheckSum = CheckSum ^ RxData[j];
    }
    
    if(CheckSum == getc()){
        get_deg = (int)RxData[6];
        get_deg = get_deg << 8;
        get_deg |= (int)RxData[5];
    }
        
    return _s16(get_deg);
}

/* -----------------------------------------------------------------------------
Arguments:
    ID ... 電流を読みたいサーボのID 

Return:
    get_mA(int) ... サーボに流れている電流量

Description:
    メーカー曰く，参考程度の値なので注意

----------------------------------------------------------------------------- */
int RS405CB::Read_mA(unsigned char ID)
{
    unsigned char CheckSum=0;
    int get_mA = 0;
    unsigned char RxData[8];
    unsigned char TxData[8];
    TxData[0] = 0xFA;
    TxData[1] = 0xAF;
    TxData[2] = ID;
    TxData[3] = 0x0F;
    TxData[4] = 0x30;
    TxData[5] = 0x02;
    TxData[6] = 0x00;

    for(int j=2; j<=6; j++)
    {
        CheckSum = CheckSum ^ TxData[j];
    }
    TxData[7]=CheckSum;

    _permit = 1;
    for (int j=0; j<=7; j++)
        putc(TxData[j]);
    wait_us(670);
    _permit = 0;

    CheckSum = 0;
    while(getc() != 0xFD);
    while(getc() != 0xDF);

    for(int j=0; j<=6; j++){
        RxData[j] = getc();
        CheckSum = CheckSum ^ RxData[j];
    }
    
    if(CheckSum == getc()){
        get_mA = (int)RxData[6];
        get_mA = get_mA << 8;
        get_mA |= (int)RxData[5];
    }
        
    return _s16(get_mA);
}


/* -----------------------------------------------------------------------------
Arguments:
    ID ... ROMをアップデートしたいサーボのID

Description:
    ROM領域の値を変更した時に実行する．

----------------------------------------------------------------------------- */
void RS405CB::_ROM_Update(unsigned char ID){
    unsigned char TxData[8];
    unsigned char CheckSum = 0;

    TxData[0] = 0xFA;
    TxData[1] = 0xAF;
    TxData[2] = ID;
    TxData[3] = 0x40;
    TxData[4] = 0xFF;
    TxData[5] = 0x00;
    TxData[6] = 0x00;
    
    for(int i=2; i<=6; i++)
    {
        CheckSum = CheckSum ^ TxData[i];
    }
    TxData[7] = CheckSum;
    
    _permit = 1;
    for(int i=0; i<=7; i++) {
        putc(TxData[i]);
    }
    wait_us(810);
    _permit = 0;
    wait(1.5);
}

int RS405CB::_s16(int value)
{
    return -(value & 0b1000000000000000)|(value & 0b0111111111111111);
}