123

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers dynamixel.cpp Source File

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 }