Martin Smith / AX12_Hardware

A library for the AX-12+ servo using external ICs instead of the SerialHalf-Duplex class

A library based on the AX-12+ library but using external hardware to convert from full to half duplex as suggested in the AX-12 datasheet. The buffers and NOT gate need to connected as shown below.

640

Don't forget the pull-up resistor!

AX12.cpp@0:d4af99baf76f, 2011-08-06 (annotated)

Committer:
ms523
Date:
Sat Aug 06 12:20:27 2011 +0000
Revision:
0:d4af99baf76f
Child:
1:1dd9cd18975d
Initial revision

Who changed what in which revision?

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