DXL_SDK_For_F446RE. Now test with MX64AR, MX28AR with 1Mbps
Dependents: RoboticArm DXL_SDK_Porting_Test
dynamixel.cpp
- Committer:
- stanley1228
- Date:
- 2017-02-08
- Revision:
- 4:44560391db99
- Parent:
- 3:e2e9064c668d
- Child:
- 5:edccfcb47ab8
File content as of revision 4:44560391db99:
#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(short int start_addr, short int data_length, short int *param, 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(); }