T Arai
/
Futaba_servo171024
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependents: ROBOCON30th_TOHOKU
Diff: Futaba485.cpp
- Revision:
- 0:9145c8c3aeca
diff -r 000000000000 -r 9145c8c3aeca Futaba485.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Futaba485.cpp Sat Dec 30 00:18:34 2017 +0000
@@ -0,0 +1,455 @@
+#include "Futaba485.h"
+#include "mbed.h"
+
+unsigned char RxData[20]; // 受信データバッファ [20byte]
+
+Futaba::Futaba(PinName tx ,PinName rx, PinName REDE) : _device(tx,rx),_REDE(REDE){
+// tx=p13;
+// rx=p14;
+// REDE = p15;
+}
+
+void Futaba::Init(void){
+ _device.baud(115200); // baud Rate = 115.2kbps [Futaba default]
+ _REDE = 0; // RS485 Transmit disable
+}
+
+
+void Futaba::SetTmax (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] = 0x23; // 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(800); // Wait for transmission
+ _REDE = 0; // RS485 Transmitt disable
+}
+
+
+
+void Futaba::TorqueOn (unsigned char ID){
+
+ 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] = 0x01; // 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(800); // Wait for transmission
+ _REDE = 0; // RS485 Transmitt disable
+}
+
+void Futaba::TorqueOff (unsigned char ID){
+
+ 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] = 0x00; // 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(800); // Wait for transmission
+ _REDE = 0; // RS485 Transmitt disable
+}
+
+
+
+
+void Futaba::SetPosition (unsigned char ID, int data){
+
+ unsigned char TxData[10]; // TransmitByteData [10byte]
+ unsigned char CheckSum = 0; // CheckSum calculation
+
+ 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
+
+ // CheckSum calculation
+ for(int i=2; i<=8; i++){
+ CheckSum = CheckSum ^ TxData[i]; // XOR from ID to Data
+ }
+ TxData[9] = CheckSum; // Sum
+ // Send Packet
+ _REDE = 1; // RS485 Transmitt Enable
+ //wait_us(100000); // Wait for transmission
+ for(int i=0; i<=9; i++){
+ _device.putc(TxData[i]);
+ }
+ wait_us(800); // Wait for transmission
+ _REDE = 0; // RS485 Transmit disable
+}
+
+
+void Futaba::SetPosition_sx2 (unsigned char ID1, unsigned char ID2, int data1, int data2){
+
+ unsigned char TxData[14]; // TransmitByteData [10byte]
+ unsigned char CheckSum = 0; // CheckSum calculation
+
+ TxData[0] = 0xFA; // Header
+ TxData[1] = 0xAF; // Header
+ TxData[2] = 0x00; // ID long packet
+ TxData[3] = 0x00; // Flags
+ TxData[4] = 0x1E; // Address
+ TxData[5] = 0x03; // Length
+ TxData[6] = 0x03; // Count
+
+ TxData[7] = ID1; //Servo ID1
+ TxData[8] = (unsigned char)0x00FF & data1; // Low byte
+ TxData[9] = (unsigned char)0x00FF & (data1 >> 8); // Hi byte
+
+ TxData[10] = ID2; //Servo ID2
+ TxData[11] = (unsigned char)0x00FF & data2; // Low byte
+ TxData[12] = (unsigned char)0x00FF & (data2 >> 8); // Hi byte
+
+
+ // CheckSum calculation
+ for(int i=2; i<=12; i++){
+ CheckSum = CheckSum ^ TxData[i]; // XOR from ID to Data
+ }
+ TxData[13] = CheckSum; // Sum
+ // Send Packet
+ _REDE = 1; // RS485 Transmitt Enable
+
+ for(int i=0; i<=13; i++){
+ _device.putc(TxData[i]);
+ }
+ wait_us(2000); // Wait for transmission
+ _REDE = 1; // RS485 Transmit disable
+}
+
+void Futaba::SetPosition_sx4 (unsigned char ID1,unsigned char ID2,unsigned char ID3,unsigned char ID4, int data1, int data2, int data3, int data4){
+
+ unsigned char TxData[20]; // TransmitByteData [10byte]
+ unsigned char CheckSum = 0; // CheckSum calculation
+
+ TxData[0] = 0xFA; // Header
+ TxData[1] = 0xAF; // Header
+ TxData[2] = 0x00; // ID long packet
+ TxData[3] = 0x00; // Flags
+ TxData[4] = 0x1E; // Address
+ TxData[5] = 0x03; // Length
+ TxData[6] = 0x03; // Count
+
+ TxData[7] = ID1; //Servo ID1
+ TxData[8] = (unsigned char)0x00FF & data1; // Low byte
+ TxData[9] = (unsigned char)0x00FF & (data1 >> 8); // Hi byte
+
+ TxData[10] = ID2; //Servo ID2
+ TxData[11] = (unsigned char)0x00FF & data2; // Low byte
+ TxData[12] = (unsigned char)0x00FF & (data2 >> 8); // Hi byte
+
+ TxData[13] = ID3; //Servo ID31
+ TxData[14] = (unsigned char)0x00FF & data3; // Low byte
+ TxData[15] = (unsigned char)0x00FF & (data3 >> 8); // Hi byte
+
+ TxData[16] = ID4; //Servo ID4
+ TxData[17] = (unsigned char)0x00FF & data4; // Low byte
+ TxData[18] = (unsigned char)0x00FF & (data4 >> 8); // Hi byte
+
+ // CheckSum calculation
+ for(int i=2; i<=12; i++){
+ CheckSum = CheckSum ^ TxData[i]; // XOR from ID to Data
+ }
+ TxData[19] = CheckSum; // Sum
+ // Send Packet
+ _REDE = 1; // RS485 Transmitt Enable
+
+ for(int i=0; i<=19; i++){
+ _device.putc(TxData[i]);
+ }
+ wait_us(1700); // Wait for transmission
+ _REDE = 0; // RS485 Transmit disable
+}
+
+
+
+void Futaba::SetTslope (unsigned char ID, unsigned char CW, unsigned char CCW){
+
+ unsigned char TxData[10]; // 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] = 0x1A; // Address
+ TxData[5] = 0x02; // Length
+ TxData[6] = 0x01; // Count
+ TxData[7] = CW; // Data
+ TxData[8] = CCW; // Data
+ // CheckSum calculation
+ for(int i=2; i<=8; i++){
+ CheckSum = CheckSum ^ TxData[i]; // XOR from ID to Data
+ }
+
+ TxData[9] = CheckSum; // Sum
+
+ // Send Packet
+ _REDE = 1; // RS485 Transmit Enable
+
+ for(int i=0; i<=9; i++){
+ _device.putc(TxData[i]);
+ }
+ wait_us(800); // Wait for transmission
+ _REDE = 0; // RS485 Transmitt disable
+}
+
+void Futaba::SetMargin (unsigned char ID, unsigned char CW, unsigned char CCW){
+
+ unsigned char TxData[10]; // 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] = 0x18; // Address
+ TxData[5] = 0x02; // Length
+ TxData[6] = 0x01; // Count
+ TxData[7] = CW; // Data
+ TxData[8] = CCW; // Data
+ // CheckSum calculation
+ for(int i=2; i<=8; i++){
+ CheckSum = CheckSum ^ TxData[i]; // XOR from ID to Data
+ }
+
+ TxData[9] = CheckSum; // Sum
+
+ // Send Packet
+ _REDE = 1; // RS485 Transmit Enable
+
+ for(int i=0; i<=9; i++){
+ _device.putc(TxData[i]);
+ }
+ wait_us(800); // Wait for transmission
+ _REDE = 0; // RS485 Transmitt disable
+}
+
+
+
+/*-------------------------------------------------*/
+/* 機能 : リターンパケット要求 */
+/* 名前 : RequestReply */
+/* 引数 : ID (Servo ID) */
+/* : address (指定アドレス) */
+/* : Lenge (指定バイト数) */
+/* 戻り値 : 無し */
+/*-------------------------------------------------*/
+void Futaba::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); // Wait for transmission
+ _REDE = 0; // RS485 Transmitt disable 受信待ち
+}
+
+
+
+/*----------------------------------------------------*/
+/* 機能 : リターンパケット受信待ち(2byte専用) */
+/* 名前 : WaitReply */
+/* 引数 : 無し */
+/* 戻り値 : 受信完了(0xAB) */
+/* : タイムアウト(0xCD) */
+/*----------------------------------------------------*/
+unsigned char Futaba::WaitReply (void){
+ unsigned char result = 0xAB; // 戻り値一時格納
+ int TimeOut = 0; // リターンパケット受信タイムアウト
+
+ // ヘッダー上位バイト(0xFD)待ち
+ while(0xFD != _device.getc()){
+ if(TimeOut < 300){
+ TimeOut++;
+ }else{
+ result = 0xCD;
+ break;
+ }
+ }
+ RxData[0] = 0xFD;
+
+ // ヘッダー下位バイト(0xDF)待ち
+ while(0xDF != _device.getc()){
+ if(TimeOut < 300){
+ TimeOut++;
+ }else{
+ result = 0xCD;
+ break;
+ }
+ }
+ RxData[1] = 0xDF;
+
+ // 残データ待ち
+ //while(Serial.available() < 8){
+ while( _device.readable()){
+ if(TimeOut < 300){
+ TimeOut++;
+ }else{
+ result = 0xCD;
+ break;
+ }
+ }
+
+ // 受信バッファからデータ取り出し
+ for(int i=2; i<10; i++){
+ RxData[i] = _device.getc();
+ }
+
+ return result; // 受信データを戻す
+}
+
+
+
+/*----------------------------------------------------*/
+/* 機能 : リターンパケット解析(2byte専用) */
+/* 名前 : AnalysisReply */
+/* 引数 : 無し */
+/* 戻り値 : 受信データ内容 */
+/* ｺﾒﾝﾄ : ヘッダー・チェックサム等異常があった場合、*/
+/* resultを「3600」とする */
+/*----------------------------------------------------*/
+int Futaba::AnalysisReply (void){
+ unsigned char CheckSum = 0; // チェックサム計算用変数
+ int result; // 受信データ内容格納
+ int ErrorFlag;
+
+ // ヘッダ、チェックサム確認
+ 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"で無い
+ }
+ if(ErrorFlag == true) result=3600;
+ return result; // 受信データを戻す
+}
+
+
+/*----------------------------------------------------*/
+/* 機能 : 角度データ取得(2byte専用) */
+/* 名前 : GetAngle */
+/* 引数 : ID */
+/* 戻り値 : 角度データ 0x0000~FFFF */
+/* ｺﾒﾝﾄ : 不感帯 : Angle=0x5555 */
+/* 受信エラー : Angle=0x6666 */
+/*----------------------------------------------------*/
+int Futaba::GetAngle(unsigned char ID){
+ int result; // 受信データ内容格納
+ int ErrorFlag = 0;
+ int Angle;
+
+ Futaba::RequestReply(ID,0x2A,0x02); // ID = 1, address = 42 (2A)(現在位置), Lenge = 2(2byte)
+
+ // リターンパケット受信待ち
+ result = Futaba::WaitReply();
+ if(result == 0xCD){
+ ErrorFlag = true; // リターンパケット受信エラー
+ }
+
+ Angle = int(AnalysisReply());
+ /*
+ if(Angle==5555){
+ ;
+ }else if(Angle>1800){
+ Angle = Angle - 0xFFFF;
+ }
+ */
+ if (ErrorFlag == true){
+ Angle = 6666;
+ }
+
+ return Angle; // 受信データを戻す
+}
\ No newline at end of file