DXL_SDK_For_F446RE. Now test with MX64AR, MX28AR with 1Mbps
Dependents: RoboticArm DXL_SDK_Porting_Test
Diff: dynamixel.cpp
- Revision:
- 0:bf4774b25188
- Child:
- 3:e2e9064c668d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/dynamixel.cpp Wed Feb 08 02:49:39 2017 +0000 @@ -0,0 +1,421 @@ +#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( int id, 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( int id, int address, 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_16bit(int start_addr, int data_length, int *param, 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; + + 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(); + +}