Team DIANA
/
AX12_v2
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
AX12
by 
Chris Styles
AX12.cpp@1:93ad80f5fde7, 2011-03-30 (annotated)
- Committer:
- chris
- Date:
- Wed Mar 30 16:08:02 2011 +0000
- Revision:
- 1:93ad80f5fde7
- Parent:
- 0:be51952765ec
- Child:
- 2:5ea99c37a2d7
- Child:
- 4:36f451ee0a3c
Who changed what in which revision?
| User | Revision | Line number | New 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 | |
| chris | 0:be51952765ec | 27 | AX12::AX12(PinName tx, PinName rx, int ID) |
| chris | 0:be51952765ec | 28 | : _ax12(tx,rx) { |
| chris | 0:be51952765ec | 29 | |
| chris | 0:be51952765ec | 30 | _ax12.baud(1000000); |
| chris | 0:be51952765ec | 31 | _ID = ID; |
| chris | 0:be51952765ec | 32 | } |
| chris | 0:be51952765ec | 33 | |
| chris | 1:93ad80f5fde7 | 34 | // Set the mode of the servo |
| chris | 1:93ad80f5fde7 | 35 | // 0 = Positional (0-300 degrees) |
| chris | 1:93ad80f5fde7 | 36 | // 1 = Rotational -1 to 1 speed |
| chris | 1:93ad80f5fde7 | 37 | int AX12::SetMode(int mode) { |
| chris | 1:93ad80f5fde7 | 38 | |
| chris | 1:93ad80f5fde7 | 39 | if (mode == 1) { // set CR |
| chris | 1:93ad80f5fde7 | 40 | SetCWLimit(0); |
| chris | 1:93ad80f5fde7 | 41 | SetCCWLimit(0); |
| chris | 1:93ad80f5fde7 | 42 | SetCRSpeed(0.0); |
| chris | 1:93ad80f5fde7 | 43 | } else { |
| chris | 1:93ad80f5fde7 | 44 | SetCWLimit(0); |
| chris | 1:93ad80f5fde7 | 45 | SetCCWLimit(300); |
| chris | 1:93ad80f5fde7 | 46 | SetCRSpeed(0.0); |
| chris | 1:93ad80f5fde7 | 47 | } |
| chris | 1:93ad80f5fde7 | 48 | return(0); |
| chris | 1:93ad80f5fde7 | 49 | } |
| chris | 1:93ad80f5fde7 | 50 | |
| chris | 1:93ad80f5fde7 | 51 | |
| chris | 1:93ad80f5fde7 | 52 | // if flag[0] is set, were blocking |
| chris | 1:93ad80f5fde7 | 53 | // if flag[1] is set, we're registering |
| chris | 1:93ad80f5fde7 | 54 | // they are mutually exclusive operations |
| chris | 1:93ad80f5fde7 | 55 | int AX12::SetGoal(int degrees, int flags) { |
| chris | 0:be51952765ec | 56 | |
| chris | 1:93ad80f5fde7 | 57 | char reg_flag = 0; |
| chris | 1:93ad80f5fde7 | 58 | char data[2]; |
| chris | 1:93ad80f5fde7 | 59 | |
| chris | 1:93ad80f5fde7 | 60 | // set the flag is only the register bit is set in the flag |
| chris | 1:93ad80f5fde7 | 61 | if (flags == 0x2) { |
| chris | 1:93ad80f5fde7 | 62 | reg_flag = 1; |
| chris | 1:93ad80f5fde7 | 63 | } |
| chris | 1:93ad80f5fde7 | 64 | |
| chris | 1:93ad80f5fde7 | 65 | // 1023 / 300 * degrees |
| chris | 1:93ad80f5fde7 | 66 | short goal = (1023 * degrees) / 300; |
| chris | 1:93ad80f5fde7 | 67 | if (AX12_DEBUG) { |
| chris | 1:93ad80f5fde7 | 68 | printf("SetGoal to 0x%x\n",goal); |
| chris | 1:93ad80f5fde7 | 69 | } |
| chris | 1:93ad80f5fde7 | 70 | |
| chris | 1:93ad80f5fde7 | 71 | data[0] = goal & 0xff; // bottom 8 bits |
| chris | 1:93ad80f5fde7 | 72 | data[1] = goal >> 8; // top 8 bits |
| chris | 1:93ad80f5fde7 | 73 | |
| chris | 1:93ad80f5fde7 | 74 | // write the packet, return the error code |
| chris | 1:93ad80f5fde7 | 75 | int rVal = write(_ID, AX12_REG_GOAL_POSITION, 2, data, reg_flag); |
| chris | 1:93ad80f5fde7 | 76 | |
| chris | 1:93ad80f5fde7 | 77 | if (flags == 1) { |
| chris | 1:93ad80f5fde7 | 78 | // block until it comes to a halt |
| chris | 1:93ad80f5fde7 | 79 | while (isMoving()) {} |
| chris | 1:93ad80f5fde7 | 80 | } |
| chris | 1:93ad80f5fde7 | 81 | return(rVal); |
| chris | 0:be51952765ec | 82 | } |
| chris | 0:be51952765ec | 83 | |
| chris | 1:93ad80f5fde7 | 84 | |
| chris | 0:be51952765ec | 85 | // Set continuous rotation speed from -1 to 1 |
| chris | 0:be51952765ec | 86 | int AX12::SetCRSpeed(float speed) { |
| chris | 0:be51952765ec | 87 | |
| chris | 0:be51952765ec | 88 | // bit 10 = direction, 0 = CCW, 1=CW |
| chris | 0:be51952765ec | 89 | // bits 9-0 = Speed |
| chris | 0:be51952765ec | 90 | char data[2]; |
| chris | 0:be51952765ec | 91 | |
| chris | 0:be51952765ec | 92 | int goal = (0x3ff * abs(speed)); |
| chris | 0:be51952765ec | 93 | |
| chris | 0:be51952765ec | 94 | // Set direction CW if we have a negative speed |
| chris | 0:be51952765ec | 95 | if (speed < 0) { |
| chris | 0:be51952765ec | 96 | goal |= (0x1 << 10); |
| chris | 0:be51952765ec | 97 | } |
| chris | 0:be51952765ec | 98 | |
| chris | 0:be51952765ec | 99 | data[0] = goal & 0xff; // bottom 8 bits |
| chris | 0:be51952765ec | 100 | data[1] = goal >> 8; // top 8 bits |
| chris | 0:be51952765ec | 101 | |
| chris | 0:be51952765ec | 102 | // write the packet, return the error code |
| chris | 0:be51952765ec | 103 | int rVal = write(_ID, 0x20, 2, data); |
| chris | 0:be51952765ec | 104 | |
| chris | 0:be51952765ec | 105 | return(rVal); |
| chris | 0:be51952765ec | 106 | } |
| chris | 0:be51952765ec | 107 | |
| chris | 1:93ad80f5fde7 | 108 | |
| chris | 1:93ad80f5fde7 | 109 | int AX12::SetCWLimit (int degrees) { |
| chris | 1:93ad80f5fde7 | 110 | |
| chris | 1:93ad80f5fde7 | 111 | char data[2]; |
| chris | 1:93ad80f5fde7 | 112 | |
| chris | 1:93ad80f5fde7 | 113 | // 1023 / 300 * degrees |
| chris | 1:93ad80f5fde7 | 114 | short limit = (1023 * degrees) / 300; |
| chris | 0:be51952765ec | 115 | |
| chris | 1:93ad80f5fde7 | 116 | if (AX12_DEBUG) { |
| chris | 1:93ad80f5fde7 | 117 | printf("SetCWLimit to 0x%x\n",limit); |
| chris | 0:be51952765ec | 118 | } |
| chris | 1:93ad80f5fde7 | 119 | |
| chris | 1:93ad80f5fde7 | 120 | data[0] = limit & 0xff; // bottom 8 bits |
| chris | 1:93ad80f5fde7 | 121 | data[1] = limit >> 8; // top 8 bits |
| chris | 1:93ad80f5fde7 | 122 | |
| chris | 1:93ad80f5fde7 | 123 | // write the packet, return the error code |
| chris | 1:93ad80f5fde7 | 124 | return (write(_ID, AX12_REG_CW_LIMIT, 2, data)); |
| chris | 1:93ad80f5fde7 | 125 | |
| chris | 0:be51952765ec | 126 | } |
| chris | 0:be51952765ec | 127 | |
| chris | 1:93ad80f5fde7 | 128 | int AX12::SetCCWLimit (int degrees) { |
| chris | 1:93ad80f5fde7 | 129 | |
| chris | 1:93ad80f5fde7 | 130 | char data[2]; |
| chris | 1:93ad80f5fde7 | 131 | |
| chris | 1:93ad80f5fde7 | 132 | // 1023 / 300 * degrees |
| chris | 1:93ad80f5fde7 | 133 | short limit = (1023 * degrees) / 300; |
| chris | 0:be51952765ec | 134 | |
| chris | 0:be51952765ec | 135 | if (AX12_DEBUG) { |
| chris | 1:93ad80f5fde7 | 136 | printf("SetCCWLimit to 0x%x\n",limit); |
| chris | 0:be51952765ec | 137 | } |
| chris | 1:93ad80f5fde7 | 138 | |
| chris | 1:93ad80f5fde7 | 139 | data[0] = limit & 0xff; // bottom 8 bits |
| chris | 1:93ad80f5fde7 | 140 | data[1] = limit >> 8; // top 8 bits |
| chris | 1:93ad80f5fde7 | 141 | |
| chris | 1:93ad80f5fde7 | 142 | // write the packet, return the error code |
| chris | 1:93ad80f5fde7 | 143 | return (write(_ID, AX12_REG_CCW_LIMIT, 2, data)); |
| chris | 0:be51952765ec | 144 | } |
| chris | 0:be51952765ec | 145 | |
| chris | 1:93ad80f5fde7 | 146 | |
| chris | 1:93ad80f5fde7 | 147 | int AX12::SetID (int CurrentID, int NewID) { |
| chris | 1:93ad80f5fde7 | 148 | |
| chris | 0:be51952765ec | 149 | char data[1]; |
| chris | 1:93ad80f5fde7 | 150 | data[0] = NewID; |
| chris | 1:93ad80f5fde7 | 151 | if (AX12_DEBUG) { |
| chris | 1:93ad80f5fde7 | 152 | printf("Setting ID from 0x%x to 0x%x\n",CurrentID,NewID); |
| chris | 1:93ad80f5fde7 | 153 | } |
| chris | 1:93ad80f5fde7 | 154 | return (write(CurrentID, AX12_REG_ID, 1, data)); |
| chris | 1:93ad80f5fde7 | 155 | |
| chris | 0:be51952765ec | 156 | } |
| chris | 0:be51952765ec | 157 | |
| chris | 1:93ad80f5fde7 | 158 | |
| chris | 1:93ad80f5fde7 | 159 | // return 1 is the servo is still in flight |
| chris | 1:93ad80f5fde7 | 160 | int AX12::isMoving(void) { |
| chris | 1:93ad80f5fde7 | 161 | |
| chris | 1:93ad80f5fde7 | 162 | char data[1]; |
| chris | 1:93ad80f5fde7 | 163 | read(_ID,AX12_REG_MOVING,1,data); |
| chris | 1:93ad80f5fde7 | 164 | return(data[0]); |
| chris | 1:93ad80f5fde7 | 165 | } |
| chris | 1:93ad80f5fde7 | 166 | |
| chris | 1:93ad80f5fde7 | 167 | |
| chris | 0:be51952765ec | 168 | void AX12::trigger(void) { |
| chris | 0:be51952765ec | 169 | |
| chris | 0:be51952765ec | 170 | char TxBuf[16]; |
| chris | 0:be51952765ec | 171 | char sum = 0; |
| chris | 0:be51952765ec | 172 | |
| chris | 0:be51952765ec | 173 | if (AX12_TRIGGER_DEBUG) { |
| chris | 0:be51952765ec | 174 | printf("\nTriggered\n"); |
| chris | 0:be51952765ec | 175 | } |
| chris | 0:be51952765ec | 176 | |
| chris | 0:be51952765ec | 177 | // Build the TxPacket first in RAM, then we'll send in one go |
| chris | 0:be51952765ec | 178 | if (AX12_TRIGGER_DEBUG) { |
| chris | 0:be51952765ec | 179 | printf("\nTrigger Packet\n Header : 0xFF, 0xFF\n"); |
| chris | 0:be51952765ec | 180 | } |
| chris | 0:be51952765ec | 181 | |
| chris | 0:be51952765ec | 182 | TxBuf[0] = 0xFF; |
| chris | 0:be51952765ec | 183 | TxBuf[1] = 0xFF; |
| chris | 0:be51952765ec | 184 | |
| chris | 0:be51952765ec | 185 | // ID - Broadcast |
| chris | 0:be51952765ec | 186 | TxBuf[2] = 0xFE; |
| chris | 0:be51952765ec | 187 | sum += TxBuf[2]; |
| chris | 0:be51952765ec | 188 | |
| chris | 0:be51952765ec | 189 | if (AX12_TRIGGER_DEBUG) { |
| chris | 0:be51952765ec | 190 | printf(" ID : %d\n",TxBuf[2]); |
| chris | 0:be51952765ec | 191 | } |
| chris | 0:be51952765ec | 192 | |
| chris | 0:be51952765ec | 193 | // Length |
| chris | 0:be51952765ec | 194 | TxBuf[3] = 0x02; |
| chris | 0:be51952765ec | 195 | sum += TxBuf[3]; |
| chris | 0:be51952765ec | 196 | if (AX12_TRIGGER_DEBUG) { |
| chris | 0:be51952765ec | 197 | printf(" Length %d\n",TxBuf[3]); |
| chris | 0:be51952765ec | 198 | } |
| chris | 0:be51952765ec | 199 | |
| chris | 0:be51952765ec | 200 | // Instruction - ACTION |
| chris | 0:be51952765ec | 201 | TxBuf[4] = 0x04; |
| chris | 0:be51952765ec | 202 | sum += TxBuf[4]; |
| chris | 0:be51952765ec | 203 | if (AX12_TRIGGER_DEBUG) { |
| chris | 0:be51952765ec | 204 | printf(" Instruction 0x%X\n",TxBuf[5]); |
| chris | 0:be51952765ec | 205 | } |
| chris | 0:be51952765ec | 206 | |
| chris | 0:be51952765ec | 207 | // Checksum |
| chris | 0:be51952765ec | 208 | TxBuf[5] = 0xFF - sum; |
| chris | 0:be51952765ec | 209 | if (AX12_TRIGGER_DEBUG) { |
| chris | 0:be51952765ec | 210 | printf(" Checksum 0x%X\n",TxBuf[5]); |
| chris | 0:be51952765ec | 211 | } |
| chris | 0:be51952765ec | 212 | |
| chris | 0:be51952765ec | 213 | // Transmit the packet in one burst with no pausing |
| chris | 0:be51952765ec | 214 | for (int i = 0; i < 6 ; i++) { |
| chris | 0:be51952765ec | 215 | _ax12.putc(TxBuf[i]); |
| chris | 0:be51952765ec | 216 | } |
| chris | 1:93ad80f5fde7 | 217 | |
| chris | 0:be51952765ec | 218 | // This is a broadcast packet, so there will be no reply |
| chris | 0:be51952765ec | 219 | |
| chris | 0:be51952765ec | 220 | return; |
| chris | 0:be51952765ec | 221 | } |
| chris | 0:be51952765ec | 222 | |
| chris | 0:be51952765ec | 223 | |
| chris | 0:be51952765ec | 224 | float AX12::GetPosition(void) { |
| chris | 0:be51952765ec | 225 | |
| chris | 0:be51952765ec | 226 | if (AX12_DEBUG) { |
| chris | 0:be51952765ec | 227 | printf("\nGetPosition(%d)",_ID); |
| chris | 0:be51952765ec | 228 | } |
| chris | 0:be51952765ec | 229 | |
| chris | 0:be51952765ec | 230 | char data[2]; |
| chris | 0:be51952765ec | 231 | |
| chris | 0:be51952765ec | 232 | int ErrorCode = read(_ID, AX12_REG_POSITION, 2, data); |
| chris | 0:be51952765ec | 233 | short position = data[0] + (data[1] << 8); |
| chris | 0:be51952765ec | 234 | float angle = (position * 300)/1024; |
| chris | 0:be51952765ec | 235 | |
| chris | 0:be51952765ec | 236 | return (angle); |
| chris | 0:be51952765ec | 237 | } |
| chris | 0:be51952765ec | 238 | |
| chris | 0:be51952765ec | 239 | |
| chris | 1:93ad80f5fde7 | 240 | float AX12::GetTemp (void) { |
| chris | 0:be51952765ec | 241 | |
| chris | 0:be51952765ec | 242 | if (AX12_DEBUG) { |
| chris | 1:93ad80f5fde7 | 243 | printf("\nGetTemp(%d)",_ID); |
| chris | 0:be51952765ec | 244 | } |
| chris | 1:93ad80f5fde7 | 245 | char data[1]; |
| chris | 1:93ad80f5fde7 | 246 | int ErrorCode = read(_ID, AX12_REG_TEMP, 1, data); |
| chris | 1:93ad80f5fde7 | 247 | float temp = data[0]; |
| chris | 1:93ad80f5fde7 | 248 | return(temp); |
| chris | 0:be51952765ec | 249 | } |
| chris | 0:be51952765ec | 250 | |
| chris | 0:be51952765ec | 251 | |
| chris | 1:93ad80f5fde7 | 252 | float AX12::GetVolts (void) { |
| chris | 0:be51952765ec | 253 | if (AX12_DEBUG) { |
| chris | 1:93ad80f5fde7 | 254 | printf("\nGetVolts(%d)",_ID); |
| chris | 0:be51952765ec | 255 | } |
| chris | 0:be51952765ec | 256 | char data[1]; |
| chris | 1:93ad80f5fde7 | 257 | int ErrorCode = read(_ID, AX12_REG_VOLTS, 1, data); |
| chris | 1:93ad80f5fde7 | 258 | float volts = data[0]/10.0; |
| chris | 1:93ad80f5fde7 | 259 | return(volts); |
| chris | 0:be51952765ec | 260 | } |
| chris | 0:be51952765ec | 261 | |
| chris | 0:be51952765ec | 262 | |
| chris | 0:be51952765ec | 263 | int AX12::read(int ID, int start, int bytes, char* data) { |
| chris | 0:be51952765ec | 264 | |
| chris | 0:be51952765ec | 265 | char PacketLength = 0x4; |
| chris | 0:be51952765ec | 266 | char TxBuf[16]; |
| chris | 0:be51952765ec | 267 | char sum = 0; |
| chris | 0:be51952765ec | 268 | char Status[16]; |
| chris | 0:be51952765ec | 269 | |
| chris | 0:be51952765ec | 270 | Status[4] = 0xFE; // return code |
| chris | 0:be51952765ec | 271 | |
| chris | 0:be51952765ec | 272 | if (AX12_READ_DEBUG) { |
| chris | 0:be51952765ec | 273 | printf("\nread(%d,0x%x,%d,data)\n",ID,start,bytes); |
| chris | 0:be51952765ec | 274 | } |
| chris | 0:be51952765ec | 275 | |
| chris | 0:be51952765ec | 276 | // Build the TxPacket first in RAM, then we'll send in one go |
| chris | 0:be51952765ec | 277 | if (AX12_READ_DEBUG) { |
| chris | 0:be51952765ec | 278 | printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n"); |
| chris | 0:be51952765ec | 279 | } |
| chris | 0:be51952765ec | 280 | |
| chris | 0:be51952765ec | 281 | TxBuf[0] = 0xff; |
| chris | 0:be51952765ec | 282 | TxBuf[1] = 0xff; |
| chris | 0:be51952765ec | 283 | |
| chris | 0:be51952765ec | 284 | // ID |
| chris | 0:be51952765ec | 285 | TxBuf[2] = ID; |
| chris | 0:be51952765ec | 286 | sum += TxBuf[2]; |
| chris | 0:be51952765ec | 287 | if (AX12_READ_DEBUG) { |
| chris | 0:be51952765ec | 288 | printf(" ID : %d\n",TxBuf[2]); |
| chris | 0:be51952765ec | 289 | } |
| chris | 0:be51952765ec | 290 | |
| chris | 0:be51952765ec | 291 | // Packet Length |
| chris | 0:be51952765ec | 292 | TxBuf[3] = PacketLength; // Length = 4 ; 2 + 1 (start) = 1 (bytes) |
| chris | 0:be51952765ec | 293 | sum += TxBuf[3]; // Accululate the packet sum |
| chris | 0:be51952765ec | 294 | if (AX12_READ_DEBUG) { |
| chris | 0:be51952765ec | 295 | printf(" Length : 0x%x\n",TxBuf[3]); |
| chris | 0:be51952765ec | 296 | } |
| chris | 0:be51952765ec | 297 | |
| chris | 0:be51952765ec | 298 | // Instruction - Read |
| chris | 0:be51952765ec | 299 | TxBuf[4] = 0x2; |
| chris | 0:be51952765ec | 300 | sum += TxBuf[4]; |
| chris | 0:be51952765ec | 301 | if (AX12_READ_DEBUG) { |
| chris | 0:be51952765ec | 302 | printf(" Instruction : 0x%x\n",TxBuf[4]); |
| chris | 0:be51952765ec | 303 | } |
| chris | 0:be51952765ec | 304 | |
| chris | 0:be51952765ec | 305 | // Start Address |
| chris | 0:be51952765ec | 306 | TxBuf[5] = start; |
| chris | 0:be51952765ec | 307 | sum += TxBuf[5]; |
| chris | 0:be51952765ec | 308 | if (AX12_READ_DEBUG) { |
| chris | 0:be51952765ec | 309 | printf(" Start Address : 0x%x\n",TxBuf[5]); |
| chris | 0:be51952765ec | 310 | } |
| chris | 0:be51952765ec | 311 | |
| chris | 0:be51952765ec | 312 | // Bytes to read |
| chris | 0:be51952765ec | 313 | TxBuf[6] = bytes; |
| chris | 0:be51952765ec | 314 | sum += TxBuf[6]; |
| chris | 0:be51952765ec | 315 | if (AX12_READ_DEBUG) { |
| chris | 0:be51952765ec | 316 | printf(" No bytes : 0x%x\n",TxBuf[6]); |
| chris | 0:be51952765ec | 317 | } |
| chris | 0:be51952765ec | 318 | |
| chris | 0:be51952765ec | 319 | // Checksum |
| chris | 0:be51952765ec | 320 | TxBuf[7] = 0xFF - sum; |
| chris | 0:be51952765ec | 321 | if (AX12_READ_DEBUG) { |
| chris | 0:be51952765ec | 322 | printf(" Checksum : 0x%x\n",TxBuf[7]); |
| chris | 0:be51952765ec | 323 | } |
| chris | 0:be51952765ec | 324 | |
| chris | 0:be51952765ec | 325 | // Transmit the packet in one burst with no pausing |
| chris | 0:be51952765ec | 326 | for (int i = 0; i<8 ; i++) { |
| chris | 0:be51952765ec | 327 | _ax12.putc(TxBuf[i]); |
| chris | 0:be51952765ec | 328 | } |
| chris | 0:be51952765ec | 329 | |
| chris | 0:be51952765ec | 330 | // Wait for the bytes to be transmitted |
| chris | 0:be51952765ec | 331 | wait (0.00002); |
| chris | 0:be51952765ec | 332 | |
| chris | 0:be51952765ec | 333 | // Skip if the read was to the broadcast address |
| chris | 0:be51952765ec | 334 | if (_ID != 0xFE) { |
| chris | 0:be51952765ec | 335 | |
| chris | 0:be51952765ec | 336 | // Receive the Status packet 6+ number of bytes read |
| chris | 0:be51952765ec | 337 | for (int i=0; i<(6+bytes) ; i++) { |
| chris | 0:be51952765ec | 338 | Status[i] = _ax12.getc(); |
| chris | 0:be51952765ec | 339 | } |
| chris | 0:be51952765ec | 340 | |
| chris | 0:be51952765ec | 341 | // Copy the data from Status into data for return |
| chris | 0:be51952765ec | 342 | for (int i=0; i < Status[3]-2 ; i++) { |
| chris | 0:be51952765ec | 343 | data[i] = Status[5+i]; |
| chris | 0:be51952765ec | 344 | } |
| chris | 0:be51952765ec | 345 | |
| chris | 0:be51952765ec | 346 | if (AX12_READ_DEBUG) { |
| chris | 0:be51952765ec | 347 | printf("\nStatus Packet\n"); |
| chris | 0:be51952765ec | 348 | printf(" Header : 0x%x\n",Status[0]); |
| chris | 0:be51952765ec | 349 | printf(" Header : 0x%x\n",Status[1]); |
| chris | 0:be51952765ec | 350 | printf(" ID : 0x%x\n",Status[2]); |
| chris | 0:be51952765ec | 351 | printf(" Length : 0x%x\n",Status[3]); |
| chris | 0:be51952765ec | 352 | printf(" Error Code : 0x%x\n",Status[4]); |
| chris | 0:be51952765ec | 353 | |
| chris | 0:be51952765ec | 354 | for (int i=0; i < Status[3]-2 ; i++) { |
| chris | 0:be51952765ec | 355 | printf(" Data : 0x%x\n",Status[5+i]); |
| chris | 0:be51952765ec | 356 | } |
| chris | 0:be51952765ec | 357 | |
| chris | 0:be51952765ec | 358 | printf(" Checksum : 0x%x\n",Status[5+(Status[3]-2)]); |
| chris | 0:be51952765ec | 359 | } |
| chris | 0:be51952765ec | 360 | |
| chris | 0:be51952765ec | 361 | } // if (ID!=0xFE) |
| chris | 0:be51952765ec | 362 | |
| chris | 0:be51952765ec | 363 | return(Status[4]); |
| chris | 0:be51952765ec | 364 | } |
| chris | 0:be51952765ec | 365 | |
| chris | 1:93ad80f5fde7 | 366 | |
| chris | 0:be51952765ec | 367 | int AX12:: write(int ID, int start, int bytes, char* data, int flag) { |
| chris | 0:be51952765ec | 368 | // 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum |
| chris | 0:be51952765ec | 369 | |
| chris | 0:be51952765ec | 370 | char TxBuf[16]; |
| chris | 0:be51952765ec | 371 | char sum = 0; |
| chris | 0:be51952765ec | 372 | char Status[6]; |
| chris | 0:be51952765ec | 373 | |
| chris | 0:be51952765ec | 374 | if (AX12_WRITE_DEBUG) { |
| chris | 0:be51952765ec | 375 | printf("\nwrite(%d,0x%x,%d,data,%d)\n",ID,start,bytes,flag); |
| chris | 0:be51952765ec | 376 | } |
| chris | 0:be51952765ec | 377 | |
| chris | 0:be51952765ec | 378 | // Build the TxPacket first in RAM, then we'll send in one go |
| chris | 0:be51952765ec | 379 | if (AX12_WRITE_DEBUG) { |
| chris | 0:be51952765ec | 380 | printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n"); |
| chris | 0:be51952765ec | 381 | } |
| chris | 0:be51952765ec | 382 | |
| chris | 0:be51952765ec | 383 | TxBuf[0] = 0xff; |
| chris | 0:be51952765ec | 384 | TxBuf[1] = 0xff; |
| chris | 0:be51952765ec | 385 | |
| chris | 0:be51952765ec | 386 | // ID |
| chris | 0:be51952765ec | 387 | TxBuf[2] = ID; |
| chris | 0:be51952765ec | 388 | sum += TxBuf[2]; |
| chris | 0:be51952765ec | 389 | |
| chris | 0:be51952765ec | 390 | if (AX12_WRITE_DEBUG) { |
| chris | 0:be51952765ec | 391 | printf(" ID : %d\n",TxBuf[2]); |
| chris | 0:be51952765ec | 392 | } |
| chris | 0:be51952765ec | 393 | |
| chris | 0:be51952765ec | 394 | // packet Length |
| chris | 0:be51952765ec | 395 | TxBuf[3] = 3+bytes; |
| chris | 0:be51952765ec | 396 | sum += TxBuf[3]; |
| chris | 0:be51952765ec | 397 | |
| chris | 0:be51952765ec | 398 | if (AX12_WRITE_DEBUG) { |
| chris | 0:be51952765ec | 399 | printf(" Length : %d\n",TxBuf[3]); |
| chris | 0:be51952765ec | 400 | } |
| chris | 0:be51952765ec | 401 | |
| chris | 0:be51952765ec | 402 | // Instruction |
| chris | 0:be51952765ec | 403 | if (flag == 1) { |
| chris | 0:be51952765ec | 404 | TxBuf[4]=0x04; |
| chris | 0:be51952765ec | 405 | sum += TxBuf[4]; |
| chris | 0:be51952765ec | 406 | } else { |
| chris | 0:be51952765ec | 407 | TxBuf[4]=0x03; |
| chris | 0:be51952765ec | 408 | sum += TxBuf[4]; |
| chris | 0:be51952765ec | 409 | } |
| chris | 0:be51952765ec | 410 | |
| chris | 0:be51952765ec | 411 | if (AX12_WRITE_DEBUG) { |
| chris | 0:be51952765ec | 412 | printf(" Instruction : 0x%x\n",TxBuf[4]); |
| chris | 0:be51952765ec | 413 | } |
| chris | 0:be51952765ec | 414 | |
| chris | 0:be51952765ec | 415 | // Start Address |
| chris | 0:be51952765ec | 416 | TxBuf[5] = start; |
| chris | 0:be51952765ec | 417 | sum += TxBuf[5]; |
| chris | 0:be51952765ec | 418 | if (AX12_WRITE_DEBUG) { |
| chris | 0:be51952765ec | 419 | printf(" Start : 0x%x\n",TxBuf[5]); |
| chris | 0:be51952765ec | 420 | } |
| chris | 0:be51952765ec | 421 | |
| chris | 0:be51952765ec | 422 | // data |
| chris | 0:be51952765ec | 423 | for (char i=0; i<bytes ; i++) { |
| chris | 0:be51952765ec | 424 | TxBuf[6+i] = data[i]; |
| chris | 0:be51952765ec | 425 | sum += TxBuf[6+i]; |
| chris | 0:be51952765ec | 426 | if (AX12_WRITE_DEBUG) { |
| chris | 0:be51952765ec | 427 | printf(" Data : 0x%x\n",TxBuf[6+i]); |
| chris | 0:be51952765ec | 428 | } |
| chris | 0:be51952765ec | 429 | } |
| chris | 0:be51952765ec | 430 | |
| chris | 0:be51952765ec | 431 | // checksum |
| chris | 0:be51952765ec | 432 | TxBuf[6+bytes] = 0xFF - sum; |
| chris | 0:be51952765ec | 433 | if (AX12_WRITE_DEBUG) { |
| chris | 0:be51952765ec | 434 | printf(" Checksum : 0x%x\n",TxBuf[6+bytes]); |
| chris | 0:be51952765ec | 435 | } |
| chris | 0:be51952765ec | 436 | |
| chris | 0:be51952765ec | 437 | // Transmit the packet in one burst with no pausing |
| chris | 0:be51952765ec | 438 | for (int i = 0; i < (7 + bytes) ; i++) { |
| chris | 0:be51952765ec | 439 | _ax12.putc(TxBuf[i]); |
| chris | 0:be51952765ec | 440 | } |
| chris | 0:be51952765ec | 441 | |
| chris | 0:be51952765ec | 442 | // Wait for data to transmit |
| chris | 0:be51952765ec | 443 | wait (0.00002); |
| chris | 0:be51952765ec | 444 | |
| chris | 0:be51952765ec | 445 | // make sure we have a valid return |
| chris | 0:be51952765ec | 446 | Status[4]=0x00; |
| chris | 0:be51952765ec | 447 | |
| chris | 0:be51952765ec | 448 | // we'll only get a reply if it was not broadcast |
| chris | 0:be51952765ec | 449 | if (_ID!=0xFE) { |
| chris | 0:be51952765ec | 450 | |
| chris | 0:be51952765ec | 451 | // response is always 6 bytes |
| chris | 0:be51952765ec | 452 | // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum |
| chris | 0:be51952765ec | 453 | for (int i=0; i < 6 ; i++) { |
| chris | 0:be51952765ec | 454 | Status[i] = _ax12.getc(); |
| chris | 0:be51952765ec | 455 | } |
| chris | 0:be51952765ec | 456 | |
| chris | 0:be51952765ec | 457 | // Build the TxPacket first in RAM, then we'll send in one go |
| chris | 0:be51952765ec | 458 | if (AX12_WRITE_DEBUG) { |
| chris | 0:be51952765ec | 459 | printf("\nStatus Packet\n Header : 0x%X, 0x%X\n",Status[0],Status[1]); |
| chris | 0:be51952765ec | 460 | printf(" ID : %d\n",Status[2]); |
| chris | 0:be51952765ec | 461 | printf(" Length : %d\n",Status[3]); |
| chris | 0:be51952765ec | 462 | printf(" Error : 0x%x\n",Status[4]); |
| chris | 0:be51952765ec | 463 | printf(" Checksum : 0x%x\n",Status[5]); |
| chris | 0:be51952765ec | 464 | } |
| chris | 0:be51952765ec | 465 | |
| chris | 0:be51952765ec | 466 | |
| chris | 0:be51952765ec | 467 | } |
| chris | 0:be51952765ec | 468 | |
| chris | 0:be51952765ec | 469 | return(Status[4]); // return error code |
| chris | 0:be51952765ec | 470 | } |