Team DIANA / AX12_final

Debugged library

Dependents:   MX106-finaltest dynamixel Arm_dynamixel_can Arm_dynamixel_can_procedurale

Fork of AX12_v2 by Team DIANA

AX12.cpp@11:9532fc2ca8b1, 2016-06-26 (annotated)

Committer:
mattiasub
Date:
Sun Jun 26 21:14:17 2016 +0000
Revision:
11:9532fc2ca8b1
Parent:
8:ae950cba6bc9
Child:
14:4487db847117
Risolti errori su Communication_1 . Da risolvere ancora MX106

Who changed what in which revision?

UserRevisionLine numberNew contents of line
chris 0:be51952765ec 1 #include "AX12.h"
chris 0:be51952765ec 2 #include "mbed.h"
ilaria 4:36f451ee0a3c 3 #define AX12_DEBUG 1
chris 0:be51952765ec 4
chris 0:be51952765ec 5 AX12::AX12(PinName tx, PinName rx, int ID)
chris 0:be51952765ec 6 : _ax12(tx,rx) {
chris 0:be51952765ec 7
dconsoli 7:ac233ac0ba11 8 _ax12.baud(9600);
chris 0:be51952765ec 9 _ID = ID;
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);
mattiasub 11:9532fc2ca8b1 23 SetCCWLimit(360);
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
mattiasub 8:ae950cba6bc9 44 short goal = (4093 * degrees) / 300;
chris 1:93ad80f5fde7 45 if (AX12_DEBUG) {
chris 1:93ad80f5fde7 46 printf("SetGoal to 0x%x\n",goal);
chris 1:93ad80f5fde7 47 }
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
dconsoli 7:ac233ac0ba11 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 1:93ad80f5fde7 90
chris 1:93ad80f5fde7 91 // 1023 / 300 * degrees
mattiasub 8:ae950cba6bc9 92 short limit = (4093 * degrees) / 300;
chris 0:be51952765ec 93
chris 1:93ad80f5fde7 94 if (AX12_DEBUG) {
chris 1:93ad80f5fde7 95 printf("SetCWLimit to 0x%x\n",limit);
chris 0:be51952765ec 96 }
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
mattiasub 8:ae950cba6bc9 111 short limit = (4093 * degrees) / 300;
chris 0:be51952765ec 112
chris 0:be51952765ec 113 if (AX12_DEBUG) {
chris 1:93ad80f5fde7 114 printf("SetCCWLimit to 0x%x\n",limit);
chris 0:be51952765ec 115 }
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 1:93ad80f5fde7 129 if (AX12_DEBUG) {
chris 1:93ad80f5fde7 130 printf("Setting ID from 0x%x to 0x%x\n",CurrentID,NewID);
chris 1:93ad80f5fde7 131 }
chris 1:93ad80f5fde7 132 return (write(CurrentID, AX12_REG_ID, 1, data));
chris 1:93ad80f5fde7 133
chris 0:be51952765ec 134 }
chris 0:be51952765ec 135
chris 1:93ad80f5fde7 136
chris 1:93ad80f5fde7 137 // return 1 is the servo is still in flight
chris 1:93ad80f5fde7 138 int AX12::isMoving(void) {
chris 1:93ad80f5fde7 139
chris 1:93ad80f5fde7 140 char data[1];
chris 1:93ad80f5fde7 141 read(_ID,AX12_REG_MOVING,1,data);
chris 1:93ad80f5fde7 142 return(data[0]);
chris 1:93ad80f5fde7 143 }
chris 1:93ad80f5fde7 144
chris 1:93ad80f5fde7 145
chris 0:be51952765ec 146 void AX12::trigger(void) {
chris 0:be51952765ec 147
chris 0:be51952765ec 148 char TxBuf[16];
chris 0:be51952765ec 149 char sum = 0;
chris 0:be51952765ec 150
chris 0:be51952765ec 151 if (AX12_TRIGGER_DEBUG) {
chris 0:be51952765ec 152 printf("\nTriggered\n");
chris 0:be51952765ec 153 }
chris 0:be51952765ec 154
chris 0:be51952765ec 155 // Build the TxPacket first in RAM, then we'll send in one go
chris 0:be51952765ec 156 if (AX12_TRIGGER_DEBUG) {
chris 0:be51952765ec 157 printf("\nTrigger Packet\n Header : 0xFF, 0xFF\n");
chris 0:be51952765ec 158 }
chris 0:be51952765ec 159
chris 0:be51952765ec 160 TxBuf[0] = 0xFF;
chris 0:be51952765ec 161 TxBuf[1] = 0xFF;
chris 0:be51952765ec 162
chris 0:be51952765ec 163 // ID - Broadcast
chris 0:be51952765ec 164 TxBuf[2] = 0xFE;
chris 0:be51952765ec 165 sum += TxBuf[2];
chris 0:be51952765ec 166
chris 0:be51952765ec 167 if (AX12_TRIGGER_DEBUG) {
chris 0:be51952765ec 168 printf(" ID : %d\n",TxBuf[2]);
chris 0:be51952765ec 169 }
chris 0:be51952765ec 170
chris 0:be51952765ec 171 // Length
chris 0:be51952765ec 172 TxBuf[3] = 0x02;
chris 0:be51952765ec 173 sum += TxBuf[3];
chris 0:be51952765ec 174 if (AX12_TRIGGER_DEBUG) {
chris 0:be51952765ec 175 printf(" Length %d\n",TxBuf[3]);
chris 0:be51952765ec 176 }
chris 0:be51952765ec 177
chris 0:be51952765ec 178 // Instruction - ACTION
chris 0:be51952765ec 179 TxBuf[4] = 0x04;
chris 0:be51952765ec 180 sum += TxBuf[4];
chris 0:be51952765ec 181 if (AX12_TRIGGER_DEBUG) {
chris 0:be51952765ec 182 printf(" Instruction 0x%X\n",TxBuf[5]);
chris 0:be51952765ec 183 }
chris 0:be51952765ec 184
chris 0:be51952765ec 185 // Checksum
chris 0:be51952765ec 186 TxBuf[5] = 0xFF - sum;
chris 0:be51952765ec 187 if (AX12_TRIGGER_DEBUG) {
chris 0:be51952765ec 188 printf(" Checksum 0x%X\n",TxBuf[5]);
chris 0:be51952765ec 189 }
chris 0:be51952765ec 190
chris 0:be51952765ec 191 // Transmit the packet in one burst with no pausing
chris 0:be51952765ec 192 for (int i = 0; i < 6 ; i++) {
chris 0:be51952765ec 193 _ax12.putc(TxBuf[i]);
chris 0:be51952765ec 194 }
chris 1:93ad80f5fde7 195
chris 0:be51952765ec 196 // This is a broadcast packet, so there will be no reply
chris 0:be51952765ec 197
chris 0:be51952765ec 198 return;
chris 0:be51952765ec 199 }
chris 0:be51952765ec 200
chris 0:be51952765ec 201
chris 0:be51952765ec 202 float AX12::GetPosition(void) {
chris 0:be51952765ec 203
chris 0:be51952765ec 204 if (AX12_DEBUG) {
chris 0:be51952765ec 205 printf("\nGetPosition(%d)",_ID);
chris 0:be51952765ec 206 }
chris 0:be51952765ec 207
chris 0:be51952765ec 208 char data[2];
chris 0:be51952765ec 209
chris 0:be51952765ec 210 int ErrorCode = read(_ID, AX12_REG_POSITION, 2, data);
chris 0:be51952765ec 211 short position = data[0] + (data[1] << 8);
mattiasub 8:ae950cba6bc9 212 float angle = (position * 300)/4093;
chris 0:be51952765ec 213
chris 0:be51952765ec 214 return (angle);
chris 0:be51952765ec 215 }
chris 0:be51952765ec 216
chris 0:be51952765ec 217
chris 1:93ad80f5fde7 218 float AX12::GetTemp (void) {
chris 0:be51952765ec 219
chris 0:be51952765ec 220 if (AX12_DEBUG) {
chris 1:93ad80f5fde7 221 printf("\nGetTemp(%d)",_ID);
chris 0:be51952765ec 222 }
chris 1:93ad80f5fde7 223 char data[1];
chris 1:93ad80f5fde7 224 int ErrorCode = read(_ID, AX12_REG_TEMP, 1, data);
chris 1:93ad80f5fde7 225 float temp = data[0];
chris 1:93ad80f5fde7 226 return(temp);
chris 0:be51952765ec 227 }
chris 0:be51952765ec 228
chris 0:be51952765ec 229
chris 1:93ad80f5fde7 230 float AX12::GetVolts (void) {
chris 0:be51952765ec 231 if (AX12_DEBUG) {
chris 1:93ad80f5fde7 232 printf("\nGetVolts(%d)",_ID);
chris 0:be51952765ec 233 }
chris 0:be51952765ec 234 char data[1];
chris 1:93ad80f5fde7 235 int ErrorCode = read(_ID, AX12_REG_VOLTS, 1, data);
chris 1:93ad80f5fde7 236 float volts = data[0]/10.0;
chris 1:93ad80f5fde7 237 return(volts);
chris 0:be51952765ec 238 }
chris 0:be51952765ec 239
chris 0:be51952765ec 240
chris 0:be51952765ec 241 int AX12::read(int ID, int start, int bytes, char* data) {
chris 0:be51952765ec 242
chris 0:be51952765ec 243 char PacketLength = 0x4;
chris 0:be51952765ec 244 char TxBuf[16];
chris 0:be51952765ec 245 char sum = 0;
chris 0:be51952765ec 246 char Status[16];
chris 0:be51952765ec 247
chris 0:be51952765ec 248 Status[4] = 0xFE; // return code
chris 0:be51952765ec 249
chris 0:be51952765ec 250 if (AX12_READ_DEBUG) {
chris 0:be51952765ec 251 printf("\nread(%d,0x%x,%d,data)\n",ID,start,bytes);
chris 0:be51952765ec 252 }
chris 0:be51952765ec 253
chris 0:be51952765ec 254 // Build the TxPacket first in RAM, then we'll send in one go
chris 0:be51952765ec 255 if (AX12_READ_DEBUG) {
chris 0:be51952765ec 256 printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n");
chris 0:be51952765ec 257 }
chris 0:be51952765ec 258
chris 0:be51952765ec 259 TxBuf[0] = 0xff;
chris 0:be51952765ec 260 TxBuf[1] = 0xff;
chris 0:be51952765ec 261
chris 0:be51952765ec 262 // ID
chris 0:be51952765ec 263 TxBuf[2] = ID;
chris 0:be51952765ec 264 sum += TxBuf[2];
chris 0:be51952765ec 265 if (AX12_READ_DEBUG) {
chris 0:be51952765ec 266 printf(" ID : %d\n",TxBuf[2]);
chris 0:be51952765ec 267 }
chris 0:be51952765ec 268
chris 0:be51952765ec 269 // Packet Length
chris 0:be51952765ec 270 TxBuf[3] = PacketLength; // Length = 4 ; 2 + 1 (start) = 1 (bytes)
chris 0:be51952765ec 271 sum += TxBuf[3]; // Accululate the packet sum
chris 0:be51952765ec 272 if (AX12_READ_DEBUG) {
chris 0:be51952765ec 273 printf(" Length : 0x%x\n",TxBuf[3]);
chris 0:be51952765ec 274 }
chris 0:be51952765ec 275
chris 0:be51952765ec 276 // Instruction - Read
chris 0:be51952765ec 277 TxBuf[4] = 0x2;
chris 0:be51952765ec 278 sum += TxBuf[4];
chris 0:be51952765ec 279 if (AX12_READ_DEBUG) {
chris 0:be51952765ec 280 printf(" Instruction : 0x%x\n",TxBuf[4]);
chris 0:be51952765ec 281 }
chris 0:be51952765ec 282
chris 0:be51952765ec 283 // Start Address
chris 0:be51952765ec 284 TxBuf[5] = start;
chris 0:be51952765ec 285 sum += TxBuf[5];
chris 0:be51952765ec 286 if (AX12_READ_DEBUG) {
chris 0:be51952765ec 287 printf(" Start Address : 0x%x\n",TxBuf[5]);
chris 0:be51952765ec 288 }
chris 0:be51952765ec 289
chris 0:be51952765ec 290 // Bytes to read
chris 0:be51952765ec 291 TxBuf[6] = bytes;
chris 0:be51952765ec 292 sum += TxBuf[6];
chris 0:be51952765ec 293 if (AX12_READ_DEBUG) {
chris 0:be51952765ec 294 printf(" No bytes : 0x%x\n",TxBuf[6]);
chris 0:be51952765ec 295 }
chris 0:be51952765ec 296
chris 0:be51952765ec 297 // Checksum
chris 0:be51952765ec 298 TxBuf[7] = 0xFF - sum;
chris 0:be51952765ec 299 if (AX12_READ_DEBUG) {
chris 0:be51952765ec 300 printf(" Checksum : 0x%x\n",TxBuf[7]);
chris 0:be51952765ec 301 }
chris 0:be51952765ec 302
chris 0:be51952765ec 303 // Transmit the packet in one burst with no pausing
chris 0:be51952765ec 304 for (int i = 0; i<8 ; i++) {
ilaria 4:36f451ee0a3c 305 // printf("Inizio scrittura numero %d\n",i);
chris 0:be51952765ec 306 _ax12.putc(TxBuf[i]);
ilaria 4:36f451ee0a3c 307 //printf("Fine scrittura numero %d\n",i);
chris 0:be51952765ec 308 }
chris 0:be51952765ec 309
chris 0:be51952765ec 310 // Wait for the bytes to be transmitted
dconsoli 7:ac233ac0ba11 311 // wait (0.00002);
chris 0:be51952765ec 312
chris 0:be51952765ec 313 // Skip if the read was to the broadcast address
chris 0:be51952765ec 314 if (_ID != 0xFE) {
chris 0:be51952765ec 315
chris 0:be51952765ec 316 // Receive the Status packet 6+ number of bytes read
chris 0:be51952765ec 317 for (int i=0; i<(6+bytes) ; i++) {
dconsoli 7:ac233ac0ba11 318 //printf("Inizio lettura numero %d\n",i);
chris 0:be51952765ec 319 Status[i] = _ax12.getc();
dconsoli 7:ac233ac0ba11 320
chris 0:be51952765ec 321 }
chris 0:be51952765ec 322
chris 0:be51952765ec 323 // Copy the data from Status into data for return
chris 0:be51952765ec 324 for (int i=0; i < Status[3]-2 ; i++) {
chris 0:be51952765ec 325 data[i] = Status[5+i];
chris 0:be51952765ec 326 }
chris 0:be51952765ec 327
chris 0:be51952765ec 328 if (AX12_READ_DEBUG) {
chris 0:be51952765ec 329 printf("\nStatus Packet\n");
chris 0:be51952765ec 330 printf(" Header : 0x%x\n",Status[0]);
chris 0:be51952765ec 331 printf(" Header : 0x%x\n",Status[1]);
chris 0:be51952765ec 332 printf(" ID : 0x%x\n",Status[2]);
chris 0:be51952765ec 333 printf(" Length : 0x%x\n",Status[3]);
chris 0:be51952765ec 334 printf(" Error Code : 0x%x\n",Status[4]);
chris 0:be51952765ec 335
chris 0:be51952765ec 336 for (int i=0; i < Status[3]-2 ; i++) {
chris 0:be51952765ec 337 printf(" Data : 0x%x\n",Status[5+i]);
chris 0:be51952765ec 338 }
chris 0:be51952765ec 339
chris 0:be51952765ec 340 printf(" Checksum : 0x%x\n",Status[5+(Status[3]-2)]);
chris 0:be51952765ec 341 }
chris 0:be51952765ec 342
chris 0:be51952765ec 343 } // if (ID!=0xFE)
chris 0:be51952765ec 344
chris 0:be51952765ec 345 return(Status[4]);
chris 0:be51952765ec 346 }
chris 0:be51952765ec 347
chris 1:93ad80f5fde7 348
chris 0:be51952765ec 349 int AX12:: write(int ID, int start, int bytes, char* data, int flag) {
chris 0:be51952765ec 350 // 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum
chris 0:be51952765ec 351
chris 0:be51952765ec 352 char TxBuf[16];
chris 0:be51952765ec 353 char sum = 0;
chris 0:be51952765ec 354 char Status[6];
chris 0:be51952765ec 355
chris 0:be51952765ec 356 if (AX12_WRITE_DEBUG) {
chris 0:be51952765ec 357 printf("\nwrite(%d,0x%x,%d,data,%d)\n",ID,start,bytes,flag);
chris 0:be51952765ec 358 }
chris 0:be51952765ec 359
chris 0:be51952765ec 360 // Build the TxPacket first in RAM, then we'll send in one go
chris 0:be51952765ec 361 if (AX12_WRITE_DEBUG) {
chris 0:be51952765ec 362 printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n");
chris 0:be51952765ec 363 }
chris 0:be51952765ec 364
chris 0:be51952765ec 365 TxBuf[0] = 0xff;
chris 0:be51952765ec 366 TxBuf[1] = 0xff;
chris 0:be51952765ec 367
chris 0:be51952765ec 368 // ID
chris 0:be51952765ec 369 TxBuf[2] = ID;
chris 0:be51952765ec 370 sum += TxBuf[2];
chris 0:be51952765ec 371
chris 0:be51952765ec 372 if (AX12_WRITE_DEBUG) {
chris 0:be51952765ec 373 printf(" ID : %d\n",TxBuf[2]);
chris 0:be51952765ec 374 }
chris 0:be51952765ec 375
chris 0:be51952765ec 376 // packet Length
chris 0:be51952765ec 377 TxBuf[3] = 3+bytes;
chris 0:be51952765ec 378 sum += TxBuf[3];
chris 0:be51952765ec 379
chris 0:be51952765ec 380 if (AX12_WRITE_DEBUG) {
chris 0:be51952765ec 381 printf(" Length : %d\n",TxBuf[3]);
chris 0:be51952765ec 382 }
chris 0:be51952765ec 383
chris 0:be51952765ec 384 // Instruction
chris 0:be51952765ec 385 if (flag == 1) {
chris 0:be51952765ec 386 TxBuf[4]=0x04;
chris 0:be51952765ec 387 sum += TxBuf[4];
chris 0:be51952765ec 388 } else {
chris 0:be51952765ec 389 TxBuf[4]=0x03;
chris 0:be51952765ec 390 sum += TxBuf[4];
chris 0:be51952765ec 391 }
chris 0:be51952765ec 392
chris 0:be51952765ec 393 if (AX12_WRITE_DEBUG) {
chris 0:be51952765ec 394 printf(" Instruction : 0x%x\n",TxBuf[4]);
chris 0:be51952765ec 395 }
chris 0:be51952765ec 396
chris 0:be51952765ec 397 // Start Address
chris 0:be51952765ec 398 TxBuf[5] = start;
chris 0:be51952765ec 399 sum += TxBuf[5];
chris 0:be51952765ec 400 if (AX12_WRITE_DEBUG) {
chris 0:be51952765ec 401 printf(" Start : 0x%x\n",TxBuf[5]);
chris 0:be51952765ec 402 }
chris 0:be51952765ec 403
chris 0:be51952765ec 404 // data
chris 0:be51952765ec 405 for (char i=0; i<bytes ; i++) {
chris 0:be51952765ec 406 TxBuf[6+i] = data[i];
chris 0:be51952765ec 407 sum += TxBuf[6+i];
chris 0:be51952765ec 408 if (AX12_WRITE_DEBUG) {
chris 0:be51952765ec 409 printf(" Data : 0x%x\n",TxBuf[6+i]);
chris 0:be51952765ec 410 }
chris 0:be51952765ec 411 }
chris 0:be51952765ec 412
chris 0:be51952765ec 413 // checksum
chris 0:be51952765ec 414 TxBuf[6+bytes] = 0xFF - sum;
chris 0:be51952765ec 415 if (AX12_WRITE_DEBUG) {
chris 0:be51952765ec 416 printf(" Checksum : 0x%x\n",TxBuf[6+bytes]);
chris 0:be51952765ec 417 }
chris 0:be51952765ec 418
chris 0:be51952765ec 419 // Transmit the packet in one burst with no pausing
chris 0:be51952765ec 420 for (int i = 0; i < (7 + bytes) ; i++) {
chris 0:be51952765ec 421 _ax12.putc(TxBuf[i]);
chris 0:be51952765ec 422 }
chris 0:be51952765ec 423
chris 0:be51952765ec 424 // Wait for data to transmit
chris 0:be51952765ec 425 wait (0.00002);
chris 0:be51952765ec 426
chris 0:be51952765ec 427 // make sure we have a valid return
chris 0:be51952765ec 428 Status[4]=0x00;
chris 0:be51952765ec 429
chris 0:be51952765ec 430 // we'll only get a reply if it was not broadcast
chris 0:be51952765ec 431 if (_ID!=0xFE) {
chris 0:be51952765ec 432
chris 0:be51952765ec 433 // response is always 6 bytes
chris 0:be51952765ec 434 // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum
chris 0:be51952765ec 435 for (int i=0; i < 6 ; i++) {
chris 0:be51952765ec 436 Status[i] = _ax12.getc();
chris 0:be51952765ec 437 }
chris 0:be51952765ec 438
chris 0:be51952765ec 439 // Build the TxPacket first in RAM, then we'll send in one go
chris 0:be51952765ec 440 if (AX12_WRITE_DEBUG) {
chris 0:be51952765ec 441 printf("\nStatus Packet\n Header : 0x%X, 0x%X\n",Status[0],Status[1]);
chris 0:be51952765ec 442 printf(" ID : %d\n",Status[2]);
chris 0:be51952765ec 443 printf(" Length : %d\n",Status[3]);
chris 0:be51952765ec 444 printf(" Error : 0x%x\n",Status[4]);
chris 0:be51952765ec 445 printf(" Checksum : 0x%x\n",Status[5]);
chris 0:be51952765ec 446 }
chris 0:be51952765ec 447
chris 0:be51952765ec 448
chris 0:be51952765ec 449 }
chris 0:be51952765ec 450
chris 0:be51952765ec 451 return(Status[4]); // return error code
chris 0:be51952765ec 452 }