忍者ブログ

白黒サイクル

ロボットとジャグリングのブログ。

Futabaコマンドサーボの角度取得

だいぶ期間が開きましたが、最近はコマンドサーボ(RS301)でアーム作ったりして遊んでます。




先輩から頂いたArduinoのサンプルコードをいじって、mbedでサーボの角度を取得するプログラムがやっと動きました。

コマンドサーボ配線図: http://futaba.co.jp/robot/systems
※プログラムと配線図でピンが違う&送信後の待ち時間が足りないので要注意です。


私が引っ掛かったのは
・device.readable()の使い方 →ネットで調べました
・リターン要求送信後の待ち時間 → オシロで波形を表示して測定しました。
といった感じです。

リターン要求送信後の待ち時間については、私の条件では700usくらいがちょうどいいみたいです。
REDE変数で送信と受信を入れ替えているので
待ち時間が短い→送信送信が途中で切れる
待ち時間が長い→受信信号の頭が切れる
となります。

コード載せておくので、ご参考までに。
あと、送信待ち時間の上手い設定方法がわかれば教えてもらえると嬉しいです。



ID1のサーボの角度を読み取り、ID20 を動かすプログラムです。
(+ID20の角度を読み、ID1を動かせばマスタ・スレーブっぽい動きになる)

※コピペしたらインデントが消滅しました・・・。
以下、コード
------------------------------------------
#include "mbed.h"

/******************************************************************/
/* Futabaコマンドサーボ */
/* ID1のサーボの角度を読み取り、ID20 に送る */
/* [ID] 1(0x01) / 20(0x14) */
/* */
/******************************************************************/

/* セットアップ */
DigitalOut led1(LED1),led2(LED2),led3(LED3),led4(LED4);
Serial PC(USBTX, USBRX);
DigitalOut LED_red(p5), LED_blue(p6), LED_green(p7);

Serial device(p13,p14);//コマンドサーボ // tx = P13, rx = P14
DigitalOut REDE(p15);

/* グローバル変数定義 */
int ErrorFlag = false; // エラーフラグ
unsigned char RxData[20]; // 受信データバッファ [20byte]


/*-------------------------------------------------*/
/* Funcyion : mbed initialize */
/* NAME : init */
/* Argument : --- */
/* Return value : --- */
/*-------------------------------------------------*/
void init(void){
device.baud(115200); // baud Rate = 115.2kbps [Futaba default]
REDE = 0; // RS485 Transmit disable
}



/*-------------------------------------------------*/
/* Funcyion : servo torque enable */
/* NAME : torque */
/* Argument : ID (Servo ID) */
/* : data (Torque enable) */
/* Return value : --- */
/*-------------------------------------------------*/
void torque (unsigned char ID, unsigned char data){

unsigned char TxData[9]; // TransmitByteData [9byte]
unsigned char CheckSum = 0; // CheckSum calculation

TxData[0] = 0xFA; // Header
TxData[1] = 0xAF; // Header
TxData[2] = ID; // ID
TxData[3] = 0x00; // Flags
TxData[4] = 0x24; // Address
TxData[5] = 0x01; // Length
TxData[6] = 0x01; // Count
TxData[7] = data; // Data

// CheckSum calculation
for(int i=2; i<=7; i++){
CheckSum = CheckSum ^ TxData[i]; // チェックサム計算(XOR from ID to Data)
}

TxData[8] = CheckSum; // Sum

// Send Packet
REDE = 1; // RS485 Transmit Enable
for(int i=0; i<=8; i++){
device.putc(TxData[i]);
}
wait_us(750); // Wait for transmission
REDE = 0; // RS485 Transmitt disable
}


/*-------------------------------------------------*/
/* 機能 : サーボ角度指定 */
/* 名前 : GoalPosition */
/* 引数 : ID (Servo ID) */
/* : data (Goal Position) */
/* 戻り値 : 無し */
/*-------------------------------------------------*/
void GoalPosition (unsigned char ID, int data){
unsigned char TxData[10]; // 送信データバッファ [10byte]
unsigned char CheckSum = 0; // チェックサム計算用変数

TxData[0] = 0xFA; // Header
TxData[1] = 0xAF; // Header
TxData[2] = ID; // ID
TxData[3] = 0x00; // Flags
TxData[4] = 0x1E; // Address
TxData[5] = 0x02; // Length
TxData[6] = 0x01; // Count
// Data
TxData[7] = (unsigned char)0x00FF & data; // Low byte
TxData[8] = (unsigned char)0x00FF & (data >> 8); // Hi byte
// チェックサム計算
for(int i=2; i<=8; i++){
CheckSum = CheckSum ^ TxData[i]; // ID~DATAまでのXOR
}
TxData[9] = CheckSum; // Sum

// パケットデータ送信
REDE = 1; // RS485 送信許可
for(int i=0; i<=9; i++){
device.putc(TxData[i]);
}
wait_us(800); // データ送信完了待ち
REDE = 0; // RS485 受信許可
}

