ARES / Mbed 2 deprecated Timer

Time is good

Dependencies:   RoboClaw mbed

Fork of Robot2016_2-0 by ARES

AX12/AX12.cpp@85:38cbf5fd22e1, 2016-05-06 (annotated)

Committer:
IceTeam
Date:
Fri May 06 09:49:01 2016 +0200
Revision:
85:38cbf5fd22e1
Rajout des AX12

Who changed what in which revision?

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