pierre Rousselin / Mbed 2 deprecated AX12

a

Dependencies:   mbed

Fork of AX12 by Chris Styles

AX12.cpp@4:a7c95f3012db, 2017-10-18 (annotated)

Committer:
ROUSSELIN
Date:
Wed Oct 18 11:58:25 2017 +0000
Revision:
4:a7c95f3012db
Parent:
3:ced71d1b2558 
For Apollo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ROUSSELIN 4:a7c95f3012db 1 #include "AX12.h"
ROUSSELIN 4:a7c95f3012db 2
chris 0:be51952765ec 3
ROUSSELIN 4:a7c95f3012db 4 AX12::AX12(int tx, int rx, int ID, int baud)
chris 0:be51952765ec 5 : _ax12(tx,rx) {
ROUSSELIN 4:a7c95f3012db 6 _baud = baud;
ROUSSELIN 4:a7c95f3012db 7 _ID = ID;
ROUSSELIN 4:a7c95f3012db 8 int _ax12.baud(_baud);
chris 0:be51952765ec 9
chris 0:be51952765ec 10 }
chris 0:be51952765ec 11
chris 1:93ad80f5fde7 12 // Set the mode of the servo
chris 1:93ad80f5fde7 13 // 0 = Positional (0-300 degrees)
chris 1:93ad80f5fde7 14 // 1 = Rotational -1 to 1 speed
chris 1:93ad80f5fde7 15 int AX12::SetMode(int mode) {
chris 1:93ad80f5fde7 16
chris 1:93ad80f5fde7 17 if (mode == 1) { // set CR
chris 1:93ad80f5fde7 18 SetCWLimit(0);
chris 1:93ad80f5fde7 19 SetCCWLimit(0);
chris 1:93ad80f5fde7 20 SetCRSpeed(0.0);
chris 1:93ad80f5fde7 21 } else {
chris 1:93ad80f5fde7 22 SetCWLimit(0);
chris 1:93ad80f5fde7 23 SetCCWLimit(300);
chris 1:93ad80f5fde7 24 SetCRSpeed(0.0);
chris 1:93ad80f5fde7 25 }
chris 1:93ad80f5fde7 26 return(0);
chris 1:93ad80f5fde7 27 }
chris 1:93ad80f5fde7 28
chris 1:93ad80f5fde7 29
chris 1:93ad80f5fde7 30 // if flag[0] is set, were blocking
chris 1:93ad80f5fde7 31 // if flag[1] is set, we're registering
chris 1:93ad80f5fde7 32 // they are mutually exclusive operations
chris 1:93ad80f5fde7 33 int AX12::SetGoal(int degrees, int flags) {
chris 0:be51952765ec 34
chris 1:93ad80f5fde7 35 char reg_flag = 0;
chris 1:93ad80f5fde7 36 char data[2];
chris 1:93ad80f5fde7 37
chris 1:93ad80f5fde7 38 // set the flag is only the register bit is set in the flag
chris 1:93ad80f5fde7 39 if (flags == 0x2) {
chris 1:93ad80f5fde7 40 reg_flag = 1;
chris 1:93ad80f5fde7 41 }
chris 1:93ad80f5fde7 42
chris 1:93ad80f5fde7 43 // 1023 / 300 * degrees
chris 1:93ad80f5fde7 44 short goal = (1023 * degrees) / 300;
chris 2:5ea99c37a2d7 45 #ifdef AX12_DEBUG
chris 2:5ea99c37a2d7 46 printf("SetGoal to 0x%x\n",goal);
chris 2:5ea99c37a2d7 47 #endif
chris 1:93ad80f5fde7 48
chris 1:93ad80f5fde7 49 data[0] = goal & 0xff; // bottom 8 bits
chris 1:93ad80f5fde7 50 data[1] = goal >> 8; // top 8 bits
chris 1:93ad80f5fde7 51
chris 1:93ad80f5fde7 52 // write the packet, return the error code
chris 1:93ad80f5fde7 53 int rVal = write(_ID, AX12_REG_GOAL_POSITION, 2, data, reg_flag);
chris 1:93ad80f5fde7 54
chris 1:93ad80f5fde7 55 if (flags == 1) {
chris 1:93ad80f5fde7 56 // block until it comes to a halt
chris 1:93ad80f5fde7 57 while (isMoving()) {}
chris 1:93ad80f5fde7 58 }
chris 1:93ad80f5fde7 59 return(rVal);
chris 0:be51952765ec 60 }
chris 0:be51952765ec 61
chris 1:93ad80f5fde7 62
chris 0:be51952765ec 63 // Set continuous rotation speed from -1 to 1
chris 0:be51952765ec 64 int AX12::SetCRSpeed(float speed) {
chris 0:be51952765ec 65
chris 0:be51952765ec 66 // bit 10 = direction, 0 = CCW, 1=CW
chris 0:be51952765ec 67 // bits 9-0 = Speed
chris 0:be51952765ec 68 char data[2];
chris 0:be51952765ec 69
chris 0:be51952765ec 70 int goal = (0x3ff * abs(speed));
chris 0:be51952765ec 71
chris 0:be51952765ec 72 // Set direction CW if we have a negative speed
chris 0:be51952765ec 73 if (speed < 0) {
chris 0:be51952765ec 74 goal |= (0x1 << 10);
chris 0:be51952765ec 75 }
chris 0:be51952765ec 76
chris 0:be51952765ec 77 data[0] = goal & 0xff; // bottom 8 bits
chris 0:be51952765ec 78 data[1] = goal >> 8; // top 8 bits
chris 0:be51952765ec 79
chris 0:be51952765ec 80 // write the packet, return the error code
chris 0:be51952765ec 81 int rVal = write(_ID, 0x20, 2, data);
chris 0:be51952765ec 82
chris 0:be51952765ec 83 return(rVal);
chris 0:be51952765ec 84 }
chris 0:be51952765ec 85
chris 1:93ad80f5fde7 86
chris 1:93ad80f5fde7 87 int AX12::SetCWLimit (int degrees) {
chris 1:93ad80f5fde7 88
chris 1:93ad80f5fde7 89 char data[2];
chris 2:5ea99c37a2d7 90
chris 1:93ad80f5fde7 91 // 1023 / 300 * degrees
chris 1:93ad80f5fde7 92 short limit = (1023 * degrees) / 300;
chris 0:be51952765ec 93
chris 2:5ea99c37a2d7 94 #ifdef AX12_DEBUG
chris 2:5ea99c37a2d7 95 printf("SetCWLimit to 0x%x\n",limit);
chris 2:5ea99c37a2d7 96 #endif
chris 1:93ad80f5fde7 97
chris 1:93ad80f5fde7 98 data[0] = limit & 0xff; // bottom 8 bits
chris 1:93ad80f5fde7 99 data[1] = limit >> 8; // top 8 bits
chris 1:93ad80f5fde7 100
chris 1:93ad80f5fde7 101 // write the packet, return the error code
chris 1:93ad80f5fde7 102 return (write(_ID, AX12_REG_CW_LIMIT, 2, data));
chris 1:93ad80f5fde7 103
chris 0:be51952765ec 104 }
chris 0:be51952765ec 105
chris 1:93ad80f5fde7 106 int AX12::SetCCWLimit (int degrees) {
chris 1:93ad80f5fde7 107
chris 1:93ad80f5fde7 108 char data[2];
chris 1:93ad80f5fde7 109
chris 1:93ad80f5fde7 110 // 1023 / 300 * degrees
chris 1:93ad80f5fde7 111 short limit = (1023 * degrees) / 300;
chris 0:be51952765ec 112
chris 2:5ea99c37a2d7 113 #ifdef AX12_DEBUG
chris 2:5ea99c37a2d7 114 printf("SetCCWLimit to 0x%x\n",limit);
chris 2:5ea99c37a2d7 115 #endif
chris 1:93ad80f5fde7 116
chris 1:93ad80f5fde7 117 data[0] = limit & 0xff; // bottom 8 bits
chris 1:93ad80f5fde7 118 data[1] = limit >> 8; // top 8 bits
chris 1:93ad80f5fde7 119
chris 1:93ad80f5fde7 120 // write the packet, return the error code
chris 1:93ad80f5fde7 121 return (write(_ID, AX12_REG_CCW_LIMIT, 2, data));
chris 0:be51952765ec 122 }
chris 0:be51952765ec 123
chris 1:93ad80f5fde7 124
chris 1:93ad80f5fde7 125 int AX12::SetID (int CurrentID, int NewID) {
chris 1:93ad80f5fde7 126
chris 0:be51952765ec 127 char data[1];
chris 1:93ad80f5fde7 128 data[0] = NewID;
chris 2:5ea99c37a2d7 129
chris 2:5ea99c37a2d7 130 #ifdef AX12_DEBUG
chris 2:5ea99c37a2d7 131 printf("Setting ID from 0x%x to 0x%x\n",CurrentID,NewID);
chris 2:5ea99c37a2d7 132 #endif
chris 2:5ea99c37a2d7 133
chris 1:93ad80f5fde7 134 return (write(CurrentID, AX12_REG_ID, 1, data));
chris 1:93ad80f5fde7 135
chris 0:be51952765ec 136 }
chris 0:be51952765ec 137
chris 1:93ad80f5fde7 138
chris 3:ced71d1b2558 139 int AX12::SetBaud (int baud) {
chris 3:ced71d1b2558 140
chris 3:ced71d1b2558 141 char data[1];
chris 3:ced71d1b2558 142 data[0] = baud;
chris 3:ced71d1b2558 143
chris 3:ced71d1b2558 144 #ifdef AX12_DEBUG
chris 3:ced71d1b2558 145 printf("Setting Baud rate to %d\n",baud);
chris 3:ced71d1b2558 146 #endif
chris 3:ced71d1b2558 147
chris 3:ced71d1b2558 148 return (write(0xFE, AX12_REG_BAUD, 1, data));
chris 3:ced71d1b2558 149
chris 3:ced71d1b2558 150 }
chris 3:ced71d1b2558 151
chris 3:ced71d1b2558 152
chris 3:ced71d1b2558 153
chris 1:93ad80f5fde7 154 // return 1 is the servo is still in flight
chris 1:93ad80f5fde7 155 int AX12::isMoving(void) {
chris 1:93ad80f5fde7 156
chris 1:93ad80f5fde7 157 char data[1];
chris 1:93ad80f5fde7 158 read(_ID,AX12_REG_MOVING,1,data);
chris 1:93ad80f5fde7 159 return(data[0]);
chris 1:93ad80f5fde7 160 }
chris 1:93ad80f5fde7 161
chris 1:93ad80f5fde7 162
chris 0:be51952765ec 163 void AX12::trigger(void) {
chris 0:be51952765ec 164
chris 0:be51952765ec 165 char TxBuf[16];
chris 0:be51952765ec 166 char sum = 0;
chris 0:be51952765ec 167
chris 2:5ea99c37a2d7 168 #ifdef AX12_TRIGGER_DEBUG
chris 0:be51952765ec 169 // Build the TxPacket first in RAM, then we'll send in one go
chris 2:5ea99c37a2d7 170 printf("\nTriggered\n");
chris 2:5ea99c37a2d7 171 printf("\nTrigger Packet\n Header : 0xFF, 0xFF\n");
chris 2:5ea99c37a2d7 172 #endif
chris 0:be51952765ec 173
chris 0:be51952765ec 174 TxBuf[0] = 0xFF;
chris 0:be51952765ec 175 TxBuf[1] = 0xFF;
chris 0:be51952765ec 176
chris 0:be51952765ec 177 // ID - Broadcast
chris 0:be51952765ec 178 TxBuf[2] = 0xFE;
chris 0:be51952765ec 179 sum += TxBuf[2];
chris 0:be51952765ec 180
chris 2:5ea99c37a2d7 181 #ifdef AX12_TRIGGER_DEBUG
chris 2:5ea99c37a2d7 182 printf(" ID : %d\n",TxBuf[2]);
chris 2:5ea99c37a2d7 183 #endif
chris 0:be51952765ec 184
chris 0:be51952765ec 185 // Length
chris 0:be51952765ec 186 TxBuf[3] = 0x02;
chris 0:be51952765ec 187 sum += TxBuf[3];
chris 2:5ea99c37a2d7 188
chris 2:5ea99c37a2d7 189 #ifdef AX12_TRIGGER_DEBUG
chris 2:5ea99c37a2d7 190 printf(" Length %d\n",TxBuf[3]);
chris 2:5ea99c37a2d7 191 #endif
chris 0:be51952765ec 192
chris 0:be51952765ec 193 // Instruction - ACTION
chris 0:be51952765ec 194 TxBuf[4] = 0x04;
chris 0:be51952765ec 195 sum += TxBuf[4];
chris 2:5ea99c37a2d7 196
chris 2:5ea99c37a2d7 197 #ifdef AX12_TRIGGER_DEBUG
chris 2:5ea99c37a2d7 198 printf(" Instruction 0x%X\n",TxBuf[5]);
chris 2:5ea99c37a2d7 199 #endif
chris 0:be51952765ec 200
chris 0:be51952765ec 201 // Checksum
chris 0:be51952765ec 202 TxBuf[5] = 0xFF - sum;
chris 2:5ea99c37a2d7 203 #ifdef AX12_TRIGGER_DEBUG
chris 2:5ea99c37a2d7 204 printf(" Checksum 0x%X\n",TxBuf[5]);
chris 2:5ea99c37a2d7 205 #endif
chris 0:be51952765ec 206
chris 0:be51952765ec 207 // Transmit the packet in one burst with no pausing
chris 0:be51952765ec 208 for (int i = 0; i < 6 ; i++) {
chris 0:be51952765ec 209 _ax12.putc(TxBuf[i]);
chris 0:be51952765ec 210 }
chris 1:93ad80f5fde7 211
chris 0:be51952765ec 212 // This is a broadcast packet, so there will be no reply
chris 0:be51952765ec 213 return;
chris 0:be51952765ec 214 }
chris 0:be51952765ec 215
chris 0:be51952765ec 216
chris 0:be51952765ec 217 float AX12::GetPosition(void) {
chris 0:be51952765ec 218
chris 2:5ea99c37a2d7 219 #ifdef AX12_DEBUG
chris 2:5ea99c37a2d7 220 printf("\nGetPosition(%d)",_ID);
chris 2:5ea99c37a2d7 221 #endif
chris 0:be51952765ec 222
chris 0:be51952765ec 223 char data[2];
chris 0:be51952765ec 224
chris 0:be51952765ec 225 int ErrorCode = read(_ID, AX12_REG_POSITION, 2, data);
chris 0:be51952765ec 226 short position = data[0] + (data[1] << 8);
chris 0:be51952765ec 227 float angle = (position * 300)/1024;
chris 0:be51952765ec 228
chris 0:be51952765ec 229 return (angle);
chris 0:be51952765ec 230 }
chris 0:be51952765ec 231
chris 0:be51952765ec 232
chris 1:93ad80f5fde7 233 float AX12::GetTemp (void) {
chris 0:be51952765ec 234
chris 2:5ea99c37a2d7 235 #ifdef AX12_DEBUG
chris 2:5ea99c37a2d7 236 printf("\nGetTemp(%d)",_ID);
chris 2:5ea99c37a2d7 237 #endif
chris 2:5ea99c37a2d7 238
chris 1:93ad80f5fde7 239 char data[1];
chris 1:93ad80f5fde7 240 int ErrorCode = read(_ID, AX12_REG_TEMP, 1, data);
chris 1:93ad80f5fde7 241 float temp = data[0];
chris 1:93ad80f5fde7 242 return(temp);
chris 0:be51952765ec 243 }
chris 0:be51952765ec 244
chris 0:be51952765ec 245
chris 1:93ad80f5fde7 246 float AX12::GetVolts (void) {
chris 2:5ea99c37a2d7 247
chris 2:5ea99c37a2d7 248 #ifdef AX12_DEBUG
chris 2:5ea99c37a2d7 249 printf("\nGetVolts(%d)",_ID);
chris 2:5ea99c37a2d7 250 #endif
chris 2:5ea99c37a2d7 251
chris 0:be51952765ec 252 char data[1];
chris 1:93ad80f5fde7 253 int ErrorCode = read(_ID, AX12_REG_VOLTS, 1, data);
chris 1:93ad80f5fde7 254 float volts = data[0]/10.0;
chris 1:93ad80f5fde7 255 return(volts);
chris 0:be51952765ec 256 }
chris 0:be51952765ec 257
chris 0:be51952765ec 258
chris 0:be51952765ec 259 int AX12::read(int ID, int start, int bytes, char* data) {
chris 0:be51952765ec 260
chris 0:be51952765ec 261 char PacketLength = 0x4;
chris 0:be51952765ec 262 char TxBuf[16];
chris 0:be51952765ec 263 char sum = 0;
chris 0:be51952765ec 264 char Status[16];
chris 0:be51952765ec 265
chris 0:be51952765ec 266 Status[4] = 0xFE; // return code
chris 0:be51952765ec 267
chris 2:5ea99c37a2d7 268 #ifdef AX12_READ_DEBUG
chris 2:5ea99c37a2d7 269 printf("\nread(%d,0x%x,%d,data)\n",ID,start,bytes);
chris 2:5ea99c37a2d7 270 #endif
chris 0:be51952765ec 271
chris 0:be51952765ec 272 // Build the TxPacket first in RAM, then we'll send in one go
chris 2:5ea99c37a2d7 273 #ifdef AX12_READ_DEBUG
chris 2:5ea99c37a2d7 274 printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n");
chris 2:5ea99c37a2d7 275 #endif
chris 0:be51952765ec 276
chris 0:be51952765ec 277 TxBuf[0] = 0xff;
chris 0:be51952765ec 278 TxBuf[1] = 0xff;
chris 0:be51952765ec 279
chris 0:be51952765ec 280 // ID
chris 0:be51952765ec 281 TxBuf[2] = ID;
chris 0:be51952765ec 282 sum += TxBuf[2];
chris 2:5ea99c37a2d7 283
chris 2:5ea99c37a2d7 284 #ifdef AX12_READ_DEBUG
chris 2:5ea99c37a2d7 285 printf(" ID : %d\n",TxBuf[2]);
chris 2:5ea99c37a2d7 286 #endif
chris 0:be51952765ec 287
chris 0:be51952765ec 288 // Packet Length
chris 0:be51952765ec 289 TxBuf[3] = PacketLength; // Length = 4 ; 2 + 1 (start) = 1 (bytes)
chris 0:be51952765ec 290 sum += TxBuf[3]; // Accululate the packet sum
chris 2:5ea99c37a2d7 291
chris 2:5ea99c37a2d7 292 #ifdef AX12_READ_DEBUG
chris 2:5ea99c37a2d7 293 printf(" Length : 0x%x\n",TxBuf[3]);
chris 2:5ea99c37a2d7 294 #endif
chris 0:be51952765ec 295
chris 0:be51952765ec 296 // Instruction - Read
chris 0:be51952765ec 297 TxBuf[4] = 0x2;
chris 0:be51952765ec 298 sum += TxBuf[4];
chris 2:5ea99c37a2d7 299
chris 2:5ea99c37a2d7 300 #ifdef AX12_READ_DEBUG
chris 2:5ea99c37a2d7 301 printf(" Instruction : 0x%x\n",TxBuf[4]);
chris 2:5ea99c37a2d7 302 #endif
chris 0:be51952765ec 303
chris 0:be51952765ec 304 // Start Address
chris 0:be51952765ec 305 TxBuf[5] = start;
chris 0:be51952765ec 306 sum += TxBuf[5];
chris 2:5ea99c37a2d7 307
chris 2:5ea99c37a2d7 308 #ifdef AX12_READ_DEBUG
chris 2:5ea99c37a2d7 309 printf(" Start Address : 0x%x\n",TxBuf[5]);
chris 2:5ea99c37a2d7 310 #endif
chris 0:be51952765ec 311
chris 0:be51952765ec 312 // Bytes to read
chris 0:be51952765ec 313 TxBuf[6] = bytes;
chris 0:be51952765ec 314 sum += TxBuf[6];
chris 2:5ea99c37a2d7 315
chris 2:5ea99c37a2d7 316 #ifdef AX12_READ_DEBUG
chris 2:5ea99c37a2d7 317 printf(" No bytes : 0x%x\n",TxBuf[6]);
chris 2:5ea99c37a2d7 318 #endif
chris 0:be51952765ec 319
chris 0:be51952765ec 320 // Checksum
chris 0:be51952765ec 321 TxBuf[7] = 0xFF - sum;
chris 2:5ea99c37a2d7 322 #ifdef AX12_READ_DEBUG
chris 2:5ea99c37a2d7 323 printf(" Checksum : 0x%x\n",TxBuf[7]);
chris 2:5ea99c37a2d7 324 #endif
chris 0:be51952765ec 325
chris 0:be51952765ec 326 // Transmit the packet in one burst with no pausing
chris 0:be51952765ec 327 for (int i = 0; i<8 ; i++) {
chris 0:be51952765ec 328 _ax12.putc(TxBuf[i]);
chris 0:be51952765ec 329 }
chris 0:be51952765ec 330
chris 0:be51952765ec 331 // Skip if the read was to the broadcast address
chris 0:be51952765ec 332 if (_ID != 0xFE) {
chris 0:be51952765ec 333
chris 2:5ea99c37a2d7 334
chris 2:5ea99c37a2d7 335
chris 2:5ea99c37a2d7 336 // response packet is always 6 + bytes
chris 2:5ea99c37a2d7 337 // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum
chris 2:5ea99c37a2d7 338 // timeout is a little more than the time to transmit
chris 2:5ea99c37a2d7 339 // the packet back, i.e. (6+bytes)*10 bit periods
chris 2:5ea99c37a2d7 340
chris 2:5ea99c37a2d7 341 int timeout = 0;
chris 2:5ea99c37a2d7 342 int plen = 0;
chris 2:5ea99c37a2d7 343 while ((timeout < ((6+bytes)*10)) && (plen<(6+bytes))) {
chris 2:5ea99c37a2d7 344
chris 2:5ea99c37a2d7 345 if (_ax12.readable()) {
chris 2:5ea99c37a2d7 346 Status[plen] = _ax12.getc();
chris 2:5ea99c37a2d7 347 plen++;
chris 2:5ea99c37a2d7 348 timeout = 0;
chris 2:5ea99c37a2d7 349 }
chris 2:5ea99c37a2d7 350 timeout++;
chris 2:5ea99c37a2d7 351 }
chris 2:5ea99c37a2d7 352
chris 2:5ea99c37a2d7 353 if (timeout == ((6+bytes)*10) ) {
chris 2:5ea99c37a2d7 354 return(-1);
chris 0:be51952765ec 355 }
chris 0:be51952765ec 356
chris 0:be51952765ec 357 // Copy the data from Status into data for return
chris 0:be51952765ec 358 for (int i=0; i < Status[3]-2 ; i++) {
chris 0:be51952765ec 359 data[i] = Status[5+i];
chris 0:be51952765ec 360 }
chris 0:be51952765ec 361
chris 2:5ea99c37a2d7 362 #ifdef AX12_READ_DEBUG
chris 2:5ea99c37a2d7 363 printf("\nStatus Packet\n");
chris 2:5ea99c37a2d7 364 printf(" Header : 0x%x\n",Status[0]);
chris 2:5ea99c37a2d7 365 printf(" Header : 0x%x\n",Status[1]);
chris 2:5ea99c37a2d7 366 printf(" ID : 0x%x\n",Status[2]);
chris 2:5ea99c37a2d7 367 printf(" Length : 0x%x\n",Status[3]);
chris 2:5ea99c37a2d7 368 printf(" Error Code : 0x%x\n",Status[4]);
chris 0:be51952765ec 369
chris 2:5ea99c37a2d7 370 for (int i=0; i < Status[3]-2 ; i++) {
chris 2:5ea99c37a2d7 371 printf(" Data : 0x%x\n",Status[5+i]);
chris 2:5ea99c37a2d7 372 }
chris 0:be51952765ec 373
chris 2:5ea99c37a2d7 374 printf(" Checksum : 0x%x\n",Status[5+(Status[3]-2)]);
chris 2:5ea99c37a2d7 375 #endif
chris 0:be51952765ec 376
chris 0:be51952765ec 377 } // if (ID!=0xFE)
chris 0:be51952765ec 378
chris 0:be51952765ec 379 return(Status[4]);
chris 0:be51952765ec 380 }
chris 0:be51952765ec 381
chris 1:93ad80f5fde7 382
chris 2:5ea99c37a2d7 383 int AX12::write(int ID, int start, int bytes, char* data, int flag) {
chris 0:be51952765ec 384 // 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum
chris 0:be51952765ec 385
chris 0:be51952765ec 386 char TxBuf[16];
chris 0:be51952765ec 387 char sum = 0;
chris 0:be51952765ec 388 char Status[6];
chris 0:be51952765ec 389
chris 2:5ea99c37a2d7 390 #ifdef AX12_WRITE_DEBUG
chris 2:5ea99c37a2d7 391 printf("\nwrite(%d,0x%x,%d,data,%d)\n",ID,start,bytes,flag);
chris 2:5ea99c37a2d7 392 #endif
chris 0:be51952765ec 393
chris 0:be51952765ec 394 // Build the TxPacket first in RAM, then we'll send in one go
chris 2:5ea99c37a2d7 395 #ifdef AX12_WRITE_DEBUG
chris 2:5ea99c37a2d7 396 printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n");
chris 2:5ea99c37a2d7 397 #endif
chris 0:be51952765ec 398
chris 0:be51952765ec 399 TxBuf[0] = 0xff;
chris 0:be51952765ec 400 TxBuf[1] = 0xff;
chris 0:be51952765ec 401
chris 0:be51952765ec 402 // ID
chris 0:be51952765ec 403 TxBuf[2] = ID;
chris 0:be51952765ec 404 sum += TxBuf[2];
chris 0:be51952765ec 405
chris 2:5ea99c37a2d7 406 #ifdef AX12_WRITE_DEBUG
chris 2:5ea99c37a2d7 407 printf(" ID : %d\n",TxBuf[2]);
chris 2:5ea99c37a2d7 408 #endif
chris 0:be51952765ec 409
chris 0:be51952765ec 410 // packet Length
chris 0:be51952765ec 411 TxBuf[3] = 3+bytes;
chris 0:be51952765ec 412 sum += TxBuf[3];
chris 0:be51952765ec 413
chris 2:5ea99c37a2d7 414 #ifdef AX12_WRITE_DEBUG
chris 2:5ea99c37a2d7 415 printf(" Length : %d\n",TxBuf[3]);
chris 2:5ea99c37a2d7 416 #endif
chris 0:be51952765ec 417
chris 0:be51952765ec 418 // Instruction
chris 0:be51952765ec 419 if (flag == 1) {
chris 0:be51952765ec 420 TxBuf[4]=0x04;
chris 0:be51952765ec 421 sum += TxBuf[4];
chris 0:be51952765ec 422 } else {
chris 0:be51952765ec 423 TxBuf[4]=0x03;
chris 0:be51952765ec 424 sum += TxBuf[4];
chris 0:be51952765ec 425 }
chris 0:be51952765ec 426
chris 2:5ea99c37a2d7 427 #ifdef AX12_WRITE_DEBUG
chris 2:5ea99c37a2d7 428 printf(" Instruction : 0x%x\n",TxBuf[4]);
chris 2:5ea99c37a2d7 429 #endif
chris 0:be51952765ec 430
chris 0:be51952765ec 431 // Start Address
chris 0:be51952765ec 432 TxBuf[5] = start;
chris 0:be51952765ec 433 sum += TxBuf[5];
chris 2:5ea99c37a2d7 434
chris 2:5ea99c37a2d7 435 #ifdef AX12_WRITE_DEBUG
chris 2:5ea99c37a2d7 436 printf(" Start : 0x%x\n",TxBuf[5]);
chris 2:5ea99c37a2d7 437 #endif
chris 0:be51952765ec 438
chris 0:be51952765ec 439 // data
chris 0:be51952765ec 440 for (char i=0; i<bytes ; i++) {
chris 0:be51952765ec 441 TxBuf[6+i] = data[i];
chris 0:be51952765ec 442 sum += TxBuf[6+i];
chris 2:5ea99c37a2d7 443
chris 2:5ea99c37a2d7 444 #ifdef AX12_WRITE_DEBUG
chris 2:5ea99c37a2d7 445 printf(" Data : 0x%x\n",TxBuf[6+i]);
chris 2:5ea99c37a2d7 446 #endif
chris 2:5ea99c37a2d7 447
chris 0:be51952765ec 448 }
chris 0:be51952765ec 449
chris 0:be51952765ec 450 // checksum
chris 0:be51952765ec 451 TxBuf[6+bytes] = 0xFF - sum;
chris 2:5ea99c37a2d7 452
chris 2:5ea99c37a2d7 453 #ifdef AX12_WRITE_DEBUG
chris 2:5ea99c37a2d7 454 printf(" Checksum : 0x%x\n",TxBuf[6+bytes]);
chris 2:5ea99c37a2d7 455 #endif
chris 0:be51952765ec 456
chris 0:be51952765ec 457 // Transmit the packet in one burst with no pausing
chris 0:be51952765ec 458 for (int i = 0; i < (7 + bytes) ; i++) {
chris 0:be51952765ec 459 _ax12.putc(TxBuf[i]);
chris 0:be51952765ec 460 }
chris 0:be51952765ec 461
chris 0:be51952765ec 462
chris 0:be51952765ec 463 // make sure we have a valid return
chris 0:be51952765ec 464 Status[4]=0x00;
chris 0:be51952765ec 465
chris 0:be51952765ec 466 // we'll only get a reply if it was not broadcast
chris 0:be51952765ec 467 if (_ID!=0xFE) {
chris 0:be51952765ec 468
chris 2:5ea99c37a2d7 469
chris 2:5ea99c37a2d7 470 // response packet is always 6 bytes
chris 0:be51952765ec 471 // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum
chris 2:5ea99c37a2d7 472 // timeout is a little more than the time to transmit
chris 2:5ea99c37a2d7 473 // the packet back, i.e. 60 bit periods, round up to 100
chris 2:5ea99c37a2d7 474 int timeout = 0;
chris 2:5ea99c37a2d7 475 int plen = 0;
chris 2:5ea99c37a2d7 476 while ((timeout < 100) && (plen<6)) {
chris 2:5ea99c37a2d7 477
chris 2:5ea99c37a2d7 478 if (_ax12.readable()) {
chris 2:5ea99c37a2d7 479 Status[plen] = _ax12.getc();
chris 2:5ea99c37a2d7 480 plen++;
chris 2:5ea99c37a2d7 481 timeout = 0;
chris 2:5ea99c37a2d7 482 }
chris 2:5ea99c37a2d7 483 timeout++;
chris 0:be51952765ec 484 }
chris 0:be51952765ec 485
chris 2:5ea99c37a2d7 486
chris 0:be51952765ec 487 // Build the TxPacket first in RAM, then we'll send in one go
chris 2:5ea99c37a2d7 488 #ifdef AX12_WRITE_DEBUG
chris 2:5ea99c37a2d7 489 printf("\nStatus Packet\n Header : 0x%X, 0x%X\n",Status[0],Status[1]);
chris 2:5ea99c37a2d7 490 printf(" ID : %d\n",Status[2]);
chris 2:5ea99c37a2d7 491 printf(" Length : %d\n",Status[3]);
chris 2:5ea99c37a2d7 492 printf(" Error : 0x%x\n",Status[4]);
chris 2:5ea99c37a2d7 493 printf(" Checksum : 0x%x\n",Status[5]);
chris 2:5ea99c37a2d7 494 #endif
chris 0:be51952765ec 495
chris 0:be51952765ec 496
chris 0:be51952765ec 497 }
chris 0:be51952765ec 498
chris 0:be51952765ec 499 return(Status[4]); // return error code
chris 0:be51952765ec 500 }