hsu han-lin / DXL_SDK_For_F446RE

DXL_SDK_For_F446RE. Now test with MX64AR, MX28AR with 1Mbps

Dependents:   RoboticArm DXL_SDK_Porting_Test

dynamixel.cpp@0:bf4774b25188, 2017-02-08 (annotated)

Committer:
stanley1228
Date:
Wed Feb 08 02:49:39 2017 +0000
Revision:
0:bf4774b25188
Child:
3:e2e9064c668d
ver1 lib

Who changed what in which revision?

UserRevisionLine numberNew contents of line
stanley1228 0:bf4774b25188 1 #include "dxl_hal.h"
stanley1228 0:bf4774b25188 2 #include "dynamixel.h"
stanley1228 0:bf4774b25188 3
stanley1228 0:bf4774b25188 4 #define ID (2)
stanley1228 0:bf4774b25188 5 #define LENGTH (3)
stanley1228 0:bf4774b25188 6 #define INSTRUCTION (4)
stanley1228 0:bf4774b25188 7 #define ERRBIT (4)
stanley1228 0:bf4774b25188 8 #define PARAMETER (5)
stanley1228 0:bf4774b25188 9 #define DEFAULT_BAUDNUMBER (1)
stanley1228 0:bf4774b25188 10
stanley1228 0:bf4774b25188 11 unsigned char gbInstructionPacket[MAXNUM_TXPARAM+10] = {0};
stanley1228 0:bf4774b25188 12 unsigned char gbStatusPacket[MAXNUM_RXPARAM+10] = {0};
stanley1228 0:bf4774b25188 13 unsigned char gbRxPacketLength = 0;
stanley1228 0:bf4774b25188 14 unsigned char gbRxGetLength = 0;
stanley1228 0:bf4774b25188 15 int gbCommStatus = COMM_RXSUCCESS;
stanley1228 0:bf4774b25188 16 int giBusUsing = 0;
stanley1228 0:bf4774b25188 17
stanley1228 0:bf4774b25188 18 #include "mbed.h"
stanley1228 0:bf4774b25188 19 #include "Serial.h"
stanley1228 0:bf4774b25188 20 extern Serial pc;//stanley
stanley1228 0:bf4774b25188 21
stanley1228 0:bf4774b25188 22
stanley1228 0:bf4774b25188 23 //DigitalOut gTest(D4);
stanley1228 0:bf4774b25188 24
stanley1228 0:bf4774b25188 25 int dxl_initialize( int devIndex, int baudnum )
stanley1228 0:bf4774b25188 26 {
stanley1228 0:bf4774b25188 27 float baudrate;
stanley1228 0:bf4774b25188 28 baudrate = 2000000.0f / (float)(baudnum + 1);
stanley1228 0:bf4774b25188 29
stanley1228 0:bf4774b25188 30 //gTest=0;//stanley
stanley1228 0:bf4774b25188 31
stanley1228 0:bf4774b25188 32 if( dxl_hal_open(devIndex, baudrate) == 0 )
stanley1228 0:bf4774b25188 33 return 0;
stanley1228 0:bf4774b25188 34
stanley1228 0:bf4774b25188 35 gbCommStatus = COMM_RXSUCCESS;
stanley1228 0:bf4774b25188 36 giBusUsing = 0;
stanley1228 0:bf4774b25188 37 return 1;
stanley1228 0:bf4774b25188 38 }
stanley1228 0:bf4774b25188 39
stanley1228 0:bf4774b25188 40 void dxl_terminate()
stanley1228 0:bf4774b25188 41 {
stanley1228 0:bf4774b25188 42 dxl_hal_close();
stanley1228 0:bf4774b25188 43 }
stanley1228 0:bf4774b25188 44
stanley1228 0:bf4774b25188 45 void dxl_tx_packet()
stanley1228 0:bf4774b25188 46 {
stanley1228 0:bf4774b25188 47 unsigned char i;
stanley1228 0:bf4774b25188 48 unsigned char TxNumByte, RealTxNumByte;
stanley1228 0:bf4774b25188 49 unsigned char checksum = 0;
stanley1228 0:bf4774b25188 50
stanley1228 0:bf4774b25188 51 if( giBusUsing == 1 )
stanley1228 0:bf4774b25188 52 return;
stanley1228 0:bf4774b25188 53
stanley1228 0:bf4774b25188 54 giBusUsing = 1;
stanley1228 0:bf4774b25188 55
stanley1228 0:bf4774b25188 56 if( gbInstructionPacket[LENGTH] > (MAXNUM_TXPARAM+2) )
stanley1228 0:bf4774b25188 57 {
stanley1228 0:bf4774b25188 58 gbCommStatus = COMM_TXERROR;
stanley1228 0:bf4774b25188 59 giBusUsing = 0;
stanley1228 0:bf4774b25188 60 return;
stanley1228 0:bf4774b25188 61 }
stanley1228 0:bf4774b25188 62
stanley1228 0:bf4774b25188 63 if( gbInstructionPacket[INSTRUCTION] != INST_PING
stanley1228 0:bf4774b25188 64 && gbInstructionPacket[INSTRUCTION] != INST_READ
stanley1228 0:bf4774b25188 65 && gbInstructionPacket[INSTRUCTION] != INST_WRITE
stanley1228 0:bf4774b25188 66 && gbInstructionPacket[INSTRUCTION] != INST_REG_WRITE
stanley1228 0:bf4774b25188 67 && gbInstructionPacket[INSTRUCTION] != INST_ACTION
stanley1228 0:bf4774b25188 68 && gbInstructionPacket[INSTRUCTION] != INST_RESET
stanley1228 0:bf4774b25188 69 && gbInstructionPacket[INSTRUCTION] != INST_SYNC_WRITE )
stanley1228 0:bf4774b25188 70 {
stanley1228 0:bf4774b25188 71 gbCommStatus = COMM_TXERROR;
stanley1228 0:bf4774b25188 72 giBusUsing = 0;
stanley1228 0:bf4774b25188 73 return;
stanley1228 0:bf4774b25188 74 }
stanley1228 0:bf4774b25188 75
stanley1228 0:bf4774b25188 76 gbInstructionPacket[0] = 0xff;
stanley1228 0:bf4774b25188 77 gbInstructionPacket[1] = 0xff;
stanley1228 0:bf4774b25188 78 for( i=0; i<(gbInstructionPacket[LENGTH]+1); i++ )
stanley1228 0:bf4774b25188 79 checksum += gbInstructionPacket[i+2];
stanley1228 0:bf4774b25188 80 gbInstructionPacket[gbInstructionPacket[LENGTH]+3] = ~checksum;
stanley1228 0:bf4774b25188 81
stanley1228 0:bf4774b25188 82 if( gbCommStatus == COMM_RXTIMEOUT || gbCommStatus == COMM_RXCORRUPT )
stanley1228 0:bf4774b25188 83 dxl_hal_clear();
stanley1228 0:bf4774b25188 84
stanley1228 0:bf4774b25188 85 TxNumByte = gbInstructionPacket[LENGTH] + 4;
stanley1228 0:bf4774b25188 86 RealTxNumByte = dxl_hal_tx( (unsigned char*)gbInstructionPacket, TxNumByte );
stanley1228 0:bf4774b25188 87
stanley1228 0:bf4774b25188 88 if( TxNumByte != RealTxNumByte )
stanley1228 0:bf4774b25188 89 {
stanley1228 0:bf4774b25188 90 gbCommStatus = COMM_TXFAIL;
stanley1228 0:bf4774b25188 91 giBusUsing = 0;
stanley1228 0:bf4774b25188 92 return;
stanley1228 0:bf4774b25188 93 }
stanley1228 0:bf4774b25188 94
stanley1228 0:bf4774b25188 95 if( gbInstructionPacket[INSTRUCTION] == INST_READ )
stanley1228 0:bf4774b25188 96 dxl_hal_set_timeout( gbInstructionPacket[PARAMETER+1] + 6 );
stanley1228 0:bf4774b25188 97 else
stanley1228 0:bf4774b25188 98 dxl_hal_set_timeout( 6 );
stanley1228 0:bf4774b25188 99
stanley1228 0:bf4774b25188 100 gbCommStatus = COMM_TXSUCCESS;
stanley1228 0:bf4774b25188 101 }
stanley1228 0:bf4774b25188 102
stanley1228 0:bf4774b25188 103 void dxl_rx_packet()
stanley1228 0:bf4774b25188 104 {
stanley1228 0:bf4774b25188 105 unsigned char i, j, nRead;
stanley1228 0:bf4774b25188 106 unsigned char checksum = 0;
stanley1228 0:bf4774b25188 107
stanley1228 0:bf4774b25188 108 if( giBusUsing == 0 )
stanley1228 0:bf4774b25188 109 return;
stanley1228 0:bf4774b25188 110
stanley1228 0:bf4774b25188 111 if( gbInstructionPacket[ID] == BROADCAST_ID )
stanley1228 0:bf4774b25188 112 {
stanley1228 0:bf4774b25188 113 gbCommStatus = COMM_RXSUCCESS;
stanley1228 0:bf4774b25188 114 giBusUsing = 0;
stanley1228 0:bf4774b25188 115 return;
stanley1228 0:bf4774b25188 116 }
stanley1228 0:bf4774b25188 117
stanley1228 0:bf4774b25188 118 if( gbCommStatus == COMM_TXSUCCESS )
stanley1228 0:bf4774b25188 119 {
stanley1228 0:bf4774b25188 120 gbRxGetLength = 0;
stanley1228 0:bf4774b25188 121 gbRxPacketLength = 6;
stanley1228 0:bf4774b25188 122 }
stanley1228 0:bf4774b25188 123
stanley1228 0:bf4774b25188 124 nRead = dxl_hal_rx( (unsigned char*)&gbStatusPacket[gbRxGetLength], gbRxPacketLength - gbRxGetLength );
stanley1228 0:bf4774b25188 125
stanley1228 0:bf4774b25188 126 gbRxGetLength += nRead;
stanley1228 0:bf4774b25188 127 if( gbRxGetLength < gbRxPacketLength )
stanley1228 0:bf4774b25188 128 {
stanley1228 0:bf4774b25188 129 if( dxl_hal_timeout() == 1 )
stanley1228 0:bf4774b25188 130 {
stanley1228 0:bf4774b25188 131 if(gbRxGetLength == 0)
stanley1228 0:bf4774b25188 132 gbCommStatus = COMM_RXTIMEOUT;
stanley1228 0:bf4774b25188 133 else
stanley1228 0:bf4774b25188 134 gbCommStatus = COMM_RXCORRUPT;
stanley1228 0:bf4774b25188 135 giBusUsing = 0;
stanley1228 0:bf4774b25188 136 return;
stanley1228 0:bf4774b25188 137 }
stanley1228 0:bf4774b25188 138 }
stanley1228 0:bf4774b25188 139 //pc.printf("133\n");//stanley
stanley1228 0:bf4774b25188 140 // Find packet header
stanley1228 0:bf4774b25188 141 for( i=0; i<(gbRxGetLength-1); i++ )
stanley1228 0:bf4774b25188 142 {
stanley1228 0:bf4774b25188 143 if( gbStatusPacket[i] == 0xff && gbStatusPacket[i+1] == 0xff )
stanley1228 0:bf4774b25188 144 {
stanley1228 0:bf4774b25188 145 break;
stanley1228 0:bf4774b25188 146 }
stanley1228 0:bf4774b25188 147 else if( i == gbRxGetLength-2 && gbStatusPacket[gbRxGetLength-1] == 0xff )
stanley1228 0:bf4774b25188 148 {
stanley1228 0:bf4774b25188 149 break;
stanley1228 0:bf4774b25188 150 }
stanley1228 0:bf4774b25188 151 }
stanley1228 0:bf4774b25188 152 if( i > 0 )
stanley1228 0:bf4774b25188 153 {
stanley1228 0:bf4774b25188 154 for( j=0; j<(gbRxGetLength-i); j++ )
stanley1228 0:bf4774b25188 155 gbStatusPacket[j] = gbStatusPacket[j + i];
stanley1228 0:bf4774b25188 156
stanley1228 0:bf4774b25188 157 gbRxGetLength -= i;
stanley1228 0:bf4774b25188 158 }
stanley1228 0:bf4774b25188 159
stanley1228 0:bf4774b25188 160 if( gbRxGetLength < gbRxPacketLength )
stanley1228 0:bf4774b25188 161 {
stanley1228 0:bf4774b25188 162 gbCommStatus = COMM_RXWAITING;
stanley1228 0:bf4774b25188 163 return;
stanley1228 0:bf4774b25188 164 }
stanley1228 0:bf4774b25188 165
stanley1228 0:bf4774b25188 166 // Check id pairing
stanley1228 0:bf4774b25188 167 if( gbInstructionPacket[ID] != gbStatusPacket[ID])
stanley1228 0:bf4774b25188 168 {
stanley1228 0:bf4774b25188 169 gbCommStatus = COMM_RXCORRUPT;
stanley1228 0:bf4774b25188 170 giBusUsing = 0;
stanley1228 0:bf4774b25188 171 return;
stanley1228 0:bf4774b25188 172 }
stanley1228 0:bf4774b25188 173
stanley1228 0:bf4774b25188 174 gbRxPacketLength = gbStatusPacket[LENGTH] + 4;
stanley1228 0:bf4774b25188 175 if( gbRxGetLength < gbRxPacketLength )
stanley1228 0:bf4774b25188 176 {
stanley1228 0:bf4774b25188 177 nRead = dxl_hal_rx( (unsigned char*)&gbStatusPacket[gbRxGetLength], gbRxPacketLength - gbRxGetLength );
stanley1228 0:bf4774b25188 178 gbRxGetLength += nRead;
stanley1228 0:bf4774b25188 179 if( gbRxGetLength < gbRxPacketLength )
stanley1228 0:bf4774b25188 180 {
stanley1228 0:bf4774b25188 181 gbCommStatus = COMM_RXWAITING;
stanley1228 0:bf4774b25188 182 return;
stanley1228 0:bf4774b25188 183 }
stanley1228 0:bf4774b25188 184 }
stanley1228 0:bf4774b25188 185
stanley1228 0:bf4774b25188 186 // Check checksum
stanley1228 0:bf4774b25188 187 for( i=0; i<(gbStatusPacket[LENGTH]+1); i++ )
stanley1228 0:bf4774b25188 188 checksum += gbStatusPacket[i+2];
stanley1228 0:bf4774b25188 189 checksum = ~checksum;
stanley1228 0:bf4774b25188 190
stanley1228 0:bf4774b25188 191 if( gbStatusPacket[gbStatusPacket[LENGTH]+3] != checksum )
stanley1228 0:bf4774b25188 192 {
stanley1228 0:bf4774b25188 193 gbCommStatus = COMM_RXCORRUPT;
stanley1228 0:bf4774b25188 194 giBusUsing = 0;
stanley1228 0:bf4774b25188 195 return;
stanley1228 0:bf4774b25188 196 }
stanley1228 0:bf4774b25188 197
stanley1228 0:bf4774b25188 198 gbCommStatus = COMM_RXSUCCESS;
stanley1228 0:bf4774b25188 199 giBusUsing = 0;
stanley1228 0:bf4774b25188 200 }
stanley1228 0:bf4774b25188 201
stanley1228 0:bf4774b25188 202 void dxl_txrx_packet()
stanley1228 0:bf4774b25188 203 {
stanley1228 0:bf4774b25188 204
stanley1228 0:bf4774b25188 205 //pc.printf("before dxl_tx_packet\n");
stanley1228 0:bf4774b25188 206 dxl_tx_packet();
stanley1228 0:bf4774b25188 207
stanley1228 0:bf4774b25188 208 //pc.printf("after dxl_tx_packet\n");
stanley1228 0:bf4774b25188 209 if( gbCommStatus != COMM_TXSUCCESS )
stanley1228 0:bf4774b25188 210 return;
stanley1228 0:bf4774b25188 211
stanley1228 0:bf4774b25188 212
stanley1228 0:bf4774b25188 213
stanley1228 0:bf4774b25188 214 do{
stanley1228 0:bf4774b25188 215 //pc.printf("before dxl_rx_packet\n");
stanley1228 0:bf4774b25188 216 dxl_rx_packet();
stanley1228 0:bf4774b25188 217 //pc.printf("after dxl_rx_packet\n");
stanley1228 0:bf4774b25188 218 }while( gbCommStatus == COMM_RXWAITING );
stanley1228 0:bf4774b25188 219 }
stanley1228 0:bf4774b25188 220
stanley1228 0:bf4774b25188 221 int dxl_get_result()
stanley1228 0:bf4774b25188 222 {
stanley1228 0:bf4774b25188 223 return gbCommStatus;
stanley1228 0:bf4774b25188 224 }
stanley1228 0:bf4774b25188 225
stanley1228 0:bf4774b25188 226 void dxl_set_txpacket_id( int id )
stanley1228 0:bf4774b25188 227 {
stanley1228 0:bf4774b25188 228 gbInstructionPacket[ID] = (unsigned char)id;
stanley1228 0:bf4774b25188 229 }
stanley1228 0:bf4774b25188 230
stanley1228 0:bf4774b25188 231 void dxl_set_txpacket_instruction( int instruction )
stanley1228 0:bf4774b25188 232 {
stanley1228 0:bf4774b25188 233 gbInstructionPacket[INSTRUCTION] = (unsigned char)instruction;
stanley1228 0:bf4774b25188 234 }
stanley1228 0:bf4774b25188 235
stanley1228 0:bf4774b25188 236 void dxl_set_txpacket_parameter( int index, int value )
stanley1228 0:bf4774b25188 237 {
stanley1228 0:bf4774b25188 238 gbInstructionPacket[PARAMETER+index] = (unsigned char)value;
stanley1228 0:bf4774b25188 239 }
stanley1228 0:bf4774b25188 240
stanley1228 0:bf4774b25188 241 void dxl_set_txpacket_length( int length )
stanley1228 0:bf4774b25188 242 {
stanley1228 0:bf4774b25188 243 gbInstructionPacket[LENGTH] = (unsigned char)length;
stanley1228 0:bf4774b25188 244 }
stanley1228 0:bf4774b25188 245
stanley1228 0:bf4774b25188 246 int dxl_get_rxpacket_error( int errbit )
stanley1228 0:bf4774b25188 247 {
stanley1228 0:bf4774b25188 248 if( gbStatusPacket[ERRBIT] & (unsigned char)errbit )
stanley1228 0:bf4774b25188 249 return 1;
stanley1228 0:bf4774b25188 250
stanley1228 0:bf4774b25188 251 return 0;
stanley1228 0:bf4774b25188 252 }
stanley1228 0:bf4774b25188 253
stanley1228 0:bf4774b25188 254 int dxl_get_rxpacket_length()
stanley1228 0:bf4774b25188 255 {
stanley1228 0:bf4774b25188 256 return (int)gbStatusPacket[LENGTH];
stanley1228 0:bf4774b25188 257 }
stanley1228 0:bf4774b25188 258
stanley1228 0:bf4774b25188 259 int dxl_get_rxpacket_parameter( int index )
stanley1228 0:bf4774b25188 260 {
stanley1228 0:bf4774b25188 261 return (int)gbStatusPacket[PARAMETER+index];
stanley1228 0:bf4774b25188 262 }
stanley1228 0:bf4774b25188 263
stanley1228 0:bf4774b25188 264 int dxl_makeword( int lowbyte, int highbyte )
stanley1228 0:bf4774b25188 265 {
stanley1228 0:bf4774b25188 266 unsigned short word;
stanley1228 0:bf4774b25188 267
stanley1228 0:bf4774b25188 268 word = highbyte;
stanley1228 0:bf4774b25188 269 word = word << 8;
stanley1228 0:bf4774b25188 270 word = word + lowbyte;
stanley1228 0:bf4774b25188 271 return (int)word;
stanley1228 0:bf4774b25188 272 }
stanley1228 0:bf4774b25188 273
stanley1228 0:bf4774b25188 274 int dxl_get_lowbyte( int word )
stanley1228 0:bf4774b25188 275 {
stanley1228 0:bf4774b25188 276 unsigned short temp;
stanley1228 0:bf4774b25188 277
stanley1228 0:bf4774b25188 278 temp = word & 0xff;
stanley1228 0:bf4774b25188 279 return (int)temp;
stanley1228 0:bf4774b25188 280 }
stanley1228 0:bf4774b25188 281
stanley1228 0:bf4774b25188 282 int dxl_get_highbyte( int word )
stanley1228 0:bf4774b25188 283 {
stanley1228 0:bf4774b25188 284 unsigned short temp;
stanley1228 0:bf4774b25188 285
stanley1228 0:bf4774b25188 286 temp = word & 0xff00;
stanley1228 0:bf4774b25188 287 temp = temp >> 8;
stanley1228 0:bf4774b25188 288 return (int)temp;
stanley1228 0:bf4774b25188 289 }
stanley1228 0:bf4774b25188 290
stanley1228 0:bf4774b25188 291 void dxl_ping( int id )
stanley1228 0:bf4774b25188 292 {
stanley1228 0:bf4774b25188 293 while(giBusUsing);
stanley1228 0:bf4774b25188 294
stanley1228 0:bf4774b25188 295 gbInstructionPacket[ID] = (unsigned char)id;
stanley1228 0:bf4774b25188 296 gbInstructionPacket[INSTRUCTION] = INST_PING;
stanley1228 0:bf4774b25188 297 gbInstructionPacket[LENGTH] = 2;
stanley1228 0:bf4774b25188 298
stanley1228 0:bf4774b25188 299 dxl_txrx_packet();
stanley1228 0:bf4774b25188 300 }
stanley1228 0:bf4774b25188 301
stanley1228 0:bf4774b25188 302 int dxl_read_byte( int id, int address )
stanley1228 0:bf4774b25188 303 {
stanley1228 0:bf4774b25188 304 while(giBusUsing);
stanley1228 0:bf4774b25188 305
stanley1228 0:bf4774b25188 306 gbInstructionPacket[ID] = (unsigned char)id;
stanley1228 0:bf4774b25188 307 gbInstructionPacket[INSTRUCTION] = INST_READ;
stanley1228 0:bf4774b25188 308 gbInstructionPacket[PARAMETER] = (unsigned char)address;
stanley1228 0:bf4774b25188 309 gbInstructionPacket[PARAMETER+1] = 1;
stanley1228 0:bf4774b25188 310 gbInstructionPacket[LENGTH] = 4;
stanley1228 0:bf4774b25188 311
stanley1228 0:bf4774b25188 312 dxl_txrx_packet();
stanley1228 0:bf4774b25188 313
stanley1228 0:bf4774b25188 314 return (int)gbStatusPacket[PARAMETER];
stanley1228 0:bf4774b25188 315 }
stanley1228 0:bf4774b25188 316
stanley1228 0:bf4774b25188 317 void dxl_write_byte( int id, int address, int value )
stanley1228 0:bf4774b25188 318 {
stanley1228 0:bf4774b25188 319 //pc.printf("id=%d,address=%x,value=%d\n",id,address,value);//stanley
stanley1228 0:bf4774b25188 320 while(giBusUsing);
stanley1228 0:bf4774b25188 321
stanley1228 0:bf4774b25188 322 //pc.printf("after giBusUsing\n",id,address,value);//stanley
stanley1228 0:bf4774b25188 323
stanley1228 0:bf4774b25188 324 gbInstructionPacket[ID] = (unsigned char)id;
stanley1228 0:bf4774b25188 325 gbInstructionPacket[INSTRUCTION] = INST_WRITE;
stanley1228 0:bf4774b25188 326 gbInstructionPacket[PARAMETER] = (unsigned char)address;
stanley1228 0:bf4774b25188 327 gbInstructionPacket[PARAMETER+1] = (unsigned char)value;
stanley1228 0:bf4774b25188 328 gbInstructionPacket[LENGTH] = 4;
stanley1228 0:bf4774b25188 329
stanley1228 0:bf4774b25188 330 //pc.printf("before dxl_txrx_packet\n",id,address,value);//stanley
stanley1228 0:bf4774b25188 331
stanley1228 0:bf4774b25188 332 dxl_txrx_packet();
stanley1228 0:bf4774b25188 333
stanley1228 0:bf4774b25188 334 //pc.printf("after dxl_txrx_packet\n",id,address,value);//stanley
stanley1228 0:bf4774b25188 335 }
stanley1228 0:bf4774b25188 336
stanley1228 0:bf4774b25188 337 int dxl_read_word( int id, int address )
stanley1228 0:bf4774b25188 338 {
stanley1228 0:bf4774b25188 339 while(giBusUsing);
stanley1228 0:bf4774b25188 340
stanley1228 0:bf4774b25188 341 gbInstructionPacket[ID] = (unsigned char)id;
stanley1228 0:bf4774b25188 342 gbInstructionPacket[INSTRUCTION] = INST_READ;
stanley1228 0:bf4774b25188 343 gbInstructionPacket[PARAMETER] = (unsigned char)address;
stanley1228 0:bf4774b25188 344 gbInstructionPacket[PARAMETER+1] = 2;
stanley1228 0:bf4774b25188 345 gbInstructionPacket[LENGTH] = 4;
stanley1228 0:bf4774b25188 346
stanley1228 0:bf4774b25188 347 dxl_txrx_packet();
stanley1228 0:bf4774b25188 348
stanley1228 0:bf4774b25188 349 return dxl_makeword((int)gbStatusPacket[PARAMETER], (int)gbStatusPacket[PARAMETER+1]);
stanley1228 0:bf4774b25188 350 }
stanley1228 0:bf4774b25188 351
stanley1228 0:bf4774b25188 352 void dxl_write_word( int id, int address, int value )
stanley1228 0:bf4774b25188 353 {
stanley1228 0:bf4774b25188 354 while(giBusUsing);
stanley1228 0:bf4774b25188 355
stanley1228 0:bf4774b25188 356 gbInstructionPacket[ID] = (unsigned char)id;
stanley1228 0:bf4774b25188 357 gbInstructionPacket[INSTRUCTION] = INST_WRITE;
stanley1228 0:bf4774b25188 358 gbInstructionPacket[PARAMETER] = (unsigned char)address;
stanley1228 0:bf4774b25188 359 gbInstructionPacket[PARAMETER+1] = (unsigned char)dxl_get_lowbyte(value);
stanley1228 0:bf4774b25188 360 gbInstructionPacket[PARAMETER+2] = (unsigned char)dxl_get_highbyte(value);
stanley1228 0:bf4774b25188 361 gbInstructionPacket[LENGTH] = 5;
stanley1228 0:bf4774b25188 362
stanley1228 0:bf4774b25188 363 dxl_txrx_packet();
stanley1228 0:bf4774b25188 364 }
stanley1228 0:bf4774b25188 365
stanley1228 0:bf4774b25188 366 int syncWrite_16bit(int start_addr, int data_length, int *param, int param_length) // WORD(16bit) syncwrite() for DXL stanley
stanley1228 0:bf4774b25188 367 {
stanley1228 0:bf4774b25188 368 while(giBusUsing);
stanley1228 0:bf4774b25188 369
stanley1228 0:bf4774b25188 370 gbInstructionPacket[ID] = (unsigned char)BROADCAST_ID;
stanley1228 0:bf4774b25188 371 gbInstructionPacket[INSTRUCTION] = INST_SYNC_WRITE;
stanley1228 0:bf4774b25188 372 gbInstructionPacket[PARAMETER] = (unsigned char)start_addr;
stanley1228 0:bf4774b25188 373 gbInstructionPacket[PARAMETER+1] = (unsigned char)data_length*2;
stanley1228 0:bf4774b25188 374
stanley1228 0:bf4774b25188 375
stanley1228 0:bf4774b25188 376 int slaveNum=param_length/(data_length+1);
stanley1228 0:bf4774b25188 377 int OneRawByte=(1+data_length*2);//ID(1byte) + worddata*len(2byte*len)
stanley1228 0:bf4774b25188 378
stanley1228 0:bf4774b25188 379
stanley1228 0:bf4774b25188 380 int i=0; //offset of slave number(number of row)
stanley1228 0:bf4774b25188 381 int j=0; //offset of data in raw
stanley1228 0:bf4774b25188 382 int k=1;//offset of int *param
stanley1228 0:bf4774b25188 383 int index=0;
stanley1228 0:bf4774b25188 384
stanley1228 0:bf4774b25188 385 for( i=0; i<slaveNum; i++ )
stanley1228 0:bf4774b25188 386 {
stanley1228 0:bf4774b25188 387 index=PARAMETER+OneRawByte*i+2;
stanley1228 0:bf4774b25188 388 gbInstructionPacket[index] = (unsigned char)param[i*(data_length+1)];//ID
stanley1228 0:bf4774b25188 389 k=1;
stanley1228 0:bf4774b25188 390
stanley1228 0:bf4774b25188 391 for(j=1;j<OneRawByte;j+=2)
stanley1228 0:bf4774b25188 392 {
stanley1228 0:bf4774b25188 393 gbInstructionPacket[index+j]= (unsigned char)(param[i*(data_length+1)+k]&0xff); //DATA L
stanley1228 0:bf4774b25188 394 gbInstructionPacket[index+j+1]= (unsigned char)(param[i*(data_length+1)+k]>>8); //DATA H
stanley1228 0:bf4774b25188 395 k++;
stanley1228 0:bf4774b25188 396 }
stanley1228 0:bf4774b25188 397 }
stanley1228 0:bf4774b25188 398
stanley1228 0:bf4774b25188 399 gbInstructionPacket[LENGTH] = OneRawByte*slaveNum+4;
stanley1228 0:bf4774b25188 400
stanley1228 0:bf4774b25188 401 dxl_txrx_packet();
stanley1228 0:bf4774b25188 402 return 0;
stanley1228 0:bf4774b25188 403 }
stanley1228 0:bf4774b25188 404
stanley1228 0:bf4774b25188 405 void setPosition(int ServoID, int Position, int Speed)//stanley
stanley1228 0:bf4774b25188 406 {
stanley1228 0:bf4774b25188 407 while(giBusUsing);
stanley1228 0:bf4774b25188 408
stanley1228 0:bf4774b25188 409 gbInstructionPacket[ID] = (unsigned char)ServoID;
stanley1228 0:bf4774b25188 410 gbInstructionPacket[INSTRUCTION] = INST_WRITE;
stanley1228 0:bf4774b25188 411 gbInstructionPacket[PARAMETER] = (unsigned char)30;
stanley1228 0:bf4774b25188 412 gbInstructionPacket[PARAMETER+1] = (unsigned char)dxl_get_lowbyte(Position);
stanley1228 0:bf4774b25188 413 gbInstructionPacket[PARAMETER+2] = (unsigned char)dxl_get_highbyte(Position);
stanley1228 0:bf4774b25188 414 gbInstructionPacket[PARAMETER+3] = (unsigned char)dxl_get_lowbyte(Speed);
stanley1228 0:bf4774b25188 415 gbInstructionPacket[PARAMETER+4] = (unsigned char)dxl_get_highbyte(Speed);
stanley1228 0:bf4774b25188 416
stanley1228 0:bf4774b25188 417 gbInstructionPacket[LENGTH] = 7;
stanley1228 0:bf4774b25188 418
stanley1228 0:bf4774b25188 419 dxl_txrx_packet();
stanley1228 0:bf4774b25188 420
stanley1228 0:bf4774b25188 421 }