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@8:bf333220b2f1, 2014-06-20 (annotated)

Committer:
ppr2013G2
Date:
Fri Jun 20 06:25:54 2014 +0000
Revision:
8:bf333220b2f1
Parent:
5:15f8cd3b7dfb 
fix

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 5:15f8cd3b7dfb 85 // if flag[0] is set, were blocking
ppr2013G2 5:15f8cd3b7dfb 86 // if flag[1] is set, we're registering
ppr2013G2 5:15f8cd3b7dfb 87 // they are mutually exclusive operations
ppr2013G2 5:15f8cd3b7dfb 88 int MX64::SetGoalSpeed(int degrees, int speed, int flags) {
ppr2013G2 5:15f8cd3b7dfb 89
ppr2013G2 5:15f8cd3b7dfb 90 char reg_flag = 0;
ppr2013G2 5:15f8cd3b7dfb 91 char data[4];
ppr2013G2 5:15f8cd3b7dfb 92
ppr2013G2 5:15f8cd3b7dfb 93 // set the flag is only the register bit is set in the flag
ppr2013G2 5:15f8cd3b7dfb 94 if (flags == 0x2) {
ppr2013G2 5:15f8cd3b7dfb 95 reg_flag = 1;
ppr2013G2 5:15f8cd3b7dfb 96 }
ppr2013G2 5:15f8cd3b7dfb 97
ppr2013G2 5:15f8cd3b7dfb 98 // 1023 / 300 * degrees
ppr2013G2 8:bf333220b2f1 99 short goal = (1023 * degrees) / 360;
ppr2013G2 5:15f8cd3b7dfb 100 short sp = (1023 * speed) / 100;
ppr2013G2 5:15f8cd3b7dfb 101
ppr2013G2 5:15f8cd3b7dfb 102 #ifdef AX12_DEBUG
ppr2013G2 5:15f8cd3b7dfb 103 printf("SetGoalSpeed to 0x%x with speed 0x%x\n",goal,sp);
ppr2013G2 5:15f8cd3b7dfb 104 #endif
ppr2013G2 5:15f8cd3b7dfb 105
ppr2013G2 5:15f8cd3b7dfb 106 data[0] = goal & 0xff; // bottom 8 bits
ppr2013G2 5:15f8cd3b7dfb 107 data[1] = goal >> 8; // top 8 bits
ppr2013G2 5:15f8cd3b7dfb 108
ppr2013G2 5:15f8cd3b7dfb 109 data[2] = sp & 0xff;
ppr2013G2 5:15f8cd3b7dfb 110 data[3] = sp >> 8;
ppr2013G2 5:15f8cd3b7dfb 111
ppr2013G2 5:15f8cd3b7dfb 112 // write the packet, return the error code
ppr2013G2 5:15f8cd3b7dfb 113 int rVal = write(_ID, MX64_REG_GOAL_POSITION, 4, data, reg_flag);
ppr2013G2 5:15f8cd3b7dfb 114
ppr2013G2 5:15f8cd3b7dfb 115 if (flags == 1) {
ppr2013G2 5:15f8cd3b7dfb 116 // block until it comes to a halt
ppr2013G2 5:15f8cd3b7dfb 117 while (isMoving()) {}
ppr2013G2 5:15f8cd3b7dfb 118 }
ppr2013G2 5:15f8cd3b7dfb 119 return(rVal);
ppr2013G2 5:15f8cd3b7dfb 120 }
ppr2013G2 4:acc6261cf054 121
ppr2013G2 4:acc6261cf054 122 // Set continuous rotation speed from -1 to 1
ppr2013G2 4:acc6261cf054 123 int MX64::SetCRSpeed(float speed) {
ppr2013G2 4:acc6261cf054 124
ppr2013G2 4:acc6261cf054 125 // bit 10 = direction, 0 = CCW, 1=CW
ppr2013G2 4:acc6261cf054 126 // bits 9-0 = Speed
ppr2013G2 4:acc6261cf054 127 char data[2];
ppr2013G2 4:acc6261cf054 128
ppr2013G2 4:acc6261cf054 129 int goal = (0x3ff * abs(speed));
ppr2013G2 4:acc6261cf054 130
ppr2013G2 4:acc6261cf054 131 // Set direction CW if we have a negative speed
ppr2013G2 4:acc6261cf054 132 if (speed < 0) {
ppr2013G2 4:acc6261cf054 133 goal |= (0x1 << 10);
ppr2013G2 4:acc6261cf054 134 }
ppr2013G2 4:acc6261cf054 135
ppr2013G2 4:acc6261cf054 136 data[0] = goal & 0xff; // bottom 8 bits
ppr2013G2 4:acc6261cf054 137 data[1] = goal >> 8; // top 8 bits
ppr2013G2 4:acc6261cf054 138
ppr2013G2 4:acc6261cf054 139 // write the packet, return the error code
ppr2013G2 4:acc6261cf054 140 int rVal = write(_ID, 0x20, 2, data);
ppr2013G2 4:acc6261cf054 141
ppr2013G2 4:acc6261cf054 142 return(rVal);
ppr2013G2 4:acc6261cf054 143 }
ppr2013G2 4:acc6261cf054 144
ppr2013G2 4:acc6261cf054 145
ppr2013G2 4:acc6261cf054 146 int MX64::SetCWLimit (int degrees) {
ppr2013G2 4:acc6261cf054 147
ppr2013G2 4:acc6261cf054 148 char data[2];
ppr2013G2 4:acc6261cf054 149
ppr2013G2 4:acc6261cf054 150 // 1023 / 300 * degrees
ppr2013G2 4:acc6261cf054 151 short limit = (4095 * degrees) / 360;
ppr2013G2 4:acc6261cf054 152
ppr2013G2 4:acc6261cf054 153 #ifdef MX64_DEBUG
ppr2013G2 4:acc6261cf054 154 printf("SetCWLimit to 0x%x\n",limit);
ppr2013G2 4:acc6261cf054 155 #endif
ppr2013G2 4:acc6261cf054 156
ppr2013G2 4:acc6261cf054 157 data[0] = limit & 0xff; // bottom 8 bits
ppr2013G2 4:acc6261cf054 158 data[1] = limit >> 8; // top 8 bits
ppr2013G2 4:acc6261cf054 159
ppr2013G2 4:acc6261cf054 160 // write the packet, return the error code
ppr2013G2 4:acc6261cf054 161 return (write(_ID, MX64_REG_CW_LIMIT, 2, data));
ppr2013G2 4:acc6261cf054 162
ppr2013G2 4:acc6261cf054 163 }
ppr2013G2 4:acc6261cf054 164
ppr2013G2 4:acc6261cf054 165 int MX64::SetCCWLimit (int degrees) {
ppr2013G2 4:acc6261cf054 166
ppr2013G2 4:acc6261cf054 167 char data[2];
ppr2013G2 4:acc6261cf054 168
ppr2013G2 4:acc6261cf054 169 // 1023 / 300 * degrees
ppr2013G2 4:acc6261cf054 170 short limit = (4095 * degrees) / 360;
ppr2013G2 4:acc6261cf054 171
ppr2013G2 4:acc6261cf054 172 #ifdef MX64_DEBUG
ppr2013G2 4:acc6261cf054 173 printf("SetCCWLimit to 0x%x\n",limit);
ppr2013G2 4:acc6261cf054 174 #endif
ppr2013G2 4:acc6261cf054 175
ppr2013G2 4:acc6261cf054 176 data[0] = limit & 0xff; // bottom 8 bits
ppr2013G2 4:acc6261cf054 177 data[1] = limit >> 8; // top 8 bits
ppr2013G2 4:acc6261cf054 178
ppr2013G2 4:acc6261cf054 179 // write the packet, return the error code
ppr2013G2 4:acc6261cf054 180 return (write(_ID, MX64_REG_CCW_LIMIT, 2, data));
ppr2013G2 4:acc6261cf054 181 }
ppr2013G2 4:acc6261cf054 182
ppr2013G2 4:acc6261cf054 183
ppr2013G2 4:acc6261cf054 184 int MX64::SetID (int CurrentID, int NewID) {
ppr2013G2 4:acc6261cf054 185
ppr2013G2 4:acc6261cf054 186 char data[1];
ppr2013G2 4:acc6261cf054 187 data[0] = NewID;
ppr2013G2 4:acc6261cf054 188
ppr2013G2 4:acc6261cf054 189 #ifdef MX64_DEBUG
ppr2013G2 4:acc6261cf054 190 printf("Setting ID from 0x%x to 0x%x\n",CurrentID,NewID);
ppr2013G2 4:acc6261cf054 191 #endif
ppr2013G2 4:acc6261cf054 192
ppr2013G2 4:acc6261cf054 193 return (write(CurrentID, MX64_REG_ID, 1, data));
ppr2013G2 4:acc6261cf054 194
ppr2013G2 4:acc6261cf054 195 }
ppr2013G2 4:acc6261cf054 196
ppr2013G2 4:acc6261cf054 197
ppr2013G2 4:acc6261cf054 198 int MX64::SetBaud (int baud) {
ppr2013G2 4:acc6261cf054 199
ppr2013G2 4:acc6261cf054 200 char data[1];
ppr2013G2 4:acc6261cf054 201 data[0] = baud;
ppr2013G2 4:acc6261cf054 202
ppr2013G2 4:acc6261cf054 203 #ifdef MX64_DEBUG
ppr2013G2 4:acc6261cf054 204 printf("Setting Baud rate to %d\n",baud);
ppr2013G2 4:acc6261cf054 205 #endif
ppr2013G2 4:acc6261cf054 206
ppr2013G2 4:acc6261cf054 207 return (write(0xFE, MX64_REG_BAUD, 1, data));
ppr2013G2 4:acc6261cf054 208
ppr2013G2 4:acc6261cf054 209 }
ppr2013G2 4:acc6261cf054 210
ppr2013G2 4:acc6261cf054 211
ppr2013G2 4:acc6261cf054 212
ppr2013G2 4:acc6261cf054 213 // return 1 is the servo is still in flight
ppr2013G2 4:acc6261cf054 214 int MX64::isMoving(void) {
ppr2013G2 4:acc6261cf054 215
ppr2013G2 4:acc6261cf054 216 char data[1];
ppr2013G2 4:acc6261cf054 217 read(_ID,MX64_REG_MOVING,1,data);
ppr2013G2 4:acc6261cf054 218 return(data[0]);
ppr2013G2 4:acc6261cf054 219 }
ppr2013G2 4:acc6261cf054 220
ppr2013G2 4:acc6261cf054 221
ppr2013G2 4:acc6261cf054 222 void MX64::trigger(void) {
ppr2013G2 4:acc6261cf054 223
ppr2013G2 4:acc6261cf054 224 char TxBuf[16];
ppr2013G2 4:acc6261cf054 225 char sum = 0;
ppr2013G2 4:acc6261cf054 226
ppr2013G2 4:acc6261cf054 227 #ifdef MX64_TRIGGER_DEBUG
ppr2013G2 4:acc6261cf054 228 // Build the TxPacket first in RAM, then we'll send in one go
ppr2013G2 4:acc6261cf054 229 printf("\nTriggered\n");
ppr2013G2 4:acc6261cf054 230 printf("\nTrigger Packet\n Header : 0xFF, 0xFF\n");
ppr2013G2 4:acc6261cf054 231 #endif
ppr2013G2 4:acc6261cf054 232
ppr2013G2 4:acc6261cf054 233 TxBuf[0] = 0xFF;
ppr2013G2 4:acc6261cf054 234 TxBuf[1] = 0xFF;
ppr2013G2 4:acc6261cf054 235
ppr2013G2 4:acc6261cf054 236 // ID - Broadcast
ppr2013G2 4:acc6261cf054 237 TxBuf[2] = 0xFE;
ppr2013G2 4:acc6261cf054 238 sum += TxBuf[2];
ppr2013G2 4:acc6261cf054 239
ppr2013G2 4:acc6261cf054 240 #ifdef MX64_TRIGGER_DEBUG
ppr2013G2 4:acc6261cf054 241 printf(" ID : %d\n",TxBuf[2]);
ppr2013G2 4:acc6261cf054 242 #endif
ppr2013G2 4:acc6261cf054 243
ppr2013G2 4:acc6261cf054 244 // Length
ppr2013G2 4:acc6261cf054 245 TxBuf[3] = 0x02;
ppr2013G2 4:acc6261cf054 246 sum += TxBuf[3];
ppr2013G2 4:acc6261cf054 247
ppr2013G2 4:acc6261cf054 248 #ifdef MX64_TRIGGER_DEBUG
ppr2013G2 4:acc6261cf054 249 printf(" Length %d\n",TxBuf[3]);
ppr2013G2 4:acc6261cf054 250 #endif
ppr2013G2 4:acc6261cf054 251
ppr2013G2 4:acc6261cf054 252 // Instruction - ACTION
ppr2013G2 4:acc6261cf054 253 TxBuf[4] = 0x04;
ppr2013G2 4:acc6261cf054 254 sum += TxBuf[4];
ppr2013G2 4:acc6261cf054 255
ppr2013G2 4:acc6261cf054 256 #ifdef MX64_TRIGGER_DEBUG
ppr2013G2 4:acc6261cf054 257 printf(" Instruction 0x%X\n",TxBuf[5]);
ppr2013G2 4:acc6261cf054 258 #endif
ppr2013G2 4:acc6261cf054 259
ppr2013G2 4:acc6261cf054 260 // Checksum
ppr2013G2 4:acc6261cf054 261 TxBuf[5] = 0xFF - sum;
ppr2013G2 4:acc6261cf054 262 #ifdef MX64_TRIGGER_DEBUG
ppr2013G2 4:acc6261cf054 263 printf(" Checksum 0x%X\n",TxBuf[5]);
ppr2013G2 4:acc6261cf054 264 #endif
ppr2013G2 4:acc6261cf054 265
ppr2013G2 4:acc6261cf054 266 // Transmit the packet in one burst with no pausing
ppr2013G2 4:acc6261cf054 267 for (int i = 0; i < 6 ; i++) {
ppr2013G2 4:acc6261cf054 268 _mx64.putc(TxBuf[i]);
ppr2013G2 4:acc6261cf054 269 }
ppr2013G2 4:acc6261cf054 270
ppr2013G2 4:acc6261cf054 271 // This is a broadcast packet, so there will be no reply
ppr2013G2 4:acc6261cf054 272 return;
ppr2013G2 4:acc6261cf054 273 }
ppr2013G2 4:acc6261cf054 274
ppr2013G2 4:acc6261cf054 275
ppr2013G2 4:acc6261cf054 276 float MX64::GetPosition(void) {
ppr2013G2 4:acc6261cf054 277
ppr2013G2 4:acc6261cf054 278 #ifdef MX64_DEBUG
ppr2013G2 4:acc6261cf054 279 printf("\nGetPosition(%d)",_ID);
ppr2013G2 4:acc6261cf054 280 #endif
ppr2013G2 4:acc6261cf054 281
ppr2013G2 4:acc6261cf054 282 char data[2];
ppr2013G2 4:acc6261cf054 283
ppr2013G2 4:acc6261cf054 284 int ErrorCode = read(_ID, MX64_REG_POSITION, 2, data);
ppr2013G2 4:acc6261cf054 285 short position = data[0] + (data[1] << 8);
ppr2013G2 4:acc6261cf054 286 float angle = (position * 360)/4096;
ppr2013G2 4:acc6261cf054 287
ppr2013G2 4:acc6261cf054 288 return (angle);
ppr2013G2 4:acc6261cf054 289 }
ppr2013G2 4:acc6261cf054 290
ppr2013G2 4:acc6261cf054 291
ppr2013G2 4:acc6261cf054 292 float MX64::GetTemp (void) {
ppr2013G2 4:acc6261cf054 293
ppr2013G2 4:acc6261cf054 294 #ifdef MX64_DEBUG
ppr2013G2 4:acc6261cf054 295 printf("\nGetTemp(%d)",_ID);
ppr2013G2 4:acc6261cf054 296 #endif
ppr2013G2 4:acc6261cf054 297
ppr2013G2 4:acc6261cf054 298 char data[1];
ppr2013G2 4:acc6261cf054 299 int ErrorCode = read(_ID, MX64_REG_TEMP, 1, data);
ppr2013G2 4:acc6261cf054 300 float temp = data[0];
ppr2013G2 4:acc6261cf054 301 return(temp);
ppr2013G2 4:acc6261cf054 302 }
ppr2013G2 4:acc6261cf054 303
ppr2013G2 4:acc6261cf054 304
ppr2013G2 4:acc6261cf054 305 float MX64::GetVolts (void) {
ppr2013G2 4:acc6261cf054 306
ppr2013G2 4:acc6261cf054 307 #ifdef MX64_DEBUG
ppr2013G2 4:acc6261cf054 308 printf("\nGetVolts(%d)",_ID);
ppr2013G2 4:acc6261cf054 309 #endif
ppr2013G2 4:acc6261cf054 310
ppr2013G2 4:acc6261cf054 311 char data[1];
ppr2013G2 4:acc6261cf054 312 int ErrorCode = read(_ID, MX64_REG_VOLTS, 1, data);
ppr2013G2 4:acc6261cf054 313 float volts = data[0]/10.0;
ppr2013G2 4:acc6261cf054 314 return(volts);
ppr2013G2 4:acc6261cf054 315 }
ppr2013G2 4:acc6261cf054 316
ppr2013G2 4:acc6261cf054 317
ppr2013G2 4:acc6261cf054 318 int MX64::read(int ID, int start, int bytes, char* data) {
ppr2013G2 4:acc6261cf054 319
ppr2013G2 4:acc6261cf054 320 char PacketLength = 0x4;
ppr2013G2 4:acc6261cf054 321 char TxBuf[16];
ppr2013G2 4:acc6261cf054 322 char sum = 0;
ppr2013G2 4:acc6261cf054 323 char Status[16];
ppr2013G2 4:acc6261cf054 324
ppr2013G2 4:acc6261cf054 325 Status[4] = 0xFE; // return code
ppr2013G2 4:acc6261cf054 326
ppr2013G2 4:acc6261cf054 327 #ifdef MX64_READ_DEBUG
ppr2013G2 4:acc6261cf054 328 printf("\nread(%d,0x%x,%d,data)\n",ID,start,bytes);
ppr2013G2 4:acc6261cf054 329 #endif
ppr2013G2 4:acc6261cf054 330
ppr2013G2 4:acc6261cf054 331 // Build the TxPacket first in RAM, then we'll send in one go
ppr2013G2 4:acc6261cf054 332 #ifdef MX64_READ_DEBUG
ppr2013G2 4:acc6261cf054 333 printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n");
ppr2013G2 4:acc6261cf054 334 #endif
ppr2013G2 4:acc6261cf054 335
ppr2013G2 4:acc6261cf054 336 TxBuf[0] = 0xff;
ppr2013G2 4:acc6261cf054 337 TxBuf[1] = 0xff;
ppr2013G2 4:acc6261cf054 338
ppr2013G2 4:acc6261cf054 339 // ID
ppr2013G2 4:acc6261cf054 340 TxBuf[2] = ID;
ppr2013G2 4:acc6261cf054 341 sum += TxBuf[2];
ppr2013G2 4:acc6261cf054 342
ppr2013G2 4:acc6261cf054 343 #ifdef MX64_READ_DEBUG
ppr2013G2 4:acc6261cf054 344 printf(" ID : %d\n",TxBuf[2]);
ppr2013G2 4:acc6261cf054 345 #endif
ppr2013G2 4:acc6261cf054 346
ppr2013G2 4:acc6261cf054 347 // Packet Length
ppr2013G2 4:acc6261cf054 348 TxBuf[3] = PacketLength; // Length = 4 ; 2 + 1 (start) = 1 (bytes)
ppr2013G2 4:acc6261cf054 349 sum += TxBuf[3]; // Accululate the packet sum
ppr2013G2 4:acc6261cf054 350
ppr2013G2 4:acc6261cf054 351 #ifdef MX64_READ_DEBUG
ppr2013G2 4:acc6261cf054 352 printf(" Length : 0x%x\n",TxBuf[3]);
ppr2013G2 4:acc6261cf054 353 #endif
ppr2013G2 4:acc6261cf054 354
ppr2013G2 4:acc6261cf054 355 // Instruction - Read
ppr2013G2 4:acc6261cf054 356 TxBuf[4] = 0x2;
ppr2013G2 4:acc6261cf054 357 sum += TxBuf[4];
ppr2013G2 4:acc6261cf054 358
ppr2013G2 4:acc6261cf054 359 #ifdef MX64_READ_DEBUG
ppr2013G2 4:acc6261cf054 360 printf(" Instruction : 0x%x\n",TxBuf[4]);
ppr2013G2 4:acc6261cf054 361 #endif
ppr2013G2 4:acc6261cf054 362
ppr2013G2 4:acc6261cf054 363 // Start Address
ppr2013G2 4:acc6261cf054 364 TxBuf[5] = start;
ppr2013G2 4:acc6261cf054 365 sum += TxBuf[5];
ppr2013G2 4:acc6261cf054 366
ppr2013G2 4:acc6261cf054 367 #ifdef MX64_READ_DEBUG
ppr2013G2 4:acc6261cf054 368 printf(" Start Address : 0x%x\n",TxBuf[5]);
ppr2013G2 4:acc6261cf054 369 #endif
ppr2013G2 4:acc6261cf054 370
ppr2013G2 4:acc6261cf054 371 // Bytes to read
ppr2013G2 4:acc6261cf054 372 TxBuf[6] = bytes;
ppr2013G2 4:acc6261cf054 373 sum += TxBuf[6];
ppr2013G2 4:acc6261cf054 374
ppr2013G2 4:acc6261cf054 375 #ifdef MX64_READ_DEBUG
ppr2013G2 4:acc6261cf054 376 printf(" No bytes : 0x%x\n",TxBuf[6]);
ppr2013G2 4:acc6261cf054 377 #endif
ppr2013G2 4:acc6261cf054 378
ppr2013G2 4:acc6261cf054 379 // Checksum
ppr2013G2 4:acc6261cf054 380 TxBuf[7] = 0xFF - sum;
ppr2013G2 4:acc6261cf054 381 #ifdef MX64_READ_DEBUG
ppr2013G2 4:acc6261cf054 382 printf(" Checksum : 0x%x\n",TxBuf[7]);
ppr2013G2 4:acc6261cf054 383 #endif
ppr2013G2 4:acc6261cf054 384
ppr2013G2 4:acc6261cf054 385 // Transmit the packet in one burst with no pausing
ppr2013G2 4:acc6261cf054 386 for (int i = 0; i<8 ; i++) {
ppr2013G2 4:acc6261cf054 387 _mx64.putc(TxBuf[i]);
ppr2013G2 4:acc6261cf054 388 }
ppr2013G2 4:acc6261cf054 389
ppr2013G2 4:acc6261cf054 390 // Wait for the bytes to be transmitted
ppr2013G2 4:acc6261cf054 391 wait (0.00002);
ppr2013G2 4:acc6261cf054 392
ppr2013G2 4:acc6261cf054 393 // Skip if the read was to the broadcast address
ppr2013G2 4:acc6261cf054 394 if (_ID != 0xFE) {
ppr2013G2 4:acc6261cf054 395
ppr2013G2 4:acc6261cf054 396
ppr2013G2 4:acc6261cf054 397
ppr2013G2 4:acc6261cf054 398 // response packet is always 6 + bytes
ppr2013G2 4:acc6261cf054 399 // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum
ppr2013G2 4:acc6261cf054 400 // timeout is a little more than the time to transmit
ppr2013G2 4:acc6261cf054 401 // the packet back, i.e. (6+bytes)*10 bit periods
ppr2013G2 4:acc6261cf054 402
ppr2013G2 4:acc6261cf054 403 int timeout = 0;
ppr2013G2 4:acc6261cf054 404 int plen = 0;
ppr2013G2 4:acc6261cf054 405 while ((timeout < ((6+bytes)*10)) && (plen<(6+bytes))) {
ppr2013G2 4:acc6261cf054 406
ppr2013G2 4:acc6261cf054 407 if (_mx64.readable()) {
ppr2013G2 4:acc6261cf054 408 Status[plen] = _mx64.getc();
ppr2013G2 4:acc6261cf054 409 plen++;
ppr2013G2 4:acc6261cf054 410 timeout = 0;
ppr2013G2 4:acc6261cf054 411 }
ppr2013G2 4:acc6261cf054 412
ppr2013G2 4:acc6261cf054 413 // wait for the bit period
ppr2013G2 4:acc6261cf054 414 wait (1.0/_baud);
ppr2013G2 4:acc6261cf054 415 timeout++;
ppr2013G2 4:acc6261cf054 416 }
ppr2013G2 4:acc6261cf054 417
ppr2013G2 4:acc6261cf054 418 if (timeout == ((6+bytes)*10) ) {
ppr2013G2 4:acc6261cf054 419 return(-1);
ppr2013G2 4:acc6261cf054 420 }
ppr2013G2 4:acc6261cf054 421
ppr2013G2 4:acc6261cf054 422 // Copy the data from Status into data for return
ppr2013G2 4:acc6261cf054 423 for (int i=0; i < Status[3]-2 ; i++) {
ppr2013G2 4:acc6261cf054 424 data[i] = Status[5+i];
ppr2013G2 4:acc6261cf054 425 }
ppr2013G2 4:acc6261cf054 426
ppr2013G2 4:acc6261cf054 427 #ifdef MX64_READ_DEBUG
ppr2013G2 4:acc6261cf054 428 printf("\nStatus Packet\n");
ppr2013G2 4:acc6261cf054 429 printf(" Header : 0x%x\n",Status[0]);
ppr2013G2 4:acc6261cf054 430 printf(" Header : 0x%x\n",Status[1]);
ppr2013G2 4:acc6261cf054 431 printf(" ID : 0x%x\n",Status[2]);
ppr2013G2 4:acc6261cf054 432 printf(" Length : 0x%x\n",Status[3]);
ppr2013G2 4:acc6261cf054 433 printf(" Error Code : 0x%x\n",Status[4]);
ppr2013G2 4:acc6261cf054 434
ppr2013G2 4:acc6261cf054 435 for (int i=0; i < Status[3]-2 ; i++) {
ppr2013G2 4:acc6261cf054 436 printf(" Data : 0x%x\n",Status[5+i]);
ppr2013G2 4:acc6261cf054 437 }
ppr2013G2 4:acc6261cf054 438
ppr2013G2 4:acc6261cf054 439 printf(" Checksum : 0x%x\n",Status[5+(Status[3]-2)]);
ppr2013G2 4:acc6261cf054 440 #endif
ppr2013G2 4:acc6261cf054 441
ppr2013G2 4:acc6261cf054 442 } // if (ID!=0xFE)
ppr2013G2 4:acc6261cf054 443
ppr2013G2 4:acc6261cf054 444 return(Status[4]);
ppr2013G2 4:acc6261cf054 445 }
ppr2013G2 4:acc6261cf054 446
ppr2013G2 4:acc6261cf054 447
ppr2013G2 4:acc6261cf054 448 int MX64::write(int ID, int start, int bytes, char* data, int flag) {
ppr2013G2 4:acc6261cf054 449 // 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum
ppr2013G2 4:acc6261cf054 450
ppr2013G2 4:acc6261cf054 451 char TxBuf[16];
ppr2013G2 4:acc6261cf054 452 char sum = 0;
ppr2013G2 4:acc6261cf054 453 char Status[6];
ppr2013G2 4:acc6261cf054 454
ppr2013G2 4:acc6261cf054 455 #ifdef MX64_WRITE_DEBUG
ppr2013G2 4:acc6261cf054 456 printf("\nwrite(%d,0x%x,%d,data,%d)\n",ID,start,bytes,flag);
ppr2013G2 4:acc6261cf054 457 #endif
ppr2013G2 4:acc6261cf054 458
ppr2013G2 4:acc6261cf054 459 // Build the TxPacket first in RAM, then we'll send in one go
ppr2013G2 4:acc6261cf054 460 #ifdef MX64_WRITE_DEBUG
ppr2013G2 4:acc6261cf054 461 printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n");
ppr2013G2 4:acc6261cf054 462 #endif
ppr2013G2 4:acc6261cf054 463
ppr2013G2 4:acc6261cf054 464 TxBuf[0] = 0xff;
ppr2013G2 4:acc6261cf054 465 TxBuf[1] = 0xff;
ppr2013G2 4:acc6261cf054 466
ppr2013G2 4:acc6261cf054 467 // ID
ppr2013G2 4:acc6261cf054 468 TxBuf[2] = ID;
ppr2013G2 4:acc6261cf054 469 sum += TxBuf[2];
ppr2013G2 4:acc6261cf054 470
ppr2013G2 4:acc6261cf054 471 #ifdef MX64_WRITE_DEBUG
ppr2013G2 4:acc6261cf054 472 printf(" ID : %d\n",TxBuf[2]);
ppr2013G2 4:acc6261cf054 473 #endif
ppr2013G2 4:acc6261cf054 474
ppr2013G2 4:acc6261cf054 475 // packet Length
ppr2013G2 4:acc6261cf054 476 TxBuf[3] = 3+bytes;
ppr2013G2 4:acc6261cf054 477 sum += TxBuf[3];
ppr2013G2 4:acc6261cf054 478
ppr2013G2 4:acc6261cf054 479 #ifdef MX64_WRITE_DEBUG
ppr2013G2 4:acc6261cf054 480 printf(" Length : %d\n",TxBuf[3]);
ppr2013G2 4:acc6261cf054 481 #endif
ppr2013G2 4:acc6261cf054 482
ppr2013G2 4:acc6261cf054 483 // Instruction
ppr2013G2 4:acc6261cf054 484 if (flag == 1) {
ppr2013G2 4:acc6261cf054 485 TxBuf[4]=0x04;
ppr2013G2 4:acc6261cf054 486 sum += TxBuf[4];
ppr2013G2 4:acc6261cf054 487 } else {
ppr2013G2 4:acc6261cf054 488 TxBuf[4]=0x03;
ppr2013G2 4:acc6261cf054 489 sum += TxBuf[4];
ppr2013G2 4:acc6261cf054 490 }
ppr2013G2 4:acc6261cf054 491
ppr2013G2 4:acc6261cf054 492 #ifdef MX64_WRITE_DEBUG
ppr2013G2 4:acc6261cf054 493 printf(" Instruction : 0x%x\n",TxBuf[4]);
ppr2013G2 4:acc6261cf054 494 #endif
ppr2013G2 4:acc6261cf054 495
ppr2013G2 4:acc6261cf054 496 // Start Address
ppr2013G2 4:acc6261cf054 497 TxBuf[5] = start;
ppr2013G2 4:acc6261cf054 498 sum += TxBuf[5];
ppr2013G2 4:acc6261cf054 499
ppr2013G2 4:acc6261cf054 500 #ifdef MX64_WRITE_DEBUG
ppr2013G2 4:acc6261cf054 501 printf(" Start : 0x%x\n",TxBuf[5]);
ppr2013G2 4:acc6261cf054 502 #endif
ppr2013G2 4:acc6261cf054 503
ppr2013G2 4:acc6261cf054 504 // data
ppr2013G2 4:acc6261cf054 505 for (char i=0; i<bytes ; i++) {
ppr2013G2 4:acc6261cf054 506 TxBuf[6+i] = data[i];
ppr2013G2 4:acc6261cf054 507 sum += TxBuf[6+i];
ppr2013G2 4:acc6261cf054 508
ppr2013G2 4:acc6261cf054 509 #ifdef MX64_WRITE_DEBUG
ppr2013G2 4:acc6261cf054 510 printf(" Data : 0x%x\n",TxBuf[6+i]);
ppr2013G2 4:acc6261cf054 511 #endif
ppr2013G2 4:acc6261cf054 512
ppr2013G2 4:acc6261cf054 513 }
ppr2013G2 4:acc6261cf054 514
ppr2013G2 4:acc6261cf054 515 // checksum
ppr2013G2 4:acc6261cf054 516 TxBuf[6+bytes] = 0xFF - sum;
ppr2013G2 4:acc6261cf054 517
ppr2013G2 4:acc6261cf054 518 #ifdef MX64_WRITE_DEBUG
ppr2013G2 4:acc6261cf054 519 printf(" Checksum : 0x%x\n",TxBuf[6+bytes]);
ppr2013G2 4:acc6261cf054 520 #endif
ppr2013G2 4:acc6261cf054 521
ppr2013G2 4:acc6261cf054 522 // Transmit the packet in one burst with no pausing
ppr2013G2 4:acc6261cf054 523 for (int i = 0; i < (7 + bytes) ; i++) {
ppr2013G2 4:acc6261cf054 524 _mx64.putc(TxBuf[i]);
ppr2013G2 4:acc6261cf054 525 }
ppr2013G2 4:acc6261cf054 526
ppr2013G2 4:acc6261cf054 527 // Wait for data to transmit
ppr2013G2 4:acc6261cf054 528 wait (0.00002);
ppr2013G2 4:acc6261cf054 529
ppr2013G2 4:acc6261cf054 530 // make sure we have a valid return
ppr2013G2 4:acc6261cf054 531 Status[4]=0x00;
ppr2013G2 4:acc6261cf054 532
ppr2013G2 4:acc6261cf054 533 // we'll only get a reply if it was not broadcast
ppr2013G2 4:acc6261cf054 534 if (_ID!=0xFE) {
ppr2013G2 4:acc6261cf054 535
ppr2013G2 4:acc6261cf054 536
ppr2013G2 4:acc6261cf054 537 // response packet is always 6 bytes
ppr2013G2 4:acc6261cf054 538 // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum
ppr2013G2 4:acc6261cf054 539 // timeout is a little more than the time to transmit
ppr2013G2 4:acc6261cf054 540 // the packet back, i.e. 60 bit periods, round up to 100
ppr2013G2 4:acc6261cf054 541 int timeout = 0;
ppr2013G2 4:acc6261cf054 542 int plen = 0;
ppr2013G2 4:acc6261cf054 543 while ((timeout < 100) && (plen<6)) {
ppr2013G2 4:acc6261cf054 544
ppr2013G2 4:acc6261cf054 545 if (_mx64.readable()) {
ppr2013G2 4:acc6261cf054 546 Status[plen] = _mx64.getc();
ppr2013G2 4:acc6261cf054 547 plen++;
ppr2013G2 4:acc6261cf054 548 timeout = 0;
ppr2013G2 4:acc6261cf054 549 }
ppr2013G2 4:acc6261cf054 550
ppr2013G2 4:acc6261cf054 551 // wait for the bit period
ppr2013G2 4:acc6261cf054 552 wait (1.0/_baud);
ppr2013G2 4:acc6261cf054 553 timeout++;
ppr2013G2 4:acc6261cf054 554 }
ppr2013G2 4:acc6261cf054 555
ppr2013G2 4:acc6261cf054 556
ppr2013G2 4:acc6261cf054 557 // Build the TxPacket first in RAM, then we'll send in one go
ppr2013G2 4:acc6261cf054 558 #ifdef MX64_WRITE_DEBUG
ppr2013G2 4:acc6261cf054 559 printf("\nStatus Packet\n Header : 0x%X, 0x%X\n",Status[0],Status[1]);
ppr2013G2 4:acc6261cf054 560 printf(" ID : %d\n",Status[2]);
ppr2013G2 4:acc6261cf054 561 printf(" Length : %d\n",Status[3]);
ppr2013G2 4:acc6261cf054 562 printf(" Error : 0x%x\n",Status[4]);
ppr2013G2 4:acc6261cf054 563 printf(" Checksum : 0x%x\n",Status[5]);
ppr2013G2 4:acc6261cf054 564 #endif
ppr2013G2 4:acc6261cf054 565
ppr2013G2 4:acc6261cf054 566
ppr2013G2 4:acc6261cf054 567 }
ppr2013G2 4:acc6261cf054 568
ppr2013G2 4:acc6261cf054 569 return(Status[4]); // return error code
ppr2013G2 4:acc6261cf054 570 }