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

Futaba485.cpp@0:9145c8c3aeca, 2017-12-30 (annotated)

Committer:
TArai
Date:
Sat Dec 30 00:18:34 2017 +0000
Revision:
0:9145c8c3aeca
???

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TArai 0:9145c8c3aeca 1 #include "Futaba485.h"
TArai 0:9145c8c3aeca 2 #include "mbed.h"
TArai 0:9145c8c3aeca 3
TArai 0:9145c8c3aeca 4 unsigned char RxData[20]; // 受信データバッファ [20byte]
TArai 0:9145c8c3aeca 5
TArai 0:9145c8c3aeca 6 Futaba::Futaba(PinName tx ,PinName rx, PinName REDE) : _device(tx,rx),_REDE(REDE){
TArai 0:9145c8c3aeca 7 // tx=p13;
TArai 0:9145c8c3aeca 8 // rx=p14;
TArai 0:9145c8c3aeca 9 // REDE = p15;
TArai 0:9145c8c3aeca 10 }
TArai 0:9145c8c3aeca 11
TArai 0:9145c8c3aeca 12 void Futaba::Init(void){
TArai 0:9145c8c3aeca 13 _device.baud(115200); // baud Rate = 115.2kbps [Futaba default]
TArai 0:9145c8c3aeca 14 _REDE = 0; // RS485 Transmit disable
TArai 0:9145c8c3aeca 15 }
TArai 0:9145c8c3aeca 16
TArai 0:9145c8c3aeca 17
TArai 0:9145c8c3aeca 18 void Futaba::SetTmax (unsigned char ID, unsigned char data){
TArai 0:9145c8c3aeca 19
TArai 0:9145c8c3aeca 20 unsigned char TxData[9]; // TransmitByteData [9byte]
TArai 0:9145c8c3aeca 21 unsigned char CheckSum = 0; // CheckSum calculation
TArai 0:9145c8c3aeca 22
TArai 0:9145c8c3aeca 23 TxData[0] = 0xFA; // Header
TArai 0:9145c8c3aeca 24 TxData[1] = 0xAF; // Header
TArai 0:9145c8c3aeca 25 TxData[2] = ID; // ID
TArai 0:9145c8c3aeca 26 TxData[3] = 0x00; // Flags
TArai 0:9145c8c3aeca 27 TxData[4] = 0x23; // Address
TArai 0:9145c8c3aeca 28 TxData[5] = 0x01; // Length
TArai 0:9145c8c3aeca 29 TxData[6] = 0x01; // Count
TArai 0:9145c8c3aeca 30 TxData[7] = data; // Data
TArai 0:9145c8c3aeca 31
TArai 0:9145c8c3aeca 32 // CheckSum calculation
TArai 0:9145c8c3aeca 33 for(int i=2; i<=7; i++){
TArai 0:9145c8c3aeca 34 CheckSum = CheckSum ^ TxData[i]; // XOR from ID to Data
TArai 0:9145c8c3aeca 35 }
TArai 0:9145c8c3aeca 36
TArai 0:9145c8c3aeca 37 TxData[8] = CheckSum; // Sum
TArai 0:9145c8c3aeca 38
TArai 0:9145c8c3aeca 39 // Send Packet
TArai 0:9145c8c3aeca 40 _REDE = 1; // RS485 Transmit Enable
TArai 0:9145c8c3aeca 41 for(int i=0; i<=8; i++){
TArai 0:9145c8c3aeca 42 _device.putc(TxData[i]);
TArai 0:9145c8c3aeca 43 }
TArai 0:9145c8c3aeca 44 wait_us(800); // Wait for transmission
TArai 0:9145c8c3aeca 45 _REDE = 0; // RS485 Transmitt disable
TArai 0:9145c8c3aeca 46 }
TArai 0:9145c8c3aeca 47
TArai 0:9145c8c3aeca 48
TArai 0:9145c8c3aeca 49
TArai 0:9145c8c3aeca 50 void Futaba::TorqueOn (unsigned char ID){
TArai 0:9145c8c3aeca 51
TArai 0:9145c8c3aeca 52 unsigned char TxData[9]; // TransmitByteData [9byte]
TArai 0:9145c8c3aeca 53 unsigned char CheckSum = 0; // CheckSum calculation
TArai 0:9145c8c3aeca 54
TArai 0:9145c8c3aeca 55 TxData[0] = 0xFA; // Header
TArai 0:9145c8c3aeca 56 TxData[1] = 0xAF; // Header
TArai 0:9145c8c3aeca 57 TxData[2] = ID; // ID
TArai 0:9145c8c3aeca 58 TxData[3] = 0x00; // Flags
TArai 0:9145c8c3aeca 59 TxData[4] = 0x24; // Address
TArai 0:9145c8c3aeca 60 TxData[5] = 0x01; // Length
TArai 0:9145c8c3aeca 61 TxData[6] = 0x01; // Count
TArai 0:9145c8c3aeca 62 TxData[7] = 0x01; // Data
TArai 0:9145c8c3aeca 63
TArai 0:9145c8c3aeca 64 // CheckSum calculation
TArai 0:9145c8c3aeca 65 for(int i=2; i<=7; i++){
TArai 0:9145c8c3aeca 66 CheckSum = CheckSum ^ TxData[i]; // XOR from ID to Data
TArai 0:9145c8c3aeca 67 }
TArai 0:9145c8c3aeca 68
TArai 0:9145c8c3aeca 69 TxData[8] = CheckSum; // Sum
TArai 0:9145c8c3aeca 70
TArai 0:9145c8c3aeca 71 // Send Packet
TArai 0:9145c8c3aeca 72 _REDE = 1; // RS485 Transmit Enable
TArai 0:9145c8c3aeca 73 for(int i=0; i<=8; i++){
TArai 0:9145c8c3aeca 74 _device.putc(TxData[i]);
TArai 0:9145c8c3aeca 75 }
TArai 0:9145c8c3aeca 76 wait_us(800); // Wait for transmission
TArai 0:9145c8c3aeca 77 _REDE = 0; // RS485 Transmitt disable
TArai 0:9145c8c3aeca 78 }
TArai 0:9145c8c3aeca 79
TArai 0:9145c8c3aeca 80 void Futaba::TorqueOff (unsigned char ID){
TArai 0:9145c8c3aeca 81
TArai 0:9145c8c3aeca 82 unsigned char TxData[9]; // TransmitByteData [9byte]
TArai 0:9145c8c3aeca 83 unsigned char CheckSum = 0; // CheckSum calculation
TArai 0:9145c8c3aeca 84
TArai 0:9145c8c3aeca 85 TxData[0] = 0xFA; // Header
TArai 0:9145c8c3aeca 86 TxData[1] = 0xAF; // Header
TArai 0:9145c8c3aeca 87 TxData[2] = ID; // ID
TArai 0:9145c8c3aeca 88 TxData[3] = 0x00; // Flags
TArai 0:9145c8c3aeca 89 TxData[4] = 0x24; // Address
TArai 0:9145c8c3aeca 90 TxData[5] = 0x01; // Length
TArai 0:9145c8c3aeca 91 TxData[6] = 0x01; // Count
TArai 0:9145c8c3aeca 92 TxData[7] = 0x00; // Data
TArai 0:9145c8c3aeca 93
TArai 0:9145c8c3aeca 94 // CheckSum calculation
TArai 0:9145c8c3aeca 95 for(int i=2; i<=7; i++){
TArai 0:9145c8c3aeca 96 CheckSum = CheckSum ^ TxData[i]; // XOR from ID to Data
TArai 0:9145c8c3aeca 97 }
TArai 0:9145c8c3aeca 98
TArai 0:9145c8c3aeca 99 TxData[8] = CheckSum; // Sum
TArai 0:9145c8c3aeca 100
TArai 0:9145c8c3aeca 101 // Send Packet
TArai 0:9145c8c3aeca 102 _REDE = 1; // RS485 Transmit Enable
TArai 0:9145c8c3aeca 103 for(int i=0; i<=8; i++){
TArai 0:9145c8c3aeca 104 _device.putc(TxData[i]);
TArai 0:9145c8c3aeca 105 }
TArai 0:9145c8c3aeca 106 wait_us(800); // Wait for transmission
TArai 0:9145c8c3aeca 107 _REDE = 0; // RS485 Transmitt disable
TArai 0:9145c8c3aeca 108 }
TArai 0:9145c8c3aeca 109
TArai 0:9145c8c3aeca 110
TArai 0:9145c8c3aeca 111
TArai 0:9145c8c3aeca 112
TArai 0:9145c8c3aeca 113 void Futaba::SetPosition (unsigned char ID, int data){
TArai 0:9145c8c3aeca 114
TArai 0:9145c8c3aeca 115 unsigned char TxData[10]; // TransmitByteData [10byte]
TArai 0:9145c8c3aeca 116 unsigned char CheckSum = 0; // CheckSum calculation
TArai 0:9145c8c3aeca 117
TArai 0:9145c8c3aeca 118 TxData[0] = 0xFA; // Header
TArai 0:9145c8c3aeca 119 TxData[1] = 0xAF; // Header
TArai 0:9145c8c3aeca 120 TxData[2] = ID; // ID
TArai 0:9145c8c3aeca 121 TxData[3] = 0x00; // Flags
TArai 0:9145c8c3aeca 122 TxData[4] = 0x1E; // Address
TArai 0:9145c8c3aeca 123 TxData[5] = 0x02; // Length
TArai 0:9145c8c3aeca 124 TxData[6] = 0x01; // Count
TArai 0:9145c8c3aeca 125 // Data
TArai 0:9145c8c3aeca 126 TxData[7] = (unsigned char)0x00FF & data; // Low byte
TArai 0:9145c8c3aeca 127 TxData[8] = (unsigned char)0x00FF & (data >> 8); // Hi byte
TArai 0:9145c8c3aeca 128
TArai 0:9145c8c3aeca 129 // CheckSum calculation
TArai 0:9145c8c3aeca 130 for(int i=2; i<=8; i++){
TArai 0:9145c8c3aeca 131 CheckSum = CheckSum ^ TxData[i]; // XOR from ID to Data
TArai 0:9145c8c3aeca 132 }
TArai 0:9145c8c3aeca 133 TxData[9] = CheckSum; // Sum
TArai 0:9145c8c3aeca 134 // Send Packet
TArai 0:9145c8c3aeca 135 _REDE = 1; // RS485 Transmitt Enable
TArai 0:9145c8c3aeca 136 //wait_us(100000); // Wait for transmission
TArai 0:9145c8c3aeca 137 for(int i=0; i<=9; i++){
TArai 0:9145c8c3aeca 138 _device.putc(TxData[i]);
TArai 0:9145c8c3aeca 139 }
TArai 0:9145c8c3aeca 140 wait_us(800); // Wait for transmission
TArai 0:9145c8c3aeca 141 _REDE = 0; // RS485 Transmit disable
TArai 0:9145c8c3aeca 142 }
TArai 0:9145c8c3aeca 143
TArai 0:9145c8c3aeca 144
TArai 0:9145c8c3aeca 145 void Futaba::SetPosition_sx2 (unsigned char ID1, unsigned char ID2, int data1, int data2){
TArai 0:9145c8c3aeca 146
TArai 0:9145c8c3aeca 147 unsigned char TxData[14]; // TransmitByteData [10byte]
TArai 0:9145c8c3aeca 148 unsigned char CheckSum = 0; // CheckSum calculation
TArai 0:9145c8c3aeca 149
TArai 0:9145c8c3aeca 150 TxData[0] = 0xFA; // Header
TArai 0:9145c8c3aeca 151 TxData[1] = 0xAF; // Header
TArai 0:9145c8c3aeca 152 TxData[2] = 0x00; // ID long packet
TArai 0:9145c8c3aeca 153 TxData[3] = 0x00; // Flags
TArai 0:9145c8c3aeca 154 TxData[4] = 0x1E; // Address
TArai 0:9145c8c3aeca 155 TxData[5] = 0x03; // Length
TArai 0:9145c8c3aeca 156 TxData[6] = 0x03; // Count
TArai 0:9145c8c3aeca 157
TArai 0:9145c8c3aeca 158 TxData[7] = ID1; //Servo ID1
TArai 0:9145c8c3aeca 159 TxData[8] = (unsigned char)0x00FF & data1; // Low byte
TArai 0:9145c8c3aeca 160 TxData[9] = (unsigned char)0x00FF & (data1 >> 8); // Hi byte
TArai 0:9145c8c3aeca 161
TArai 0:9145c8c3aeca 162 TxData[10] = ID2; //Servo ID2
TArai 0:9145c8c3aeca 163 TxData[11] = (unsigned char)0x00FF & data2; // Low byte
TArai 0:9145c8c3aeca 164 TxData[12] = (unsigned char)0x00FF & (data2 >> 8); // Hi byte
TArai 0:9145c8c3aeca 165
TArai 0:9145c8c3aeca 166
TArai 0:9145c8c3aeca 167 // CheckSum calculation
TArai 0:9145c8c3aeca 168 for(int i=2; i<=12; i++){
TArai 0:9145c8c3aeca 169 CheckSum = CheckSum ^ TxData[i]; // XOR from ID to Data
TArai 0:9145c8c3aeca 170 }
TArai 0:9145c8c3aeca 171 TxData[13] = CheckSum; // Sum
TArai 0:9145c8c3aeca 172 // Send Packet
TArai 0:9145c8c3aeca 173 _REDE = 1; // RS485 Transmitt Enable
TArai 0:9145c8c3aeca 174
TArai 0:9145c8c3aeca 175 for(int i=0; i<=13; i++){
TArai 0:9145c8c3aeca 176 _device.putc(TxData[i]);
TArai 0:9145c8c3aeca 177 }
TArai 0:9145c8c3aeca 178 wait_us(2000); // Wait for transmission
TArai 0:9145c8c3aeca 179 _REDE = 1; // RS485 Transmit disable
TArai 0:9145c8c3aeca 180 }
TArai 0:9145c8c3aeca 181
TArai 0:9145c8c3aeca 182 void Futaba::SetPosition_sx4 (unsigned char ID1,unsigned char ID2,unsigned char ID3,unsigned char ID4, int data1, int data2, int data3, int data4){
TArai 0:9145c8c3aeca 183
TArai 0:9145c8c3aeca 184 unsigned char TxData[20]; // TransmitByteData [10byte]
TArai 0:9145c8c3aeca 185 unsigned char CheckSum = 0; // CheckSum calculation
TArai 0:9145c8c3aeca 186
TArai 0:9145c8c3aeca 187 TxData[0] = 0xFA; // Header
TArai 0:9145c8c3aeca 188 TxData[1] = 0xAF; // Header
TArai 0:9145c8c3aeca 189 TxData[2] = 0x00; // ID long packet
TArai 0:9145c8c3aeca 190 TxData[3] = 0x00; // Flags
TArai 0:9145c8c3aeca 191 TxData[4] = 0x1E; // Address
TArai 0:9145c8c3aeca 192 TxData[5] = 0x03; // Length
TArai 0:9145c8c3aeca 193 TxData[6] = 0x03; // Count
TArai 0:9145c8c3aeca 194
TArai 0:9145c8c3aeca 195 TxData[7] = ID1; //Servo ID1
TArai 0:9145c8c3aeca 196 TxData[8] = (unsigned char)0x00FF & data1; // Low byte
TArai 0:9145c8c3aeca 197 TxData[9] = (unsigned char)0x00FF & (data1 >> 8); // Hi byte
TArai 0:9145c8c3aeca 198
TArai 0:9145c8c3aeca 199 TxData[10] = ID2; //Servo ID2
TArai 0:9145c8c3aeca 200 TxData[11] = (unsigned char)0x00FF & data2; // Low byte
TArai 0:9145c8c3aeca 201 TxData[12] = (unsigned char)0x00FF & (data2 >> 8); // Hi byte
TArai 0:9145c8c3aeca 202
TArai 0:9145c8c3aeca 203 TxData[13] = ID3; //Servo ID31
TArai 0:9145c8c3aeca 204 TxData[14] = (unsigned char)0x00FF & data3; // Low byte
TArai 0:9145c8c3aeca 205 TxData[15] = (unsigned char)0x00FF & (data3 >> 8); // Hi byte
TArai 0:9145c8c3aeca 206
TArai 0:9145c8c3aeca 207 TxData[16] = ID4; //Servo ID4
TArai 0:9145c8c3aeca 208 TxData[17] = (unsigned char)0x00FF & data4; // Low byte
TArai 0:9145c8c3aeca 209 TxData[18] = (unsigned char)0x00FF & (data4 >> 8); // Hi byte
TArai 0:9145c8c3aeca 210
TArai 0:9145c8c3aeca 211 // CheckSum calculation
TArai 0:9145c8c3aeca 212 for(int i=2; i<=12; i++){
TArai 0:9145c8c3aeca 213 CheckSum = CheckSum ^ TxData[i]; // XOR from ID to Data
TArai 0:9145c8c3aeca 214 }
TArai 0:9145c8c3aeca 215 TxData[19] = CheckSum; // Sum
TArai 0:9145c8c3aeca 216 // Send Packet
TArai 0:9145c8c3aeca 217 _REDE = 1; // RS485 Transmitt Enable
TArai 0:9145c8c3aeca 218
TArai 0:9145c8c3aeca 219 for(int i=0; i<=19; i++){
TArai 0:9145c8c3aeca 220 _device.putc(TxData[i]);
TArai 0:9145c8c3aeca 221 }
TArai 0:9145c8c3aeca 222 wait_us(1700); // Wait for transmission
TArai 0:9145c8c3aeca 223 _REDE = 0; // RS485 Transmit disable
TArai 0:9145c8c3aeca 224 }
TArai 0:9145c8c3aeca 225
TArai 0:9145c8c3aeca 226
TArai 0:9145c8c3aeca 227
TArai 0:9145c8c3aeca 228 void Futaba::SetTslope (unsigned char ID, unsigned char CW, unsigned char CCW){
TArai 0:9145c8c3aeca 229
TArai 0:9145c8c3aeca 230 unsigned char TxData[10]; // TransmitByteData [9byte]
TArai 0:9145c8c3aeca 231 unsigned char CheckSum = 0; // CheckSum calculation
TArai 0:9145c8c3aeca 232
TArai 0:9145c8c3aeca 233 TxData[0] = 0xFA; // Header
TArai 0:9145c8c3aeca 234 TxData[1] = 0xAF; // Header
TArai 0:9145c8c3aeca 235 TxData[2] = ID; // ID
TArai 0:9145c8c3aeca 236 TxData[3] = 0x00; // Flags
TArai 0:9145c8c3aeca 237 TxData[4] = 0x1A; // Address
TArai 0:9145c8c3aeca 238 TxData[5] = 0x02; // Length
TArai 0:9145c8c3aeca 239 TxData[6] = 0x01; // Count
TArai 0:9145c8c3aeca 240 TxData[7] = CW; // Data
TArai 0:9145c8c3aeca 241 TxData[8] = CCW; // Data
TArai 0:9145c8c3aeca 242 // CheckSum calculation
TArai 0:9145c8c3aeca 243 for(int i=2; i<=8; i++){
TArai 0:9145c8c3aeca 244 CheckSum = CheckSum ^ TxData[i]; // XOR from ID to Data
TArai 0:9145c8c3aeca 245 }
TArai 0:9145c8c3aeca 246
TArai 0:9145c8c3aeca 247 TxData[9] = CheckSum; // Sum
TArai 0:9145c8c3aeca 248
TArai 0:9145c8c3aeca 249 // Send Packet
TArai 0:9145c8c3aeca 250 _REDE = 1; // RS485 Transmit Enable
TArai 0:9145c8c3aeca 251
TArai 0:9145c8c3aeca 252 for(int i=0; i<=9; i++){
TArai 0:9145c8c3aeca 253 _device.putc(TxData[i]);
TArai 0:9145c8c3aeca 254 }
TArai 0:9145c8c3aeca 255 wait_us(800); // Wait for transmission
TArai 0:9145c8c3aeca 256 _REDE = 0; // RS485 Transmitt disable
TArai 0:9145c8c3aeca 257 }
TArai 0:9145c8c3aeca 258
TArai 0:9145c8c3aeca 259 void Futaba::SetMargin (unsigned char ID, unsigned char CW, unsigned char CCW){
TArai 0:9145c8c3aeca 260
TArai 0:9145c8c3aeca 261 unsigned char TxData[10]; // TransmitByteData [9byte]
TArai 0:9145c8c3aeca 262 unsigned char CheckSum = 0; // CheckSum calculation
TArai 0:9145c8c3aeca 263
TArai 0:9145c8c3aeca 264 TxData[0] = 0xFA; // Header
TArai 0:9145c8c3aeca 265 TxData[1] = 0xAF; // Header
TArai 0:9145c8c3aeca 266 TxData[2] = ID; // ID
TArai 0:9145c8c3aeca 267 TxData[3] = 0x00; // Flags
TArai 0:9145c8c3aeca 268 TxData[4] = 0x18; // Address
TArai 0:9145c8c3aeca 269 TxData[5] = 0x02; // Length
TArai 0:9145c8c3aeca 270 TxData[6] = 0x01; // Count
TArai 0:9145c8c3aeca 271 TxData[7] = CW; // Data
TArai 0:9145c8c3aeca 272 TxData[8] = CCW; // Data
TArai 0:9145c8c3aeca 273 // CheckSum calculation
TArai 0:9145c8c3aeca 274 for(int i=2; i<=8; i++){
TArai 0:9145c8c3aeca 275 CheckSum = CheckSum ^ TxData[i]; // XOR from ID to Data
TArai 0:9145c8c3aeca 276 }
TArai 0:9145c8c3aeca 277
TArai 0:9145c8c3aeca 278 TxData[9] = CheckSum; // Sum
TArai 0:9145c8c3aeca 279
TArai 0:9145c8c3aeca 280 // Send Packet
TArai 0:9145c8c3aeca 281 _REDE = 1; // RS485 Transmit Enable
TArai 0:9145c8c3aeca 282
TArai 0:9145c8c3aeca 283 for(int i=0; i<=9; i++){
TArai 0:9145c8c3aeca 284 _device.putc(TxData[i]);
TArai 0:9145c8c3aeca 285 }
TArai 0:9145c8c3aeca 286 wait_us(800); // Wait for transmission
TArai 0:9145c8c3aeca 287 _REDE = 0; // RS485 Transmitt disable
TArai 0:9145c8c3aeca 288 }
TArai 0:9145c8c3aeca 289
TArai 0:9145c8c3aeca 290
TArai 0:9145c8c3aeca 291
TArai 0:9145c8c3aeca 292 /*-------------------------------------------------*/
TArai 0:9145c8c3aeca 293 /* 機能 : リターンパケット要求 */
TArai 0:9145c8c3aeca 294 /* 名前 : RequestReply */
TArai 0:9145c8c3aeca 295 /* 引数 : ID (Servo ID) */
TArai 0:9145c8c3aeca 296 /* : address (指定アドレス) */
TArai 0:9145c8c3aeca 297 /* : Lenge (指定バイト数) */
TArai 0:9145c8c3aeca 298 /* 戻り値 : 無し */
TArai 0:9145c8c3aeca 299 /*-------------------------------------------------*/
TArai 0:9145c8c3aeca 300 void Futaba::RequestReply (unsigned char ID, unsigned char address, unsigned char Lenge){
TArai 0:9145c8c3aeca 301 unsigned char TxData[8]; // 送信データバッファ [8byte]
TArai 0:9145c8c3aeca 302 unsigned char CheckSum = 0; // チェックサム計算用変数
TArai 0:9145c8c3aeca 303
TArai 0:9145c8c3aeca 304 TxData[0] = 0xFA; // Header
TArai 0:9145c8c3aeca 305 TxData[1] = 0xAF; // Header
TArai 0:9145c8c3aeca 306 TxData[2] = ID; // ID
TArai 0:9145c8c3aeca 307 TxData[3] = 0x0F; // Flags
TArai 0:9145c8c3aeca 308 TxData[4] = address; // Address
TArai 0:9145c8c3aeca 309 TxData[5] = Lenge; // Length
TArai 0:9145c8c3aeca 310 TxData[6] = 0x00; // Count
TArai 0:9145c8c3aeca 311 // チェックサム計算
TArai 0:9145c8c3aeca 312 for(int i=2; i<=6; i++){
TArai 0:9145c8c3aeca 313 CheckSum = CheckSum ^ TxData[i]; // ID～DATAまでのXOR
TArai 0:9145c8c3aeca 314 }
TArai 0:9145c8c3aeca 315 TxData[7] = CheckSum; // Sum
TArai 0:9145c8c3aeca 316
TArai 0:9145c8c3aeca 317 // パケットデータ送信
TArai 0:9145c8c3aeca 318 _REDE = 1; // RS485 送信許可
TArai 0:9145c8c3aeca 319 for(int i=0; i<=7; i++){
TArai 0:9145c8c3aeca 320 _device.putc(TxData[i]);
TArai 0:9145c8c3aeca 321 }
TArai 0:9145c8c3aeca 322 wait_us(700); // Wait for transmission
TArai 0:9145c8c3aeca 323 _REDE = 0; // RS485 Transmitt disable 受信待ち
TArai 0:9145c8c3aeca 324 }
TArai 0:9145c8c3aeca 325
TArai 0:9145c8c3aeca 326
TArai 0:9145c8c3aeca 327
TArai 0:9145c8c3aeca 328 /*----------------------------------------------------*/
TArai 0:9145c8c3aeca 329 /* 機能 : リターンパケット受信待ち(2byte専用) */
TArai 0:9145c8c3aeca 330 /* 名前 : WaitReply */
TArai 0:9145c8c3aeca 331 /* 引数 : 無し */
TArai 0:9145c8c3aeca 332 /* 戻り値 : 受信完了(0xAB) */
TArai 0:9145c8c3aeca 333 /* : タイムアウト(0xCD) */
TArai 0:9145c8c3aeca 334 /*----------------------------------------------------*/
TArai 0:9145c8c3aeca 335 unsigned char Futaba::WaitReply (void){
TArai 0:9145c8c3aeca 336 unsigned char result = 0xAB; // 戻り値一時格納
TArai 0:9145c8c3aeca 337 int TimeOut = 0; // リターンパケット受信タイムアウト
TArai 0:9145c8c3aeca 338
TArai 0:9145c8c3aeca 339 // ヘッダー上位バイト(0xFD)待ち
TArai 0:9145c8c3aeca 340 while(0xFD != _device.getc()){
TArai 0:9145c8c3aeca 341 if(TimeOut < 300){
TArai 0:9145c8c3aeca 342 TimeOut++;
TArai 0:9145c8c3aeca 343 }else{
TArai 0:9145c8c3aeca 344 result = 0xCD;
TArai 0:9145c8c3aeca 345 break;
TArai 0:9145c8c3aeca 346 }
TArai 0:9145c8c3aeca 347 }
TArai 0:9145c8c3aeca 348 RxData[0] = 0xFD;
TArai 0:9145c8c3aeca 349
TArai 0:9145c8c3aeca 350 // ヘッダー下位バイト(0xDF)待ち
TArai 0:9145c8c3aeca 351 while(0xDF != _device.getc()){
TArai 0:9145c8c3aeca 352 if(TimeOut < 300){
TArai 0:9145c8c3aeca 353 TimeOut++;
TArai 0:9145c8c3aeca 354 }else{
TArai 0:9145c8c3aeca 355 result = 0xCD;
TArai 0:9145c8c3aeca 356 break;
TArai 0:9145c8c3aeca 357 }
TArai 0:9145c8c3aeca 358 }
TArai 0:9145c8c3aeca 359 RxData[1] = 0xDF;
TArai 0:9145c8c3aeca 360
TArai 0:9145c8c3aeca 361 // 残データ待ち
TArai 0:9145c8c3aeca 362 //while(Serial.available() < 8){
TArai 0:9145c8c3aeca 363 while( _device.readable()){
TArai 0:9145c8c3aeca 364 if(TimeOut < 300){
TArai 0:9145c8c3aeca 365 TimeOut++;
TArai 0:9145c8c3aeca 366 }else{
TArai 0:9145c8c3aeca 367 result = 0xCD;
TArai 0:9145c8c3aeca 368 break;
TArai 0:9145c8c3aeca 369 }
TArai 0:9145c8c3aeca 370 }
TArai 0:9145c8c3aeca 371
TArai 0:9145c8c3aeca 372 // 受信バッファからデータ取り出し
TArai 0:9145c8c3aeca 373 for(int i=2; i<10; i++){
TArai 0:9145c8c3aeca 374 RxData[i] = _device.getc();
TArai 0:9145c8c3aeca 375 }
TArai 0:9145c8c3aeca 376
TArai 0:9145c8c3aeca 377 return result; // 受信データを戻す
TArai 0:9145c8c3aeca 378 }
TArai 0:9145c8c3aeca 379
TArai 0:9145c8c3aeca 380
TArai 0:9145c8c3aeca 381
TArai 0:9145c8c3aeca 382 /*----------------------------------------------------*/
TArai 0:9145c8c3aeca 383 /* 機能 : リターンパケット解析(2byte専用) */
TArai 0:9145c8c3aeca 384 /* 名前 : AnalysisReply */
TArai 0:9145c8c3aeca 385 /* 引数 : 無し */
TArai 0:9145c8c3aeca 386 /* 戻り値 : 受信データ内容 */
TArai 0:9145c8c3aeca 387 /* ｺﾒﾝﾄ : ヘッダー・チェックサム等異常があった場合、*/
TArai 0:9145c8c3aeca 388 /* resultを「3600」とする */
TArai 0:9145c8c3aeca 389 /*----------------------------------------------------*/
TArai 0:9145c8c3aeca 390 int Futaba::AnalysisReply (void){
TArai 0:9145c8c3aeca 391 unsigned char CheckSum = 0; // チェックサム計算用変数
TArai 0:9145c8c3aeca 392 int result; // 受信データ内容格納
TArai 0:9145c8c3aeca 393 int ErrorFlag;
TArai 0:9145c8c3aeca 394
TArai 0:9145c8c3aeca 395 // ヘッダ、チェックサム確認
TArai 0:9145c8c3aeca 396 if(RxData[0] == 0xFD){
TArai 0:9145c8c3aeca 397 if(RxData[1] == 0xDF){
TArai 0:9145c8c3aeca 398 // チェックサム計算
TArai 0:9145c8c3aeca 399 for(int i=2; i<=8; i++){
TArai 0:9145c8c3aeca 400 CheckSum = CheckSum ^ RxData[i]; // ID～DATAまでのXOR
TArai 0:9145c8c3aeca 401 }
TArai 0:9145c8c3aeca 402 if(CheckSum == RxData[9]){
TArai 0:9145c8c3aeca 403 result = RxData[8];
TArai 0:9145c8c3aeca 404 result = result << 8;
TArai 0:9145c8c3aeca 405 result |= RxData[7];
TArai 0:9145c8c3aeca 406 }else{
TArai 0:9145c8c3aeca 407 ErrorFlag = true; // チェックサムが計算結果と異なる
TArai 0:9145c8c3aeca 408 }
TArai 0:9145c8c3aeca 409 CheckSum = 0;
TArai 0:9145c8c3aeca 410 }else{
TArai 0:9145c8c3aeca 411 ErrorFlag = true; // ヘッダー2byte目が"0xDF"で無い
TArai 0:9145c8c3aeca 412 }
TArai 0:9145c8c3aeca 413 }else{
TArai 0:9145c8c3aeca 414 ErrorFlag = true; // ヘッダー1byte目が"0xFD"で無い
TArai 0:9145c8c3aeca 415 }
TArai 0:9145c8c3aeca 416 if(ErrorFlag == true) result=3600;
TArai 0:9145c8c3aeca 417 return result; // 受信データを戻す
TArai 0:9145c8c3aeca 418 }
TArai 0:9145c8c3aeca 419
TArai 0:9145c8c3aeca 420
TArai 0:9145c8c3aeca 421 /*----------------------------------------------------*/
TArai 0:9145c8c3aeca 422 /* 機能 : 角度データ取得(2byte専用) */
TArai 0:9145c8c3aeca 423 /* 名前 : GetAngle */
TArai 0:9145c8c3aeca 424 /* 引数 : ID */
TArai 0:9145c8c3aeca 425 /* 戻り値 : 角度データ 0x0000~FFFF */
TArai 0:9145c8c3aeca 426 /* ｺﾒﾝﾄ : 不感帯 : Angle=0x5555 */
TArai 0:9145c8c3aeca 427 /* 受信エラー : Angle=0x6666 */
TArai 0:9145c8c3aeca 428 /*----------------------------------------------------*/
TArai 0:9145c8c3aeca 429 int Futaba::GetAngle(unsigned char ID){
TArai 0:9145c8c3aeca 430 int result; // 受信データ内容格納
TArai 0:9145c8c3aeca 431 int ErrorFlag = 0;
TArai 0:9145c8c3aeca 432 int Angle;
TArai 0:9145c8c3aeca 433
TArai 0:9145c8c3aeca 434 Futaba::RequestReply(ID,0x2A,0x02); // ID = 1, address = 42 (2A)(現在位置), Lenge = 2(2byte)
TArai 0:9145c8c3aeca 435
TArai 0:9145c8c3aeca 436 // リターンパケット受信待ち
TArai 0:9145c8c3aeca 437 result = Futaba::WaitReply();
TArai 0:9145c8c3aeca 438 if(result == 0xCD){
TArai 0:9145c8c3aeca 439 ErrorFlag = true; // リターンパケット受信エラー
TArai 0:9145c8c3aeca 440 }
TArai 0:9145c8c3aeca 441
TArai 0:9145c8c3aeca 442 Angle = int(AnalysisReply());
TArai 0:9145c8c3aeca 443 /*
TArai 0:9145c8c3aeca 444 if(Angle==5555){
TArai 0:9145c8c3aeca 445 ;
TArai 0:9145c8c3aeca 446 }else if(Angle>1800){
TArai 0:9145c8c3aeca 447 Angle = Angle - 0xFFFF;
TArai 0:9145c8c3aeca 448 }
TArai 0:9145c8c3aeca 449 */
TArai 0:9145c8c3aeca 450 if (ErrorFlag == true){
TArai 0:9145c8c3aeca 451 Angle = 6666;
TArai 0:9145c8c3aeca 452 }
TArai 0:9145c8c3aeca 453
TArai 0:9145c8c3aeca 454 return Angle; // 受信データを戻す
TArai 0:9145c8c3aeca 455 }