erkin yucel / mbed-os

mbed os with nrf51 internal bandgap enabled to read battery level

Dependents:   BLE_file_test BLE_Blink ExternalEncoder

features/unsupported/tests/peripherals/AX12/AX12.cpp@0:f269e3021894, 2016-10-23 (annotated)

Committer:
elessair
Date:
Sun Oct 23 15:10:02 2016 +0000
Revision:
0:f269e3021894
Initial commit

Who changed what in which revision?

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