BRSRI/bellande_humanoid_robot_intelligence_control_system_framework
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

- sync with dynamixel sdk (print comm result & rx packet error function added)

Browse Source
This commit is contained in:
ROBOTIS
2016-03-09 17:38:46 +09:00
parent 8bb48fc806
commit a0f412e4dd
6 changed files with 171 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h
View File

@@ -9,6 +9,7 @@
#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_
#include <stdio.h>
#include <vector>
#include "RobotisDef.h"
#include "PortHandler.h"
@@ -65,6 +66,9 @@ public:
virtual float GetProtocolVersion() = 0;
virtual void PrintTxRxResult(int result) = 0;
virtual void PrintRxPacketError(UINT8_T error) = 0;
virtual int TxPacket (PortHandler *port, UINT8_T *txpacket) = 0;
virtual int RxPacket (PortHandler *port, UINT8_T *rxpacket) = 0;
virtual int TxRxPacket (PortHandler *port, UINT8_T *txpacket, UINT8_T *rxpacket, UINT8_T *error = 0) = 0;

3
dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h
View File

@@ -28,6 +28,9 @@ public:
float GetProtocolVersion() { return 1.0; }
void PrintTxRxResult(int result);
void PrintRxPacketError(UINT8_T error);
int TxPacket (PortHandler *port, UINT8_T *txpacket);
int RxPacket (PortHandler *port, UINT8_T *rxpacket);
int TxRxPacket (PortHandler *port, UINT8_T *txpacket, UINT8_T *rxpacket, UINT8_T *error = 0);

3
dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h
View File

@@ -32,6 +32,9 @@ public:
float GetProtocolVersion() { return 2.0; }
void PrintTxRxResult(int result);
void PrintRxPacketError(UINT8_T error);
int TxPacket (PortHandler *port, UINT8_T *txpacket);
int RxPacket (PortHandler *port, UINT8_T *rxpacket);
int TxRxPacket (PortHandler *port, UINT8_T *txpacket, UINT8_T *rxpacket, UINT8_T *error = 0);

1
dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp
View File

@@ -123,6 +123,7 @@ int GroupBulkRead::RxPacket()
{
UINT8_T _id = id_list_[_i];
port_->SetPacketTimeout((UINT16_T)(length_list_[_id] + 11));
_result = ph_->ReadRx(port_, length_list_[_id], data_list_[_id]);
if(_result != COMM_SUCCESS)
{

69
dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp
View File

@@ -36,6 +36,75 @@ Protocol1PacketHandler *Protocol1PacketHandler::unique_instance_ = new Protocol1
Protocol1PacketHandler::Protocol1PacketHandler() { }
void Protocol1PacketHandler::PrintTxRxResult(int result)
{
switch(result)
{
case COMM_SUCCESS:
printf("[TxRxResult] Communication success.\n");
break;
case COMM_PORT_BUSY:
printf("[TxRxResult] Port is in use!\n");
break;
case COMM_TX_FAIL:
printf("[TxRxResult] Failed transmit instruction packet!\n");
break;
case COMM_RX_FAIL:
printf("[TxRxResult] Failed get status packet from device!\n");
break;
case COMM_TX_ERROR:
printf("[TxRxResult] Incorrect instruction packet!\n");
break;
case COMM_RX_WAITING:
printf("[TxRxResult] Now recieving status packet!\n");
break;
case COMM_RX_TIMEOUT:
printf("[TxRxResult] There is no status packet!\n");
break;
case COMM_RX_CORRUPT:
printf("[TxRxResult] Incorrect status packet!\n");
break;
case COMM_NOT_AVAILABLE:
printf("[TxRxResult] Protocol does not support This function!\n");
break;
default:
break;
}
}
void Protocol1PacketHandler::PrintRxPacketError(UINT8_T error)
{
if(error & ERRBIT_VOLTAGE)
printf("[RxPacketError] Input voltage error!\n");
if(error & ERRBIT_ANGLE)
printf("[RxPacketError] Angle limit error!\n");
if(error & ERRBIT_OVERHEAT)
printf("[RxPacketError] Overheat error!\n");
if(error & ERRBIT_RANGE)
printf("[RxPacketError] Out of range error!\n");
if(error & ERRBIT_CHECKSUM)
printf("[RxPacketError] Checksum error!\n");
if(error & ERRBIT_OVERLOAD)
printf("[RxPacketError] Overload error!\n");
if(error & ERRBIT_INSTRUCTION)
printf("[RxPacketError] Instruction code error!\n");
}
int Protocol1PacketHandler::TxPacket(PortHandler *port, UINT8_T *txpacket)
{
UINT8_T _checksum = 0;

91
dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp
View File

@@ -42,6 +42,97 @@ Protocol2PacketHandler *Protocol2PacketHandler::unique_instance_ = new Protocol2
Protocol2PacketHandler::Protocol2PacketHandler() { }
void Protocol2PacketHandler::PrintTxRxResult(int result)
{
switch(result)
{
case COMM_SUCCESS:
printf("[TxRxResult] Communication success.\n");
break;
case COMM_PORT_BUSY:
printf("[TxRxResult] Port is in use!\n");
break;
case COMM_TX_FAIL:
printf("[TxRxResult] Failed transmit instruction packet!\n");
break;
case COMM_RX_FAIL:
printf("[TxRxResult] Failed get status packet from device!\n");
break;
case COMM_TX_ERROR:
printf("[TxRxResult] Incorrect instruction packet!\n");
break;
case COMM_RX_WAITING:
printf("[TxRxResult] Now recieving status packet!\n");
break;
case COMM_RX_TIMEOUT:
printf("[TxRxResult] There is no status packet!\n");
break;
case COMM_RX_CORRUPT:
printf("[TxRxResult] Incorrect status packet!\n");
break;
case COMM_NOT_AVAILABLE:
printf("[TxRxResult] Protocol does not support This function!\n");
break;
default:
break;
}
}
void Protocol2PacketHandler::PrintRxPacketError(UINT8_T error)
{
if(error & ERRBIT_ALERT)
printf("[RxPacketError] Hardware error occurred. Check the error at Control Table (Hardware Error Status)!\n");
int _error = error & ~ERRBIT_ALERT;
switch(_error)
{
case 0:
break;
case ERRNUM_RESULT_FAIL:
printf("[RxPacketError] Failed to process the instruction packet!\n");
break;
case ERRNUM_INSTRUCTION:
printf("[RxPacketError] Undefined instruction or incorrect instruction!\n");
break;
case ERRNUM_CRC:
printf("[RxPacketError] CRC doesn't match!\n");
break;
case ERRNUM_DATA_RANGE:
printf("[RxPacketError] The data value is out of range!\n");
break;
case ERRNUM_DATA_LENGTH:
printf("[RxPacketError] The data length does not match as expected!\n");
break;
case ERRNUM_DATA_LIMIT:
printf("[RxPacketError] The data value exceeds the limit value!\n");
break;
case ERRNUM_ACCESS:
printf("[RxPacketError] Writing or Reading is not available to target address!\n");
break;
default:
printf("[RxPacketError] Unknown error code!\n");
break;
}
}
unsigned short Protocol2PacketHandler::UpdateCRC(UINT16_T crc_accum, UINT8_T *data_blk_ptr, UINT16_T data_blk_size)
{
UINT16_T i, j;