Futabaコマンドサーボの角度取得 ロボット全般 2017年02月14日 だいぶ期間が開きましたが、最近はコマンドサーボ(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 = P14DigitalOut 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 calculationTxData[0] = 0xFA; // HeaderTxData[1] = 0xAF; // HeaderTxData[2] = ID; // IDTxData[3] = 0x00; // FlagsTxData[4] = 0x24; // AddressTxData[5] = 0x01; // LengthTxData[6] = 0x01; // CountTxData[7] = data; // Data// CheckSum calculationfor(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 Enablefor(int i=0; i<=8; i++){device.putc(TxData[i]);}wait_us(750); // Wait for transmissionREDE = 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; // HeaderTxData[1] = 0xAF; // HeaderTxData[2] = ID; // IDTxData[3] = 0x00; // FlagsTxData[4] = 0x1E; // AddressTxData[5] = 0x02; // LengthTxData[6] = 0x01; // Count// DataTxData[7] = (unsigned char)0x00FF & data; // Low byteTxData[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; // HeaderTxData[1] = 0xAF; // HeaderTxData[2] = ID; // IDTxData[3] = 0x0F; // FlagsTxData[4] = address; // AddressTxData[5] = Lenge; // LengthTxData[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