123
Embed:
(wiki syntax)
Show/hide line numbers
dynamixel.cpp
00001 #include "dxl_hal.h" 00002 #include "dynamixel.h" 00003 00004 #define ID (2) 00005 #define LENGTH (3) 00006 #define INSTRUCTION (4) 00007 #define ERRBIT (4) 00008 #define PARAMETER (5) 00009 #define DEFAULT_BAUDNUMBER (1) 00010 00011 unsigned char gbInstructionPacket[MAXNUM_TXPARAM+10] = {0}; 00012 unsigned char gbStatusPacket[MAXNUM_RXPARAM+10] = {0}; 00013 unsigned char gbRxPacketLength = 0; 00014 unsigned char gbRxGetLength = 0; 00015 int gbCommStatus = COMM_RXSUCCESS; 00016 int giBusUsing = 0; 00017 00018 #include "mbed.h" 00019 #include "Serial.h" 00020 extern Serial pc;//stanley 00021 00022 00023 //DigitalOut gTest(D4); 00024 00025 int dxl_initialize( int devIndex, int baudnum ) 00026 { 00027 float baudrate; 00028 baudrate = 2000000.0f / (float)(baudnum + 1); 00029 00030 //gTest=0;//stanley 00031 00032 if( dxl_hal_open(devIndex, baudrate) == 0 ) 00033 return 0; 00034 00035 gbCommStatus = COMM_RXSUCCESS; 00036 giBusUsing = 0; 00037 return 1; 00038 } 00039 00040 void dxl_terminate() 00041 { 00042 dxl_hal_close(); 00043 } 00044 00045 void dxl_tx_packet() 00046 { 00047 unsigned char i; 00048 unsigned char TxNumByte, RealTxNumByte; 00049 unsigned char checksum = 0; 00050 00051 if( giBusUsing == 1 ) 00052 return; 00053 00054 giBusUsing = 1; 00055 00056 if( gbInstructionPacket[LENGTH] > (MAXNUM_TXPARAM+2) ) 00057 { 00058 gbCommStatus = COMM_TXERROR; 00059 giBusUsing = 0; 00060 return; 00061 } 00062 00063 if( gbInstructionPacket[INSTRUCTION] != INST_PING 00064 && gbInstructionPacket[INSTRUCTION] != INST_READ 00065 && gbInstructionPacket[INSTRUCTION] != INST_WRITE 00066 && gbInstructionPacket[INSTRUCTION] != INST_REG_WRITE 00067 && gbInstructionPacket[INSTRUCTION] != INST_ACTION 00068 && gbInstructionPacket[INSTRUCTION] != INST_RESET 00069 && gbInstructionPacket[INSTRUCTION] != INST_SYNC_WRITE ) 00070 { 00071 gbCommStatus = COMM_TXERROR; 00072 giBusUsing = 0; 00073 return; 00074 } 00075 00076 gbInstructionPacket[0] = 0xff; 00077 gbInstructionPacket[1] = 0xff; 00078 for( i=0; i<(gbInstructionPacket[LENGTH]+1); i++ ) 00079 checksum += gbInstructionPacket[i+2]; 00080 gbInstructionPacket[gbInstructionPacket[LENGTH]+3] = ~checksum; 00081 00082 if( gbCommStatus == COMM_RXTIMEOUT || gbCommStatus == COMM_RXCORRUPT ) 00083 dxl_hal_clear(); 00084 00085 TxNumByte = gbInstructionPacket[LENGTH] + 4; 00086 RealTxNumByte = dxl_hal_tx( (unsigned char*)gbInstructionPacket, TxNumByte ); 00087 00088 if( TxNumByte != RealTxNumByte ) 00089 { 00090 gbCommStatus = COMM_TXFAIL; 00091 giBusUsing = 0; 00092 return; 00093 } 00094 00095 if( gbInstructionPacket[INSTRUCTION] == INST_READ ) 00096 dxl_hal_set_timeout( gbInstructionPacket[PARAMETER+1] + 6 ); 00097 else 00098 dxl_hal_set_timeout( 6 ); 00099 00100 gbCommStatus = COMM_TXSUCCESS; 00101 } 00102 00103 void dxl_rx_packet() 00104 { 00105 unsigned char i, j, nRead; 00106 unsigned char checksum = 0; 00107 00108 if( giBusUsing == 0 ) 00109 return; 00110 00111 if( gbInstructionPacket[ID] == BROADCAST_ID ) 00112 { 00113 gbCommStatus = COMM_RXSUCCESS; 00114 giBusUsing = 0; 00115 return; 00116 } 00117 00118 if( gbCommStatus == COMM_TXSUCCESS ) 00119 { 00120 gbRxGetLength = 0; 00121 gbRxPacketLength = 6; 00122 } 00123 00124 nRead = dxl_hal_rx( (unsigned char*)&gbStatusPacket[gbRxGetLength], gbRxPacketLength - gbRxGetLength ); 00125 00126 gbRxGetLength += nRead; 00127 if( gbRxGetLength < gbRxPacketLength ) 00128 { 00129 if( dxl_hal_timeout() == 1 ) 00130 { 00131 if(gbRxGetLength == 0) 00132 gbCommStatus = COMM_RXTIMEOUT; 00133 else 00134 gbCommStatus = COMM_RXCORRUPT; 00135 giBusUsing = 0; 00136 return; 00137 } 00138 } 00139 //pc.printf("133\n");//stanley 00140 // Find packet header 00141 for( i=0; i<(gbRxGetLength-1); i++ ) 00142 { 00143 if( gbStatusPacket[i] == 0xff && gbStatusPacket[i+1] == 0xff ) 00144 { 00145 break; 00146 } 00147 else if( i == gbRxGetLength-2 && gbStatusPacket[gbRxGetLength-1] == 0xff ) 00148 { 00149 break; 00150 } 00151 } 00152 if( i > 0 ) 00153 { 00154 for( j=0; j<(gbRxGetLength-i); j++ ) 00155 gbStatusPacket[j] = gbStatusPacket[j + i]; 00156 00157 gbRxGetLength -= i; 00158 } 00159 00160 if( gbRxGetLength < gbRxPacketLength ) 00161 { 00162 gbCommStatus = COMM_RXWAITING; 00163 return; 00164 } 00165 00166 // Check id pairing 00167 if( gbInstructionPacket[ID] != gbStatusPacket[ID]) 00168 { 00169 gbCommStatus = COMM_RXCORRUPT; 00170 giBusUsing = 0; 00171 return; 00172 } 00173 00174 gbRxPacketLength = gbStatusPacket[LENGTH] + 4; 00175 if( gbRxGetLength < gbRxPacketLength ) 00176 { 00177 nRead = dxl_hal_rx( (unsigned char*)&gbStatusPacket[gbRxGetLength], gbRxPacketLength - gbRxGetLength ); 00178 gbRxGetLength += nRead; 00179 if( gbRxGetLength < gbRxPacketLength ) 00180 { 00181 gbCommStatus = COMM_RXWAITING; 00182 return; 00183 } 00184 } 00185 00186 // Check checksum 00187 for( i=0; i<(gbStatusPacket[LENGTH]+1); i++ ) 00188 checksum += gbStatusPacket[i+2]; 00189 checksum = ~checksum; 00190 00191 if( gbStatusPacket[gbStatusPacket[LENGTH]+3] != checksum ) 00192 { 00193 gbCommStatus = COMM_RXCORRUPT; 00194 giBusUsing = 0; 00195 return; 00196 } 00197 00198 gbCommStatus = COMM_RXSUCCESS; 00199 giBusUsing = 0; 00200 } 00201 00202 void dxl_txrx_packet() 00203 { 00204 00205 //pc.printf("before dxl_tx_packet\n"); 00206 dxl_tx_packet(); 00207 00208 //pc.printf("after dxl_tx_packet\n"); 00209 if( gbCommStatus != COMM_TXSUCCESS ) 00210 return; 00211 00212 00213 00214 do{ 00215 //pc.printf("before dxl_rx_packet\n"); 00216 dxl_rx_packet(); 00217 //pc.printf("after dxl_rx_packet\n"); 00218 }while( gbCommStatus == COMM_RXWAITING ); 00219 } 00220 00221 int dxl_get_result() 00222 { 00223 return gbCommStatus; 00224 } 00225 00226 void dxl_set_txpacket_id( int id ) 00227 { 00228 gbInstructionPacket[ID] = (unsigned char)id; 00229 } 00230 00231 void dxl_set_txpacket_instruction( int instruction ) 00232 { 00233 gbInstructionPacket[INSTRUCTION] = (unsigned char)instruction; 00234 } 00235 00236 void dxl_set_txpacket_parameter( int index, int value ) 00237 { 00238 gbInstructionPacket[PARAMETER+index] = (unsigned char)value; 00239 } 00240 00241 void dxl_set_txpacket_length( int length ) 00242 { 00243 gbInstructionPacket[LENGTH] = (unsigned char)length; 00244 } 00245 00246 int dxl_get_rxpacket_error( int errbit ) 00247 { 00248 if( gbStatusPacket[ERRBIT] & (unsigned char)errbit ) 00249 return 1; 00250 00251 return 0; 00252 } 00253 00254 int dxl_get_rxpacket_length() 00255 { 00256 return (int)gbStatusPacket[LENGTH]; 00257 } 00258 00259 int dxl_get_rxpacket_parameter( int index ) 00260 { 00261 return (int)gbStatusPacket[PARAMETER+index]; 00262 } 00263 00264 int dxl_makeword( int lowbyte, int highbyte ) 00265 { 00266 unsigned short word; 00267 00268 word = highbyte; 00269 word = word << 8; 00270 word = word + lowbyte; 00271 return (int)word; 00272 } 00273 00274 int dxl_get_lowbyte( int word ) 00275 { 00276 unsigned short temp; 00277 00278 temp = word & 0xff; 00279 return (int)temp; 00280 } 00281 00282 int dxl_get_highbyte( int word ) 00283 { 00284 unsigned short temp; 00285 00286 temp = word & 0xff00; 00287 temp = temp >> 8; 00288 return (int)temp; 00289 } 00290 00291 void dxl_ping( int id ) 00292 { 00293 while(giBusUsing); 00294 00295 gbInstructionPacket[ID] = (unsigned char)id; 00296 gbInstructionPacket[INSTRUCTION] = INST_PING; 00297 gbInstructionPacket[LENGTH] = 2; 00298 00299 dxl_txrx_packet(); 00300 } 00301 00302 int dxl_read_byte( int id, int address ) 00303 { 00304 while(giBusUsing); 00305 00306 gbInstructionPacket[ID] = (unsigned char)id; 00307 gbInstructionPacket[INSTRUCTION] = INST_READ; 00308 gbInstructionPacket[PARAMETER] = (unsigned char)address; 00309 gbInstructionPacket[PARAMETER+1] = 1; 00310 gbInstructionPacket[LENGTH] = 4; 00311 00312 dxl_txrx_packet(); 00313 00314 return (int)gbStatusPacket[PARAMETER]; 00315 } 00316 00317 void dxl_write_byte( int id, int address, int value ) 00318 { 00319 //pc.printf("id=%d,address=%x,value=%d\n",id,address,value);//stanley 00320 while(giBusUsing); 00321 00322 //pc.printf("after giBusUsing\n",id,address,value);//stanley 00323 00324 gbInstructionPacket[ID] = (unsigned char)id; 00325 gbInstructionPacket[INSTRUCTION] = INST_WRITE; 00326 gbInstructionPacket[PARAMETER] = (unsigned char)address; 00327 gbInstructionPacket[PARAMETER+1] = (unsigned char)value; 00328 gbInstructionPacket[LENGTH] = 4; 00329 00330 //pc.printf("before dxl_txrx_packet\n",id,address,value);//stanley 00331 00332 dxl_txrx_packet(); 00333 00334 //pc.printf("after dxl_txrx_packet\n",id,address,value);//stanley 00335 } 00336 00337 int dxl_read_word( short int id, short int address ) 00338 { 00339 while(giBusUsing); 00340 00341 gbInstructionPacket[ID] = (unsigned char)id; 00342 gbInstructionPacket[INSTRUCTION] = INST_READ; 00343 gbInstructionPacket[PARAMETER] = (unsigned char)address; 00344 gbInstructionPacket[PARAMETER+1] = 2; 00345 gbInstructionPacket[LENGTH] = 4; 00346 00347 dxl_txrx_packet(); 00348 00349 return dxl_makeword((int)gbStatusPacket[PARAMETER], (int)gbStatusPacket[PARAMETER+1]); 00350 } 00351 00352 void dxl_write_word( short int id, short int address, short int value ) 00353 { 00354 while(giBusUsing); 00355 00356 gbInstructionPacket[ID] = (unsigned char)id; 00357 gbInstructionPacket[INSTRUCTION] = INST_WRITE; 00358 gbInstructionPacket[PARAMETER] = (unsigned char)address; 00359 gbInstructionPacket[PARAMETER+1] = (unsigned char)dxl_get_lowbyte(value); 00360 gbInstructionPacket[PARAMETER+2] = (unsigned char)dxl_get_highbyte(value); 00361 gbInstructionPacket[LENGTH] = 5; 00362 00363 dxl_txrx_packet(); 00364 } 00365 00366 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 00367 { 00368 while(giBusUsing); 00369 00370 gbInstructionPacket[ID] = (unsigned char)BROADCAST_ID; 00371 gbInstructionPacket[INSTRUCTION] = INST_SYNC_WRITE; 00372 gbInstructionPacket[PARAMETER] = (unsigned char)start_addr; 00373 gbInstructionPacket[PARAMETER+1] = (unsigned char)data_length*2; 00374 00375 00376 int slaveNum=param_length/(data_length+1); 00377 int OneRawByte=(1+data_length*2);//ID(1byte) + worddata*len(2byte*len) 00378 00379 00380 int i=0; //offset of slave number(number of row) 00381 int j=0; //offset of data in raw 00382 int k=1;//offset of int *param 00383 int index=0; 00384 00385 for( i=0; i<slaveNum; i++ ) 00386 { 00387 index=PARAMETER+OneRawByte*i+2; 00388 gbInstructionPacket[index] = (unsigned char)param[i*(data_length+1)];//ID 00389 k=1; 00390 00391 for(j=1;j<OneRawByte;j+=2) 00392 { 00393 gbInstructionPacket[index+j]= (unsigned char)(param[i*(data_length+1)+k]&0xff); //DATA L 00394 gbInstructionPacket[index+j+1]= (unsigned char)(param[i*(data_length+1)+k]>>8); //DATA H 00395 k++; 00396 } 00397 } 00398 00399 gbInstructionPacket[LENGTH] = OneRawByte*slaveNum+4; 00400 00401 //for(int i=0;i<50;i++) 00402 // pc.printf("gbInstructionPacket[%d]=%x\n",i,gbInstructionPacket[i]);//stanley test 00403 00404 00405 00406 dxl_txrx_packet(); 00407 return 0; 00408 } 00409 00410 void setPosition(int ServoID, int Position, int Speed)//stanley 00411 { 00412 while(giBusUsing); 00413 00414 gbInstructionPacket[ID] = (unsigned char)ServoID; 00415 gbInstructionPacket[INSTRUCTION] = INST_WRITE; 00416 gbInstructionPacket[PARAMETER] = (unsigned char)30; 00417 gbInstructionPacket[PARAMETER+1] = (unsigned char)dxl_get_lowbyte(Position); 00418 gbInstructionPacket[PARAMETER+2] = (unsigned char)dxl_get_highbyte(Position); 00419 gbInstructionPacket[PARAMETER+3] = (unsigned char)dxl_get_lowbyte(Speed); 00420 gbInstructionPacket[PARAMETER+4] = (unsigned char)dxl_get_highbyte(Speed); 00421 00422 gbInstructionPacket[LENGTH] = 7; 00423 00424 dxl_txrx_packet(); 00425 00426 }
Generated on Thu Jul 14 2022 17:32:04 by
1.7.2