int Dynamixel::bulkRead(byte *param, int param_length) { //mResult = 0; uint32 bulkReadlength=0; int n, i, k=0; int num = param_length / 5; // each length : 5 (ID ADDR_L ADDR_H LEN_L LEN_H) // int pkt_length = param_length + 3; // 3 : INST CHKSUM_L CHKSUM_H // unsigned char txpacket[MAXNUM_TXPACKET] = {0}; // unsigned char rxpacket[MAXNUM_RXPACKET] = {0}; for(n=0; n < param_length; n++) { mParamBuffer[n] = param[n]; } /************ TxRxPacket *************/ // Wait for Bus Idle /* while(comm->iBusUsing == 1) { //Sleep(0); }*/ //mResult = txrx_PacketEx(BROADCAST_ID, INST_BULK_READ_EX, param_length);; this->txRxPacket(BROADCAST_ID, INST_BULK_READ, param_length); for(n = 0; n < num; n++) { // int id = param[n*5+0]; bulkReadlength = this->rxPacket(param_length+11); /*result = DXL_MAKEDWORD( DXL_MAKEWORD(gbpRxBufferEx[9],gbpRxBufferEx[10]), DXL_MAKEWORD(gbpRxBufferEx[11],gbpRxBufferEx[12]) );*/ if(mRxBuffer[7] == 0x55) { //packet instruction index mBulkData[n].iID = mRxBuffer[4]; //packet ID index mBulkData[n].iAddr = DXL_MAKEWORD(mParamBuffer[5*n+1],mParamBuffer[5*n+2]); //get address mBulkData[n].iLength = DXL_MAKEWORD(mParamBuffer[5*n+3],mParamBuffer[5*n+4]);//DXL_MAKEWORD(gbpRxBufferEx[PKT_LENGTH_L],gbpRxBufferEx[PKT_LENGTH_H]); //TxDStringC("iLength = ");TxDHex8C(mBulkData[n].iLength);TxDStringC("\r\n"); mBulkData[n].iError = mRxBuffer[7+1]; //Error code for(i=0; i < mBulkData[n].iLength ; i++) { mBulkData[n].iData[i] = mRxBuffer[7+2+i]; //DATA1 } } for(k=0; k < DXL_RX_BUF_SIZE ; k++) { mRxBuffer[k] = 0; //buffer clear } this->clearBuffer(); } return bulkReadlength; }
uint32 Dynamixel::readDword( byte bID, word wAddress ) { if(mPacketType == DXL_PACKET_TYPE1) return 0xFFFFFFFF; mParamBuffer[0] = (unsigned char)DXL_LOBYTE(wAddress); mParamBuffer[1] = (unsigned char)DXL_HIBYTE(wAddress); mParamBuffer[2] = 4; //4byte mParamBuffer[3] = 0; if(this->txRxPacket(bID, INST_READ, 4)) { return DXL_MAKEDWORD( DXL_MAKEWORD( mRxBuffer[9], mRxBuffer[10]), DXL_MAKEWORD( mRxBuffer[11], mRxBuffer[12])); } else { return 0xFFFFFFFF; } }
uint16_t pingGetModelNum1(int port_num, uint8_t id) { packetData[port_num].data_read = (uint8_t *)realloc(packetData[port_num].data_read, 2 * sizeof(uint8_t)); packetData[port_num].communication_result = COMM_TX_FAIL; packetData[port_num].tx_packet = (uint8_t *)realloc(packetData[port_num].tx_packet, 6); packetData[port_num].rx_packet = (uint8_t *)realloc(packetData[port_num].rx_packet, 6); if (id >= BROADCAST_ID) { packetData[port_num].communication_result = COMM_NOT_AVAILABLE; return 0; } packetData[port_num].tx_packet[PKT_ID] = id; packetData[port_num].tx_packet[PKT_LENGTH] = 2; packetData[port_num].tx_packet[PKT_INSTRUCTION] = INST_PING; txRxPacket1(port_num); if (packetData[port_num].communication_result == COMM_SUCCESS) { readTxRx1(port_num, id, 0, 2); // Address 0 : Model Number if (packetData[port_num].communication_result == COMM_SUCCESS) return DXL_MAKEWORD(packetData[port_num].data_read[0], packetData[port_num].data_read[1]); } return 0; }
UINT16_T PingGetModelNum1(int port_num, UINT8_T id) { packetData[port_num].data_read_ = (UINT8_T *)realloc(packetData[port_num].data_read_, 2 * sizeof(UINT8_T)); packetData[port_num].communication_result_ = COMM_TX_FAIL; packetData[port_num].txpacket_ = (UINT8_T *)realloc(packetData[port_num].txpacket_, 6); packetData[port_num].rxpacket_ = (UINT8_T *)realloc(packetData[port_num].rxpacket_, 6); if (id >= BROADCAST_ID) { packetData[port_num].communication_result_ = COMM_NOT_AVAILABLE; return 0; } packetData[port_num].txpacket_[PKT_ID] = id; packetData[port_num].txpacket_[PKT_LENGTH] = 2; packetData[port_num].txpacket_[PKT_INSTRUCTION] = INST_PING; TxRxPacket1(port_num); if (packetData[port_num].communication_result_ == COMM_SUCCESS) { ReadTxRx1(port_num, id, 0, 2); // Address 0 : Model Number if (packetData[port_num].communication_result_ == COMM_SUCCESS) return DXL_MAKEWORD(packetData[port_num].data_read_[0], packetData[port_num].data_read_[1]); } return 0; }
word Dynamixel::ping(byte bID ) { if(this->txRxPacket(bID, INST_PING, 0)) { if(mPacketType == DXL_PACKET_TYPE1) return (mRxBuffer[2]); //1.0 else return DXL_MAKEWORD(mRxBuffer[9],mRxBuffer[10]); //return product code when 2.0 } else { return 0xffff; //no dxl in bus. } }
uint16_t read2ByteRx1(int port_num) { packetData[port_num].data_read = (uint8_t *)realloc(packetData[port_num].data_read, 2 * sizeof(uint8_t)); packetData[port_num].data_read[0] = 0; packetData[port_num].data_read[1] = 0; readRx1(port_num, 2); if (packetData[port_num].communication_result == COMM_SUCCESS) return DXL_MAKEWORD(packetData[port_num].data_read[0], packetData[port_num].data_read[1]); return 0; }
UINT16_T Read2ByteRx1(int port_num) { packetData[port_num].data_read_ = (UINT8_T *)realloc(packetData[port_num].data_read_, 2 * sizeof(UINT8_T)); packetData[port_num].data_read_[0] = 0; packetData[port_num].data_read_[1] = 0; ReadRx1(port_num, 2); if (packetData[port_num].communication_result_ == COMM_SUCCESS) return DXL_MAKEWORD(packetData[port_num].data_read_[0], packetData[port_num].data_read_[1]); return 0; }
word Dynamixel::readWord(byte bID, word bAddress) { this->clearBuffer(); if(mPacketType == DXL_PACKET_TYPE1) { mParamBuffer[0] = bAddress; mParamBuffer[1] = 2; if(this->txRxPacket(bID, INST_READ, 2)) { return DXL_MAKEWORD(mRxBuffer[5],mRxBuffer[6]);//( (((word)mRxBuffer[6])<<8)+ mRxBuffer[5] ); } else { return 0xffff; } } else { mParamBuffer[0] = (unsigned char)DXL_LOBYTE(bAddress); mParamBuffer[1] = (unsigned char)DXL_HIBYTE(bAddress); mParamBuffer[2] = 2; //2byte mParamBuffer[3] = 0; if(this->txRxPacket(bID, INST_READ, 4)) { return(DXL_MAKEWORD(mRxBuffer[9], mRxBuffer[10])); } else { return 0xffff; } } }
uint32_t getDataRead1(int port_num, uint16_t data_length, uint16_t data_pos) { switch (data_length) { case 1: return packetData[port_num].data_read[data_pos + 0]; case 2: return DXL_MAKEWORD(packetData[port_num].data_read[data_pos + 0], packetData[port_num].data_read[data_pos + 1]); default: printf("[Set Data Read] failed... "); return 0; } }
byte Dynamixel::txRxPacket(byte bID, byte bInst, int bTxParaLen) { mDXLtxrxStatus = 0; word bTxLen, bRxLenEx, bTryCount; mBusUsed = 1; mRxLength = bRxLenEx = bTxLen = 0; for(bTryCount = 0; bTryCount < gbDXLNumberTxRxAttempts; bTryCount++)//for(bTryCount = 0; bTryCount < TRY_NUM; bTryCount++) { //gbDXLReadPointer = gbDXLWritePointer; mDxlDevice->read_pointer = mDxlDevice->write_pointer;//[ROBOTIS]BufferClear050728 /************************************** Transfer packet ***************************************************/ bTxLen = this->txPacket(bID, bInst, bTxParaLen); if(mPacketType == DXL_PACKET_TYPE1) { //Dxl 1.0 Tx success ? if (bTxLen == (bTxParaLen+4+2)) mDXLtxrxStatus = (1<<COMM_TXSUCCESS); } else { //Dxl 2.0 Tx success? if (bTxLen == (bTxParaLen+3+7)) mDXLtxrxStatus = (1<<COMM_TXSUCCESS); } //TxDStringC("bTxLen = ");TxDHex8C(bTxLen);TxDStringC("\r\n"); if(bInst == INST_PING) { if(mPacketType == DXL_PACKET_TYPE1) { //Dxl 1.0 if(bID == BROADCAST_ID) mRxLength = bRxLenEx = 0xff; else mRxLength = bRxLenEx = 6; // basic response packet length } else { //Dxl 2.0 if(bID == BROADCAST_ID) mRxLength = bRxLenEx = 0xffff; else mRxLength = bRxLenEx = 14; } } else if(bInst == INST_READ) { if (gbDXLStatusReturnLevel > 0) { if(mPacketType == DXL_PACKET_TYPE1) mRxLength = bRxLenEx = 6+mParamBuffer[1]; else mRxLength = bRxLenEx = 11+DXL_MAKEWORD(mParamBuffer[2], mParamBuffer[3]); } else { mRxLength = bRxLenEx = 0; } } else if( bID == BROADCAST_ID ) { if(bInst == INST_SYNC_READ || bInst == INST_BULK_READ) mRxLength = bRxLenEx = 0xffff; //only 2.0 case else mRxLength = bRxLenEx = 0; // no response packet } else { if (gbDXLStatusReturnLevel>1) { if(mPacketType == DXL_PACKET_TYPE1) mRxLength = bRxLenEx = 6;//+mParamBuffer[1]; else mRxLength = bRxLenEx = 11; } else { mRxLength = bRxLenEx = 0; } } if(bRxLenEx) { if(SmartDelayFlag == 1) delay(150); /************************************** Receive packet ***************************************************/ mRxLength = this->rxPacket(bRxLenEx); }//bRxLenEx is exist } //for() gbDXLNumberTxRxAttempts //TxDStringC("\r\n TEST POINT 2");//TxDString("\r\n Err ID:0x"); mBusUsed = 0; if((mRxLength != bRxLenEx) && (mTxBuffer[mPktIdIndex] != BROADCAST_ID)) { //TxDByteC('3');// //TxDStringC("Rx Error\r\n");//TxDString("\r\n Err ID:0x"); #ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2 //TxDString("\r\n Err ID:0x"); //TxDHex8(bID); TxDStringC("\r\n ->[DXL]Err: "); printBuffer(mTxBuffer,bTxLen); TxDStringC("\r\n <-[DXL]Err: "); printBuffer(mRxBuffer,mRxLength); #endif #ifdef PRINT_OUT_TRACE_ERROR_PRINT_TO_USART2 //TxDString("\r\n {[ERROR:");TxD16Hex(0x8100);TxDByte(':');TxD16Hex(bID);TxDByte(':');TxD8Hex(bInst);TxDByte(']');TxDByte('}'); //TxDByte(bID);TxDByte(' '); //TxDByte(bInst);TxDByte(' '); //TxDByte(gbpParameter[0]);TxDByte(' '); //TxDByte(gbpParameter[1]);TxDByte(' '); #endif return 0; } else if((mRxLength == 0) && (mTxBuffer[mPktInstIndex] == INST_PING)) { //[ROBOTIS] 2013-11-22 correct response for ping instruction //return 0; } //TxDString("\r\n TEST POINT 4");//TxDString("\r\n Err ID:0x"); #ifdef PRINT_OUT_PACKET_TO_USART2 TxDStringC("\r\n ->[TX Buffer]: "); printBuffer(mTxBuffer,bTxLen); TxDStringC("\r\n <-[RX Buffer]: "); printBuffer(mRxBuffer,mRxLength); #endif mDXLtxrxStatus = (1<<COMM_RXSUCCESS); //gbLengthForPacketMaking =0; return 1; }
byte Dynamixel::rxPacket(int bRxLength) { unsigned long ulCounter, ulTimeLimit; word bCount, bLength, bChecksum; byte bTimeout; bTimeout = 0; if(bRxLength == 255 || bRxLength == 0xffff) //2014-04-03 ulTimeLimit = RX_TIMEOUT_COUNT1; else ulTimeLimit = RX_TIMEOUT_COUNT2; for(bCount = 0; bCount < bRxLength; bCount++) { ulCounter = 0; while(mDxlDevice->read_pointer == mDxlDevice->write_pointer) { nDelay(NANO_TIME_DELAY); //[ROBOTIS] porting ydh if(ulCounter++ > ulTimeLimit) { bTimeout = 1; //TxDStringC("Timeout\r\n"); break; } uDelay(0); //[ROBOTIS] porting ydh added //if exist DXL 1.0 -> ok DXL 2.0 -> ok, if not exist 1.0 not ok, 2.0 ok } if(bTimeout) break; mRxBuffer[bCount] = mDxlDevice->data_buffer[mDxlDevice->read_pointer++ & DXL_RX_BUF_SIZE]; // get packet data from USART device //TxDStringC("mRxBuffer = ");TxDHex8C(mRxBuffer[bCount]);TxDStringC("\r\n"); } bLength = bCount; bChecksum = 0; if( mTxBuffer[mPktIdIndex] != BROADCAST_ID ) { if(bTimeout && bRxLength != 255) { #ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2 TxDStringC("Rx Timeout"); TxDByteC(bLength); #endif mDXLtxrxStatus |= (1<<COMM_RXTIMEOUT); clearBuffer(); //TxDStringC("Rx Timeout"); return 0; } if(bLength > 3) //checking available length. { /*if(mPacketType == 1){ if(mRxBuffer[0] != 0xff || mRxBuffer[1] != 0xff ) mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXHEADER); else if(mRxBuffer[mPktIdIndex] != mTxBuffer[mPktIdIndex] ) mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXID); else if(mRxBuffer[mPktLengthIndex] != bLength-mPktInstIndex) mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXLENGTH); else{ for(bCount = 2; bCount < bLength; bCount++){ bChecksum += mRxBuffer[bCount]; //Calculate checksum of received data for compare } if(bChecksum != 0xff) mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXCHECKSUM); return 0; } }else{ if(mRxBuffer[0] != 0xff || mRxBuffer[1] != 0xff || mRxBuffer[2] != 0xfd) mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXHEADER); else if(mRxBuffer[mPktIdIndex] != mTxBuffer[mPktIdIndex] ) mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXID); else if(mRxBuffer[mPktLengthIndex] != bLength-mPktInstIndex) mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXLENGTH); else{ bChecksum = DXL_MAKEWORD(mRxBuffer[bRxLength-2], mRxBuffer[bRxLength-1]); if(update_crc(0, mRxBuffer, bRxLength-2) != bChecksum) mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXCHECKSUM); return 0; } } */ if(mPacketType == 1) { //Dxl 1.0 header check if(mRxBuffer[0] != 0xff || mRxBuffer[1] != 0xff ) { #ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2 TxDStringC("Wrong Header");//[Wrong Header] #endif mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXHEADER); clearBuffer(); return 0; } } else { // Dxl 2.0 header check if(mRxBuffer[0] != 0xff || mRxBuffer[1] != 0xff || mRxBuffer[2] != 0xfd) { #ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2 TxDStringC("Wrong Header");//[Wrong Header] #endif mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXHEADER); clearBuffer(); return 0; } } if(mRxBuffer[mPktIdIndex] != mTxBuffer[mPktIdIndex] ) //id check { #ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2 TxDStringC("[Error:TxID != RxID]"); #endif mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXID); clearBuffer(); return 0; } if(mRxBuffer[mPktLengthIndex] != bLength-mPktInstIndex) // status packet length check { #ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2 TxDStringC("RxLength Error"); #endif mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXLENGTH); clearBuffer(); return 0; } if(mPacketType == 1 && mRxBuffer[mPktErrorIndex] != 0) { //140512 shin #ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2 for(bTryCount = 0; bTryCount<= 6; bTryCount++) { if((mRxBuffer[mPktErrorIndex] & (1<<bTryCount)) == TRUE) { switch(bTryCount) { case 0: TxDStringC("InputVoltage Error"); break; case 1: TxDStringC("Angle Limit Error"); break; case 2: TxDStringC("Overheating Error"); break; case 3: TxDStringC("Range Error"); break; case 4: TxDStringC("Checksum Error"); break; case 5: TxDStringC("Overload Error"); break; case 6: TxDStringC("Instruction Error"); break; } } } #endif } else { //140512 shin #ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2 for(bTryCount = 1; bTryCount<= 7; bTryCount++) { if((mRxBuffer[mPktErrorIndex]) == bTryCount) { switch(bTryCount) { case 1: TxDStringC("Result Fail"); break; case 2: TxDStringC("Instruction Error"); break; case 3: TxDStringC("CRC Error"); break; case 4: TxDStringC("DataRange Error"); break; case 5: TxDStringC("DataLength Error"); break; case 6: TxDStringC("DataLimit Error"); break; case 7: TxDStringC("Accrss Error"); break; } } } #endif } if(mPacketType == 1) { // Dxl 1.0 checksum for(bCount = 2; bCount < bLength; bCount++) { bChecksum += mRxBuffer[bCount]; //Calculate checksum of received data for compare } if(bChecksum != 0xff) { #ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2 TxDStringC("[RxChksum Error]"); #endif mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXCHECKSUM); clearBuffer(); return 0; } } else { // Dxl 2.0 checksum bChecksum = DXL_MAKEWORD(mRxBuffer[bRxLength-2], mRxBuffer[bRxLength-1]); if(update_crc(0, mRxBuffer, bRxLength-2) == bChecksum) { // -2 : except CRC16 return bLength; } else { #ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2 TxDStringC("CRC-16 Error\r\n"); #endif return 0; } }//end of checksum }//(bLength > 3) }//end of Rx status packet check return bLength; }