sinyuu masahito / Mbed 2 deprecated yozakura_arm

Arm control program for Yozakura

Dependencies:   mbed

Dynamixel/AX12.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 AX12::AX12(PinName tx, PinName rx, int ID) : _ax12(tx,rx) {
masasin 0:6b3497b2f2ec 28
masasin 0:6b3497b2f2ec 29 _ax12.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 AX12::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(300);
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 AX12::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 // 1023 / 300 * degrees
masasin 0:6b3497b2f2ec 65 short goal = (1023 * degrees) / 300;
masasin 0:6b3497b2f2ec 66 if (AX12_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, AX12_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 AX12::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 AX12::SetCWLimit (int degrees) {
masasin 0:6b3497b2f2ec 109
masasin 0:6b3497b2f2ec 110 char data[2];
masasin 0:6b3497b2f2ec 111
masasin 0:6b3497b2f2ec 112 // 1023 / 300 * degrees
masasin 0:6b3497b2f2ec 113 short limit = (1023 * degrees) / 300;
masasin 0:6b3497b2f2ec 114
masasin 0:6b3497b2f2ec 115 if (AX12_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, AX12_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 AX12::SetCCWLimit (int degrees) {
masasin 0:6b3497b2f2ec 129
masasin 0:6b3497b2f2ec 130 char data[2];
masasin 0:6b3497b2f2ec 131
masasin 0:6b3497b2f2ec 132 // 1023 / 300 * degrees
masasin 0:6b3497b2f2ec 133 short limit = (1023 * degrees) / 300;
masasin 0:6b3497b2f2ec 134
masasin 0:6b3497b2f2ec 135 if (AX12_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, AX12_REG_CCW_LIMIT, 2, data));
masasin 0:6b3497b2f2ec 144 }
masasin 0:6b3497b2f2ec 145
masasin 0:6b3497b2f2ec 146
masasin 0:6b3497b2f2ec 147 int AX12::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 (AX12_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, AX12_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 AX12::isMoving(void) {
masasin 0:6b3497b2f2ec 161
masasin 0:6b3497b2f2ec 162 char data[1];
masasin 0:6b3497b2f2ec 163 read(_ID,AX12_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 AX12::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 (AX12_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 (AX12_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 (AX12_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 (AX12_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 (AX12_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 (AX12_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 _ax12.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 AX12::GetPosition(void) {
masasin 0:6b3497b2f2ec 225 if (AX12_DEBUG) {
masasin 0:6b3497b2f2ec 226 printf("\nGetPosition(%d)",_ID);
masasin 0:6b3497b2f2ec 227 }
masasin 0:6b3497b2f2ec 228
masasin 0:6b3497b2f2ec 229 char data[2];
masasin 0:6b3497b2f2ec 230
masasin 0:6b3497b2f2ec 231 int ErrorCode = read(_ID, AX12_REG_POSITION, 2, data);
masasin 0:6b3497b2f2ec 232 short position = data[0] + (data[1] << 8);
masasin 0:6b3497b2f2ec 233 float angle = (position * 300)/1024;
masasin 0:6b3497b2f2ec 234
masasin 0:6b3497b2f2ec 235 return (angle);
masasin 0:6b3497b2f2ec 236 }
masasin 0:6b3497b2f2ec 237
masasin 0:6b3497b2f2ec 238
masasin 0:6b3497b2f2ec 239 float AX12::GetTemp (void) {
masasin 0:6b3497b2f2ec 240
masasin 0:6b3497b2f2ec 241 if (AX12_DEBUG) {
masasin 0:6b3497b2f2ec 242 printf("\nGetTemp(%d)",_ID);
masasin 0:6b3497b2f2ec 243 }
masasin 0:6b3497b2f2ec 244 char data[1];
masasin 0:6b3497b2f2ec 245 int ErrorCode = read(_ID, AX12_REG_TEMP, 1, data);
masasin 0:6b3497b2f2ec 246 float temp = data[0];
masasin 0:6b3497b2f2ec 247 return(temp);
masasin 0:6b3497b2f2ec 248 }
masasin 0:6b3497b2f2ec 249
masasin 0:6b3497b2f2ec 250
masasin 0:6b3497b2f2ec 251 float AX12::GetVolts (void) {
masasin 0:6b3497b2f2ec 252 if (AX12_DEBUG) {
masasin 0:6b3497b2f2ec 253 printf("\nGetVolts(%d)",_ID);
masasin 0:6b3497b2f2ec 254 }
masasin 0:6b3497b2f2ec 255 char data[1];
masasin 0:6b3497b2f2ec 256 int ErrorCode = read(_ID, AX12_REG_VOLTS, 1, data);
masasin 0:6b3497b2f2ec 257 float volts = data[0]/10.0;
masasin 0:6b3497b2f2ec 258 return(volts);
masasin 0:6b3497b2f2ec 259 }
masasin 0:6b3497b2f2ec 260
masasin 0:6b3497b2f2ec 261 float AX12::GetCurrent (void) {
masasin 0:6b3497b2f2ec 262 return 0;
masasin 0:6b3497b2f2ec 263 }
masasin 0:6b3497b2f2ec 264
masasin 0:6b3497b2f2ec 265
masasin 0:6b3497b2f2ec 266 int AX12::TorqueEnable (int mode) {
masasin 0:6b3497b2f2ec 267
masasin 0:6b3497b2f2ec 268 char data[1];
masasin 0:6b3497b2f2ec 269 data[0] = mode;
masasin 0:6b3497b2f2ec 270
masasin 0:6b3497b2f2ec 271 return (write(_ID, AX12_REG_TORQUE_ENABLE, 1, data));
masasin 0:6b3497b2f2ec 272 }
masasin 0:6b3497b2f2ec 273
masasin 0:6b3497b2f2ec 274
masasin 0:6b3497b2f2ec 275 int AX12::SetTorqueLimit (float torque_lim) {
masasin 0:6b3497b2f2ec 276
masasin 0:6b3497b2f2ec 277 short limit = torque_lim * 1023;
masasin 0:6b3497b2f2ec 278 char data[2];
masasin 0:6b3497b2f2ec 279 data[0] = limit & 0xff; // bottom 8 bits
masasin 0:6b3497b2f2ec 280 data[1] = limit >> 8; // top 8 bits
masasin 0:6b3497b2f2ec 281
masasin 0:6b3497b2f2ec 282 return (write(_ID, AX12_REG_TORQUE_LIMIT, 2, data));
masasin 0:6b3497b2f2ec 283 }
masasin 0:6b3497b2f2ec 284
masasin 0:6b3497b2f2ec 285
masasin 0:6b3497b2f2ec 286 int AX12::read(int ID, int start, int bytes, char* data) {
masasin 0:6b3497b2f2ec 287
masasin 0:6b3497b2f2ec 288 char PacketLength = 0x4;
masasin 0:6b3497b2f2ec 289 char TxBuf[16];
masasin 0:6b3497b2f2ec 290 char sum = 0;
masasin 0:6b3497b2f2ec 291 char Status[16];
masasin 0:6b3497b2f2ec 292
masasin 0:6b3497b2f2ec 293 Status[4] = 0xFE; // return code
masasin 0:6b3497b2f2ec 294
masasin 0:6b3497b2f2ec 295 if (AX12_READ_DEBUG) {
masasin 0:6b3497b2f2ec 296 printf("\nread(%d,0x%x,%d,data)\n",ID,start,bytes);
masasin 0:6b3497b2f2ec 297 }
masasin 0:6b3497b2f2ec 298
masasin 0:6b3497b2f2ec 299 // Build the TxPacket first in RAM, then we'll send in one go
masasin 0:6b3497b2f2ec 300 if (AX12_READ_DEBUG) {
masasin 0:6b3497b2f2ec 301 printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n");
masasin 0:6b3497b2f2ec 302 }
masasin 0:6b3497b2f2ec 303
masasin 0:6b3497b2f2ec 304 TxBuf[0] = 0xff;
masasin 0:6b3497b2f2ec 305 TxBuf[1] = 0xff;
masasin 0:6b3497b2f2ec 306
masasin 0:6b3497b2f2ec 307 // ID
masasin 0:6b3497b2f2ec 308 TxBuf[2] = ID;
masasin 0:6b3497b2f2ec 309 sum += TxBuf[2];
masasin 0:6b3497b2f2ec 310 if (AX12_READ_DEBUG) {
masasin 0:6b3497b2f2ec 311 printf(" ID : %d\n",TxBuf[2]);
masasin 0:6b3497b2f2ec 312 }
masasin 0:6b3497b2f2ec 313
masasin 0:6b3497b2f2ec 314 // Packet Length
masasin 0:6b3497b2f2ec 315 TxBuf[3] = PacketLength; // Length = 4 ; 2 + 1 (start) = 1 (bytes)
masasin 0:6b3497b2f2ec 316 sum += TxBuf[3]; // Accululate the packet sum
masasin 0:6b3497b2f2ec 317 if (AX12_READ_DEBUG) {
masasin 0:6b3497b2f2ec 318 printf(" Length : 0x%x\n",TxBuf[3]);
masasin 0:6b3497b2f2ec 319 }
masasin 0:6b3497b2f2ec 320
masasin 0:6b3497b2f2ec 321 // Instruction - Read
masasin 0:6b3497b2f2ec 322 TxBuf[4] = 0x2;
masasin 0:6b3497b2f2ec 323 sum += TxBuf[4];
masasin 0:6b3497b2f2ec 324 if (AX12_READ_DEBUG) {
masasin 0:6b3497b2f2ec 325 printf(" Instruction : 0x%x\n",TxBuf[4]);
masasin 0:6b3497b2f2ec 326 }
masasin 0:6b3497b2f2ec 327
masasin 0:6b3497b2f2ec 328 // Start Address
masasin 0:6b3497b2f2ec 329 TxBuf[5] = start;
masasin 0:6b3497b2f2ec 330 sum += TxBuf[5];
masasin 0:6b3497b2f2ec 331 if (AX12_READ_DEBUG) {
masasin 0:6b3497b2f2ec 332 printf(" Start Address : 0x%x\n",TxBuf[5]);
masasin 0:6b3497b2f2ec 333 }
masasin 0:6b3497b2f2ec 334
masasin 0:6b3497b2f2ec 335 // Bytes to read
masasin 0:6b3497b2f2ec 336 TxBuf[6] = bytes;
masasin 0:6b3497b2f2ec 337 sum += TxBuf[6];
masasin 0:6b3497b2f2ec 338 if (AX12_READ_DEBUG) {
masasin 0:6b3497b2f2ec 339 printf(" No bytes : 0x%x\n",TxBuf[6]);
masasin 0:6b3497b2f2ec 340 }
masasin 0:6b3497b2f2ec 341
masasin 0:6b3497b2f2ec 342 // Checksum
masasin 0:6b3497b2f2ec 343 TxBuf[7] = 0xFF - sum;
masasin 0:6b3497b2f2ec 344 if (AX12_READ_DEBUG) {
masasin 0:6b3497b2f2ec 345 printf(" Checksum : 0x%x\n",TxBuf[7]);
masasin 0:6b3497b2f2ec 346 }
masasin 0:6b3497b2f2ec 347
masasin 0:6b3497b2f2ec 348 // Transmit the packet in one burst with no pausing
masasin 0:6b3497b2f2ec 349 for (int i = 0; i<8 ; i++) {
masasin 0:6b3497b2f2ec 350 _ax12.putc(TxBuf[i]);
masasin 0:6b3497b2f2ec 351 }
masasin 0:6b3497b2f2ec 352
masasin 0:6b3497b2f2ec 353 // Wait for the bytes to be transmitted
masasin 0:6b3497b2f2ec 354 wait (0.00002);
masasin 0:6b3497b2f2ec 355
masasin 0:6b3497b2f2ec 356 // Skip if the read was to the broadcast address
masasin 0:6b3497b2f2ec 357 if (_ID != 0xFE) {
masasin 0:6b3497b2f2ec 358
masasin 0:6b3497b2f2ec 359 // Receive the Status packet 6+ number of bytes read
masasin 0:6b3497b2f2ec 360 for (int i=0; i<(6+bytes) ; i++) {
masasin 0:6b3497b2f2ec 361 Status[i] = _ax12.getc();
masasin 0:6b3497b2f2ec 362 }
masasin 0:6b3497b2f2ec 363
masasin 0:6b3497b2f2ec 364 // Copy the data from Status into data for return
masasin 0:6b3497b2f2ec 365 for (int i=0; i < Status[3]-2 ; i++) {
masasin 0:6b3497b2f2ec 366 data[i] = Status[5+i];
masasin 0:6b3497b2f2ec 367 }
masasin 0:6b3497b2f2ec 368
masasin 0:6b3497b2f2ec 369 if (AX12_READ_DEBUG) {
masasin 0:6b3497b2f2ec 370 printf("\nStatus Packet\n");
masasin 0:6b3497b2f2ec 371 printf(" Header : 0x%x\n",Status[0]);
masasin 0:6b3497b2f2ec 372 printf(" Header : 0x%x\n",Status[1]);
masasin 0:6b3497b2f2ec 373 printf(" ID : 0x%x\n",Status[2]);
masasin 0:6b3497b2f2ec 374 printf(" Length : 0x%x\n",Status[3]);
masasin 0:6b3497b2f2ec 375
masasin 0:6b3497b2f2ec 376 for (int i=0; i < Status[3]-2 ; i++) {
masasin 0:6b3497b2f2ec 377 printf(" Data : 0x%x\n",Status[5+i]);
masasin 0:6b3497b2f2ec 378 }
masasin 0:6b3497b2f2ec 379
masasin 0:6b3497b2f2ec 380 printf(" Checksum : 0x%x\n",Status[5+(Status[3]-2)]);
masasin 0:6b3497b2f2ec 381 }
masasin 0:6b3497b2f2ec 382
masasin 0:6b3497b2f2ec 383 } // if (ID!=0xFE)
masasin 0:6b3497b2f2ec 384
masasin 0:6b3497b2f2ec 385 return(Status[4]);
masasin 0:6b3497b2f2ec 386 }
masasin 0:6b3497b2f2ec 387
masasin 0:6b3497b2f2ec 388
masasin 0:6b3497b2f2ec 389 int AX12:: write(int ID, int start, int bytes, char* data, int flag) {
masasin 0:6b3497b2f2ec 390 // 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum
masasin 0:6b3497b2f2ec 391
masasin 0:6b3497b2f2ec 392 char TxBuf[16];
masasin 0:6b3497b2f2ec 393 char sum = 0;
masasin 0:6b3497b2f2ec 394 char Status[6];
masasin 0:6b3497b2f2ec 395
masasin 0:6b3497b2f2ec 396 if (AX12_WRITE_DEBUG) {
masasin 0:6b3497b2f2ec 397 printf("\nwrite(%d,0x%x,%d,data,%d)\n",ID,start,bytes,flag);
masasin 0:6b3497b2f2ec 398 }
masasin 0:6b3497b2f2ec 399
masasin 0:6b3497b2f2ec 400 // Build the TxPacket first in RAM, then we'll send in one go
masasin 0:6b3497b2f2ec 401 if (AX12_WRITE_DEBUG) {
masasin 0:6b3497b2f2ec 402 printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n");
masasin 0:6b3497b2f2ec 403 }
masasin 0:6b3497b2f2ec 404
masasin 0:6b3497b2f2ec 405 TxBuf[0] = 0xff;
masasin 0:6b3497b2f2ec 406 TxBuf[1] = 0xff;
masasin 0:6b3497b2f2ec 407
masasin 0:6b3497b2f2ec 408 // ID
masasin 0:6b3497b2f2ec 409 TxBuf[2] = ID;
masasin 0:6b3497b2f2ec 410 sum += TxBuf[2];
masasin 0:6b3497b2f2ec 411
masasin 0:6b3497b2f2ec 412 if (AX12_WRITE_DEBUG) {
masasin 0:6b3497b2f2ec 413 printf(" ID : %d\n",TxBuf[2]);
masasin 0:6b3497b2f2ec 414 }
masasin 0:6b3497b2f2ec 415
masasin 0:6b3497b2f2ec 416 // packet Length
masasin 0:6b3497b2f2ec 417 TxBuf[3] = 3+bytes;
masasin 0:6b3497b2f2ec 418 sum += TxBuf[3];
masasin 0:6b3497b2f2ec 419
masasin 0:6b3497b2f2ec 420 if (AX12_WRITE_DEBUG) {
masasin 0:6b3497b2f2ec 421 printf(" Length : %d\n",TxBuf[3]);
masasin 0:6b3497b2f2ec 422 }
masasin 0:6b3497b2f2ec 423
masasin 0:6b3497b2f2ec 424 // Instruction
masasin 0:6b3497b2f2ec 425 if (flag == 1) {
masasin 0:6b3497b2f2ec 426 TxBuf[4]=0x04;
masasin 0:6b3497b2f2ec 427 sum += TxBuf[4];
masasin 0:6b3497b2f2ec 428 } else {
masasin 0:6b3497b2f2ec 429 TxBuf[4]=0x03;
masasin 0:6b3497b2f2ec 430 sum += TxBuf[4];
masasin 0:6b3497b2f2ec 431 }
masasin 0:6b3497b2f2ec 432
masasin 0:6b3497b2f2ec 433 if (AX12_WRITE_DEBUG) {
masasin 0:6b3497b2f2ec 434 printf(" Instruction : 0x%x\n",TxBuf[4]);
masasin 0:6b3497b2f2ec 435 }
masasin 0:6b3497b2f2ec 436
masasin 0:6b3497b2f2ec 437 // Start Address
masasin 0:6b3497b2f2ec 438 TxBuf[5] = start;
masasin 0:6b3497b2f2ec 439 sum += TxBuf[5];
masasin 0:6b3497b2f2ec 440 if (AX12_WRITE_DEBUG) {
masasin 0:6b3497b2f2ec 441 printf(" Start : 0x%x\n",TxBuf[5]);
masasin 0:6b3497b2f2ec 442 }
masasin 0:6b3497b2f2ec 443
masasin 0:6b3497b2f2ec 444 // data
masasin 0:6b3497b2f2ec 445 for (char i=0; i<bytes ; i++) {
masasin 0:6b3497b2f2ec 446 TxBuf[6+i] = data[i];
masasin 0:6b3497b2f2ec 447 sum += TxBuf[6+i];
masasin 0:6b3497b2f2ec 448 if (AX12_WRITE_DEBUG) {
masasin 0:6b3497b2f2ec 449 printf(" Data : 0x%x\n",TxBuf[6+i]);
masasin 0:6b3497b2f2ec 450 }
masasin 0:6b3497b2f2ec 451 }
masasin 0:6b3497b2f2ec 452
masasin 0:6b3497b2f2ec 453 // checksum
masasin 0:6b3497b2f2ec 454 TxBuf[6+bytes] = 0xFF - sum;
masasin 0:6b3497b2f2ec 455 if (AX12_WRITE_DEBUG) {
masasin 0:6b3497b2f2ec 456 printf(" Checksum : 0x%x\n",TxBuf[6+bytes]);
masasin 0:6b3497b2f2ec 457 }
masasin 0:6b3497b2f2ec 458
masasin 0:6b3497b2f2ec 459 // Transmit the packet in one burst with no pausing
masasin 0:6b3497b2f2ec 460 for (int i = 0; i < (7 + bytes) ; i++) {
masasin 0:6b3497b2f2ec 461 _ax12.putc(TxBuf[i]);
masasin 0:6b3497b2f2ec 462 }
masasin 0:6b3497b2f2ec 463
masasin 0:6b3497b2f2ec 464 // Wait for data to transmit
masasin 0:6b3497b2f2ec 465 wait (0.00002);
masasin 0:6b3497b2f2ec 466
masasin 0:6b3497b2f2ec 467 // make sure we have a valid return
masasin 0:6b3497b2f2ec 468 Status[4]=0x00;
masasin 0:6b3497b2f2ec 469
masasin 0:6b3497b2f2ec 470 // we'll only get a reply if it was not broadcast
masasin 0:6b3497b2f2ec 471 if (_ID!=0xFE) {
masasin 0:6b3497b2f2ec 472
masasin 0:6b3497b2f2ec 473 // response is always 6 bytes
masasin 0:6b3497b2f2ec 474 // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum
masasin 0:6b3497b2f2ec 475 for (int i=0; i < 6 ; i++) {
masasin 0:6b3497b2f2ec 476 Status[i] = _ax12.getc();
masasin 0:6b3497b2f2ec 477 }
masasin 0:6b3497b2f2ec 478
masasin 0:6b3497b2f2ec 479 // Build the TxPacket first in RAM, then we'll send in one go
masasin 0:6b3497b2f2ec 480 if (AX12_WRITE_DEBUG) {
masasin 0:6b3497b2f2ec 481 printf("\nStatus Packet\n Header : 0x%X, 0x%X\n",Status[0],Status[1]);
masasin 0:6b3497b2f2ec 482 printf(" ID : %d\n",Status[2]);
masasin 0:6b3497b2f2ec 483 printf(" Length : %d\n",Status[3]);
masasin 0:6b3497b2f2ec 484 printf(" Error : 0x%x\n",Status[4]);
masasin 0:6b3497b2f2ec 485 printf(" Checksum : 0x%x\n",Status[5]);
masasin 0:6b3497b2f2ec 486 }
masasin 0:6b3497b2f2ec 487
masasin 0:6b3497b2f2ec 488
masasin 0:6b3497b2f2ec 489 }
masasin 0:6b3497b2f2ec 490
masasin 0:6b3497b2f2ec 491 return(Status[4]); // return error code
masasin 0:6b3497b2f2ec 492 }