T Arai
/
Futaba_servo140922
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.
Diff: Futaba485.cpp
- Revision:
- 0:5387fecc249c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Futaba485.cpp Sat Dec 26 14:12:18 2015 +0000
@@ -0,0 +1,214 @@
+#include "Futaba485.h"
+#include "mbed.h"
+
+Futaba::Futaba(PinName tx ,PinName rx, PinName REDE) : _device(tx,rx),_REDE(REDE){
+// tx=p9;
+// rx=p10;
+// REDE = p11;
+}
+
+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(250); // 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(250); // 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(250); // 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
+ for(int i=0; i<=9; i++){
+ _device.putc(TxData[i]);
+ }
+ wait_us(250); // Wait for transmission
+ _REDE = 0; // RS485 Transmit disable
+}
+
+
+
+void Futaba::setPosition_long123 (int data1,int data2, int data3){
+
+ unsigned char TxData[17]; // 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] = 0x01; //Servo ID1
+ TxData[8] = (unsigned char)0x00FF & data1; // Low byte
+ TxData[9] = (unsigned char)0x00FF & (data1 >> 8); // Hi byte
+
+ TxData[10] = 0x02; //Servo ID2
+ TxData[11] = (unsigned char)0x00FF & data2; // Low byte
+ TxData[12] = (unsigned char)0x00FF & (data2 >> 8); // Hi byte
+
+ TxData[13] = 0x03; //Servo ID3
+ TxData[14] = (unsigned char)0x00FF & data3; // Low byte
+ TxData[15] = (unsigned char)0x00FF & (data3 >> 8); // Hi byte
+
+ // CheckSum calculation
+ for(int i=2; i<=15; i++){
+ CheckSum = CheckSum ^ TxData[i]; // XOR from ID to Data
+ }
+ TxData[16] = CheckSum; // Sum
+ // Send Packet
+ _REDE = 1; // RS485 Transmitt Enable
+ for(int i=0; i<=16; i++){
+ _device.putc(TxData[i]);
+ }
+ wait_us(500); // 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(250); // Wait for transmission
+ _REDE = 0; // RS485 Transmitt disable
+}
\ No newline at end of file