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.
Revision 0:5387fecc249c, committed 2015-12-26
- Comitter:
- TArai
- Date:
- Sat Dec 26 14:12:18 2015 +0000
- Commit message:
- ???????????????????
Changed in this revision
| Futaba485.cpp | Show annotated file Show diff for this revision Revisions of this file |
| Futaba485.h | Show annotated file Show diff for this revision Revisions of this file |
--- /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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Futaba485.h Sat Dec 26 14:12:18 2015 +0000
@@ -0,0 +1,22 @@
+#ifndef MBED_FUTABA_SERVO_H
+#define MBED_FUTABA_SERVO_H
+
+#include "mbed.h"
+
+class Futaba {
+private:
+ Serial _device; // tx, rx
+ DigitalOut _REDE; //transmitt enable or disenable
+
+public:
+ Futaba(PinName tx ,PinName rx, PinName REDE);
+ void init();
+ void setTmax(unsigned char ID, unsigned char data);
+ void torqueOn (unsigned char ID);
+ void torqueOff (unsigned char ID);
+ void setPosition (unsigned char ID, int data);
+ void setPosition_long123 (int data1,int data2, int data3); //ID 1~3
+ void setTslope(unsigned char ID, unsigned char CW, unsigned char CCW);//00H~FFH 0deg~255deg
+};
+
+#endif
\ No newline at end of file