sinyuu masahito / Mbed 2 deprecated yozakura_arm

Arm control program for Yozakura

Dependencies:   mbed

Dynamixel/MX28.cpp@0:6b3497b2f2ec, 2015-04-24 (annotated)

Committer:
masasin
Date:
Fri Apr 24 01:55:32 2015 +0000
Revision:
0:6b3497b2f2ec
Initial commit

Who changed what in which revision?

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