/*-------------------------------------------------*/
/* 機能 : リターンパケット要求 */
/* 名前 : RequestReply */
/* 引数 : ID (Servo ID) */
/* : address (指定アドレス) */
/* : Lenge (指定バイト数) */
/* 戻り値 : 無し */
/*-------------------------------------------------*/
void RequestReply (unsigned char ID, unsigned char address, unsigned char Lenge){
unsigned char TxData[8]; // 送信データバッファ [8byte]
unsigned char CheckSum = 0; // チェックサム計算用変数


TxData[0] = 0xFA; // Header
TxData[1] = 0xAF; // Header
TxData[2] = ID; // ID
TxData[3] = 0x0F; // Flags
TxData[4] = address; // Address
TxData[5] = Lenge; // Length
TxData[6] = 0x00; // Count
// チェックサム計算
for(int i=2; i<=6; i++){
CheckSum = CheckSum ^ TxData[i]; // ID~DATAまでのXOR
}
TxData[7] = CheckSum; // Sum

// パケットデータ送信
REDE = 1; // RS485 送信許可
for(int i=0; i<=7; i++){
device.putc(TxData[i]);
}
wait_us(700); // データ送信完了待ち ※重要
REDE = 0; // RS485 受信許可
}

/*----------------------------------------------------*/
/* 機能 : リターンパケット受信待ち(2byte専用) */
/* 名前 : WaitReply */
/* 引数 : 無し */
/* 戻り値 : 受信完了(0xAB) */
/* : タイムアウト(0xCD) */
/*----------------------------------------------------*/

unsigned char WaitReply (void){
unsigned char result = 0xAB; // 戻り値一時格納
int TimeOut = 0; // リターンパケット受信タイムアウト

while(!device.readable()){
if(TimeOut < 300){
TimeOut++;
wait_ms(10);
}else{
result = 0xCD;
PC.printf("NOT READABLE\n");
break;
}
}

// ヘッダー上位バイト(0xFD)待ち
while(0xFD != device.getc()){
if(TimeOut < 300){
TimeOut++;
}else{
result = 0xCD;
PC.printf("TimeOUT [0]\n");
break;
}
}
RxData[0] = 0xFD;

// ヘッダー下位バイト(0xDF)待ち
while(0xDF != device.getc()){
if(TimeOut < 300){
TimeOut++;
}else{
result = 0xCD;
PC.printf("TimeOUT [1]\n");
break;
}
}
RxData[1] = 0xDF;

// 受信バッファからデータ取り出し
for(int i=2; i<10; i++){
if(device.readable()){
RxData[i] = device.getc();
}
}

return result; // 受信データを戻す
}


/*----------------------------------------------------*/
/* 機能 : リターンパケット解析(2byte専用) */
/* 名前 : AnalysisReply */
/* 引数 : 無し */
/* 戻り値 : 受信データ内容 */
/* コメント : ヘッダー・チェックサム等異常があった場合、*/
/* 「ErrorFlag (グローバル変数)」を立てる */
/*----------------------------------------------------*/
int AnalysisReply (void){
unsigned char CheckSum = 0; // チェックサム計算用変数
int result; // 受信データ内容格納

// ヘッダ、チェックサム確認
if(RxData[0] == 0xFD){
if(RxData[1] == 0xDF){
// チェックサム計算
for(int i=2; i<=8; i++){
CheckSum = CheckSum ^ RxData[i]; // ID~DATAまでのXOR
}
if(CheckSum == RxData[9]){
result = RxData[8];
result = result << 8;
result |= RxData[7];
}else{
ErrorFlag = true; // チェックサムが計算結果と異なる
}
CheckSum = 0;
}else{
ErrorFlag = true; // ヘッダー2byte目が"0xDF"で無い
}
}else{
ErrorFlag = true; // ヘッダー1byte目が"0xFD"で無い
}

return result; // 受信データを戻す
}
/*-------------------------------------------------*/
/* 機能 : メインプログラム */
/* 名前 : loop */
/* 引数 : 無し */
/* 戻り値 : 無し */
/*-------------------------------------------------*/


int main(){
int Angle = 0; // 角度データ読込値格納
unsigned char result = 0; // 演算結果一時格納

init(); //通信関係の設定
PC.printf("servo init\n");


wait_ms(500); // wait (100msec)
//torque(0xFF, 0x01); // ID = 1(0x01) , torque = OFF (0x00)
// torque = OFF(0x00), ON(0x01), BRAKE(0x02)
torque(0x14, 0x01); // ID = 20(0x14)
wait_ms(100); // wait (100msec)

while(1){
// エラーフラグクリア
ErrorFlag = false;

// リターンパケット要求(現在位置 2byte)
RequestReply(0x01,0x2A,0x02); // ID = 1, address = 42 (2A)(現在位置), Lenge = 2(2byte)

// リターンパケット受信待ち
result = WaitReply();
if(result == 0xCD){
ErrorFlag = true; // リターンパケット受信エラー
}

// 他サーボへ角度指示
if(ErrorFlag == false){
GoalPosition(0x14,Angle);
}
wait_ms(10); // wait (10msec)


// 角度データ表示(見やすいように符号計算)
Angle = int(AnalysisReply());
if(Angle==5555){ //ポテンショメータ不感帯の場合は5555となる
;
}else if (Angle>1800){
Angle = Angle - 0xFFFF;//負
}

PC.printf("angle %d\n", Angle);

}
}
PR