Team DIANA / Mbed 2 deprecated AX12

ok

Dependencies:   mbed

Fork of AX12 by Chris Styles

AX12.cpp@4:1200e221d94d, 2016-04-22 (annotated)

Committer:
dconsoli
Date:
Fri Apr 22 17:52:24 2016 +0000
Revision:
4:1200e221d94d
Parent:
3:ced71d1b2558 
ok;

Who changed what in which revision?

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