Gong Chen / MX64

Fork from Dynamixel AX12 Servo for MX64 use and not-finishi now

Dependents:   2014-Mx64 2014-mx64-test

Fork of AX12 by Chris Styles

This Library was Fork From Chris Styles's AX12 Library .

Dynamixel MX64 Servo

MX64 is like a new generation Dynamixel Servo Use TTL 2.0 bus, Half-duplex Serial just like a pro-version AX12 300

Quote:

The MX-64T Dynamixel Robot Servo Actuator is the newest generation of Robotis Dynamixel actuator; equipped with an onboard 32bit 72mhz Cortex M3, a contact-less magnetic encoder with 4x the resolution over the AX/RX series, and up to 3mpbs using the new TTL 2.0 bus. Each servo has the ability to track its speed, temperature, shaft position, voltage, and load.

Information

In cace AX12 you can use This Library

Import libraryAX12

new ax12 lib

Last commit 26 Jun 2014 by Gong Chen

You need this Dependence Library SerialHalfDuplex library too

Import librarySerialHalfDuplex

Serial Half Duplex implementation

Last commit 01 Apr 2014 by Aimen Al-Refai

MX64.cpp@4:acc6261cf054, 2014-05-23 (annotated)

Committer:
ppr2013G2
Date:
Fri May 23 09:02:04 2014 +0000
Revision:
4:acc6261cf054
Child:
5:15f8cd3b7dfb
fork ax12 lib for mx64

Who changed what in which revision?

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