00001 // Dynamixel control code - C++ file 00002 // Copyright (c) 2008 Erik Schuitema, Eelko van Breda 00003 // Delft University of Technology 00004 // www.dbl.tudelft.nl 00005 00006 #include <threemxl/platform/hardware/dynamixel/CDxlCom.h> 00007 #include <threemxl/platform/hardware/dynamixel/CDxlSerialPacketHandler.h> 00008 00009 //***********************************************************// 00010 //********************* CDxlCom *********************// 00011 //***********************************************************// 00012 00013 CDxlCom::CDxlCom() : mLog("CDxlCom") 00014 { 00015 // mLog.setLevel(llCrawl); 00016 // mLog.enableConsoleOutput("false"); 00017 mInitialized = false; 00018 mLastError = 0; 00019 mSerialPort = NULL; 00020 mPacketHandler = new CDxlSerialPacketHandler(mSerialPort); 00021 } 00022 00023 int CDxlCom::setPacketHandler(CDxlPacketHandler *packetHandler) 00024 { 00025 if (packetHandler == NULL) 00026 return DXL_INVALID_PARAMETER; 00027 00028 delete mPacketHandler; 00029 mPacketHandler = packetHandler; 00030 00031 return DXL_SUCCESS; 00032 } 00033 00034 int CDxlCom::initPacketHandler() 00035 { 00036 if (mPacketHandler == NULL) 00037 return DXL_INVALID_PARAMETER; 00038 00039 return mPacketHandler->init(); 00040 } 00041 00042 const char* CDxlCom::translateErrorCode(int errorCode) 00043 { 00044 switch (errorCode) 00045 { 00046 case DXL_SUCCESS: return "DXL_SUCCESS"; break; 00047 case DXL_PKT_RECV_ERROR: return "DXL_PKT_RECV_ERROR"; break; 00048 case DXL_PKT_RECV_CHECKSUM_ERR: return "DXL_PKT_RECV_CHECKSUM_ERR"; break; 00049 case DXL_PKT_RECV_LENGTH_ERR: return "DXL_PKT_RECV_LENGTH_ERR"; break; 00050 case DXL_PKT_RECV_ID_ERR: return "DXL_PKT_RECV_ID_ERR"; break; 00051 case DXL_PKT_RECV_TIMEOUT: return "DXL_PKT_RECV_TIMEOUT" ; break; 00052 case DXL_ALREADY_INITIALIZED: return "DXL_ALREADY_INITIALIZED"; break; 00053 case DXL_NOT_INITIALIZED: return "DXL_NOT_INITIALIZED"; break; 00054 case DXL_NO_SERIAL_PORT: return "DXL_NO_SERIAL_PORT"; break; 00055 case DXL_INVALID_PARAMETER: return "DXL_INVALID_PARAMETER"; break; 00056 case DXL_PKT_SEND_ERROR: return "DXL_PKT_SEND_ERROR"; break; 00057 case DXL_PKT_SEND_LENGTH_ERR: return "DXL_PKT_SEND_LENGTH_ERR"; break; 00058 case M3XL_INSTRUCTION_ERROR : return "M3XL_INSTRUCTION_ERROR ";break; 00059 case M3XL_OVERLOAD_ERROR : return "M3XL_OVERLOAD_ERROR ";break; 00060 case M3XL_CHECKSUM_ERROR : return "M3XL_CHECKSUM_ERROR ";break; 00061 case M3XL_RANGE_ERROR : return "M3XL_RANGE_ERROR ";break; 00062 case M3XL_OVERHEATING_ERROR : return "M3XL_OVERHEATING_ERROR ";break; 00063 case M3XL_ANGLE_LIMIT_ERROR : return "M3XL_ANGLE_LIMIT_ERROR ";break; 00064 case M3XL_INPUT_VOLTAGE_ERROR : return "M3XL_INPUT_VOLTAGE_ERROR ";break; 00065 case M3XL_STATUS_EEPROM_ERROR : return "M3XL_STATUS_EEPROM_ERROR ";break; 00066 case M3XL_STATUS_NOT_INITIALIZED : return "M3XL_STATUS_NOT_INITIALIZED ";break; 00067 case M3XL_STATUS_EM_STOP_ERROR : return "M3XL_STATUS_EM_STOP_ERROR ";break; 00068 case M3XL_STATUS_INIT_TIME_OUT_ERROR : return "M3XL_STATUS_INIT_TIME_OUT_ERROR ";break; 00069 case M3XL_STATUS_MAX_POS_ERROR : return "M3XL_STATUS_MAX_POS_ERROR ";break; 00070 case M3XL_STATUS_MAX_TORQUE_ERROR : return "M3XL_STATUS_MAX_TORQUE_ERROR ";break; 00071 case M3XL_STATUS_MAX_CURRENT_ERROR : return "M3XL_STATUS_MAX_CURRENT_ERROR ";break; 00072 case M3XL_STATUS_MOTOR_STUCK_ERROR : return "M3XL_STATUS_MOTOR_STUCK_ERROR ";break; 00073 case M3XL_STATUS_JOINT_STUCK_ERROR : return "M3XL_STATUS_JOINT_STUCK_ERROR ";break; 00074 case M3XL_STATUS_PROTOCOL_TIME_OUT_ERROR: return "M3XL_STATUS_PROTOCOL_TIME_OUT_ERROR";break; 00075 00076 default: return "UNKOWN ERROR"; 00077 } 00078 } 00079 00080 int CDxlCom::sendPacket(CDxlPacket *packet, bool replyExpected) // Returns true if no. of bytes sent equals packet size 00081 { 00082 int result = mPacketHandler->sendPacket(packet, replyExpected); 00083 00084 // std::cout << "Sent packet " << packet->getPktString() << std::endl; 00085 00086 if (result != DXL_SUCCESS) 00087 mLastError = mPacketHandler->getLastError(); 00088 return result; 00089 } 00090 00091 //int CDxlCom::receivePacket(CDxlStatusPacket *packet) 00092 //{ 00093 // int numBytesRead = mSerialPort->port_read(packet->data(), packet->length()); 00094 // if (numBytesRead < 0 ) 00095 // { 00096 // mLastError = numBytesRead; 00097 // return DXL_PKT_RECV_ERROR; 00098 // } 00099 // else if (numBytesRead != packet->length()) 00100 // { 00101 // mLastError = numBytesRead; 00102 // return DXL_PKT_RECV_LENGTH_ERR; 00103 // } 00104 // 00105 // if ( packet->calcChecksum() != packet->readChecksum() ) 00106 // { 00107 // #ifdef __DBG__ 00108 // printf("warning: CDxlCom::receivePacket: checksum error in dlx package. RecvCS: %02X CalcCS: %02X\n",packet->readChecksum(), packet->calcChecksum()); 00109 // #endif 00110 // 00111 // return DXL_PKT_RECV_CHECKSUM_ERR; 00112 // } 00113 // return DXL_SUCCESS; 00114 //} 00115 00116 int CDxlCom::receivePacketWait(CDxlStatusPacket *packet, int seconds, int microseconds) 00117 { 00118 int result = mPacketHandler->receivePacketWait(packet, seconds, microseconds); 00119 00120 if (result != DXL_SUCCESS) 00121 mLastError = mPacketHandler->getLastError(); 00122 return result; 00123 }