![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
123
Dependencies: 3W_8Dir_p2pcontrol mbed
Fork of DXL_SDK_For_F446RE by
Diff: dynamixel.cpp
- Revision:
- 6:1fe7b6875e86
- Parent:
- 5:edccfcb47ab8
diff -r edccfcb47ab8 -r 1fe7b6875e86 dynamixel.cpp --- a/dynamixel.cpp Fri Feb 10 22:55:30 2017 +0800 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,426 +0,0 @@ -#include "dxl_hal.h" -#include "dynamixel.h" - -#define ID (2) -#define LENGTH (3) -#define INSTRUCTION (4) -#define ERRBIT (4) -#define PARAMETER (5) -#define DEFAULT_BAUDNUMBER (1) - -unsigned char gbInstructionPacket[MAXNUM_TXPARAM+10] = {0}; -unsigned char gbStatusPacket[MAXNUM_RXPARAM+10] = {0}; -unsigned char gbRxPacketLength = 0; -unsigned char gbRxGetLength = 0; -int gbCommStatus = COMM_RXSUCCESS; -int giBusUsing = 0; - -#include "mbed.h" -#include "Serial.h" -extern Serial pc;//stanley - - -//DigitalOut gTest(D4); - -int dxl_initialize( int devIndex, int baudnum ) -{ - float baudrate; - baudrate = 2000000.0f / (float)(baudnum + 1); - - //gTest=0;//stanley - - if( dxl_hal_open(devIndex, baudrate) == 0 ) - return 0; - - gbCommStatus = COMM_RXSUCCESS; - giBusUsing = 0; - return 1; -} - -void dxl_terminate() -{ - dxl_hal_close(); -} - -void dxl_tx_packet() -{ - unsigned char i; - unsigned char TxNumByte, RealTxNumByte; - unsigned char checksum = 0; - - if( giBusUsing == 1 ) - return; - - giBusUsing = 1; - - if( gbInstructionPacket[LENGTH] > (MAXNUM_TXPARAM+2) ) - { - gbCommStatus = COMM_TXERROR; - giBusUsing = 0; - return; - } - - if( gbInstructionPacket[INSTRUCTION] != INST_PING - && gbInstructionPacket[INSTRUCTION] != INST_READ - && gbInstructionPacket[INSTRUCTION] != INST_WRITE - && gbInstructionPacket[INSTRUCTION] != INST_REG_WRITE - && gbInstructionPacket[INSTRUCTION] != INST_ACTION - && gbInstructionPacket[INSTRUCTION] != INST_RESET - && gbInstructionPacket[INSTRUCTION] != INST_SYNC_WRITE ) - { - gbCommStatus = COMM_TXERROR; - giBusUsing = 0; - return; - } - - gbInstructionPacket[0] = 0xff; - gbInstructionPacket[1] = 0xff; - for( i=0; i<(gbInstructionPacket[LENGTH]+1); i++ ) - checksum += gbInstructionPacket[i+2]; - gbInstructionPacket[gbInstructionPacket[LENGTH]+3] = ~checksum; - - if( gbCommStatus == COMM_RXTIMEOUT || gbCommStatus == COMM_RXCORRUPT ) - dxl_hal_clear(); - - TxNumByte = gbInstructionPacket[LENGTH] + 4; - RealTxNumByte = dxl_hal_tx( (unsigned char*)gbInstructionPacket, TxNumByte ); - - if( TxNumByte != RealTxNumByte ) - { - gbCommStatus = COMM_TXFAIL; - giBusUsing = 0; - return; - } - - if( gbInstructionPacket[INSTRUCTION] == INST_READ ) - dxl_hal_set_timeout( gbInstructionPacket[PARAMETER+1] + 6 ); - else - dxl_hal_set_timeout( 6 ); - - gbCommStatus = COMM_TXSUCCESS; -} - -void dxl_rx_packet() -{ - unsigned char i, j, nRead; - unsigned char checksum = 0; - - if( giBusUsing == 0 ) - return; - - if( gbInstructionPacket[ID] == BROADCAST_ID ) - { - gbCommStatus = COMM_RXSUCCESS; - giBusUsing = 0; - return; - } - - if( gbCommStatus == COMM_TXSUCCESS ) - { - gbRxGetLength = 0; - gbRxPacketLength = 6; - } - - nRead = dxl_hal_rx( (unsigned char*)&gbStatusPacket[gbRxGetLength], gbRxPacketLength - gbRxGetLength ); - - gbRxGetLength += nRead; - if( gbRxGetLength < gbRxPacketLength ) - { - if( dxl_hal_timeout() == 1 ) - { - if(gbRxGetLength == 0) - gbCommStatus = COMM_RXTIMEOUT; - else - gbCommStatus = COMM_RXCORRUPT; - giBusUsing = 0; - return; - } - } - //pc.printf("133\n");//stanley - // Find packet header - for( i=0; i<(gbRxGetLength-1); i++ ) - { - if( gbStatusPacket[i] == 0xff && gbStatusPacket[i+1] == 0xff ) - { - break; - } - else if( i == gbRxGetLength-2 && gbStatusPacket[gbRxGetLength-1] == 0xff ) - { - break; - } - } - if( i > 0 ) - { - for( j=0; j<(gbRxGetLength-i); j++ ) - gbStatusPacket[j] = gbStatusPacket[j + i]; - - gbRxGetLength -= i; - } - - if( gbRxGetLength < gbRxPacketLength ) - { - gbCommStatus = COMM_RXWAITING; - return; - } - - // Check id pairing - if( gbInstructionPacket[ID] != gbStatusPacket[ID]) - { - gbCommStatus = COMM_RXCORRUPT; - giBusUsing = 0; - return; - } - - gbRxPacketLength = gbStatusPacket[LENGTH] + 4; - if( gbRxGetLength < gbRxPacketLength ) - { - nRead = dxl_hal_rx( (unsigned char*)&gbStatusPacket[gbRxGetLength], gbRxPacketLength - gbRxGetLength ); - gbRxGetLength += nRead; - if( gbRxGetLength < gbRxPacketLength ) - { - gbCommStatus = COMM_RXWAITING; - return; - } - } - - // Check checksum - for( i=0; i<(gbStatusPacket[LENGTH]+1); i++ ) - checksum += gbStatusPacket[i+2]; - checksum = ~checksum; - - if( gbStatusPacket[gbStatusPacket[LENGTH]+3] != checksum ) - { - gbCommStatus = COMM_RXCORRUPT; - giBusUsing = 0; - return; - } - - gbCommStatus = COMM_RXSUCCESS; - giBusUsing = 0; -} - -void dxl_txrx_packet() -{ - - //pc.printf("before dxl_tx_packet\n"); - dxl_tx_packet(); - - //pc.printf("after dxl_tx_packet\n"); - if( gbCommStatus != COMM_TXSUCCESS ) - return; - - - - do{ - //pc.printf("before dxl_rx_packet\n"); - dxl_rx_packet(); - //pc.printf("after dxl_rx_packet\n"); - }while( gbCommStatus == COMM_RXWAITING ); -} - -int dxl_get_result() -{ - return gbCommStatus; -} - -void dxl_set_txpacket_id( int id ) -{ - gbInstructionPacket[ID] = (unsigned char)id; -} - -void dxl_set_txpacket_instruction( int instruction ) -{ - gbInstructionPacket[INSTRUCTION] = (unsigned char)instruction; -} - -void dxl_set_txpacket_parameter( int index, int value ) -{ - gbInstructionPacket[PARAMETER+index] = (unsigned char)value; -} - -void dxl_set_txpacket_length( int length ) -{ - gbInstructionPacket[LENGTH] = (unsigned char)length; -} - -int dxl_get_rxpacket_error( int errbit ) -{ - if( gbStatusPacket[ERRBIT] & (unsigned char)errbit ) - return 1; - - return 0; -} - -int dxl_get_rxpacket_length() -{ - return (int)gbStatusPacket[LENGTH]; -} - -int dxl_get_rxpacket_parameter( int index ) -{ - return (int)gbStatusPacket[PARAMETER+index]; -} - -int dxl_makeword( int lowbyte, int highbyte ) -{ - unsigned short word; - - word = highbyte; - word = word << 8; - word = word + lowbyte; - return (int)word; -} - -int dxl_get_lowbyte( int word ) -{ - unsigned short temp; - - temp = word & 0xff; - return (int)temp; -} - -int dxl_get_highbyte( int word ) -{ - unsigned short temp; - - temp = word & 0xff00; - temp = temp >> 8; - return (int)temp; -} - -void dxl_ping( int id ) -{ - while(giBusUsing); - - gbInstructionPacket[ID] = (unsigned char)id; - gbInstructionPacket[INSTRUCTION] = INST_PING; - gbInstructionPacket[LENGTH] = 2; - - dxl_txrx_packet(); -} - -int dxl_read_byte( int id, int address ) -{ - while(giBusUsing); - - gbInstructionPacket[ID] = (unsigned char)id; - gbInstructionPacket[INSTRUCTION] = INST_READ; - gbInstructionPacket[PARAMETER] = (unsigned char)address; - gbInstructionPacket[PARAMETER+1] = 1; - gbInstructionPacket[LENGTH] = 4; - - dxl_txrx_packet(); - - return (int)gbStatusPacket[PARAMETER]; -} - -void dxl_write_byte( int id, int address, int value ) -{ - //pc.printf("id=%d,address=%x,value=%d\n",id,address,value);//stanley - while(giBusUsing); - - //pc.printf("after giBusUsing\n",id,address,value);//stanley - - gbInstructionPacket[ID] = (unsigned char)id; - gbInstructionPacket[INSTRUCTION] = INST_WRITE; - gbInstructionPacket[PARAMETER] = (unsigned char)address; - gbInstructionPacket[PARAMETER+1] = (unsigned char)value; - gbInstructionPacket[LENGTH] = 4; - - //pc.printf("before dxl_txrx_packet\n",id,address,value);//stanley - - dxl_txrx_packet(); - - //pc.printf("after dxl_txrx_packet\n",id,address,value);//stanley -} - -int dxl_read_word( short int id, short int address ) -{ - while(giBusUsing); - - gbInstructionPacket[ID] = (unsigned char)id; - gbInstructionPacket[INSTRUCTION] = INST_READ; - gbInstructionPacket[PARAMETER] = (unsigned char)address; - gbInstructionPacket[PARAMETER+1] = 2; - gbInstructionPacket[LENGTH] = 4; - - dxl_txrx_packet(); - - return dxl_makeword((int)gbStatusPacket[PARAMETER], (int)gbStatusPacket[PARAMETER+1]); -} - -void dxl_write_word( short int id, short int address, short int value ) -{ - while(giBusUsing); - - gbInstructionPacket[ID] = (unsigned char)id; - gbInstructionPacket[INSTRUCTION] = INST_WRITE; - gbInstructionPacket[PARAMETER] = (unsigned char)address; - gbInstructionPacket[PARAMETER+1] = (unsigned char)dxl_get_lowbyte(value); - gbInstructionPacket[PARAMETER+2] = (unsigned char)dxl_get_highbyte(value); - gbInstructionPacket[LENGTH] = 5; - - dxl_txrx_packet(); -} - -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 -{ - while(giBusUsing); - - gbInstructionPacket[ID] = (unsigned char)BROADCAST_ID; - gbInstructionPacket[INSTRUCTION] = INST_SYNC_WRITE; - gbInstructionPacket[PARAMETER] = (unsigned char)start_addr; - gbInstructionPacket[PARAMETER+1] = (unsigned char)data_length*2; - - - int slaveNum=param_length/(data_length+1); - int OneRawByte=(1+data_length*2);//ID(1byte) + worddata*len(2byte*len) - - - int i=0; //offset of slave number(number of row) - int j=0; //offset of data in raw - int k=1;//offset of int *param - int index=0; - - for( i=0; i<slaveNum; i++ ) - { - index=PARAMETER+OneRawByte*i+2; - gbInstructionPacket[index] = (unsigned char)param[i*(data_length+1)];//ID - k=1; - - for(j=1;j<OneRawByte;j+=2) - { - gbInstructionPacket[index+j]= (unsigned char)(param[i*(data_length+1)+k]&0xff); //DATA L - gbInstructionPacket[index+j+1]= (unsigned char)(param[i*(data_length+1)+k]>>8); //DATA H - k++; - } - } - - gbInstructionPacket[LENGTH] = OneRawByte*slaveNum+4; - - //for(int i=0;i<50;i++) - // pc.printf("gbInstructionPacket[%d]=%x\n",i,gbInstructionPacket[i]);//stanley test - - - - dxl_txrx_packet(); - return 0; -} - -void setPosition(int ServoID, int Position, int Speed)//stanley -{ - while(giBusUsing); - - gbInstructionPacket[ID] = (unsigned char)ServoID; - gbInstructionPacket[INSTRUCTION] = INST_WRITE; - gbInstructionPacket[PARAMETER] = (unsigned char)30; - gbInstructionPacket[PARAMETER+1] = (unsigned char)dxl_get_lowbyte(Position); - gbInstructionPacket[PARAMETER+2] = (unsigned char)dxl_get_highbyte(Position); - gbInstructionPacket[PARAMETER+3] = (unsigned char)dxl_get_lowbyte(Speed); - gbInstructionPacket[PARAMETER+4] = (unsigned char)dxl_get_highbyte(Speed); - - gbInstructionPacket[LENGTH] = 7; - - dxl_txrx_packet(); - -}