AECL 601 / 3W_8Dir_p2pcontrol

123

dynamixel.cpp@0:a49e4e666e2d, 2017-09-12 (annotated)

Committer:
peter16688
Date:
Tue Sep 12 13:22:08 2017 +0000
Revision:
0:a49e4e666e2d
3WOK_8dir_1m_PD_p2pcontrol

Who changed what in which revision?

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