sugiyama tatsuya / AX12

a

Fork of AX12 by Chris Styles

RX64.cpp@4:40a609b85463, 2016-02-29 (annotated)

Committer:
452R142039
Date:
Mon Feb 29 08:47:07 2016 +0000
Revision:
4:40a609b85463
?

Who changed what in which revision?

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