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.

Futaba485.cpp@0:5387fecc249c, 2015-12-26 (annotated)

Committer:
TArai
Date:
Sat Dec 26 14:12:18 2015 +0000
Revision:
0:5387fecc249c
???????????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TArai 0:5387fecc249c 1 #include "Futaba485.h"
TArai 0:5387fecc249c 2 #include "mbed.h"
TArai 0:5387fecc249c 3
TArai 0:5387fecc249c 4 Futaba::Futaba(PinName tx ,PinName rx, PinName REDE) : _device(tx,rx),_REDE(REDE){
TArai 0:5387fecc249c 5 // tx=p9;
TArai 0:5387fecc249c 6 // rx=p10;
TArai 0:5387fecc249c 7 // REDE = p11;
TArai 0:5387fecc249c 8 }
TArai 0:5387fecc249c 9
TArai 0:5387fecc249c 10 void Futaba::init(void){
TArai 0:5387fecc249c 11 _device.baud(115200); // baud Rate = 115.2kbps [Futaba default]
TArai 0:5387fecc249c 12 _REDE = 0; // RS485 Transmit disable
TArai 0:5387fecc249c 13 }
TArai 0:5387fecc249c 14
TArai 0:5387fecc249c 15
TArai 0:5387fecc249c 16 void Futaba::setTmax (unsigned char ID, unsigned char data){
TArai 0:5387fecc249c 17
TArai 0:5387fecc249c 18 unsigned char TxData[9]; // TransmitByteData [9byte]
TArai 0:5387fecc249c 19 unsigned char CheckSum = 0; // CheckSum calculation
TArai 0:5387fecc249c 20
TArai 0:5387fecc249c 21 TxData[0] = 0xFA; // Header
TArai 0:5387fecc249c 22 TxData[1] = 0xAF; // Header
TArai 0:5387fecc249c 23 TxData[2] = ID; // ID
TArai 0:5387fecc249c 24 TxData[3] = 0x00; // Flags
TArai 0:5387fecc249c 25 TxData[4] = 0x23; // Address
TArai 0:5387fecc249c 26 TxData[5] = 0x01; // Length
TArai 0:5387fecc249c 27 TxData[6] = 0x01; // Count
TArai 0:5387fecc249c 28 TxData[7] = data; // Data
TArai 0:5387fecc249c 29
TArai 0:5387fecc249c 30 // CheckSum calculation
TArai 0:5387fecc249c 31 for(int i=2; i<=7; i++){
TArai 0:5387fecc249c 32 CheckSum = CheckSum ^ TxData[i]; // XOR from ID to Data
TArai 0:5387fecc249c 33 }
TArai 0:5387fecc249c 34
TArai 0:5387fecc249c 35 TxData[8] = CheckSum; // Sum
TArai 0:5387fecc249c 36
TArai 0:5387fecc249c 37 // Send Packet
TArai 0:5387fecc249c 38 _REDE = 1; // RS485 Transmit Enable
TArai 0:5387fecc249c 39 for(int i=0; i<=8; i++){
TArai 0:5387fecc249c 40 _device.putc(TxData[i]);
TArai 0:5387fecc249c 41 }
TArai 0:5387fecc249c 42 wait_us(250); // Wait for transmission
TArai 0:5387fecc249c 43 _REDE = 0; // RS485 Transmitt disable
TArai 0:5387fecc249c 44 }
TArai 0:5387fecc249c 45
TArai 0:5387fecc249c 46
TArai 0:5387fecc249c 47
TArai 0:5387fecc249c 48 void Futaba::torqueOn (unsigned char ID){
TArai 0:5387fecc249c 49
TArai 0:5387fecc249c 50 unsigned char TxData[9]; // TransmitByteData [9byte]
TArai 0:5387fecc249c 51 unsigned char CheckSum = 0; // CheckSum calculation
TArai 0:5387fecc249c 52
TArai 0:5387fecc249c 53 TxData[0] = 0xFA; // Header
TArai 0:5387fecc249c 54 TxData[1] = 0xAF; // Header
TArai 0:5387fecc249c 55 TxData[2] = ID; // ID
TArai 0:5387fecc249c 56 TxData[3] = 0x00; // Flags
TArai 0:5387fecc249c 57 TxData[4] = 0x24; // Address
TArai 0:5387fecc249c 58 TxData[5] = 0x01; // Length
TArai 0:5387fecc249c 59 TxData[6] = 0x01; // Count
TArai 0:5387fecc249c 60 TxData[7] = 0x01; // Data
TArai 0:5387fecc249c 61
TArai 0:5387fecc249c 62 // CheckSum calculation
TArai 0:5387fecc249c 63 for(int i=2; i<=7; i++){
TArai 0:5387fecc249c 64 CheckSum = CheckSum ^ TxData[i]; // XOR from ID to Data
TArai 0:5387fecc249c 65 }
TArai 0:5387fecc249c 66
TArai 0:5387fecc249c 67 TxData[8] = CheckSum; // Sum
TArai 0:5387fecc249c 68
TArai 0:5387fecc249c 69 // Send Packet
TArai 0:5387fecc249c 70 _REDE = 1; // RS485 Transmit Enable
TArai 0:5387fecc249c 71 for(int i=0; i<=8; i++){
TArai 0:5387fecc249c 72 _device.putc(TxData[i]);
TArai 0:5387fecc249c 73 }
TArai 0:5387fecc249c 74 wait_us(250); // Wait for transmission
TArai 0:5387fecc249c 75 _REDE = 0; // RS485 Transmitt disable
TArai 0:5387fecc249c 76 }
TArai 0:5387fecc249c 77
TArai 0:5387fecc249c 78 void Futaba::torqueOff (unsigned char ID){
TArai 0:5387fecc249c 79
TArai 0:5387fecc249c 80 unsigned char TxData[9]; // TransmitByteData [9byte]
TArai 0:5387fecc249c 81 unsigned char CheckSum = 0; // CheckSum calculation
TArai 0:5387fecc249c 82
TArai 0:5387fecc249c 83 TxData[0] = 0xFA; // Header
TArai 0:5387fecc249c 84 TxData[1] = 0xAF; // Header
TArai 0:5387fecc249c 85 TxData[2] = ID; // ID
TArai 0:5387fecc249c 86 TxData[3] = 0x00; // Flags
TArai 0:5387fecc249c 87 TxData[4] = 0x24; // Address
TArai 0:5387fecc249c 88 TxData[5] = 0x01; // Length
TArai 0:5387fecc249c 89 TxData[6] = 0x01; // Count
TArai 0:5387fecc249c 90 TxData[7] = 0x00; // Data
TArai 0:5387fecc249c 91
TArai 0:5387fecc249c 92 // CheckSum calculation
TArai 0:5387fecc249c 93 for(int i=2; i<=7; i++){
TArai 0:5387fecc249c 94 CheckSum = CheckSum ^ TxData[i]; // XOR from ID to Data
TArai 0:5387fecc249c 95 }
TArai 0:5387fecc249c 96
TArai 0:5387fecc249c 97 TxData[8] = CheckSum; // Sum
TArai 0:5387fecc249c 98
TArai 0:5387fecc249c 99 // Send Packet
TArai 0:5387fecc249c 100 _REDE = 1; // RS485 Transmit Enable
TArai 0:5387fecc249c 101 for(int i=0; i<=8; i++){
TArai 0:5387fecc249c 102 _device.putc(TxData[i]);
TArai 0:5387fecc249c 103 }
TArai 0:5387fecc249c 104 wait_us(250); // Wait for transmission
TArai 0:5387fecc249c 105 _REDE = 0; // RS485 Transmitt disable
TArai 0:5387fecc249c 106 }
TArai 0:5387fecc249c 107
TArai 0:5387fecc249c 108
TArai 0:5387fecc249c 109
TArai 0:5387fecc249c 110
TArai 0:5387fecc249c 111 void Futaba::setPosition (unsigned char ID, int data){
TArai 0:5387fecc249c 112
TArai 0:5387fecc249c 113 unsigned char TxData[10]; // TransmitByteData [10byte]
TArai 0:5387fecc249c 114 unsigned char CheckSum = 0; // CheckSum calculation
TArai 0:5387fecc249c 115
TArai 0:5387fecc249c 116 TxData[0] = 0xFA; // Header
TArai 0:5387fecc249c 117 TxData[1] = 0xAF; // Header
TArai 0:5387fecc249c 118 TxData[2] = ID; // ID
TArai 0:5387fecc249c 119 TxData[3] = 0x00; // Flags
TArai 0:5387fecc249c 120 TxData[4] = 0x1E; // Address
TArai 0:5387fecc249c 121 TxData[5] = 0x02; // Length
TArai 0:5387fecc249c 122 TxData[6] = 0x01; // Count
TArai 0:5387fecc249c 123 // Data
TArai 0:5387fecc249c 124 TxData[7] = (unsigned char)0x00FF & data; // Low byte
TArai 0:5387fecc249c 125 TxData[8] = (unsigned char)0x00FF & (data >> 8); // Hi byte
TArai 0:5387fecc249c 126
TArai 0:5387fecc249c 127 // CheckSum calculation
TArai 0:5387fecc249c 128 for(int i=2; i<=8; i++){
TArai 0:5387fecc249c 129 CheckSum = CheckSum ^ TxData[i]; // XOR from ID to Data
TArai 0:5387fecc249c 130 }
TArai 0:5387fecc249c 131 TxData[9] = CheckSum; // Sum
TArai 0:5387fecc249c 132 // Send Packet
TArai 0:5387fecc249c 133 _REDE = 1; // RS485 Transmitt Enable
TArai 0:5387fecc249c 134 for(int i=0; i<=9; i++){
TArai 0:5387fecc249c 135 _device.putc(TxData[i]);
TArai 0:5387fecc249c 136 }
TArai 0:5387fecc249c 137 wait_us(250); // Wait for transmission
TArai 0:5387fecc249c 138 _REDE = 0; // RS485 Transmit disable
TArai 0:5387fecc249c 139 }
TArai 0:5387fecc249c 140
TArai 0:5387fecc249c 141
TArai 0:5387fecc249c 142
TArai 0:5387fecc249c 143 void Futaba::setPosition_long123 (int data1,int data2, int data3){
TArai 0:5387fecc249c 144
TArai 0:5387fecc249c 145 unsigned char TxData[17]; // TransmitByteData [10byte]
TArai 0:5387fecc249c 146 unsigned char CheckSum = 0; // CheckSum calculation
TArai 0:5387fecc249c 147
TArai 0:5387fecc249c 148 TxData[0] = 0xFA; // Header
TArai 0:5387fecc249c 149 TxData[1] = 0xAF; // Header
TArai 0:5387fecc249c 150 TxData[2] = 0x00; // ID long packet
TArai 0:5387fecc249c 151 TxData[3] = 0x00; // Flags
TArai 0:5387fecc249c 152 TxData[4] = 0x1E; // Address
TArai 0:5387fecc249c 153 TxData[5] = 0x03; // Length
TArai 0:5387fecc249c 154 TxData[6] = 0x03; // Count
TArai 0:5387fecc249c 155
TArai 0:5387fecc249c 156 TxData[7] = 0x01; //Servo ID1
TArai 0:5387fecc249c 157 TxData[8] = (unsigned char)0x00FF & data1; // Low byte
TArai 0:5387fecc249c 158 TxData[9] = (unsigned char)0x00FF & (data1 >> 8); // Hi byte
TArai 0:5387fecc249c 159
TArai 0:5387fecc249c 160 TxData[10] = 0x02; //Servo ID2
TArai 0:5387fecc249c 161 TxData[11] = (unsigned char)0x00FF & data2; // Low byte
TArai 0:5387fecc249c 162 TxData[12] = (unsigned char)0x00FF & (data2 >> 8); // Hi byte
TArai 0:5387fecc249c 163
TArai 0:5387fecc249c 164 TxData[13] = 0x03; //Servo ID3
TArai 0:5387fecc249c 165 TxData[14] = (unsigned char)0x00FF & data3; // Low byte
TArai 0:5387fecc249c 166 TxData[15] = (unsigned char)0x00FF & (data3 >> 8); // Hi byte
TArai 0:5387fecc249c 167
TArai 0:5387fecc249c 168 // CheckSum calculation
TArai 0:5387fecc249c 169 for(int i=2; i<=15; i++){
TArai 0:5387fecc249c 170 CheckSum = CheckSum ^ TxData[i]; // XOR from ID to Data
TArai 0:5387fecc249c 171 }
TArai 0:5387fecc249c 172 TxData[16] = CheckSum; // Sum
TArai 0:5387fecc249c 173 // Send Packet
TArai 0:5387fecc249c 174 _REDE = 1; // RS485 Transmitt Enable
TArai 0:5387fecc249c 175 for(int i=0; i<=16; i++){
TArai 0:5387fecc249c 176 _device.putc(TxData[i]);
TArai 0:5387fecc249c 177 }
TArai 0:5387fecc249c 178 wait_us(500); // Wait for transmission
TArai 0:5387fecc249c 179 _REDE = 0; // RS485 Transmit disable
TArai 0:5387fecc249c 180 }
TArai 0:5387fecc249c 181
TArai 0:5387fecc249c 182
TArai 0:5387fecc249c 183
TArai 0:5387fecc249c 184
TArai 0:5387fecc249c 185
TArai 0:5387fecc249c 186 void Futaba::setTslope (unsigned char ID, unsigned char CW, unsigned char CCW){
TArai 0:5387fecc249c 187
TArai 0:5387fecc249c 188 unsigned char TxData[10]; // TransmitByteData [9byte]
TArai 0:5387fecc249c 189 unsigned char CheckSum = 0; // CheckSum calculation
TArai 0:5387fecc249c 190
TArai 0:5387fecc249c 191 TxData[0] = 0xFA; // Header
TArai 0:5387fecc249c 192 TxData[1] = 0xAF; // Header
TArai 0:5387fecc249c 193 TxData[2] = ID; // ID
TArai 0:5387fecc249c 194 TxData[3] = 0x00; // Flags
TArai 0:5387fecc249c 195 TxData[4] = 0x1A; // Address
TArai 0:5387fecc249c 196 TxData[5] = 0x02; // Length
TArai 0:5387fecc249c 197 TxData[6] = 0x01; // Count
TArai 0:5387fecc249c 198 TxData[7] = CW; // Data
TArai 0:5387fecc249c 199 TxData[8] = CCW; // Data
TArai 0:5387fecc249c 200 // CheckSum calculation
TArai 0:5387fecc249c 201 for(int i=2; i<=8; i++){
TArai 0:5387fecc249c 202 CheckSum = CheckSum ^ TxData[i]; // XOR from ID to Data
TArai 0:5387fecc249c 203 }
TArai 0:5387fecc249c 204
TArai 0:5387fecc249c 205 TxData[9] = CheckSum; // Sum
TArai 0:5387fecc249c 206
TArai 0:5387fecc249c 207 // Send Packet
TArai 0:5387fecc249c 208 _REDE = 1; // RS485 Transmit Enable
TArai 0:5387fecc249c 209 for(int i=0; i<=9; i++){
TArai 0:5387fecc249c 210 _device.putc(TxData[i]);
TArai 0:5387fecc249c 211 }
TArai 0:5387fecc249c 212 wait_us(250); // Wait for transmission
TArai 0:5387fecc249c 213 _REDE = 0; // RS485 Transmitt disable
TArai 0:5387fecc249c 214 }