はじめに
RX631/63Nでfutaba rs304をシリアル通信で動かす時のサンプルコード集．UARTの端子設定をしておく必要がある．
futaba rs304
基本的には取り扱い説明書 に従って，端子設定及び関数の作成を行う．パケットの書式に従い，サーボにやらせたいことを通信する．
動作手順としてはトルクON(No.36)→角度・速度指令(No.31-34)
複数のサーボを扱う場合は，ID設定(No.4)→ROM保存の順番に書き込みを行う必要がある．デフォルトのサーボIDは 1 である．
小ネタ
- 全てのサーボに指令を出すときは，IDの箇所に
0xFFor255を記入する． - トルクONにしないまま，角度指令を出しても動かない．
サンプルコード
このサンプルコードは，シリアル通信はSCI0を設定して使っている．
サーボ角度・速度指定
/*=================================
* 関数名 : Servo_write
* 機能 : サーボ角度・速度指定
* 引数 : サーボID, 角度=angle*0.1[deg],スピード=speed*10[ms]
* 戻り値 : なし
* 備考 :
* ===============================*/
void Servo_write(unsigned char id,int angle,int speed)
{
unsigned char TxData[12];
unsigned char CheckSum = 0;
int i;
/*=== Header ===*/
TxData[0] = 0xFA;
TxData[1] = 0xAF;
/*=== ID ===*/
TxData[2] = id;
/*=== 指示 ===*/
TxData[3] = 0x00;
/*=== アドレス ===*/
TxData[4] = 0x1E;
/*=== データのバイト数 ===*/
TxData[5] = 0x04;
/*=== サーボ数 ===*/
TxData[6] = 0x01;
/*=== 角度 ===*/
TxData[7] = (unsigned char)0x00FF & angle; //Low byte
TxData[8] = (unsigned char)0x00FF & (angle >> 8); //High byte
/*=== 速度 ===*/
TxData[9] = (unsigned char)0x00FF & speed; //Low byte
TxData[10] = (unsigned char)0x00FF & (speed >> 8); //High byte
/*=== Sum ===*/
for(i = 2; i <=10; i++)
{
CheckSum = CheckSum^TxData[i];
}
TxData[11] = CheckSum;
/*=== データ送信 ===*/
for(i = 0; i <=11; i++)
{
/*=== 送信完了待ち ===*/
while(SCI0.SSR.BIT.TEND == 0);
/*=== 送信 ===*/
SCI0.TDR = TxData[i];
/*=== フラグクリア ===*/
SCI0.SSR.BIT.TEND = 0;
}
}
サーボトルク設定
/*=================================
* 関数名 : Servo_torque
* 機能 : サーボトルク設定
* 引数 : サーボID, トルク設定データ
* 戻り値 : なし
* 備考 : 1>ON 0>OFF
* ===============================*/
void Servo_torque(unsigned char id, unsigned char data)
{
unsigned char TxData[10];
unsigned char CheckSum = 0;
int i;
/*=== Header ===*/
TxData[0] = 0xFA;
TxData[1] = 0xAF;
/*=== ID ===*/
TxData[2] = id;
/*=== 指示 ===*/
TxData[3] = 0x00;
/*=== アドレス ===*/
TxData[4] = 0x24;
/*=== データのバイト数 ===*/
TxData[5] = 0x01;
/*=== サーボ数 ===*/
TxData[6] = 0x01;
/*=== トルクON/OFF ===*/
TxData[7] = data;
/*=== Sum ===*/
for(i = 2; i <=7; i++)
{
CheckSum = CheckSum^TxData[i];
}
TxData[8] = CheckSum;
/*=== データ送信 ===*/
for(i = 0; i <=8; i++)
{
/*=== 送信完了待ち ===*/
while(SCI0.SSR.BIT.TEND == 0);
/*=== 送信 ===*/
SCI0.TDR = TxData[i];
/*=== フラグクリア ===*/
SCI0.SSR.BIT.TEND = 0;
}
}
サーボID設定
/*=================================
* 関数名 : Servo_IDwrite
* 機能 : サーボID設定
* 引数 : 変更前サーボID, 変更後サーボID
* 戻り値 : なし
* 備考 :
* ===============================*/
void Servo_IDwrite(unsigned char id, unsigned char chgid)
{
unsigned char TxData[10];
unsigned char CheckSum = 0;
int i;
/*=== Header ===*/
TxData[0] = 0xFA;
TxData[1] = 0xAF;
/*=== ID ===*/
TxData[2] = id;
/*=== 指示 ===*/
TxData[3] = 0x00;
/*=== アドレス ===*/
TxData[4] = 0x04;
/*=== データのバイト数 ===*/
TxData[5] = 0x01;
/*=== サーボ数 ===*/
TxData[6] = 0x01;
/*=== 変更後ID ===*/
TxData[7] = chgid;
/*=== Sum ===*/
for(i = 2; i <=7; i++)
{
CheckSum = CheckSum^TxData[i];
}
TxData[8] = CheckSum;
/*=== データ送信 ===*/
for(i = 0; i <=8; i++)
{
/*=== 送信完了待ち ===*/
while(SCI0.SSR.BIT.TEND == 0);
/*=== 送信 ===*/
SCI0.TDR = TxData[i];
/*=== フラグクリア ===*/
SCI0.SSR.BIT.TEND = 0;
}
}
書き込み内容保存
/*=================================
* 関数名 : Servo_ROMwrite
* 機能 : 書き込み内容保存
* 引数 :サーボID
* 戻り値 : なし
* 備考 :
* ===============================*/
void Servo_ROMwrite(unsigned char id)
{
unsigned char TxData[10];
unsigned char CheckSum = 0;
int i;
/*=== Header ===*/
TxData[0] = 0xFA;
TxData[1] = 0xAF;
/*=== ID ===*/
TxData[2] = id;
/*=== 指示 ===*/
TxData[3] = 0x40;
/*=== アドレス ===*/
TxData[4] = 0xFF;
/*=== データのバイト数 ===*/
TxData[5] = 0x00;
/*=== サーボ数 ===*/
TxData[6] = 0x00;
/*=== Sum ===*/
for(i = 2; i <=6; i++)
{
CheckSum = CheckSum^TxData[i];
}
TxData[7] = CheckSum;
/*=== データ送信 ===*/
for(i = 0; i <=7; i++)
{
/*=== 送信完了待ち ===*/
while(SCI0.SSR.BIT.TEND == 0);
/*=== 送信 ===*/
SCI0.TDR = TxData[i];
/*=== フラグクリア ===*/
SCI0.SSR.BIT.TEND = 0;
}
}