From 30586ed499852d60d7948ffb7bae2890262834c4 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Wed, 25 May 2016 11:37:07 +0900 Subject: [PATCH 01/33] - added position_p_gain sync write - MotionModule/SensorModule member variable access changed (public -> protected). - some bug fixed. --- .../robotis_controller/RobotisController.h | 6 +- .../robotis_controller/RobotisController.cpp | 332 +++++++++++++----- .../include/robotis_device/Dynamixel.h | 1 + .../include/robotis_device/DynamixelState.h | 2 + .../src/robotis_device/Dynamixel.cpp | 3 +- robotis_device/src/robotis_device/Robot.cpp | 10 +- .../robotis_framework_common/MotionModule.h | 22 +- .../robotis_framework_common/SensorModule.h | 5 +- 8 files changed, 293 insertions(+), 88 deletions(-) diff --git a/robotis_controller/include/robotis_controller/RobotisController.h b/robotis_controller/include/robotis_controller/RobotisController.h index 46eb104..d974a3e 100644 --- a/robotis_controller/include/robotis_controller/RobotisController.h +++ b/robotis_controller/include/robotis_controller/RobotisController.h @@ -24,7 +24,6 @@ #include "robotis_controller_msgs/JointCtrlModule.h" #include "robotis_controller_msgs/GetJointModule.h" -// TODO: TEMPORARY CODE !! #include "dynamixel_sdk/GroupBulkRead.h" #include "dynamixel_sdk/GroupSyncWrite.h" @@ -73,11 +72,14 @@ class RobotisController : public Singleton bool gazebo_mode; std::string gazebo_robot_name; - // TODO: TEMPORARY CODE !! + /* bulk read */ std::map port_to_bulk_read; + + /* sync write */ std::map port_to_sync_write_position; std::map port_to_sync_write_velocity; std::map port_to_sync_write_torque; + std::map port_to_sync_write_position_p_gain; /* publisher */ ros::Publisher goal_joint_state_pub; diff --git a/robotis_controller/src/robotis_controller/RobotisController.cpp b/robotis_controller/src/robotis_controller/RobotisController.cpp index e93e58c..d6dfda1 100644 --- a/robotis_controller/src/robotis_controller/RobotisController.cpp +++ b/robotis_controller/src/robotis_controller/RobotisController.cpp @@ -13,15 +13,15 @@ using namespace ROBOTIS; RobotisController::RobotisController() -: is_timer_running_(false), - stop_timer_(false), - init_pose_loaded_(false), - timer_thread_(0), - controller_mode_(MOTION_MODULE_MODE), - DEBUG_PRINT(false), - robot(0), - gazebo_mode(false), - gazebo_robot_name("robotis") + : is_timer_running_(false), + stop_timer_(false), + init_pose_loaded_(false), + timer_thread_(0), + controller_mode_(MOTION_MODULE_MODE), + DEBUG_PRINT(false), + robot(0), + gazebo_mode(false), + gazebo_robot_name("robotis") { direct_sync_write_.clear(); } @@ -54,11 +54,25 @@ void RobotisController::InitSyncWrite() // clear syncwrite param setting for(std::map::iterator _it = port_to_sync_write_position.begin(); _it != port_to_sync_write_position.end(); _it++) - _it->second->ClearParam(); + { + if(_it->second != NULL) + _it->second->ClearParam(); + } + for(std::map::iterator _it = port_to_sync_write_position_p_gain.begin(); _it != port_to_sync_write_position_p_gain.end(); _it++) + { + if(_it->second != NULL) + _it->second->ClearParam(); + } for(std::map::iterator _it = port_to_sync_write_velocity.begin(); _it != port_to_sync_write_velocity.end(); _it++) - _it->second->ClearParam(); + { + if(_it->second != NULL) + _it->second->ClearParam(); + } for(std::map::iterator _it = port_to_sync_write_torque.begin(); _it != port_to_sync_write_torque.end(); _it++) - _it->second->ClearParam(); + { + if(_it->second != NULL) + _it->second->ClearParam(); + } // set init syncwrite param(from data of bulkread) for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) @@ -91,6 +105,10 @@ void RobotisController::InitSyncWrite() port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); } + else if(_dxl->position_p_gain_item != 0 && _dxl->bulk_read_items[_i]->item_name == _dxl->position_p_gain_item->item_name) + { + _dxl->dxl_state->position_p_gain = _read_data; + } else if(_dxl->present_velocity_item != 0 && _dxl->bulk_read_items[_i]->item_name == _dxl->present_velocity_item->item_name) { _dxl->dxl_state->present_velocity = _dxl->ConvertValue2Velocity(_read_data); @@ -141,20 +159,37 @@ bool RobotisController::Initialize(const std::string robot_file_path, const std: Dynamixel *_default_device = _dxl_it->second; _default_pkt_handler = PacketHandler::GetPacketHandler(_default_device->protocol_version); - port_to_sync_write_position[_port_name] = new GroupSyncWrite(_port, - _default_pkt_handler, - _default_device->goal_position_item->address, - _default_device->goal_position_item->data_length); + if(_default_device->goal_position_item != 0) + { + port_to_sync_write_position[_port_name] = new GroupSyncWrite(_port, + _default_pkt_handler, + _default_device->goal_position_item->address, + _default_device->goal_position_item->data_length); + } - port_to_sync_write_velocity[_port_name] = new GroupSyncWrite(_port, - _default_pkt_handler, - _default_device->goal_velocity_item->address, - _default_device->goal_velocity_item->data_length); + if(_default_device->position_p_gain_item != 0) + { + port_to_sync_write_position_p_gain[_port_name] = new GroupSyncWrite(_port, + _default_pkt_handler, + _default_device->position_p_gain_item->address, + _default_device->position_p_gain_item->data_length); + } - port_to_sync_write_torque[_port_name] = new GroupSyncWrite(_port, - _default_pkt_handler, - _default_device->goal_current_item->address, - _default_device->goal_current_item->data_length); + if(_default_device->goal_velocity_item != 0) + { + port_to_sync_write_velocity[_port_name] = new GroupSyncWrite(_port, + _default_pkt_handler, + _default_device->goal_velocity_item->address, + _default_device->goal_velocity_item->data_length); + } + + if(_default_device->goal_current_item != 0) + { + port_to_sync_write_torque[_port_name] = new GroupSyncWrite(_port, + _default_pkt_handler, + _default_device->goal_current_item->address, + _default_device->goal_current_item->data_length); + } } else if(_sensor_it != robot->sensors.end()) { @@ -193,14 +228,14 @@ bool RobotisController::Initialize(const std::string robot_file_path, const std: int _bulkread_start_addr = 0; int _bulkread_data_length = 0; - // bulk read default : present position - if(_dxl->present_position_item != 0) - { - _bulkread_start_addr = _dxl->present_position_item->address; - _bulkread_data_length = _dxl->present_position_item->data_length; - } +// // bulk read default : present position +// if(_dxl->present_position_item != 0) +// { +// _bulkread_start_addr = _dxl->present_position_item->address; +// _bulkread_data_length = _dxl->present_position_item->data_length; +// } - // TODO: modifing + // calculate bulk read start address & data length std::map::iterator _indirect_addr_it = _dxl->ctrl_table.find(INDIRECT_ADDRESS_1); if(_indirect_addr_it != _dxl->ctrl_table.end()) // INDIRECT_ADDRESS_1 exist { @@ -248,13 +283,77 @@ bool RobotisController::Initialize(const std::string robot_file_path, const std: } // ROS_WARN("[%12s] start_addr: %d, data_length: %d", _joint_name.c_str(), _bulkread_start_addr, _bulkread_data_length); - port_to_bulk_read[_dxl->port_name]->AddParam(_dxl->id, _bulkread_start_addr, _bulkread_data_length); + if(_bulkread_start_addr != 0) + port_to_bulk_read[_dxl->port_name]->AddParam(_dxl->id, _bulkread_start_addr, _bulkread_data_length); // Torque ON if(WriteCtrlItem(_joint_name, _dxl->torque_enable_item->item_name, 1) != COMM_SUCCESS) WriteCtrlItem(_joint_name, _dxl->torque_enable_item->item_name, 1); } + for(std::map::iterator _it = robot->sensors.begin(); _it != robot->sensors.end(); _it++) + { + std::string _sensor_name = _it->first; + Sensor *_sensor = _it->second; + + if(_sensor == NULL) + continue; + + int _bulkread_start_addr = 0; + int _bulkread_data_length = 0; + + // calculate bulk read start address & data length + std::map::iterator _indirect_addr_it = _sensor->ctrl_table.find(INDIRECT_ADDRESS_1); + if(_indirect_addr_it != _sensor->ctrl_table.end()) // INDIRECT_ADDRESS_1 exist + { + if(_sensor->bulk_read_items.size() != 0) + { + _bulkread_start_addr = _sensor->bulk_read_items[0]->address; + _bulkread_data_length = 0; + + // set indirect address + int _indirect_addr = _indirect_addr_it->second->address; + for(int _i = 0; _i < _sensor->bulk_read_items.size(); _i++) + { + int _addr_leng = _sensor->bulk_read_items[_i]->data_length; + + _bulkread_data_length += _addr_leng; + for(int _l = 0; _l < _addr_leng; _l++) + { +// ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", _sensor_name.c_str(), _indirect_addr, _sensor->ctrl_table[_sensor->bulk_read_items[_i]->item_name]->address + _l); + Write2Byte(_sensor_name, _indirect_addr, _sensor->ctrl_table[_sensor->bulk_read_items[_i]->item_name]->address + _l); + _indirect_addr += 2; + } + } + } + } + else // INDIRECT_ADDRESS_1 NOT exist + { + if(_sensor->bulk_read_items.size() != 0) + { + _bulkread_start_addr = _sensor->bulk_read_items[0]->address; + _bulkread_data_length = 0; + + ControlTableItem *_last_item = _sensor->bulk_read_items[0]; + + for(int _i = 0; _i < _sensor->bulk_read_items.size(); _i++) + { + int _addr = _sensor->bulk_read_items[_i]->address; + if(_addr < _bulkread_start_addr) + _bulkread_start_addr = _addr; + else if(_last_item->address < _addr) + _last_item = _sensor->bulk_read_items[_i]; + } + + _bulkread_data_length = _last_item->address - _bulkread_start_addr + _last_item->data_length; + } + } + + //ROS_WARN("[%12s] start_addr: %d, data_length: %d", _sensor_name.c_str(), _bulkread_start_addr, _bulkread_data_length); + if(_bulkread_start_addr != 0) + port_to_bulk_read[_sensor->port_name]->AddParam(_sensor->id, _bulkread_start_addr, _bulkread_data_length); + } + queue_thread_ = boost::thread(boost::bind(&RobotisController::QueueThread, this)); return true; } @@ -503,14 +602,31 @@ void RobotisController::StopTimer() exit(-1); for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) - _it->second->RxPacket(); + { + if(_it->second != NULL) + _it->second->RxPacket(); + } for(std::map::iterator _it = port_to_sync_write_position.begin(); _it != port_to_sync_write_position.end(); _it++) - _it->second->ClearParam(); + { + if(_it->second != NULL) + _it->second->ClearParam(); + } + for(std::map::iterator _it = port_to_sync_write_position_p_gain.begin(); _it != port_to_sync_write_position_p_gain.end(); _it++) + { + if(_it->second != NULL) + _it->second->ClearParam(); + } for(std::map::iterator _it = port_to_sync_write_velocity.begin(); _it != port_to_sync_write_velocity.end(); _it++) - _it->second->ClearParam(); + { + if(_it->second != NULL) + _it->second->ClearParam(); + } for(std::map::iterator _it = port_to_sync_write_torque.begin(); _it != port_to_sync_write_torque.end(); _it++) - _it->second->ClearParam(); + { + if(_it->second != NULL) + _it->second->ClearParam(); + } } else { @@ -607,7 +723,7 @@ void RobotisController::Process() { _data = port_to_bulk_read[_port_name]->GetData(_dxl->id, _item->address, _item->data_length); - // TODO: change dxl_state + // change dxl_state if(_dxl->present_position_item != 0 && _item->item_name == _dxl->present_position_item->item_name) _dxl->dxl_state->present_position = _dxl->ConvertValue2Radian(_data) - _dxl->dxl_state->position_offset; // remove offset else if(_dxl->present_velocity_item != 0 && _item->item_name == _dxl->present_velocity_item->item_name) @@ -630,7 +746,6 @@ void RobotisController::Process() } _present_state.name.push_back(_joint_name); - // TODO: should be converted to rad, rad/s, Nm _present_state.position.push_back(_dxl->dxl_state->present_position); _present_state.velocity.push_back(_dxl->dxl_state->present_velocity); _present_state.effort.push_back(_dxl->dxl_state->present_current); @@ -665,7 +780,7 @@ void RobotisController::Process() { _data = port_to_bulk_read[_port_name]->GetData(_sensor->id, _item->address, _item->data_length); - // TODO: change sensor_state + // change sensor_state _sensor->sensor_state->bulk_read_table[_item->item_name] = _data; } } @@ -702,7 +817,7 @@ void RobotisController::Process() { // ros::Time _before = ros::Time::now(); - if((*module_it)->enable == false) + if((*module_it)->GetModuleEnable() == false) continue; (*module_it)->Process(robot->dxls, sensor_result_); @@ -722,20 +837,20 @@ void RobotisController::Process() Dynamixel *_dxl = dxl_it->second; DynamixelState *_dxl_state = _dxl->dxl_state; - if(_dxl->ctrl_module_name == (*module_it)->module_name) + if(_dxl->ctrl_module_name == (*module_it)->GetModuleName()) { _do_sync_write = true; // ROS_INFO("Set goal value"); DynamixelState *_result_state = (*module_it)->result[_joint_name]; if(_result_state == NULL) { - ROS_INFO("[%s] %s", (*module_it)->module_name.c_str(), _joint_name.c_str()); + ROS_INFO("[%s] %s", (*module_it)->GetModuleName().c_str(), _joint_name.c_str()); continue; } // TODO: check update time stamp ? - if((*module_it)->control_mode == POSITION_CONTROL) + if((*module_it)->GetControlMode() == POSITION_CONTROL) { // if(_result_state->goal_position == 0 && _dxl->id == 3) // ROS_INFO("[MODULE:%s][ID:3] goal position = %f", (*module_it)->module_name.c_str(), _dxl_state->goal_position); @@ -745,7 +860,7 @@ void RobotisController::Process() { // add offset UINT32_T _pos_data = _dxl->ConvertRadian2Value(_dxl_state->goal_position + _dxl_state->position_offset); - UINT8_T _sync_write_data[4]; + UINT8_T _sync_write_data[4] = {0}; _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data)); _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data)); @@ -754,10 +869,26 @@ void RobotisController::Process() if(abs(_pos_data) > 151800) printf("goal_pos : %f | position_offset : %f | pos_data : %d\n", _dxl_state->goal_position , _dxl_state->position_offset, _pos_data); - port_to_sync_write_position[_dxl->port_name]->ChangeParam(_dxl->id, _sync_write_data); + if(port_to_sync_write_position[_dxl->port_name] != NULL) + port_to_sync_write_position[_dxl->port_name]->ChangeParam(_dxl->id, _sync_write_data); + + // if position p gain value is changed -> sync write + if(_dxl_state->position_p_gain != _result_state->position_p_gain) + { + _dxl_state->position_p_gain = _result_state->position_p_gain; + + UINT8_T _sync_write_p_gain[4] = {0}; + _sync_write_p_gain[0] = DXL_LOBYTE(DXL_LOWORD(_dxl_state->position_p_gain)); + _sync_write_p_gain[1] = DXL_HIBYTE(DXL_LOWORD(_dxl_state->position_p_gain)); + _sync_write_p_gain[2] = DXL_LOBYTE(DXL_HIWORD(_dxl_state->position_p_gain)); + _sync_write_p_gain[3] = DXL_HIBYTE(DXL_HIWORD(_dxl_state->position_p_gain)); + + if(port_to_sync_write_position_p_gain[_dxl->port_name] != NULL) + port_to_sync_write_position_p_gain[_dxl->port_name]->AddParam(_dxl->id, _sync_write_p_gain); + } } } - else if((*module_it)->control_mode == VELOCITY_CONTROL) + else if((*module_it)->GetControlMode() == VELOCITY_CONTROL) { _dxl_state->goal_velocity = _result_state->goal_velocity; @@ -770,10 +901,11 @@ void RobotisController::Process() _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_vel_data)); _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_vel_data)); - port_to_sync_write_velocity[_dxl->port_name]->ChangeParam(_dxl->id, _sync_write_data); + if(port_to_sync_write_velocity[_dxl->port_name] != NULL) + port_to_sync_write_velocity[_dxl->port_name]->ChangeParam(_dxl->id, _sync_write_data); } } - else if((*module_it)->control_mode == CURRENT_CONTROL) + else if((*module_it)->GetControlMode() == CURRENT_CONTROL) { _dxl_state->goal_current = _result_state->goal_current; @@ -784,7 +916,8 @@ void RobotisController::Process() _sync_write_data[0] = DXL_LOBYTE(_torq_data); _sync_write_data[1] = DXL_HIBYTE(_torq_data); - port_to_sync_write_torque[_dxl->port_name]->ChangeParam(_dxl->id, _sync_write_data); + if(port_to_sync_write_torque[_dxl->port_name] != NULL) + port_to_sync_write_torque[_dxl->port_name]->ChangeParam(_dxl->id, _sync_write_data); } } } @@ -801,16 +934,23 @@ void RobotisController::Process() queue_mutex_.unlock(); } - // TODO: Combine the result && SyncWrite - // -> SyncWrite + // SyncWrite if(gazebo_mode == false && _do_sync_write) { + if(port_to_sync_write_position_p_gain.size() > 0) + { + for(std::map::iterator _it = port_to_sync_write_position_p_gain.begin(); _it != port_to_sync_write_position_p_gain.end(); _it++) + { + _it->second->TxPacket(); + _it->second->ClearParam(); + } + } for(std::map::iterator _it = port_to_sync_write_position.begin(); _it != port_to_sync_write_position.end(); _it++) - _it->second->TxPacket(); + if(_it->second != NULL) _it->second->TxPacket(); for(std::map::iterator _it = port_to_sync_write_velocity.begin(); _it != port_to_sync_write_velocity.end(); _it++) - _it->second->TxPacket(); + if(_it->second != NULL) _it->second->TxPacket(); for(std::map::iterator _it = port_to_sync_write_torque.begin(); _it != port_to_sync_write_torque.end(); _it++) - _it->second->TxPacket(); + if(_it->second != NULL) _it->second->TxPacket(); } else if(gazebo_mode == true) { @@ -855,16 +995,26 @@ void RobotisController::Process() _it->second->TxPacket(); } - // ros::Duration _dur = ros::Time::now() - _now; - // double _msec = _dur.nsec * 0.000001; - // - // if(_msec > 8) std::cout << "Process duration : " << _msec << std::endl; +// ros::Duration _dur = ros::Time::now() - _now; +// double _msec = _dur.nsec * 0.000001; +// +// if(_msec > 8) std::cout << "Process duration : " << _msec << std::endl; _is_process_running = false; } void RobotisController::AddMotionModule(MotionModule *module) { + // check whether the module name already exists + for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) + { + if((*_m_it)->GetModuleName() == module->GetModuleName()) + { + ROS_ERROR("Motion Module Name [%s] already exist !!", module->GetModuleName().c_str()); + return; + } + } + module->Initialize(CONTROL_CYCLE_MSEC, robot); motion_modules_.push_back(module); motion_modules_.unique(); @@ -877,6 +1027,16 @@ void RobotisController::RemoveMotionModule(MotionModule *module) void RobotisController::AddSensorModule(SensorModule *module) { + // check whether the module name already exists + for(std::list::iterator _m_it = sensor_modules_.begin(); _m_it != sensor_modules_.end(); _m_it++) + { + if((*_m_it)->GetModuleName() == module->GetModuleName()) + { + ROS_ERROR("Sensor Module Name [%s] already exist !!", module->GetModuleName().c_str()); + return; + } + } + module->Initialize(CONTROL_CYCLE_MSEC, robot); sensor_modules_.push_back(module); sensor_modules_.unique(); @@ -971,7 +1131,8 @@ void RobotisController::SetJointStatesCallback(const sensor_msgs::JointState::Co _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos)); _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos)); - port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); + if(port_to_sync_write_position[_dxl->port_name] != NULL) + port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); } queue_mutex_.unlock(); @@ -1010,7 +1171,7 @@ void RobotisController::SetJointCtrlModuleCallback(const robotis_controller_msgs // check whether the module is using this joint for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) { - if((*_m_it)->module_name == msg->module_name[idx]) + if((*_m_it)->GetModuleName() == msg->module_name[idx]) { if((*_m_it)->result.find(msg->joint_name[idx]) != (*_m_it)->result.end()) { @@ -1024,14 +1185,14 @@ void RobotisController::SetJointCtrlModuleCallback(const robotis_controller_msgs for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) { // set all modules -> disable - (*_m_it)->enable = false; + (*_m_it)->SetModuleEnable(false); // set all used modules -> enable for(std::map::iterator _d_it = robot->dxls.begin(); _d_it != robot->dxls.end(); _d_it++) { - if(_d_it->second->ctrl_module_name == (*_m_it)->module_name) + if(_d_it->second->ctrl_module_name == (*_m_it)->GetModuleName()) { - (*_m_it)->enable = true; + (*_m_it)->SetModuleEnable(true); break; } } @@ -1080,7 +1241,7 @@ void RobotisController::SetCtrlModuleThread(std::string ctrl_module) // enqueue all modules in order to stop for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) { - if((*_m_it)->enable == true) _stop_modules.push_back(*_m_it); + if((*_m_it)->GetModuleEnable() == true) _stop_modules.push_back(*_m_it); } } else @@ -1088,7 +1249,7 @@ void RobotisController::SetCtrlModuleThread(std::string ctrl_module) for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) { // if it exist - if((*_m_it)->module_name == ctrl_module) + if((*_m_it)->GetModuleName() == ctrl_module) { // enqueue the module which lost control of joint in order to stop for(std::map::iterator _result_it = (*_m_it)->result.begin(); _result_it != (*_m_it)->result.end(); _result_it++) @@ -1102,7 +1263,7 @@ void RobotisController::SetCtrlModuleThread(std::string ctrl_module) { for(std::list::iterator _stop_m_it = motion_modules_.begin(); _stop_m_it != motion_modules_.end(); _stop_m_it++) { - if((*_stop_m_it)->module_name == _d_it->second->ctrl_module_name && (*_stop_m_it)->enable == true) + if((*_stop_m_it)->GetModuleName() == _d_it->second->ctrl_module_name && (*_stop_m_it)->GetModuleEnable() == true) _stop_modules.push_back(*_stop_m_it); } } @@ -1139,7 +1300,7 @@ void RobotisController::SetCtrlModuleThread(std::string ctrl_module) // set all modules -> disable for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) { - (*_m_it)->enable = false; + (*_m_it)->SetModuleEnable(false); } // set dxl's control module to "none" @@ -1167,9 +1328,9 @@ void RobotisController::SetCtrlModuleThread(std::string ctrl_module) for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) { // if it exist - if((*_m_it)->module_name == ctrl_module) + if((*_m_it)->GetModuleName() == ctrl_module) { - CONTROL_MODE _mode = (*_m_it)->control_mode; + CONTROL_MODE _mode = (*_m_it)->GetControlMode(); for(std::map::iterator _result_it = (*_m_it)->result.begin(); _result_it != (*_m_it)->result.end(); _result_it++) { std::map::iterator _d_it = robot->dxls.find(_result_it->first); @@ -1187,10 +1348,13 @@ void RobotisController::SetCtrlModuleThread(std::string ctrl_module) _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data)); _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data)); - port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); + if(port_to_sync_write_position[_dxl->port_name] != NULL) + port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); - port_to_sync_write_torque[_dxl->port_name]->RemoveParam(_dxl->id); - port_to_sync_write_velocity[_dxl->port_name]->RemoveParam(_dxl->id); + if(port_to_sync_write_torque[_dxl->port_name] != NULL) + port_to_sync_write_torque[_dxl->port_name]->RemoveParam(_dxl->id); + if(port_to_sync_write_velocity[_dxl->port_name] != NULL) + port_to_sync_write_velocity[_dxl->port_name]->RemoveParam(_dxl->id); } else if(_mode == VELOCITY_CONTROL) { @@ -1201,10 +1365,13 @@ void RobotisController::SetCtrlModuleThread(std::string ctrl_module) _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_vel_data)); _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_vel_data)); - port_to_sync_write_velocity[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); + if(port_to_sync_write_velocity[_dxl->port_name] != NULL) + port_to_sync_write_velocity[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); - port_to_sync_write_torque[_dxl->port_name]->RemoveParam(_dxl->id); - port_to_sync_write_position[_dxl->port_name]->RemoveParam(_dxl->id); + if(port_to_sync_write_torque[_dxl->port_name] != NULL) + port_to_sync_write_torque[_dxl->port_name]->RemoveParam(_dxl->id); + if(port_to_sync_write_position[_dxl->port_name] != NULL) + port_to_sync_write_position[_dxl->port_name]->RemoveParam(_dxl->id); } else if(_mode == CURRENT_CONTROL) { @@ -1215,10 +1382,13 @@ void RobotisController::SetCtrlModuleThread(std::string ctrl_module) _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_curr_data)); _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_curr_data)); - port_to_sync_write_torque[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); + if(port_to_sync_write_torque[_dxl->port_name] != NULL) + port_to_sync_write_torque[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); - port_to_sync_write_velocity[_dxl->port_name]->RemoveParam(_dxl->id); - port_to_sync_write_position[_dxl->port_name]->RemoveParam(_dxl->id); + if(port_to_sync_write_velocity[_dxl->port_name] != NULL) + port_to_sync_write_velocity[_dxl->port_name]->RemoveParam(_dxl->id); + if(port_to_sync_write_position[_dxl->port_name] != NULL) + port_to_sync_write_position[_dxl->port_name]->RemoveParam(_dxl->id); } } } @@ -1231,14 +1401,14 @@ void RobotisController::SetCtrlModuleThread(std::string ctrl_module) for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) { // set all modules -> disable - (*_m_it)->enable = false; + (*_m_it)->SetModuleEnable(false); // set all used modules -> enable for(std::map::iterator _d_it = robot->dxls.begin(); _d_it != robot->dxls.end(); _d_it++) { - if(_d_it->second->ctrl_module_name == (*_m_it)->module_name) + if(_d_it->second->ctrl_module_name == (*_m_it)->GetModuleName()) { - (*_m_it)->enable = true; + (*_m_it)->SetModuleEnable(true); break; } } diff --git a/robotis_device/include/robotis_device/Dynamixel.h b/robotis_device/include/robotis_device/Dynamixel.h index 64d4a4a..a0a9beb 100644 --- a/robotis_device/include/robotis_device/Dynamixel.h +++ b/robotis_device/include/robotis_device/Dynamixel.h @@ -41,6 +41,7 @@ class Dynamixel : public Device ControlTableItem *goal_position_item; ControlTableItem *goal_velocity_item; ControlTableItem *goal_current_item; + ControlTableItem *position_p_gain_item; Dynamixel(int id, std::string model_name, float protocol_version); diff --git a/robotis_device/include/robotis_device/DynamixelState.h b/robotis_device/include/robotis_device/DynamixelState.h index 29212c4..e3768d4 100644 --- a/robotis_device/include/robotis_device/DynamixelState.h +++ b/robotis_device/include/robotis_device/DynamixelState.h @@ -30,6 +30,7 @@ class DynamixelState double goal_position; double goal_velocity; double goal_current; + double position_p_gain; std::map bulk_read_table; @@ -43,6 +44,7 @@ class DynamixelState goal_position(0.0), goal_velocity(0.0), goal_current(0.0), + position_p_gain(0.0), position_offset(0.0) { bulk_read_table.clear(); diff --git a/robotis_device/src/robotis_device/Dynamixel.cpp b/robotis_device/src/robotis_device/Dynamixel.cpp index daf454d..55108ca 100644 --- a/robotis_device/src/robotis_device/Dynamixel.cpp +++ b/robotis_device/src/robotis_device/Dynamixel.cpp @@ -24,7 +24,8 @@ Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version) present_current_item(0), goal_position_item(0), goal_velocity_item(0), - goal_current_item(0) + goal_current_item(0), + position_p_gain_item(0) { this->id = id; this->model_name = model_name; diff --git a/robotis_device/src/robotis_device/Robot.cpp b/robotis_device/src/robotis_device/Robot.cpp index dc1b50e..caa0463 100644 --- a/robotis_device/src/robotis_device/Robot.cpp +++ b/robotis_device/src/robotis_device/Robot.cpp @@ -106,6 +106,9 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) UINT16_T _indirect_data_addr = _dxl->ctrl_table[INDIRECT_DATA_1]->address; for(int _i = 0; _i < sub_tokens.size(); _i++) { + if(_dxl->bulk_read_items[_i] == NULL) + continue; + _dxl->bulk_read_items.push_back(new ControlTableItem()); ControlTableItem *_dest_item = _dxl->bulk_read_items[_i]; ControlTableItem *_src_item = _dxl->ctrl_table[sub_tokens[_i]]; @@ -122,10 +125,13 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) _indirect_data_addr += _dest_item->data_length; } } - else // INDIRECT_ADDRESS_1 exist + else // INDIRECT_ADDRESS_1 not exist { for(int _i = 0; _i < sub_tokens.size(); _i++) - _dxl->bulk_read_items.push_back(_dxl->ctrl_table[sub_tokens[_i]]); + { + if(_dxl->ctrl_table[sub_tokens[_i]] != NULL) + _dxl->bulk_read_items.push_back(_dxl->ctrl_table[sub_tokens[_i]]); + } } } } diff --git a/robotis_framework_common/include/robotis_framework_common/MotionModule.h b/robotis_framework_common/include/robotis_framework_common/MotionModule.h index a305a6a..0d0fb0f 100644 --- a/robotis_framework_common/include/robotis_framework_common/MotionModule.h +++ b/robotis_framework_common/include/robotis_framework_common/MotionModule.h @@ -28,15 +28,35 @@ enum CONTROL_MODE class MotionModule { -public: +protected: bool enable; std::string module_name; CONTROL_MODE control_mode; +public: std::map result; virtual ~MotionModule() { } + std::string GetModuleName() { return module_name; } + CONTROL_MODE GetControlMode() { return control_mode; } + + void SetModuleEnable(bool enable) + { + if(this->enable == enable) + return; + + this->enable = enable; + if(enable) + OnModuleEnable(); + else + OnModuleDisable(); + } + bool GetModuleEnable() { return enable; } + + virtual void OnModuleEnable() { } + virtual void OnModuleDisable() { } + virtual void Initialize(const int control_cycle_msec, Robot *robot) = 0; virtual void Process(std::map dxls, std::map sensors) = 0; diff --git a/robotis_framework_common/include/robotis_framework_common/SensorModule.h b/robotis_framework_common/include/robotis_framework_common/SensorModule.h index 6111391..1c604e9 100644 --- a/robotis_framework_common/include/robotis_framework_common/SensorModule.h +++ b/robotis_framework_common/include/robotis_framework_common/SensorModule.h @@ -21,13 +21,16 @@ namespace ROBOTIS class SensorModule { -public: +protected: std::string module_name; +public: std::map result; virtual ~SensorModule() { } + std::string GetModuleName() { return module_name; } + virtual void Initialize(const int control_cycle_msec, Robot *robot) = 0; virtual void Process(std::map dxls, std::map sensors) = 0; }; From 0e2805e17ed4e4daa2700f479dc46b317fd35239 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Thu, 26 May 2016 11:11:29 +0900 Subject: [PATCH 02/33] - sync write bug fix. --- .../src/robotis_controller/RobotisController.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/robotis_controller/src/robotis_controller/RobotisController.cpp b/robotis_controller/src/robotis_controller/RobotisController.cpp index d6dfda1..f69624a 100644 --- a/robotis_controller/src/robotis_controller/RobotisController.cpp +++ b/robotis_controller/src/robotis_controller/RobotisController.cpp @@ -1316,10 +1316,13 @@ void RobotisController::SetCtrlModuleThread(std::string ctrl_module) _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data)); _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data)); - port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); + if(port_to_sync_write_position[_dxl->port_name] != NULL) + port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); - port_to_sync_write_torque[_dxl->port_name]->RemoveParam(_dxl->id); - port_to_sync_write_velocity[_dxl->port_name]->RemoveParam(_dxl->id); + if(port_to_sync_write_torque[_dxl->port_name] != NULL) + port_to_sync_write_torque[_dxl->port_name]->RemoveParam(_dxl->id); + if(port_to_sync_write_velocity[_dxl->port_name] != NULL) + port_to_sync_write_velocity[_dxl->port_name]->RemoveParam(_dxl->id); } } else From 79c945f8a0ff7927646af01f0274d759970e58da Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Fri, 27 May 2016 18:32:44 +0900 Subject: [PATCH 03/33] - Robot() bug fixed. --- robotis_device/src/robotis_device/Robot.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/robotis_device/src/robotis_device/Robot.cpp b/robotis_device/src/robotis_device/Robot.cpp index caa0463..ff4d86b 100644 --- a/robotis_device/src/robotis_device/Robot.cpp +++ b/robotis_device/src/robotis_device/Robot.cpp @@ -106,9 +106,6 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) UINT16_T _indirect_data_addr = _dxl->ctrl_table[INDIRECT_DATA_1]->address; for(int _i = 0; _i < sub_tokens.size(); _i++) { - if(_dxl->bulk_read_items[_i] == NULL) - continue; - _dxl->bulk_read_items.push_back(new ControlTableItem()); ControlTableItem *_dest_item = _dxl->bulk_read_items[_i]; ControlTableItem *_src_item = _dxl->ctrl_table[sub_tokens[_i]]; From 11b864aa05576d5e8beadf6bcef5beb1a8d1d251 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Mon, 30 May 2016 21:22:24 +0900 Subject: [PATCH 04/33] - cleanup code. - change the order of processing in the Process() function. - added missing mutex for gazebo - fixed crash when running in gazebo simulation --- .../robotis_controller/RobotisController.cpp | 247 ++++++++++-------- 1 file changed, 135 insertions(+), 112 deletions(-) diff --git a/robotis_controller/src/robotis_controller/RobotisController.cpp b/robotis_controller/src/robotis_controller/RobotisController.cpp index f69624a..97d283a 100644 --- a/robotis_controller/src/robotis_controller/RobotisController.cpp +++ b/robotis_controller/src/robotis_controller/RobotisController.cpp @@ -483,16 +483,17 @@ void RobotisController::QueueThread() ros::Subscriber _control_mode_sub = _ros_node.subscribe("/robotis/set_control_mode", 10, &RobotisController::SetControllerModeCallback, this); ros::Subscriber _joint_states_sub = _ros_node.subscribe("/robotis/set_joint_states", 10, &RobotisController::SetJointStatesCallback, this); + ros::Subscriber _gazebo_joint_states_sub; + if(gazebo_mode == true) + _gazebo_joint_states_sub = _ros_node.subscribe("/" + gazebo_robot_name + "/joint_states", 10, &RobotisController::GazeboJointStatesCallback, this); + /* publisher */ goal_joint_state_pub = _ros_node.advertise("/robotis/goal_joint_states", 10); present_joint_state_pub = _ros_node.advertise("/robotis/present_joint_states", 10); current_module_pub = _ros_node.advertise("/robotis/present_joint_ctrl_modules", 10); - ros::Subscriber _gazebo_joint_states_sub; if(gazebo_mode == true) { - _gazebo_joint_states_sub = _ros_node.subscribe("/" + gazebo_robot_name + "/joint_states", 10, &RobotisController::GazeboJointStatesCallback, this); - for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) gazebo_joint_pub[_it->first] = _ros_node.advertise("/" + gazebo_robot_name + "/" + _it->first + "_pos/command", 1); } @@ -681,117 +682,107 @@ void RobotisController::Process() // ROS_INFO("Controller::Process()"); bool _do_sync_write = false; - sensor_msgs::JointState _goal_state; - sensor_msgs::JointState _present_state; + ros::Time _start_time; + ros::Duration _time_duration; + if(DEBUG_PRINT) + _start_time = ros::Time::now(); -// ros::Time _now = ros::Time::now(); + sensor_msgs::JointState _goal_state; + sensor_msgs::JointState _present_state; + _present_state.header.stamp = ros::Time::now(); + _goal_state.header.stamp = _present_state.header.stamp; // BulkRead Rx + // -> save to Robot->dxls[]->dynamixel_state.present_position if(gazebo_mode == false) { + // BulkRead Rx for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) + { + robot->ports[_it->first]->SetPacketTimeout(0.0); _it->second->RxPacket(); - } - -// ros::Duration _dur = ros::Time::now() - _now; -// double _msec = _dur.nsec * 0.000001; -// -// if(_msec > 8) std::cout << "Process duration : " << _msec << std::endl; + } - // -> save to Robot->dxls[]->dynamixel_state.present_position - if(robot->dxls.size() > 0) - { - for(std::map::iterator dxl_it = robot->dxls.begin(); dxl_it != robot->dxls.end(); dxl_it++) + if(robot->dxls.size() > 0) { - UINT32_T _data = 0; - - std::string _port_name = dxl_it->second->port_name; - std::string _joint_name = dxl_it->first; - Dynamixel *_dxl = dxl_it->second; - - _present_state.header.stamp = ros::Time::now(); - _goal_state.header.stamp = _present_state.header.stamp; - - if(gazebo_mode == false && _dxl->bulk_read_items.size() > 0) + for(std::map::iterator dxl_it = robot->dxls.begin(); dxl_it != robot->dxls.end(); dxl_it++) { - for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++) + Dynamixel *_dxl = dxl_it->second; + std::string _port_name = dxl_it->second->port_name; + std::string _joint_name = dxl_it->first; + + if(_dxl->bulk_read_items.size() > 0) { - ControlTableItem *_item = _dxl->bulk_read_items[_i]; - if(port_to_bulk_read[_port_name]->IsAvailable(_dxl->id, _item->address, _item->data_length) == true) + UINT32_T _data = 0; + for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++) { - _data = port_to_bulk_read[_port_name]->GetData(_dxl->id, _item->address, _item->data_length); - - // change dxl_state - if(_dxl->present_position_item != 0 && _item->item_name == _dxl->present_position_item->item_name) - _dxl->dxl_state->present_position = _dxl->ConvertValue2Radian(_data) - _dxl->dxl_state->position_offset; // remove offset - else if(_dxl->present_velocity_item != 0 && _item->item_name == _dxl->present_velocity_item->item_name) - _dxl->dxl_state->present_velocity = _dxl->ConvertValue2Velocity(_data); - else if(_dxl->present_current_item != 0 && _item->item_name == _dxl->present_current_item->item_name) - _dxl->dxl_state->present_current = _dxl->ConvertValue2Current(_data); - else if(_dxl->goal_position_item != 0 && _item->item_name == _dxl->goal_position_item->item_name) - _dxl->dxl_state->goal_position = _dxl->ConvertValue2Radian(_data) - _dxl->dxl_state->position_offset; // remove offset - else if(_dxl->goal_velocity_item != 0 && _item->item_name == _dxl->goal_velocity_item->item_name) - _dxl->dxl_state->goal_velocity = _dxl->ConvertValue2Velocity(_data); - else if(_dxl->goal_current_item != 0 && _item->item_name == _dxl->goal_current_item->item_name) - _dxl->dxl_state->goal_current = _dxl->ConvertValue2Current(_data); - else - _dxl->dxl_state->bulk_read_table[_item->item_name] = _data; + ControlTableItem *_item = _dxl->bulk_read_items[_i]; + if(port_to_bulk_read[_port_name]->IsAvailable(_dxl->id, _item->address, _item->data_length) == true) + { + _data = port_to_bulk_read[_port_name]->GetData(_dxl->id, _item->address, _item->data_length); + + // change dxl_state + if(_dxl->present_position_item != 0 && _item->item_name == _dxl->present_position_item->item_name) + _dxl->dxl_state->present_position = _dxl->ConvertValue2Radian(_data) - _dxl->dxl_state->position_offset; // remove offset + else if(_dxl->present_velocity_item != 0 && _item->item_name == _dxl->present_velocity_item->item_name) + _dxl->dxl_state->present_velocity = _dxl->ConvertValue2Velocity(_data); + else if(_dxl->present_current_item != 0 && _item->item_name == _dxl->present_current_item->item_name) + _dxl->dxl_state->present_current = _dxl->ConvertValue2Current(_data); + else if(_dxl->goal_position_item != 0 && _item->item_name == _dxl->goal_position_item->item_name) + _dxl->dxl_state->goal_position = _dxl->ConvertValue2Radian(_data) - _dxl->dxl_state->position_offset; // remove offset + else if(_dxl->goal_velocity_item != 0 && _item->item_name == _dxl->goal_velocity_item->item_name) + _dxl->dxl_state->goal_velocity = _dxl->ConvertValue2Velocity(_data); + else if(_dxl->goal_current_item != 0 && _item->item_name == _dxl->goal_current_item->item_name) + _dxl->dxl_state->goal_current = _dxl->ConvertValue2Current(_data); + else + _dxl->dxl_state->bulk_read_table[_item->item_name] = _data; + } } - } - // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp - _dxl->dxl_state->update_time_stamp = TimeStamp(_present_state.header.stamp.sec, _present_state.header.stamp.nsec); + // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp + _dxl->dxl_state->update_time_stamp = TimeStamp(_present_state.header.stamp.sec, _present_state.header.stamp.nsec); + } } - - _present_state.name.push_back(_joint_name); - _present_state.position.push_back(_dxl->dxl_state->present_position); - _present_state.velocity.push_back(_dxl->dxl_state->present_velocity); - _present_state.effort.push_back(_dxl->dxl_state->present_current); - - _goal_state.name.push_back(_joint_name); - _goal_state.position.push_back(_dxl->dxl_state->goal_position); - _goal_state.velocity.push_back(_dxl->dxl_state->goal_velocity); - _goal_state.effort.push_back(_dxl->dxl_state->goal_current); } - } - // -> publish present joint_states & goal joint states topic - present_joint_state_pub.publish(_present_state); - goal_joint_state_pub.publish(_goal_state); - - if(robot->sensors.size() > 0) - { - for(std::map::iterator sensor_it = robot->sensors.begin(); sensor_it != robot->sensors.end(); sensor_it++) + if(robot->sensors.size() > 0) { - UINT32_T _data = 0; - - std::string _port_name = sensor_it->second->port_name; - std::string _sensor_name = sensor_it->first; - Sensor *_sensor = sensor_it->second; - - if(gazebo_mode == false && _sensor->bulk_read_items.size() > 0) + for(std::map::iterator sensor_it = robot->sensors.begin(); sensor_it != robot->sensors.end(); sensor_it++) { - for(int _i = 0; _i < _sensor->bulk_read_items.size(); _i++) + Sensor *_sensor = sensor_it->second; + std::string _port_name = sensor_it->second->port_name; + std::string _sensor_name = sensor_it->first; + + if(_sensor->bulk_read_items.size() > 0) { - ControlTableItem *_item = _sensor->bulk_read_items[_i]; - if(port_to_bulk_read[_port_name]->IsAvailable(_sensor->id, _item->address, _item->data_length) == true) + UINT32_T _data = 0; + for(int _i = 0; _i < _sensor->bulk_read_items.size(); _i++) { - _data = port_to_bulk_read[_port_name]->GetData(_sensor->id, _item->address, _item->data_length); + ControlTableItem *_item = _sensor->bulk_read_items[_i]; + if(port_to_bulk_read[_port_name]->IsAvailable(_sensor->id, _item->address, _item->data_length) == true) + { + _data = port_to_bulk_read[_port_name]->GetData(_sensor->id, _item->address, _item->data_length); - // change sensor_state - _sensor->sensor_state->bulk_read_table[_item->item_name] = _data; + // change sensor_state + _sensor->sensor_state->bulk_read_table[_item->item_name] = _data; + } } - } - // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp - ros::Time _now = ros::Time::now(); - _sensor->sensor_state->update_time_stamp = TimeStamp(_now.sec, _now.nsec); + // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp + _sensor->sensor_state->update_time_stamp = TimeStamp(_present_state.header.stamp.sec, _present_state.header.stamp.nsec); + } } } } + if(DEBUG_PRINT) + { + _time_duration = ros::Time::now() - _start_time; + ROS_INFO("(%2.6f) BulkRead Rx & update state", _time_duration.nsec * 0.000001); + } + // Call SensorModule Process() // -> for loop : call SensorModule list -> Process() if(sensor_modules_.size() > 0) @@ -805,6 +796,12 @@ void RobotisController::Process() } } + if(DEBUG_PRINT) + { + _time_duration = ros::Time::now() - _start_time; + ROS_INFO("(%2.6f) SensorModule Process() & save result", _time_duration.nsec * 0.000001); + } + if(controller_mode_ == MOTION_MODULE_MODE) { // Call MotionModule Process() @@ -815,32 +812,21 @@ void RobotisController::Process() for(std::list::iterator module_it = motion_modules_.begin(); module_it != motion_modules_.end(); module_it++) { -// ros::Time _before = ros::Time::now(); - if((*module_it)->GetModuleEnable() == false) continue; (*module_it)->Process(robot->dxls, sensor_result_); -// ros::Duration _dur = ros::Time::now() - _before; -// double _msec = _dur.sec * 1e+3 + _dur.nsec * 0.000001; - -// std::cout << "Process duration ["<< (*module_it)->module_name << "] : " << _msec << std::endl; - - - ros::Time _before = ros::Time::now(); - // for loop : joint list for(std::map::iterator dxl_it = robot->dxls.begin(); dxl_it != robot->dxls.end(); dxl_it++) { - std::string _joint_name = dxl_it->first; - Dynamixel *_dxl = dxl_it->second; + std::string _joint_name = dxl_it->first; + Dynamixel *_dxl = dxl_it->second; + DynamixelState *_dxl_state = dxl_it->second->dxl_state; - DynamixelState *_dxl_state = _dxl->dxl_state; if(_dxl->ctrl_module_name == (*module_it)->GetModuleName()) { _do_sync_write = true; - // ROS_INFO("Set goal value"); DynamixelState *_result_state = (*module_it)->result[_joint_name]; if(_result_state == NULL) { @@ -853,14 +839,14 @@ void RobotisController::Process() if((*module_it)->GetControlMode() == POSITION_CONTROL) { // if(_result_state->goal_position == 0 && _dxl->id == 3) -// ROS_INFO("[MODULE:%s][ID:3] goal position = %f", (*module_it)->module_name.c_str(), _dxl_state->goal_position); +// ROS_INFO("[MODULE:%s][ID:%2d] goal position = %f", (*module_it)->GetModuleName().c_str(), _dxl->id, _dxl_state->goal_position); _dxl_state->goal_position = _result_state->goal_position; if(gazebo_mode == false) { // add offset UINT32_T _pos_data = _dxl->ConvertRadian2Value(_dxl_state->goal_position + _dxl_state->position_offset); - UINT8_T _sync_write_data[4] = {0}; + UINT8_T _sync_write_data[4] = {0}; _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data)); _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data)); @@ -895,7 +881,7 @@ void RobotisController::Process() if(gazebo_mode == false) { UINT32_T _vel_data = _dxl->ConvertVelocity2Value(_dxl_state->goal_velocity); - UINT8_T _sync_write_data[4]; + UINT8_T _sync_write_data[4] = {0}; _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_vel_data)); _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_vel_data)); _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_vel_data)); @@ -922,18 +908,17 @@ void RobotisController::Process() } } } - - ros::Duration _dur = ros::Time::now() - _before; - double _msec = _dur.sec * 1e+3 + _dur.nsec * 0.000001; - - //std::cout << "Process duration ["<< (*module_it)->module_name << "] : " << _msec << std::endl; - } - // std::cout << " ------------------------------------------- " << std::endl; queue_mutex_.unlock(); } + if(DEBUG_PRINT) + { + _time_duration = ros::Time::now() - _start_time; + ROS_INFO("(%2.6f) MotionModule Process() & save result", _time_duration.nsec * 0.000001); + } + // SyncWrite if(gazebo_mode == false && _do_sync_write) { @@ -995,10 +980,38 @@ void RobotisController::Process() _it->second->TxPacket(); } -// ros::Duration _dur = ros::Time::now() - _now; -// double _msec = _dur.nsec * 0.000001; -// -// if(_msec > 8) std::cout << "Process duration : " << _msec << std::endl; + if(DEBUG_PRINT) + { + _time_duration = ros::Time::now() - _start_time; + ROS_INFO("(%2.6f) SyncWrite & BulkRead Tx", _time_duration.nsec * 0.000001); + } + + // publish present & goal position + for(std::map::iterator dxl_it = robot->dxls.begin(); dxl_it != robot->dxls.end(); dxl_it++) + { + std::string _joint_name = dxl_it->first; + Dynamixel *_dxl = dxl_it->second; + + _present_state.name.push_back(_joint_name); + _present_state.position.push_back(_dxl->dxl_state->present_position); + _present_state.velocity.push_back(_dxl->dxl_state->present_velocity); + _present_state.effort.push_back(_dxl->dxl_state->present_current); + + _goal_state.name.push_back(_joint_name); + _goal_state.position.push_back(_dxl->dxl_state->goal_position); + _goal_state.velocity.push_back(_dxl->dxl_state->goal_velocity); + _goal_state.effort.push_back(_dxl->dxl_state->goal_current); + } + + // -> publish present joint_states & goal joint states topic + present_joint_state_pub.publish(_present_state); + goal_joint_state_pub.publish(_goal_state); + + if(DEBUG_PRINT) + { + _time_duration = ros::Time::now() - _start_time; + ROS_WARN("(%2.6f) Process() DONE", _time_duration.nsec * 0.000001); + } _is_process_running = false; } @@ -1309,6 +1322,9 @@ void RobotisController::SetCtrlModuleThread(std::string ctrl_module) Dynamixel *_dxl = _d_it->second; _dxl->ctrl_module_name = "none"; + if(gazebo_mode == true) + continue; + UINT32_T _pos_data = _dxl->ConvertRadian2Value(_dxl->dxl_state->goal_position + _dxl->dxl_state->position_offset); UINT8_T _sync_write_data[4]; _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); @@ -1342,6 +1358,9 @@ void RobotisController::SetCtrlModuleThread(std::string ctrl_module) Dynamixel *_dxl = _d_it->second; _dxl->ctrl_module_name = ctrl_module; + if(gazebo_mode == true) + continue; + if(_mode == POSITION_CONTROL) { UINT32_T _pos_data = _dxl->ConvertRadian2Value(_dxl->dxl_state->goal_position + _dxl->dxl_state->position_offset); @@ -1436,6 +1455,8 @@ void RobotisController::SetCtrlModuleThread(std::string ctrl_module) void RobotisController::GazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) { + queue_mutex_.lock(); + for(unsigned int _i = 0; _i < msg->name.size(); _i++) { std::map::iterator _d_it = robot->dxls.find((std::string)msg->name[_i]); @@ -1453,6 +1474,8 @@ void RobotisController::GazeboJointStatesCallback(const sensor_msgs::JointState: _it->second->dxl_state->goal_position = _it->second->dxl_state->present_position; init_pose_loaded_ = true; } + + queue_mutex_.unlock(); } bool RobotisController::CheckTimerStop() From 7ad51f94ea713106b4d9c9f0204e738b8804bde3 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Wed, 1 Jun 2016 16:53:03 +0900 Subject: [PATCH 05/33] - License changed (to BSD) --- LICENSE | 365 ++------------------------- dynamixel_sdk/package.xml | 2 +- robotis_controller/package.xml | 2 +- robotis_controller_msgs/package.xml | 2 +- robotis_device/package.xml | 2 +- robotis_framework_common/package.xml | 2 +- robotis_math/package.xml | 2 +- 7 files changed, 32 insertions(+), 345 deletions(-) diff --git a/LICENSE b/LICENSE index 23cb790..1d93559 100644 --- a/LICENSE +++ b/LICENSE @@ -1,339 +1,26 @@ - GNU GENERAL PUBLIC LICENSE - Version 2, June 1991 - - Copyright (C) 1989, 1991 Free Software Foundation, Inc., - 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA - Everyone is permitted to copy and distribute verbatim copies - of this license document, but changing it is not allowed. - - Preamble - - The licenses for most software are designed to take away your -freedom to share and change it. By contrast, the GNU General Public -License is intended to guarantee your freedom to share and change free -software--to make sure the software is free for all its users. This -General Public License applies to most of the Free Software -Foundation's software and to any other program whose authors commit to -using it. (Some other Free Software Foundation software is covered by -the GNU Lesser General Public License instead.) You can apply it to -your programs, too. - - When we speak of free software, we are referring to freedom, not -price. Our General Public Licenses are designed to make sure that you -have the freedom to distribute copies of free software (and charge for -this service if you wish), that you receive source code or can get it -if you want it, that you can change the software or use pieces of it -in new free programs; and that you know you can do these things. - - To protect your rights, we need to make restrictions that forbid -anyone to deny you these rights or to ask you to surrender the rights. -These restrictions translate to certain responsibilities for you if you -distribute copies of the software, or if you modify it. - - For example, if you distribute copies of such a program, whether -gratis or for a fee, you must give the recipients all the rights that -you have. You must make sure that they, too, receive or can get the -source code. And you must show them these terms so they know their -rights. - - We protect your rights with two steps: (1) copyright the software, and -(2) offer you this license which gives you legal permission to copy, -distribute and/or modify the software. - - Also, for each author's protection and ours, we want to make certain -that everyone understands that there is no warranty for this free -software. If the software is modified by someone else and passed on, we -want its recipients to know that what they have is not the original, so -that any problems introduced by others will not reflect on the original -authors' reputations. - - Finally, any free program is threatened constantly by software -patents. We wish to avoid the danger that redistributors of a free -program will individually obtain patent licenses, in effect making the -program proprietary. To prevent this, we have made it clear that any -patent must be licensed for everyone's free use or not licensed at all. - - The precise terms and conditions for copying, distribution and -modification follow. - - GNU GENERAL PUBLIC LICENSE - TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION - - 0. This License applies to any program or other work which contains -a notice placed by the copyright holder saying it may be distributed -under the terms of this General Public License. The "Program", below, -refers to any such program or work, and a "work based on the Program" -means either the Program or any derivative work under copyright law: -that is to say, a work containing the Program or a portion of it, -either verbatim or with modifications and/or translated into another -language. (Hereinafter, translation is included without limitation in -the term "modification".) Each licensee is addressed as "you". - -Activities other than copying, distribution and modification are not -covered by this License; they are outside its scope. The act of -running the Program is not restricted, and the output from the Program -is covered only if its contents constitute a work based on the -Program (independent of having been made by running the Program). -Whether that is true depends on what the Program does. - - 1. You may copy and distribute verbatim copies of the Program's -source code as you receive it, in any medium, provided that you -conspicuously and appropriately publish on each copy an appropriate -copyright notice and disclaimer of warranty; keep intact all the -notices that refer to this License and to the absence of any warranty; -and give any other recipients of the Program a copy of this License -along with the Program. - -You may charge a fee for the physical act of transferring a copy, and -you may at your option offer warranty protection in exchange for a fee. - - 2. You may modify your copy or copies of the Program or any portion -of it, thus forming a work based on the Program, and copy and -distribute such modifications or work under the terms of Section 1 -above, provided that you also meet all of these conditions: - - a) You must cause the modified files to carry prominent notices - stating that you changed the files and the date of any change. - - b) You must cause any work that you distribute or publish, that in - whole or in part contains or is derived from the Program or any - part thereof, to be licensed as a whole at no charge to all third - parties under the terms of this License. - - c) If the modified program normally reads commands interactively - when run, you must cause it, when started running for such - interactive use in the most ordinary way, to print or display an - announcement including an appropriate copyright notice and a - notice that there is no warranty (or else, saying that you provide - a warranty) and that users may redistribute the program under - these conditions, and telling the user how to view a copy of this - License. (Exception: if the Program itself is interactive but - does not normally print such an announcement, your work based on - the Program is not required to print an announcement.) - -These requirements apply to the modified work as a whole. If -identifiable sections of that work are not derived from the Program, -and can be reasonably considered independent and separate works in -themselves, then this License, and its terms, do not apply to those -sections when you distribute them as separate works. But when you -distribute the same sections as part of a whole which is a work based -on the Program, the distribution of the whole must be on the terms of -this License, whose permissions for other licensees extend to the -entire whole, and thus to each and every part regardless of who wrote it. - -Thus, it is not the intent of this section to claim rights or contest -your rights to work written entirely by you; rather, the intent is to -exercise the right to control the distribution of derivative or -collective works based on the Program. - -In addition, mere aggregation of another work not based on the Program -with the Program (or with a work based on the Program) on a volume of -a storage or distribution medium does not bring the other work under -the scope of this License. - - 3. You may copy and distribute the Program (or a work based on it, -under Section 2) in object code or executable form under the terms of -Sections 1 and 2 above provided that you also do one of the following: - - a) Accompany it with the complete corresponding machine-readable - source code, which must be distributed under the terms of Sections - 1 and 2 above on a medium customarily used for software interchange; or, - - b) Accompany it with a written offer, valid for at least three - years, to give any third party, for a charge no more than your - cost of physically performing source distribution, a complete - machine-readable copy of the corresponding source code, to be - distributed under the terms of Sections 1 and 2 above on a medium - customarily used for software interchange; or, - - c) Accompany it with the information you received as to the offer - to distribute corresponding source code. (This alternative is - allowed only for noncommercial distribution and only if you - received the program in object code or executable form with such - an offer, in accord with Subsection b above.) - -The source code for a work means the preferred form of the work for -making modifications to it. For an executable work, complete source -code means all the source code for all modules it contains, plus any -associated interface definition files, plus the scripts used to -control compilation and installation of the executable. However, as a -special exception, the source code distributed need not include -anything that is normally distributed (in either source or binary -form) with the major components (compiler, kernel, and so on) of the -operating system on which the executable runs, unless that component -itself accompanies the executable. - -If distribution of executable or object code is made by offering -access to copy from a designated place, then offering equivalent -access to copy the source code from the same place counts as -distribution of the source code, even though third parties are not -compelled to copy the source along with the object code. - - 4. You may not copy, modify, sublicense, or distribute the Program -except as expressly provided under this License. Any attempt -otherwise to copy, modify, sublicense or distribute the Program is -void, and will automatically terminate your rights under this License. -However, parties who have received copies, or rights, from you under -this License will not have their licenses terminated so long as such -parties remain in full compliance. - - 5. You are not required to accept this License, since you have not -signed it. However, nothing else grants you permission to modify or -distribute the Program or its derivative works. These actions are -prohibited by law if you do not accept this License. Therefore, by -modifying or distributing the Program (or any work based on the -Program), you indicate your acceptance of this License to do so, and -all its terms and conditions for copying, distributing or modifying -the Program or works based on it. - - 6. Each time you redistribute the Program (or any work based on the -Program), the recipient automatically receives a license from the -original licensor to copy, distribute or modify the Program subject to -these terms and conditions. You may not impose any further -restrictions on the recipients' exercise of the rights granted herein. -You are not responsible for enforcing compliance by third parties to -this License. - - 7. If, as a consequence of a court judgment or allegation of patent -infringement or for any other reason (not limited to patent issues), -conditions are imposed on you (whether by court order, agreement or -otherwise) that contradict the conditions of this License, they do not -excuse you from the conditions of this License. If you cannot -distribute so as to satisfy simultaneously your obligations under this -License and any other pertinent obligations, then as a consequence you -may not distribute the Program at all. For example, if a patent -license would not permit royalty-free redistribution of the Program by -all those who receive copies directly or indirectly through you, then -the only way you could satisfy both it and this License would be to -refrain entirely from distribution of the Program. - -If any portion of this section is held invalid or unenforceable under -any particular circumstance, the balance of the section is intended to -apply and the section as a whole is intended to apply in other -circumstances. - -It is not the purpose of this section to induce you to infringe any -patents or other property right claims or to contest validity of any -such claims; this section has the sole purpose of protecting the -integrity of the free software distribution system, which is -implemented by public license practices. Many people have made -generous contributions to the wide range of software distributed -through that system in reliance on consistent application of that -system; it is up to the author/donor to decide if he or she is willing -to distribute software through any other system and a licensee cannot -impose that choice. - -This section is intended to make thoroughly clear what is believed to -be a consequence of the rest of this License. - - 8. If the distribution and/or use of the Program is restricted in -certain countries either by patents or by copyrighted interfaces, the -original copyright holder who places the Program under this License -may add an explicit geographical distribution limitation excluding -those countries, so that distribution is permitted only in or among -countries not thus excluded. In such case, this License incorporates -the limitation as if written in the body of this License. - - 9. The Free Software Foundation may publish revised and/or new versions -of the General Public License from time to time. Such new versions will -be similar in spirit to the present version, but may differ in detail to -address new problems or concerns. - -Each version is given a distinguishing version number. If the Program -specifies a version number of this License which applies to it and "any -later version", you have the option of following the terms and conditions -either of that version or of any later version published by the Free -Software Foundation. If the Program does not specify a version number of -this License, you may choose any version ever published by the Free Software -Foundation. - - 10. If you wish to incorporate parts of the Program into other free -programs whose distribution conditions are different, write to the author -to ask for permission. For software which is copyrighted by the Free -Software Foundation, write to the Free Software Foundation; we sometimes -make exceptions for this. Our decision will be guided by the two goals -of preserving the free status of all derivatives of our free software and -of promoting the sharing and reuse of software generally. - - NO WARRANTY - - 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY -FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN -OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES -PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED -OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF -MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS -TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE -PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, -REPAIR OR CORRECTION. - - 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING -WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR -REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, -INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING -OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED -TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY -YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER -PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE -POSSIBILITY OF SUCH DAMAGES. - - END OF TERMS AND CONDITIONS - - How to Apply These Terms to Your New Programs - - If you develop a new program, and you want it to be of the greatest -possible use to the public, the best way to achieve this is to make it -free software which everyone can redistribute and change under these terms. - - To do so, attach the following notices to the program. It is safest -to attach them to the start of each source file to most effectively -convey the exclusion of warranty; and each file should have at least -the "copyright" line and a pointer to where the full notice is found. - - {description} - Copyright (C) {year} {fullname} - - This program is free software; you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation; either version 2 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License along - with this program; if not, write to the Free Software Foundation, Inc., - 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - -Also add information on how to contact you by electronic and paper mail. - -If the program is interactive, make it output a short notice like this -when it starts in an interactive mode: - - Gnomovision version 69, Copyright (C) year name of author - Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. - This is free software, and you are welcome to redistribute it - under certain conditions; type `show c' for details. - -The hypothetical commands `show w' and `show c' should show the appropriate -parts of the General Public License. Of course, the commands you use may -be called something other than `show w' and `show c'; they could even be -mouse-clicks or menu items--whatever suits your program. - -You should also get your employer (if you work as a programmer) or your -school, if any, to sign a "copyright disclaimer" for the program, if -necessary. Here is a sample; alter the names: - - Yoyodyne, Inc., hereby disclaims all copyright interest in the program - `Gnomovision' (which makes passes at compilers) written by James Hacker. - - {signature of Ty Coon}, 1 April 1989 - Ty Coon, President of Vice - -This General Public License does not permit incorporating your program into -proprietary programs. If your program is a subroutine library, you may -consider it more useful to permit linking proprietary applications with the -library. If this is what you want to do, use the GNU Lesser General -Public License instead of this License. +Software License Agreement (BSD License) + +Copyright (c) 2014, ROBOTIS Inc. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of ROBOTIS nor the names of its contributors may be + used to endorse or promote products derived from this software + without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY ROBOTIS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. +IN NO EVENT SHALL ROBOTIS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, +PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; +OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR +OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/dynamixel_sdk/package.xml b/dynamixel_sdk/package.xml index f21a3d3..ae8e683 100644 --- a/dynamixel_sdk/package.xml +++ b/dynamixel_sdk/package.xml @@ -5,7 +5,7 @@ The dynamixel_sdk package robotis - GPLv2 + BSD ROBOTIS diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index c504764..8a25f7e 100644 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -7,7 +7,7 @@ ROBOTIS - GPLv2 + BSD diff --git a/robotis_controller_msgs/package.xml b/robotis_controller_msgs/package.xml index 88a4148..3ce5db7 100644 --- a/robotis_controller_msgs/package.xml +++ b/robotis_controller_msgs/package.xml @@ -5,7 +5,7 @@ The robotis_controller_msgs package robotis - GPLv2 + BSD diff --git a/robotis_device/package.xml b/robotis_device/package.xml index 06de7e5..be72665 100644 --- a/robotis_device/package.xml +++ b/robotis_device/package.xml @@ -7,7 +7,7 @@ robotis - GPLv2 + BSD diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml index 7863f09..feec7f8 100644 --- a/robotis_framework_common/package.xml +++ b/robotis_framework_common/package.xml @@ -13,7 +13,7 @@ - TODO + BSD diff --git a/robotis_math/package.xml b/robotis_math/package.xml index 895ab75..506f261 100644 --- a/robotis_math/package.xml +++ b/robotis_math/package.xml @@ -13,7 +13,7 @@ - TODO + BSD From bd9bf5dcb2124f687a4579269cff2c20076dbfbe Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Fri, 3 Jun 2016 19:54:29 +0900 Subject: [PATCH 06/33] - file name rename (according to the ROS C++ Coding Style) --- ...botisController.h => robotis_controller.h} | 0 ...sController.cpp => robotis_controller.cpp} | 0 ...ontrolTableItem.h => control_table_item.h} | 0 .../robotis_device/{Device.h => device.h} | 0 .../{Dynamixel.h => dynamixel.h} | 0 .../{DynamixelState.h => dynamixel_state.h} | 0 .../robotis_device/{Robot.h => robot.h} | 0 .../robotis_device/{Sensor.h => sensor.h} | 0 .../{SensorState.h => sensor_state.h} | 0 .../{TimeStamp.h => time_stamp.h} | 0 .../{Dynamixel.cpp => dynamixel.cpp} | 0 .../robotis_device/{Robot.cpp => robot.cpp} | 0 .../robotis_device/{Sensor.cpp => sensor.cpp} | 0 .../robotis_framework_common/RobotisDef.h | 21 ------------------- .../{MotionModule.h => motion_module.h} | 0 .../{SensorModule.h => sensor_module.h} | 0 .../{Singleton.h => singleton.h} | 0 ...nearAlgebra.h => robotis_linear_algebra.h} | 0 .../{RobotisMath.h => robotis_math.h} | 0 ...{RobotisMathBase.h => robotis_math_base.h} | 0 ...ator.h => robotis_trajectory_calculator.h} | 0 ...Algebra.cpp => robotis_linear_algebra.cpp} | 0 ...otisMathBase.cpp => robotis_math_base.cpp} | 0 ....cpp => robotis_trajectory_calculator.cpp} | 0 24 files changed, 21 deletions(-) rename robotis_controller/include/robotis_controller/{RobotisController.h => robotis_controller.h} (100%) rename robotis_controller/src/robotis_controller/{RobotisController.cpp => robotis_controller.cpp} (100%) rename robotis_device/include/robotis_device/{ControlTableItem.h => control_table_item.h} (100%) rename robotis_device/include/robotis_device/{Device.h => device.h} (100%) rename robotis_device/include/robotis_device/{Dynamixel.h => dynamixel.h} (100%) rename robotis_device/include/robotis_device/{DynamixelState.h => dynamixel_state.h} (100%) rename robotis_device/include/robotis_device/{Robot.h => robot.h} (100%) rename robotis_device/include/robotis_device/{Sensor.h => sensor.h} (100%) rename robotis_device/include/robotis_device/{SensorState.h => sensor_state.h} (100%) rename robotis_device/include/robotis_device/{TimeStamp.h => time_stamp.h} (100%) rename robotis_device/src/robotis_device/{Dynamixel.cpp => dynamixel.cpp} (100%) rename robotis_device/src/robotis_device/{Robot.cpp => robot.cpp} (100%) rename robotis_device/src/robotis_device/{Sensor.cpp => sensor.cpp} (100%) delete mode 100644 robotis_framework_common/include/robotis_framework_common/RobotisDef.h rename robotis_framework_common/include/robotis_framework_common/{MotionModule.h => motion_module.h} (100%) rename robotis_framework_common/include/robotis_framework_common/{SensorModule.h => sensor_module.h} (100%) rename robotis_framework_common/include/robotis_framework_common/{Singleton.h => singleton.h} (100%) rename robotis_math/include/robotis_math/{RobotisLinearAlgebra.h => robotis_linear_algebra.h} (100%) rename robotis_math/include/robotis_math/{RobotisMath.h => robotis_math.h} (100%) rename robotis_math/include/robotis_math/{RobotisMathBase.h => robotis_math_base.h} (100%) rename robotis_math/include/robotis_math/{RobotisTrajectoryCalculator.h => robotis_trajectory_calculator.h} (100%) rename robotis_math/src/{RobotisLinearAlgebra.cpp => robotis_linear_algebra.cpp} (100%) rename robotis_math/src/{RobotisMathBase.cpp => robotis_math_base.cpp} (100%) rename robotis_math/src/{RobotisTrajectoryCalculator.cpp => robotis_trajectory_calculator.cpp} (100%) diff --git a/robotis_controller/include/robotis_controller/RobotisController.h b/robotis_controller/include/robotis_controller/robotis_controller.h similarity index 100% rename from robotis_controller/include/robotis_controller/RobotisController.h rename to robotis_controller/include/robotis_controller/robotis_controller.h diff --git a/robotis_controller/src/robotis_controller/RobotisController.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp similarity index 100% rename from robotis_controller/src/robotis_controller/RobotisController.cpp rename to robotis_controller/src/robotis_controller/robotis_controller.cpp diff --git a/robotis_device/include/robotis_device/ControlTableItem.h b/robotis_device/include/robotis_device/control_table_item.h similarity index 100% rename from robotis_device/include/robotis_device/ControlTableItem.h rename to robotis_device/include/robotis_device/control_table_item.h diff --git a/robotis_device/include/robotis_device/Device.h b/robotis_device/include/robotis_device/device.h similarity index 100% rename from robotis_device/include/robotis_device/Device.h rename to robotis_device/include/robotis_device/device.h diff --git a/robotis_device/include/robotis_device/Dynamixel.h b/robotis_device/include/robotis_device/dynamixel.h similarity index 100% rename from robotis_device/include/robotis_device/Dynamixel.h rename to robotis_device/include/robotis_device/dynamixel.h diff --git a/robotis_device/include/robotis_device/DynamixelState.h b/robotis_device/include/robotis_device/dynamixel_state.h similarity index 100% rename from robotis_device/include/robotis_device/DynamixelState.h rename to robotis_device/include/robotis_device/dynamixel_state.h diff --git a/robotis_device/include/robotis_device/Robot.h b/robotis_device/include/robotis_device/robot.h similarity index 100% rename from robotis_device/include/robotis_device/Robot.h rename to robotis_device/include/robotis_device/robot.h diff --git a/robotis_device/include/robotis_device/Sensor.h b/robotis_device/include/robotis_device/sensor.h similarity index 100% rename from robotis_device/include/robotis_device/Sensor.h rename to robotis_device/include/robotis_device/sensor.h diff --git a/robotis_device/include/robotis_device/SensorState.h b/robotis_device/include/robotis_device/sensor_state.h similarity index 100% rename from robotis_device/include/robotis_device/SensorState.h rename to robotis_device/include/robotis_device/sensor_state.h diff --git a/robotis_device/include/robotis_device/TimeStamp.h b/robotis_device/include/robotis_device/time_stamp.h similarity index 100% rename from robotis_device/include/robotis_device/TimeStamp.h rename to robotis_device/include/robotis_device/time_stamp.h diff --git a/robotis_device/src/robotis_device/Dynamixel.cpp b/robotis_device/src/robotis_device/dynamixel.cpp similarity index 100% rename from robotis_device/src/robotis_device/Dynamixel.cpp rename to robotis_device/src/robotis_device/dynamixel.cpp diff --git a/robotis_device/src/robotis_device/Robot.cpp b/robotis_device/src/robotis_device/robot.cpp similarity index 100% rename from robotis_device/src/robotis_device/Robot.cpp rename to robotis_device/src/robotis_device/robot.cpp diff --git a/robotis_device/src/robotis_device/Sensor.cpp b/robotis_device/src/robotis_device/sensor.cpp similarity index 100% rename from robotis_device/src/robotis_device/Sensor.cpp rename to robotis_device/src/robotis_device/sensor.cpp diff --git a/robotis_framework_common/include/robotis_framework_common/RobotisDef.h b/robotis_framework_common/include/robotis_framework_common/RobotisDef.h deleted file mode 100644 index a860e9d..0000000 --- a/robotis_framework_common/include/robotis_framework_common/RobotisDef.h +++ /dev/null @@ -1,21 +0,0 @@ -/* - * RobotisDef.h - * - * Created on: 2016. 1. 27. - * Author: zerom - */ - -#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ -#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ - - -typedef char INT8_T; -typedef short int INT16_T; -typedef int INT32_T; - -typedef unsigned char UINT8_T; -typedef unsigned short int UINT16_T; -typedef unsigned int UINT32_T; - - -#endif /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ */ diff --git a/robotis_framework_common/include/robotis_framework_common/MotionModule.h b/robotis_framework_common/include/robotis_framework_common/motion_module.h similarity index 100% rename from robotis_framework_common/include/robotis_framework_common/MotionModule.h rename to robotis_framework_common/include/robotis_framework_common/motion_module.h diff --git a/robotis_framework_common/include/robotis_framework_common/SensorModule.h b/robotis_framework_common/include/robotis_framework_common/sensor_module.h similarity index 100% rename from robotis_framework_common/include/robotis_framework_common/SensorModule.h rename to robotis_framework_common/include/robotis_framework_common/sensor_module.h diff --git a/robotis_framework_common/include/robotis_framework_common/Singleton.h b/robotis_framework_common/include/robotis_framework_common/singleton.h similarity index 100% rename from robotis_framework_common/include/robotis_framework_common/Singleton.h rename to robotis_framework_common/include/robotis_framework_common/singleton.h diff --git a/robotis_math/include/robotis_math/RobotisLinearAlgebra.h b/robotis_math/include/robotis_math/robotis_linear_algebra.h similarity index 100% rename from robotis_math/include/robotis_math/RobotisLinearAlgebra.h rename to robotis_math/include/robotis_math/robotis_linear_algebra.h diff --git a/robotis_math/include/robotis_math/RobotisMath.h b/robotis_math/include/robotis_math/robotis_math.h similarity index 100% rename from robotis_math/include/robotis_math/RobotisMath.h rename to robotis_math/include/robotis_math/robotis_math.h diff --git a/robotis_math/include/robotis_math/RobotisMathBase.h b/robotis_math/include/robotis_math/robotis_math_base.h similarity index 100% rename from robotis_math/include/robotis_math/RobotisMathBase.h rename to robotis_math/include/robotis_math/robotis_math_base.h diff --git a/robotis_math/include/robotis_math/RobotisTrajectoryCalculator.h b/robotis_math/include/robotis_math/robotis_trajectory_calculator.h similarity index 100% rename from robotis_math/include/robotis_math/RobotisTrajectoryCalculator.h rename to robotis_math/include/robotis_math/robotis_trajectory_calculator.h diff --git a/robotis_math/src/RobotisLinearAlgebra.cpp b/robotis_math/src/robotis_linear_algebra.cpp similarity index 100% rename from robotis_math/src/RobotisLinearAlgebra.cpp rename to robotis_math/src/robotis_linear_algebra.cpp diff --git a/robotis_math/src/RobotisMathBase.cpp b/robotis_math/src/robotis_math_base.cpp similarity index 100% rename from robotis_math/src/RobotisMathBase.cpp rename to robotis_math/src/robotis_math_base.cpp diff --git a/robotis_math/src/RobotisTrajectoryCalculator.cpp b/robotis_math/src/robotis_trajectory_calculator.cpp similarity index 100% rename from robotis_math/src/RobotisTrajectoryCalculator.cpp rename to robotis_math/src/robotis_trajectory_calculator.cpp From 4dcaa5900fe8ed89a4be401078115be70a060595 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Fri, 3 Jun 2016 20:03:18 +0900 Subject: [PATCH 07/33] - rename the files (according to ROS C++ Coding Style) --- dynamixel_sdk/CMakeLists.txt | 18 +- .../{DynamixelSDK.h => dynamixel_sdk.h} | 0 .../include/dynamixel_sdk/RobotisDef.h | 21 -- .../{GroupBulkRead.h => group_bulk_read.h} | 0 .../{GroupBulkWrite.h => group_bulk_write.h} | 0 .../{GroupSyncRead.h => group_sync_read.h} | 0 .../{GroupSyncWrite.h => group_sync_write.h} | 0 .../{PacketHandler.h => packet_handler.h} | 0 .../{PortHandler.h => port_handler.h} | 0 ...etHandler.h => protocol1_packet_handler.h} | 0 ...etHandler.h => protocol2_packet_handler.h} | 0 ...ortHandlerLinux.h => port_handler_linux.h} | 0 .../port_handler_windows.h | 93 +++++++ ...{GroupBulkRead.cpp => group_bulk_read.cpp} | 0 ...roupBulkWrite.cpp => group_bulk_write.cpp} | 0 ...{GroupSyncRead.cpp => group_sync_read.cpp} | 0 ...roupSyncWrite.cpp => group_sync_write.cpp} | 0 .../{PacketHandler.cpp => packet_handler.cpp} | 0 .../{PortHandler.cpp => port_handler.cpp} | 0 ...ndler.cpp => protocol1_packet_handler.cpp} | 0 ...ndler.cpp => protocol2_packet_handler.cpp} | 0 ...andlerLinux.cpp => port_handler_linux.cpp} | 0 .../port_handler_windows.cpp | 245 ++++++++++++++++++ 23 files changed, 347 insertions(+), 30 deletions(-) rename dynamixel_sdk/include/{DynamixelSDK.h => dynamixel_sdk.h} (100%) delete mode 100644 dynamixel_sdk/include/dynamixel_sdk/RobotisDef.h rename dynamixel_sdk/include/dynamixel_sdk/{GroupBulkRead.h => group_bulk_read.h} (100%) rename dynamixel_sdk/include/dynamixel_sdk/{GroupBulkWrite.h => group_bulk_write.h} (100%) rename dynamixel_sdk/include/dynamixel_sdk/{GroupSyncRead.h => group_sync_read.h} (100%) rename dynamixel_sdk/include/dynamixel_sdk/{GroupSyncWrite.h => group_sync_write.h} (100%) rename dynamixel_sdk/include/dynamixel_sdk/{PacketHandler.h => packet_handler.h} (100%) rename dynamixel_sdk/include/dynamixel_sdk/{PortHandler.h => port_handler.h} (100%) rename dynamixel_sdk/include/dynamixel_sdk/{Protocol1PacketHandler.h => protocol1_packet_handler.h} (100%) rename dynamixel_sdk/include/dynamixel_sdk/{Protocol2PacketHandler.h => protocol2_packet_handler.h} (100%) rename dynamixel_sdk/include/dynamixel_sdk_linux/{PortHandlerLinux.h => port_handler_linux.h} (100%) create mode 100644 dynamixel_sdk/include/dynamixel_sdk_windows/port_handler_windows.h rename dynamixel_sdk/src/dynamixel_sdk/{GroupBulkRead.cpp => group_bulk_read.cpp} (100%) rename dynamixel_sdk/src/dynamixel_sdk/{GroupBulkWrite.cpp => group_bulk_write.cpp} (100%) rename dynamixel_sdk/src/dynamixel_sdk/{GroupSyncRead.cpp => group_sync_read.cpp} (100%) rename dynamixel_sdk/src/dynamixel_sdk/{GroupSyncWrite.cpp => group_sync_write.cpp} (100%) rename dynamixel_sdk/src/dynamixel_sdk/{PacketHandler.cpp => packet_handler.cpp} (100%) rename dynamixel_sdk/src/dynamixel_sdk/{PortHandler.cpp => port_handler.cpp} (100%) rename dynamixel_sdk/src/dynamixel_sdk/{Protocol1PacketHandler.cpp => protocol1_packet_handler.cpp} (100%) rename dynamixel_sdk/src/dynamixel_sdk/{Protocol2PacketHandler.cpp => protocol2_packet_handler.cpp} (100%) rename dynamixel_sdk/src/dynamixel_sdk_linux/{PortHandlerLinux.cpp => port_handler_linux.cpp} (100%) create mode 100644 dynamixel_sdk/src/dynamixel_sdk_windows/port_handler_windows.cpp diff --git a/dynamixel_sdk/CMakeLists.txt b/dynamixel_sdk/CMakeLists.txt index b3b5c84..658dfd4 100644 --- a/dynamixel_sdk/CMakeLists.txt +++ b/dynamixel_sdk/CMakeLists.txt @@ -20,15 +20,15 @@ include_directories( ## Declare a C++ library add_library(dynamixel_sdk - src/${PROJECT_NAME}/PacketHandler.cpp - src/${PROJECT_NAME}/Protocol1PacketHandler.cpp - src/${PROJECT_NAME}/Protocol2PacketHandler.cpp - src/${PROJECT_NAME}/GroupSyncRead.cpp - src/${PROJECT_NAME}/GroupSyncWrite.cpp - src/${PROJECT_NAME}/GroupBulkRead.cpp - src/${PROJECT_NAME}/GroupBulkWrite.cpp - src/${PROJECT_NAME}/PortHandler.cpp - src/${PROJECT_NAME}_linux/PortHandlerLinux.cpp + src/${PROJECT_NAME}/packet_handler.cpp + src/${PROJECT_NAME}/protocol1_packet_handler.cpp + src/${PROJECT_NAME}/protocol2_packet_handler.cpp + src/${PROJECT_NAME}/group_sync_read.cpp + src/${PROJECT_NAME}/group_sync_write.cpp + src/${PROJECT_NAME}/group_bulk_read.cpp + src/${PROJECT_NAME}/group_bulk_write.cpp + src/${PROJECT_NAME}/port_handler.cpp + src/${PROJECT_NAME}_linux/port_handler_linux.cpp ) ## Specify libraries to link a library or executable target against diff --git a/dynamixel_sdk/include/DynamixelSDK.h b/dynamixel_sdk/include/dynamixel_sdk.h similarity index 100% rename from dynamixel_sdk/include/DynamixelSDK.h rename to dynamixel_sdk/include/dynamixel_sdk.h diff --git a/dynamixel_sdk/include/dynamixel_sdk/RobotisDef.h b/dynamixel_sdk/include/dynamixel_sdk/RobotisDef.h deleted file mode 100644 index 95ab804..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/RobotisDef.h +++ /dev/null @@ -1,21 +0,0 @@ -/* - * RobotisDef.h - * - * Created on: 2016. 1. 27. - * Author: zerom, leon - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ - - -typedef char INT8_T; -typedef short int INT16_T; -typedef int INT32_T; - -typedef unsigned char UINT8_T; -typedef unsigned short int UINT16_T; -typedef unsigned int UINT32_T; - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h b/dynamixel_sdk/include/dynamixel_sdk/group_bulk_read.h similarity index 100% rename from dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h rename to dynamixel_sdk/include/dynamixel_sdk/group_bulk_read.h diff --git a/dynamixel_sdk/include/dynamixel_sdk/GroupBulkWrite.h b/dynamixel_sdk/include/dynamixel_sdk/group_bulk_write.h similarity index 100% rename from dynamixel_sdk/include/dynamixel_sdk/GroupBulkWrite.h rename to dynamixel_sdk/include/dynamixel_sdk/group_bulk_write.h diff --git a/dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h b/dynamixel_sdk/include/dynamixel_sdk/group_sync_read.h similarity index 100% rename from dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h rename to dynamixel_sdk/include/dynamixel_sdk/group_sync_read.h diff --git a/dynamixel_sdk/include/dynamixel_sdk/GroupSyncWrite.h b/dynamixel_sdk/include/dynamixel_sdk/group_sync_write.h similarity index 100% rename from dynamixel_sdk/include/dynamixel_sdk/GroupSyncWrite.h rename to dynamixel_sdk/include/dynamixel_sdk/group_sync_write.h diff --git a/dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h b/dynamixel_sdk/include/dynamixel_sdk/packet_handler.h similarity index 100% rename from dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h rename to dynamixel_sdk/include/dynamixel_sdk/packet_handler.h diff --git a/dynamixel_sdk/include/dynamixel_sdk/PortHandler.h b/dynamixel_sdk/include/dynamixel_sdk/port_handler.h similarity index 100% rename from dynamixel_sdk/include/dynamixel_sdk/PortHandler.h rename to dynamixel_sdk/include/dynamixel_sdk/port_handler.h diff --git a/dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h b/dynamixel_sdk/include/dynamixel_sdk/protocol1_packet_handler.h similarity index 100% rename from dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h rename to dynamixel_sdk/include/dynamixel_sdk/protocol1_packet_handler.h diff --git a/dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h b/dynamixel_sdk/include/dynamixel_sdk/protocol2_packet_handler.h similarity index 100% rename from dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h rename to dynamixel_sdk/include/dynamixel_sdk/protocol2_packet_handler.h diff --git a/dynamixel_sdk/include/dynamixel_sdk_linux/PortHandlerLinux.h b/dynamixel_sdk/include/dynamixel_sdk_linux/port_handler_linux.h similarity index 100% rename from dynamixel_sdk/include/dynamixel_sdk_linux/PortHandlerLinux.h rename to dynamixel_sdk/include/dynamixel_sdk_linux/port_handler_linux.h diff --git a/dynamixel_sdk/include/dynamixel_sdk_windows/port_handler_windows.h b/dynamixel_sdk/include/dynamixel_sdk_windows/port_handler_windows.h new file mode 100644 index 0000000..09583cb --- /dev/null +++ b/dynamixel_sdk/include/dynamixel_sdk_windows/port_handler_windows.h @@ -0,0 +1,93 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* * Redistributions of source code must retain the above copyright notice, this +* list of conditions and the following disclaimer. +* +* * Redistributions in binary form must reproduce the above copyright notice, +* this list of conditions and the following disclaimer in the documentation +* and/or other materials provided with the distribution. +* +* * Neither the name of ROBOTIS nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*******************************************************************************/ + +/* Author: Leon Ryu Woon Jung */ + +/* +* port_handler_windows.h +* +* Created on: 2016. 4. 26. +*/ + +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_WINDOWS_PORTHANDLERWINDOWS_H_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_WINDOWS_PORTHANDLERWINDOWS_H_ + +#include + +#include "dynamixel_sdk/port_handler.h" + +namespace dynamixel +{ +class WINDECLSPEC PortHandlerWindows : public PortHandler +{ + private: + HANDLE serial_handle_; + LARGE_INTEGER freq_, counter_; + + int baudrate_; + char port_name_[30]; + + double packet_start_time_; + double packet_timeout_; + double tx_time_per_byte_; + + bool setupPort(const int baudrate); + + double getCurrentTime(); + double getTimeSinceStart(); + + public: + PortHandlerWindows(const char *port_name); + virtual ~PortHandlerWindows() { closePort(); } + + bool openPort(); + void closePort(); + void clearPort(); + + void setPortName(const char *port_name); + char *getPortName(); + + bool setBaudRate(const int baudrate); + int getBaudRate(); + + int getBytesAvailable(); + + int readPort(uint8_t *packet, int length); + int writePort(uint8_t *packet, int length); + + void setPacketTimeout(uint16_t packet_length); + void setPacketTimeout(double msec); + bool isPacketTimeout(); +}; + +} + + +#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERWINDOWS_H_ */ diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp b/dynamixel_sdk/src/dynamixel_sdk/group_bulk_read.cpp similarity index 100% rename from dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp rename to dynamixel_sdk/src/dynamixel_sdk/group_bulk_read.cpp diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp b/dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp similarity index 100% rename from dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp rename to dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp b/dynamixel_sdk/src/dynamixel_sdk/group_sync_read.cpp similarity index 100% rename from dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp rename to dynamixel_sdk/src/dynamixel_sdk/group_sync_read.cpp diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp b/dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp similarity index 100% rename from dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp rename to dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp diff --git a/dynamixel_sdk/src/dynamixel_sdk/PacketHandler.cpp b/dynamixel_sdk/src/dynamixel_sdk/packet_handler.cpp similarity index 100% rename from dynamixel_sdk/src/dynamixel_sdk/PacketHandler.cpp rename to dynamixel_sdk/src/dynamixel_sdk/packet_handler.cpp diff --git a/dynamixel_sdk/src/dynamixel_sdk/PortHandler.cpp b/dynamixel_sdk/src/dynamixel_sdk/port_handler.cpp similarity index 100% rename from dynamixel_sdk/src/dynamixel_sdk/PortHandler.cpp rename to dynamixel_sdk/src/dynamixel_sdk/port_handler.cpp diff --git a/dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp b/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp similarity index 100% rename from dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp rename to dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp diff --git a/dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp b/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp similarity index 100% rename from dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp rename to dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp diff --git a/dynamixel_sdk/src/dynamixel_sdk_linux/PortHandlerLinux.cpp b/dynamixel_sdk/src/dynamixel_sdk_linux/port_handler_linux.cpp similarity index 100% rename from dynamixel_sdk/src/dynamixel_sdk_linux/PortHandlerLinux.cpp rename to dynamixel_sdk/src/dynamixel_sdk_linux/port_handler_linux.cpp diff --git a/dynamixel_sdk/src/dynamixel_sdk_windows/port_handler_windows.cpp b/dynamixel_sdk/src/dynamixel_sdk_windows/port_handler_windows.cpp new file mode 100644 index 0000000..c47e76f --- /dev/null +++ b/dynamixel_sdk/src/dynamixel_sdk_windows/port_handler_windows.cpp @@ -0,0 +1,245 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* * Redistributions of source code must retain the above copyright notice, this +* list of conditions and the following disclaimer. +* +* * Redistributions in binary form must reproduce the above copyright notice, +* this list of conditions and the following disclaimer in the documentation +* and/or other materials provided with the distribution. +* +* * Neither the name of ROBOTIS nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*******************************************************************************/ + +/* Author: Leon Ryu Woon Jung */ + +/* +* PortHandlerWindows.cpp +* +* Created on: 2016. 4. 06. +*/ +#if defined(_WIN32) || defined(_WIN64) +#define WINDLLEXPORT +#endif + +#include "dynamixel_sdk_windows/port_handler_windows.h" + +#include +#include +#include + +#define LATENCY_TIMER 16 // msec (USB latency timer) + +using namespace dynamixel; + +PortHandlerWindows::PortHandlerWindows(const char *port_name) + : serial_handle_(INVALID_HANDLE_VALUE), + baudrate_(DEFAULT_BAUDRATE_), + packet_start_time_(0.0), + packet_timeout_(0.0), + tx_time_per_byte_(0.0) +{ + is_using_ = false; + + char buffer[15]; + sprintf_s(buffer, sizeof(buffer), "\\\\.\\", port_name); + setPortName(port_name); +} + +bool PortHandlerWindows::openPort() +{ + return setBaudRate(baudrate_); +} + +void PortHandlerWindows::closePort() +{ + if (serial_handle_ != INVALID_HANDLE_VALUE) + { + CloseHandle(serial_handle_); + serial_handle_ = INVALID_HANDLE_VALUE; + } +} + +void PortHandlerWindows::clearPort() +{ + PurgeComm(serial_handle_, PURGE_RXABORT | PURGE_RXCLEAR); +} + +void PortHandlerWindows::setPortName(const char *port_name) +{ + strcpy_s(port_name_, sizeof(port_name_), port_name); +} + +char *PortHandlerWindows::getPortName() +{ + return port_name_; +} + +bool PortHandlerWindows::setBaudRate(const int baudrate) +{ + closePort(); + + baudrate_ = baudrate; + return setupPort(baudrate); +} + +int PortHandlerWindows::getBaudRate() +{ + return baudrate_; +} + +int PortHandlerWindows::getBytesAvailable() +{ + DWORD retbyte = 2; + BOOL res = DeviceIoControl(serial_handle_, GENERIC_READ | GENERIC_WRITE, NULL, 0, 0, 0, &retbyte, (LPOVERLAPPED)NULL); + + printf("%d", (int)res); + return (int)retbyte; +} + +int PortHandlerWindows::readPort(uint8_t *packet, int length) +{ + DWORD dwRead = 0; + + if (ReadFile(serial_handle_, packet, (DWORD)length, &dwRead, NULL) == FALSE) + return -1; + + return (int)dwRead; +} + +int PortHandlerWindows::writePort(uint8_t *packet, int length) +{ + DWORD dwWrite = 0; + + if (WriteFile(serial_handle_, packet, (DWORD)length, &dwWrite, NULL) == FALSE) + return -1; + + return (int)dwWrite; +} + +void PortHandlerWindows::setPacketTimeout(uint16_t packet_length) +{ + packet_start_time_ = getCurrentTime(); + packet_timeout_ = (tx_time_per_byte_ * (double)packet_length) + (LATENCY_TIMER * 2.0) + 2.0; +} + +void PortHandlerWindows::setPacketTimeout(double msec) +{ + packet_start_time_ = getCurrentTime(); + packet_timeout_ = msec; +} + +bool PortHandlerWindows::isPacketTimeout() +{ + if (getTimeSinceStart() > packet_timeout_) + { + packet_timeout_ = 0; + return true; + } + return false; +} + +double PortHandlerWindows::getCurrentTime() +{ + QueryPerformanceCounter(&counter_); + QueryPerformanceFrequency(&freq_); + return (double)counter_.QuadPart / (double)freq_.QuadPart * 1000.0; +} + +double PortHandlerWindows::getTimeSinceStart() +{ + double time; + + time = getCurrentTime() - packet_start_time_; + if (time < 0.0) packet_start_time_ = getCurrentTime(); + + return time; +} + +bool PortHandlerWindows::setupPort(int baudrate) +{ + DCB dcb; + COMMTIMEOUTS timeouts; + DWORD dwError; + + closePort(); + + serial_handle_ = CreateFileA(port_name_, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, NULL); + if (serial_handle_ == INVALID_HANDLE_VALUE) + { + printf("[PortHandlerWindows::SetupPort] Error opening serial port!\n"); + return false; + } + + dcb.DCBlength = sizeof(DCB); + if (GetCommState(serial_handle_, &dcb) == FALSE) + goto DXL_HAL_OPEN_ERROR; + + // Set baudrate + dcb.BaudRate = (DWORD)baudrate; + dcb.ByteSize = 8; // Data bit = 8bit + dcb.Parity = NOPARITY; // No parity + dcb.StopBits = ONESTOPBIT; // Stop bit = 1 + dcb.fParity = NOPARITY; // No Parity check + dcb.fBinary = 1; // Binary mode + dcb.fNull = 0; // Get Null byte + dcb.fAbortOnError = 0; + dcb.fErrorChar = 0; + // Not using XOn/XOff + dcb.fOutX = 0; + dcb.fInX = 0; + // Not using H/W flow control + dcb.fDtrControl = DTR_CONTROL_DISABLE; + dcb.fRtsControl = RTS_CONTROL_DISABLE; + dcb.fDsrSensitivity = 0; + dcb.fOutxDsrFlow = 0; + dcb.fOutxCtsFlow = 0; + + if (SetCommState(serial_handle_, &dcb) == FALSE) + goto DXL_HAL_OPEN_ERROR; + + if (SetCommMask(serial_handle_, 0) == FALSE) // Not using Comm event + goto DXL_HAL_OPEN_ERROR; + if (SetupComm(serial_handle_, 4096, 4096) == FALSE) // Buffer size (Rx,Tx) + goto DXL_HAL_OPEN_ERROR; + if (PurgeComm(serial_handle_, PURGE_TXABORT | PURGE_TXCLEAR | PURGE_RXABORT | PURGE_RXCLEAR) == FALSE) // Clear buffer + goto DXL_HAL_OPEN_ERROR; + if (ClearCommError(serial_handle_, &dwError, NULL) == FALSE) + goto DXL_HAL_OPEN_ERROR; + + if (GetCommTimeouts(serial_handle_, &timeouts) == FALSE) + goto DXL_HAL_OPEN_ERROR; + // Timeout (Not using timeout) + // Immediatly return + timeouts.ReadIntervalTimeout = 0; + timeouts.ReadTotalTimeoutMultiplier = 0; + timeouts.ReadTotalTimeoutConstant = 1; // must not be zero. + timeouts.WriteTotalTimeoutMultiplier = 0; + timeouts.WriteTotalTimeoutConstant = 0; + if (SetCommTimeouts(serial_handle_, &timeouts) == FALSE) + goto DXL_HAL_OPEN_ERROR; + + tx_time_per_byte_ = (1000.0 / (double)baudrate_) * 10.0; + return true; + +DXL_HAL_OPEN_ERROR: + closePort(); + return false; +} From 1ed888af00c114f359628bf8665cd03bbb571f9c Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Fri, 3 Jun 2016 20:05:21 +0900 Subject: [PATCH 08/33] - ROS C++ Coding Style is applied. --- dynamixel_sdk/include/dynamixel_sdk.h | 52 +- .../include/dynamixel_sdk/group_bulk_read.h | 89 +- .../include/dynamixel_sdk/group_bulk_write.h | 84 +- .../include/dynamixel_sdk/group_sync_read.h | 90 +- .../include/dynamixel_sdk/group_sync_write.h | 82 +- .../include/dynamixel_sdk/packet_handler.h | 136 +- .../include/dynamixel_sdk/port_handler.h | 78 +- .../dynamixel_sdk/protocol1_packet_handler.h | 141 +- .../dynamixel_sdk/protocol2_packet_handler.h | 149 +- .../dynamixel_sdk_linux/port_handler_linux.h | 95 +- .../src/dynamixel_sdk/group_bulk_read.cpp | 294 +- .../src/dynamixel_sdk/group_bulk_write.cpp | 233 +- .../src/dynamixel_sdk/group_sync_read.cpp | 237 +- .../src/dynamixel_sdk/group_sync_write.cpp | 182 +- .../src/dynamixel_sdk/packet_handler.cpp | 59 +- .../src/dynamixel_sdk/port_handler.cpp | 49 +- .../protocol1_packet_handler.cpp | 899 ++--- .../protocol2_packet_handler.cpp | 1471 +++++---- .../port_handler_linux.cpp | 307 +- robotis_controller/CMakeLists.txt | 20 +- .../robotis_controller/robotis_controller.h | 201 +- robotis_controller/package.xml | 4 - .../robotis_controller/robotis_controller.cpp | 2916 +++++++++-------- robotis_controller_msgs/package.xml | 8 +- robotis_device/CMakeLists.txt | 34 +- .../robotis_device/control_table_item.h | 92 +- .../include/robotis_device/device.h | 57 +- .../include/robotis_device/dynamixel.h | 95 +- .../include/robotis_device/dynamixel_state.h | 97 +- robotis_device/include/robotis_device/robot.h | 70 +- .../include/robotis_device/sensor.h | 51 +- .../include/robotis_device/sensor_state.h | 57 +- .../include/robotis_device/time_stamp.h | 50 +- robotis_device/package.xml | 9 - .../src/robotis_device/dynamixel.cpp | 196 +- robotis_device/src/robotis_device/robot.cpp | 776 ++--- robotis_device/src/robotis_device/sensor.cpp | 50 +- robotis_framework_common/CMakeLists.txt | 38 - .../robotis_framework_common/motion_module.h | 116 +- .../robotis_framework_common/sensor_module.h | 58 +- .../robotis_framework_common/singleton.h | 72 +- robotis_framework_common/package.xml | 45 +- robotis_math/CMakeLists.txt | 6 +- .../robotis_math/robotis_linear_algebra.h | 2 +- .../include/robotis_math/robotis_math.h | 2 +- .../include/robotis_math/robotis_math_base.h | 2 +- .../robotis_trajectory_calculator.h | 6 +- robotis_math/src/robotis_linear_algebra.cpp | 4 +- robotis_math/src/robotis_math_base.cpp | 4 +- .../src/robotis_trajectory_calculator.cpp | 4 +- 50 files changed, 5480 insertions(+), 4389 deletions(-) diff --git a/dynamixel_sdk/include/dynamixel_sdk.h b/dynamixel_sdk/include/dynamixel_sdk.h index 0d34655..a0fdf2a 100644 --- a/dynamixel_sdk/include/dynamixel_sdk.h +++ b/dynamixel_sdk/include/dynamixel_sdk.h @@ -1,28 +1,58 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* * Redistributions of source code must retain the above copyright notice, this +* list of conditions and the following disclaimer. +* +* * Redistributions in binary form must reproduce the above copyright notice, +* this list of conditions and the following disclaimer in the documentation +* and/or other materials provided with the distribution. +* +* * Neither the name of ROBOTIS nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*******************************************************************************/ + +/* Author: zerom, Leon Ryu Woon Jung */ + /* - * DynamixelSDK.h + * dynamixel_sdk.h * * Created on: 2016. 3. 8. - * Author: zerom, leon */ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_DYNAMIXELSDK_H_ #define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_DYNAMIXELSDK_H_ -#include "dynamixel_sdk/RobotisDef.h" -#include "dynamixel_sdk/GroupBulkRead.h" -#include "dynamixel_sdk/GroupBulkWrite.h" -#include "dynamixel_sdk/GroupSyncRead.h" -#include "dynamixel_sdk/GroupSyncWrite.h" -#include "dynamixel_sdk/Protocol1PacketHandler.h" -#include "dynamixel_sdk/Protocol2PacketHandler.h" +#include "dynamixel_sdk/group_bulk_read.h" +#include "dynamixel_sdk/group_bulk_write.h" +#include "dynamixel_sdk/group_sync_read.h" +#include "dynamixel_sdk/group_sync_write.h" +#include "dynamixel_sdk/protocol1_packet_handler.h" +#include "dynamixel_sdk/protocol2_packet_handler.h" #ifdef __linux__ - #include "dynamixel_sdk_linux/PortHandlerLinux.h" + #include "dynamixel_sdk_linux/port_handler_linux.h" #endif #if defined(_WIN32) || defined(_WIN64) - #include "dynamixel_sdk_windows/PortHandlerWindows.h" + #include "dynamixel_sdk_windows/port_handler_windows.h" #endif diff --git a/dynamixel_sdk/include/dynamixel_sdk/group_bulk_read.h b/dynamixel_sdk/include/dynamixel_sdk/group_bulk_read.h index bd3374b..29b0dba 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/group_bulk_read.h +++ b/dynamixel_sdk/include/dynamixel_sdk/group_bulk_read.h @@ -1,5 +1,37 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* * Redistributions of source code must retain the above copyright notice, this +* list of conditions and the following disclaimer. +* +* * Redistributions in binary form must reproduce the above copyright notice, +* this list of conditions and the following disclaimer in the documentation +* and/or other materials provided with the distribution. +* +* * Neither the name of ROBOTIS nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*******************************************************************************/ + +/* Author: zerom, Leon Ryu Woon Jung */ + /* - * GroupBulkRead.h + * group_bulk_read.h * * Created on: 2016. 1. 28. * Author: zerom, leon @@ -11,48 +43,47 @@ #include #include -#include "RobotisDef.h" -#include "PortHandler.h" -#include "PacketHandler.h" +#include "port_handler.h" +#include "packet_handler.h" -namespace ROBOTIS +namespace dynamixel { class WINDECLSPEC GroupBulkRead { -private: - PortHandler *port_; - PacketHandler *ph_; + private: + PortHandler *port_; + PacketHandler *ph_; - std::vector id_list_; - std::map address_list_; // - std::map length_list_; // - std::map data_list_; // + std::vector id_list_; + std::map address_list_; // + std::map length_list_; // + std::map data_list_; // - bool last_result_; - bool is_param_changed_; + bool last_result_; + bool is_param_changed_; - UINT8_T *param_; + uint8_t *param_; - void MakeParam(); + void makeParam(); -public: - GroupBulkRead(PortHandler *port, PacketHandler *ph); - ~GroupBulkRead() { ClearParam(); } + public: + GroupBulkRead(PortHandler *port, PacketHandler *ph); + ~GroupBulkRead() { clearParam(); } - PortHandler *GetPortHandler() { return port_; } - PacketHandler *GetPacketHandler() { return ph_; } + PortHandler *getPortHandler() { return port_; } + PacketHandler *getPacketHandler() { return ph_; } - bool AddParam (UINT8_T id, UINT16_T start_address, UINT16_T data_length); - void RemoveParam (UINT8_T id); - void ClearParam (); + bool addParam (uint8_t id, uint16_t start_address, uint16_t data_length); + void removeParam (uint8_t id); + void clearParam (); - int TxPacket(); - int RxPacket(); - int TxRxPacket(); + int txPacket(); + int rxPacket(); + int txRxPacket(); - bool IsAvailable (UINT8_T id, UINT16_T address, UINT16_T data_length); - UINT32_T GetData (UINT8_T id, UINT16_T address, UINT16_T data_length); + bool isAvailable (uint8_t id, uint16_t address, uint16_t data_length); + uint32_t getData (uint8_t id, uint16_t address, uint16_t data_length); }; } diff --git a/dynamixel_sdk/include/dynamixel_sdk/group_bulk_write.h b/dynamixel_sdk/include/dynamixel_sdk/group_bulk_write.h index 3efbb80..71bd9ec 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/group_bulk_write.h +++ b/dynamixel_sdk/include/dynamixel_sdk/group_bulk_write.h @@ -1,8 +1,39 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* * Redistributions of source code must retain the above copyright notice, this +* list of conditions and the following disclaimer. +* +* * Redistributions in binary form must reproduce the above copyright notice, +* this list of conditions and the following disclaimer in the documentation +* and/or other materials provided with the distribution. +* +* * Neither the name of ROBOTIS nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*******************************************************************************/ + +/* Author: zerom, Leon Ryu Woon Jung */ + /* - * GroupBulkWrite.h + * group_bulk_write.h * * Created on: 2016. 2. 2. - * Author: zerom, leon */ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_ @@ -11,44 +42,43 @@ #include #include -#include "RobotisDef.h" -#include "PortHandler.h" -#include "PacketHandler.h" +#include "port_handler.h" +#include "packet_handler.h" -namespace ROBOTIS +namespace dynamixel { class WINDECLSPEC GroupBulkWrite { -private: - PortHandler *port_; - PacketHandler *ph_; + private: + PortHandler *port_; + PacketHandler *ph_; - std::vector id_list_; - std::map address_list_; // - std::map length_list_; // - std::map data_list_; // + std::vector id_list_; + std::map address_list_; // + std::map length_list_; // + std::map data_list_; // - bool is_param_changed_; + bool is_param_changed_; - UINT8_T *param_; - UINT16_T param_length_; + uint8_t *param_; + uint16_t param_length_; - void MakeParam(); + void makeParam(); -public: - GroupBulkWrite(PortHandler *port, PacketHandler *ph); - ~GroupBulkWrite() { ClearParam(); } + public: + GroupBulkWrite(PortHandler *port, PacketHandler *ph); + ~GroupBulkWrite() { clearParam(); } - PortHandler *GetPortHandler() { return port_; } - PacketHandler *GetPacketHandler() { return ph_; } + PortHandler *getPortHandler() { return port_; } + PacketHandler *getPacketHandler() { return ph_; } - bool AddParam (UINT8_T id, UINT16_T start_address, UINT16_T data_length, UINT8_T *data); - void RemoveParam (UINT8_T id); - bool ChangeParam (UINT8_T id, UINT16_T start_address, UINT16_T data_length, UINT8_T *data); - void ClearParam (); + bool addParam (uint8_t id, uint16_t start_address, uint16_t data_length, uint8_t *data); + void removeParam (uint8_t id); + bool changeParam (uint8_t id, uint16_t start_address, uint16_t data_length, uint8_t *data); + void clearParam (); - int TxPacket(); + int txPacket(); }; } diff --git a/dynamixel_sdk/include/dynamixel_sdk/group_sync_read.h b/dynamixel_sdk/include/dynamixel_sdk/group_sync_read.h index b6599de..ec2a208 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/group_sync_read.h +++ b/dynamixel_sdk/include/dynamixel_sdk/group_sync_read.h @@ -1,8 +1,39 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* * Redistributions of source code must retain the above copyright notice, this +* list of conditions and the following disclaimer. +* +* * Redistributions in binary form must reproduce the above copyright notice, +* this list of conditions and the following disclaimer in the documentation +* and/or other materials provided with the distribution. +* +* * Neither the name of ROBOTIS nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*******************************************************************************/ + +/* Author: zerom, Leon Ryu Woon Jung */ + /* - * GroupSyncRead.h + * group_sync_read.h * * Created on: 2016. 2. 2. - * Author: zerom, leon */ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_ @@ -11,48 +42,47 @@ #include #include -#include "RobotisDef.h" -#include "PortHandler.h" -#include "PacketHandler.h" +#include "port_handler.h" +#include "packet_handler.h" -namespace ROBOTIS +namespace dynamixel { class WINDECLSPEC GroupSyncRead { -private: - PortHandler *port_; - PacketHandler *ph_; + private: + PortHandler *port_; + PacketHandler *ph_; - std::vector id_list_; - std::map data_list_; // + std::vector id_list_; + std::map data_list_; // - bool last_result_; - bool is_param_changed_; + bool last_result_; + bool is_param_changed_; - UINT8_T *param_; - UINT16_T start_address_; - UINT16_T data_length_; + uint8_t *param_; + uint16_t start_address_; + uint16_t data_length_; - void MakeParam(); + void makeParam(); -public: - GroupSyncRead(PortHandler *port, PacketHandler *ph, UINT16_T start_address, UINT16_T data_length); - ~GroupSyncRead() { ClearParam(); } + public: + GroupSyncRead(PortHandler *port, PacketHandler *ph, uint16_t start_address, uint16_t data_length); + ~GroupSyncRead() { clearParam(); } - PortHandler *GetPortHandler() { return port_; } - PacketHandler *GetPacketHandler() { return ph_; } + PortHandler *getPortHandler() { return port_; } + PacketHandler *getPacketHandler() { return ph_; } - bool AddParam (UINT8_T id); - void RemoveParam (UINT8_T id); - void ClearParam (); + bool addParam (uint8_t id); + void removeParam (uint8_t id); + void clearParam (); - int TxPacket(); - int RxPacket(); - int TxRxPacket(); + int txPacket(); + int rxPacket(); + int txRxPacket(); - bool IsAvailable (UINT8_T id, UINT16_T address, UINT16_T data_length); - UINT32_T GetData (UINT8_T id, UINT16_T address, UINT16_T data_length); + bool isAvailable (uint8_t id, uint16_t address, uint16_t data_length); + uint32_t getData (uint8_t id, uint16_t address, uint16_t data_length); }; } diff --git a/dynamixel_sdk/include/dynamixel_sdk/group_sync_write.h b/dynamixel_sdk/include/dynamixel_sdk/group_sync_write.h index 1ff03b2..1c85666 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/group_sync_write.h +++ b/dynamixel_sdk/include/dynamixel_sdk/group_sync_write.h @@ -1,8 +1,39 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* * Redistributions of source code must retain the above copyright notice, this +* list of conditions and the following disclaimer. +* +* * Redistributions in binary form must reproduce the above copyright notice, +* this list of conditions and the following disclaimer in the documentation +* and/or other materials provided with the distribution. +* +* * Neither the name of ROBOTIS nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*******************************************************************************/ + +/* Author: zerom, Leon Ryu Woon Jung */ + /* - * GroupSyncWrite.h + * group_sync_write.h * * Created on: 2016. 1. 28. - * Author: zerom, leon */ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_ @@ -11,43 +42,42 @@ #include #include -#include "RobotisDef.h" -#include "PortHandler.h" -#include "PacketHandler.h" +#include "port_handler.h" +#include "packet_handler.h" -namespace ROBOTIS +namespace dynamixel { class WINDECLSPEC GroupSyncWrite { -private: - PortHandler *port_; - PacketHandler *ph_; + private: + PortHandler *port_; + PacketHandler *ph_; - std::vector id_list_; - std::map data_list_; // + std::vector id_list_; + std::map data_list_; // - bool is_param_changed_; + bool is_param_changed_; - UINT8_T *param_; - UINT16_T start_address_; - UINT16_T data_length_; + uint8_t *param_; + uint16_t start_address_; + uint16_t data_length_; - void MakeParam(); + void makeParam(); -public: - GroupSyncWrite(PortHandler *port, PacketHandler *ph, UINT16_T start_address, UINT16_T data_length); - ~GroupSyncWrite() { ClearParam(); } + public: + GroupSyncWrite(PortHandler *port, PacketHandler *ph, uint16_t start_address, uint16_t data_length); + ~GroupSyncWrite() { clearParam(); } - PortHandler *GetPortHandler() { return port_; } - PacketHandler *GetPacketHandler() { return ph_; } + PortHandler *getPortHandler() { return port_; } + PacketHandler *getPacketHandler() { return ph_; } - bool AddParam (UINT8_T id, UINT8_T *data); - void RemoveParam (UINT8_T id); - bool ChangeParam (UINT8_T id, UINT8_T *data); - void ClearParam (); + bool addParam (uint8_t id, uint8_t *data); + void removeParam (uint8_t id); + bool changeParam (uint8_t id, uint8_t *data); + void clearParam (); - int TxPacket(); + int txPacket(); }; } diff --git a/dynamixel_sdk/include/dynamixel_sdk/packet_handler.h b/dynamixel_sdk/include/dynamixel_sdk/packet_handler.h index 921d380..3816cbb 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/packet_handler.h +++ b/dynamixel_sdk/include/dynamixel_sdk/packet_handler.h @@ -1,8 +1,39 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* * Redistributions of source code must retain the above copyright notice, this +* list of conditions and the following disclaimer. +* +* * Redistributions in binary form must reproduce the above copyright notice, +* this list of conditions and the following disclaimer in the documentation +* and/or other materials provided with the distribution. +* +* * Neither the name of ROBOTIS nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*******************************************************************************/ + +/* Author: zerom, Leon Ryu Woon Jung */ + /* - * PacketHandler.h + * packet_handler.h * * Created on: 2016. 1. 26. - * Author: zerom, leon */ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ @@ -11,8 +42,7 @@ #include #include -#include "RobotisDef.h" -#include "PortHandler.h" +#include "port_handler.h" #define BROADCAST_ID 0xFE // 254 #define MAX_ID 0xFC // 252 @@ -51,81 +81,81 @@ #define COMM_RX_CORRUPT -3002 // Incorrect status packet #define COMM_NOT_AVAILABLE -9000 // -namespace ROBOTIS +namespace dynamixel { class WINDECLSPEC PacketHandler { -protected: - PacketHandler() { } + protected: + PacketHandler() { } -public: - static PacketHandler *GetPacketHandler(float protocol_version = 2.0); + public: + static PacketHandler *getPacketHandler(float protocol_version = 2.0); - virtual ~PacketHandler() { } + virtual ~PacketHandler() { } - virtual float GetProtocolVersion() = 0; + virtual float getProtocolVersion() = 0; - virtual void PrintTxRxResult(int result) = 0; - virtual void PrintRxPacketError(UINT8_T error) = 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; + 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; - virtual int Ping (PortHandler *port, UINT8_T id, UINT8_T *error = 0) = 0; - virtual int Ping (PortHandler *port, UINT8_T id, UINT16_T *model_number, UINT8_T *error = 0) = 0; + virtual int ping (PortHandler *port, uint8_t id, uint8_t *error = 0) = 0; + virtual int ping (PortHandler *port, uint8_t id, uint16_t *model_number, uint8_t *error = 0) = 0; - // BroadcastPing - virtual int BroadcastPing (PortHandler *port, std::vector &id_list) = 0; + // broadcastPing + virtual int broadcastPing (PortHandler *port, std::vector &id_list) = 0; - virtual int Action (PortHandler *port, UINT8_T id) = 0; - virtual int Reboot (PortHandler *port, UINT8_T id, UINT8_T *error = 0) = 0; - virtual int FactoryReset (PortHandler *port, UINT8_T id, UINT8_T option = 0, UINT8_T *error = 0) = 0; + virtual int action (PortHandler *port, uint8_t id) = 0; + virtual int reboot (PortHandler *port, uint8_t id, uint8_t *error = 0) = 0; + virtual int factoryReset (PortHandler *port, uint8_t id, uint8_t option = 0, uint8_t *error = 0) = 0; - virtual int ReadTx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length) = 0; - virtual int ReadRx (PortHandler *port, UINT16_T length, UINT8_T *data, UINT8_T *error = 0) = 0; - virtual int ReadTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0) = 0; + virtual int readTx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length) = 0; + virtual int readRx (PortHandler *port, uint16_t length, uint8_t *data, uint8_t *error = 0) = 0; + virtual int readTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0) = 0; - virtual int Read1ByteTx (PortHandler *port, UINT8_T id, UINT16_T address) = 0; - virtual int Read1ByteRx (PortHandler *port, UINT8_T *data, UINT8_T *error = 0) = 0; - virtual int Read1ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T *data, UINT8_T *error = 0) = 0; + virtual int read1ByteTx (PortHandler *port, uint8_t id, uint16_t address) = 0; + virtual int read1ByteRx (PortHandler *port, uint8_t *data, uint8_t *error = 0) = 0; + virtual int read1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error = 0) = 0; - virtual int Read2ByteTx (PortHandler *port, UINT8_T id, UINT16_T address) = 0; - virtual int Read2ByteRx (PortHandler *port, UINT16_T *data, UINT8_T *error = 0) = 0; - virtual int Read2ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T *data, UINT8_T *error = 0) = 0; + virtual int read2ByteTx (PortHandler *port, uint8_t id, uint16_t address) = 0; + virtual int read2ByteRx (PortHandler *port, uint16_t *data, uint8_t *error = 0) = 0; + virtual int read2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error = 0) = 0; - virtual int Read4ByteTx (PortHandler *port, UINT8_T id, UINT16_T address) = 0; - virtual int Read4ByteRx (PortHandler *port, UINT32_T *data, UINT8_T *error = 0) = 0; - virtual int Read4ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T *data, UINT8_T *error = 0) = 0; + virtual int read4ByteTx (PortHandler *port, uint8_t id, uint16_t address) = 0; + virtual int read4ByteRx (PortHandler *port, uint32_t *data, uint8_t *error = 0) = 0; + virtual int read4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error = 0) = 0; - virtual int WriteTxOnly (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data) = 0; - virtual int WriteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0) = 0; + virtual int writeTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) = 0; + virtual int writeTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0) = 0; - virtual int Write1ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data) = 0; - virtual int Write1ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data, UINT8_T *error = 0) = 0; + virtual int write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data) = 0; + virtual int write1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error = 0) = 0; - virtual int Write2ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data) = 0; - virtual int Write2ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data, UINT8_T *error = 0) = 0; + virtual int write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data) = 0; + virtual int write2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error = 0) = 0; - virtual int Write4ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data) = 0; - virtual int Write4ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data, UINT8_T *error = 0) = 0; + virtual int write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data) = 0; + virtual int write4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error = 0) = 0; - virtual int RegWriteTxOnly (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data) = 0; - virtual int RegWriteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0) = 0; + virtual int regWriteTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) = 0; + virtual int regWriteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0) = 0; - virtual int SyncReadTx (PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length) = 0; - // SyncReadRx -> GroupSyncRead class - // SyncReadTxRx -> GroupSyncRead class + virtual int syncReadTx (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) = 0; + // SyncReadRx -> GroupSyncRead class + // SyncReadTxRx -> GroupSyncRead class - virtual int SyncWriteTxOnly (PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length) = 0; + virtual int syncWriteTxOnly (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) = 0; - virtual int BulkReadTx (PortHandler *port, UINT8_T *param, UINT16_T param_length) = 0; - // BulkReadRx -> GroupBulkRead class - // BulkReadTxRx -> GroupBulkRead class + virtual int bulkReadTx (PortHandler *port, uint8_t *param, uint16_t param_length) = 0; + // BulkReadRx -> GroupBulkRead class + // BulkReadTxRx -> GroupBulkRead class - virtual int BulkWriteTxOnly (PortHandler *port, UINT8_T *param, UINT16_T param_length) = 0; + virtual int bulkWriteTxOnly (PortHandler *port, uint8_t *param, uint16_t param_length) = 0; }; } diff --git a/dynamixel_sdk/include/dynamixel_sdk/port_handler.h b/dynamixel_sdk/include/dynamixel_sdk/port_handler.h index 6d3e6dd..dfc83fd 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/port_handler.h +++ b/dynamixel_sdk/include/dynamixel_sdk/port_handler.h @@ -1,13 +1,47 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* * Redistributions of source code must retain the above copyright notice, this +* list of conditions and the following disclaimer. +* +* * Redistributions in binary form must reproduce the above copyright notice, +* this list of conditions and the following disclaimer in the documentation +* and/or other materials provided with the distribution. +* +* * Neither the name of ROBOTIS nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*******************************************************************************/ + +/* Author: zerom, Leon Ryu Woon Jung */ + /* - * PortHandler.h + * port_handler.h * * Created on: 2016. 1. 26. - * Author: zerom, leon */ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_ #define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_ +#undef __linux__ +#define __linux__ + #ifdef __linux__ #define WINDECLSPEC #elif defined(_WIN32) || defined(_WIN64) @@ -18,40 +52,40 @@ #endif #endif -#include "RobotisDef.h" +#include -namespace ROBOTIS +namespace dynamixel { class WINDECLSPEC PortHandler { -public: - static const int DEFAULT_BAUDRATE = 1000000; + public: + static const int DEFAULT_BAUDRATE_ = 1000000; - static PortHandler *GetPortHandler(const char *port_name); + static PortHandler *getPortHandler(const char *port_name); - bool is_using; + bool is_using_; - virtual ~PortHandler() { } + virtual ~PortHandler() { } - virtual bool OpenPort() = 0; - virtual void ClosePort() = 0; - virtual void ClearPort() = 0; + virtual bool openPort() = 0; + virtual void closePort() = 0; + virtual void clearPort() = 0; - virtual void SetPortName(const char* port_name) = 0; - virtual char *GetPortName() = 0; + virtual void setPortName(const char* port_name) = 0; + virtual char *getPortName() = 0; - virtual bool SetBaudRate(const int baudrate) = 0; - virtual int GetBaudRate() = 0; + virtual bool setBaudRate(const int baudrate) = 0; + virtual int getBaudRate() = 0; - virtual int GetBytesAvailable() = 0; + virtual int getBytesAvailable() = 0; - virtual int ReadPort(UINT8_T *packet, int length) = 0; - virtual int WritePort(UINT8_T *packet, int length) = 0; + virtual int readPort(uint8_t *packet, int length) = 0; + virtual int writePort(uint8_t *packet, int length) = 0; - virtual void SetPacketTimeout(UINT16_T packet_length) = 0; - virtual void SetPacketTimeout(double msec) = 0; - virtual bool IsPacketTimeout() = 0; + virtual void setPacketTimeout(uint16_t packet_length) = 0; + virtual void setPacketTimeout(double msec) = 0; + virtual bool isPacketTimeout() = 0; }; } diff --git a/dynamixel_sdk/include/dynamixel_sdk/protocol1_packet_handler.h b/dynamixel_sdk/include/dynamixel_sdk/protocol1_packet_handler.h index 1932d27..758556f 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/protocol1_packet_handler.h +++ b/dynamixel_sdk/include/dynamixel_sdk/protocol1_packet_handler.h @@ -1,95 +1,126 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* * Redistributions of source code must retain the above copyright notice, this +* list of conditions and the following disclaimer. +* +* * Redistributions in binary form must reproduce the above copyright notice, +* this list of conditions and the following disclaimer in the documentation +* and/or other materials provided with the distribution. +* +* * Neither the name of ROBOTIS nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*******************************************************************************/ + +/* Author: zerom, Leon Ryu Woon Jung */ + /* - * Protocol1PacketHandler.h + * protocol1_packet_handler.h * * Created on: 2016. 1. 26. - * Author: zerom, leon */ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ #define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ -#include "PacketHandler.h" +#include "packet_handler.h" -namespace ROBOTIS +namespace dynamixel { class WINDECLSPEC Protocol1PacketHandler : public PacketHandler { -private: - static Protocol1PacketHandler *unique_instance_; + private: + static Protocol1PacketHandler *unique_instance_; - Protocol1PacketHandler(); + Protocol1PacketHandler(); -public: - static Protocol1PacketHandler *GetInstance() { return unique_instance_; } + public: + static Protocol1PacketHandler *getInstance() { return unique_instance_; } - virtual ~Protocol1PacketHandler() { } + virtual ~Protocol1PacketHandler() { } - float GetProtocolVersion() { return 1.0; } + float getProtocolVersion() { return 1.0; } - void PrintTxRxResult(int result); - void PrintRxPacketError(UINT8_T error); + 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); + 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); - int Ping (PortHandler *port, UINT8_T id, UINT8_T *error = 0); - int Ping (PortHandler *port, UINT8_T id, UINT16_T *model_number, UINT8_T *error = 0); + int ping (PortHandler *port, uint8_t id, uint8_t *error = 0); + int ping (PortHandler *port, uint8_t id, uint16_t *model_number, uint8_t *error = 0); - // BroadcastPing - int BroadcastPing (PortHandler *port, std::vector &id_list); + // broadcastPing + int broadcastPing (PortHandler *port, std::vector &id_list); - int Action (PortHandler *port, UINT8_T id); - int Reboot (PortHandler *port, UINT8_T id, UINT8_T *error = 0); - int FactoryReset (PortHandler *port, UINT8_T id, UINT8_T option, UINT8_T *error = 0); + int action (PortHandler *port, uint8_t id); + int reboot (PortHandler *port, uint8_t id, uint8_t *error = 0); + int factoryReset (PortHandler *port, uint8_t id, uint8_t option, uint8_t *error = 0); - int ReadTx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length); - int ReadRx (PortHandler *port, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); - int ReadTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); + int readTx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length); + int readRx (PortHandler *port, uint16_t length, uint8_t *data, uint8_t *error = 0); + int readTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - int Read1ByteTx (PortHandler *port, UINT8_T id, UINT16_T address); - int Read1ByteRx (PortHandler *port, UINT8_T *data, UINT8_T *error = 0); - int Read1ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T *data, UINT8_T *error = 0); + int read1ByteTx (PortHandler *port, uint8_t id, uint16_t address); + int read1ByteRx (PortHandler *port, uint8_t *data, uint8_t *error = 0); + int read1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error = 0); - int Read2ByteTx (PortHandler *port, UINT8_T id, UINT16_T address); - int Read2ByteRx (PortHandler *port, UINT16_T *data, UINT8_T *error = 0); - int Read2ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T *data, UINT8_T *error = 0); + int read2ByteTx (PortHandler *port, uint8_t id, uint16_t address); + int read2ByteRx (PortHandler *port, uint16_t *data, uint8_t *error = 0); + int read2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error = 0); - int Read4ByteTx (PortHandler *port, UINT8_T id, UINT16_T address); - int Read4ByteRx (PortHandler *port, UINT32_T *data, UINT8_T *error = 0); - int Read4ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T *data, UINT8_T *error = 0); + int read4ByteTx (PortHandler *port, uint8_t id, uint16_t address); + int read4ByteRx (PortHandler *port, uint32_t *data, uint8_t *error = 0); + int read4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error = 0); - int WriteTxOnly (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data); - int WriteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); + int writeTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data); + int writeTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - int Write1ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data); - int Write1ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data, UINT8_T *error = 0); + int write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data); + int write1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error = 0); - int Write2ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data); - int Write2ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data, UINT8_T *error = 0); + int write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data); + int write2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error = 0); - int Write4ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data); - int Write4ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data, UINT8_T *error = 0); + int write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data); + int write4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error = 0); - int RegWriteTxOnly (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data); - int RegWriteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); + int regWriteTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data); + int regWriteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - int SyncReadTx (PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length); - // SyncReadRx -> GroupSyncRead class - // SyncReadTxRx -> GroupSyncRead class + int syncReadTx (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length); + // SyncReadRx -> GroupSyncRead class + // SyncReadTxRx -> GroupSyncRead class - // param : ID1 DATA0 DATA1 ... DATAn ID2 DATA0 DATA1 ... DATAn ID3 DATA0 DATA1 ... DATAn - int SyncWriteTxOnly (PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length); + // param : ID1 DATA0 DATA1 ... DATAn ID2 DATA0 DATA1 ... DATAn ID3 DATA0 DATA1 ... DATAn + int syncWriteTxOnly (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length); - // param : LEN1 ID1 ADDR1 LEN2 ID2 ADDR2 ... - int BulkReadTx (PortHandler *port, UINT8_T *param, UINT16_T param_length); - // BulkReadRx -> GroupBulkRead class - // BulkReadTxRx -> GroupBulkRead class + // param : LEN1 ID1 ADDR1 LEN2 ID2 ADDR2 ... + int bulkReadTx (PortHandler *port, uint8_t *param, uint16_t param_length); + // BulkReadRx -> GroupBulkRead class + // BulkReadTxRx -> GroupBulkRead class - int BulkWriteTxOnly (PortHandler *port, UINT8_T *param, UINT16_T param_length); + int bulkWriteTxOnly (PortHandler *port, uint8_t *param, uint16_t param_length); }; } diff --git a/dynamixel_sdk/include/dynamixel_sdk/protocol2_packet_handler.h b/dynamixel_sdk/include/dynamixel_sdk/protocol2_packet_handler.h index 101413c..ff7e2d1 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/protocol2_packet_handler.h +++ b/dynamixel_sdk/include/dynamixel_sdk/protocol2_packet_handler.h @@ -1,100 +1,131 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* * Redistributions of source code must retain the above copyright notice, this +* list of conditions and the following disclaimer. +* +* * Redistributions in binary form must reproduce the above copyright notice, +* this list of conditions and the following disclaimer in the documentation +* and/or other materials provided with the distribution. +* +* * Neither the name of ROBOTIS nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*******************************************************************************/ + +/* Author: zerom, Leon Ryu Woon Jung */ + /* - * Protocol2PacketHandler.h + * protocol2_packet_handler.h * * Created on: 2016. 1. 26. - * Author: zerom, leon */ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ #define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ -#include "PacketHandler.h" +#include "packet_handler.h" -namespace ROBOTIS +namespace dynamixel { class WINDECLSPEC Protocol2PacketHandler : public PacketHandler { -private: - static Protocol2PacketHandler *unique_instance_; + private: + static Protocol2PacketHandler *unique_instance_; - Protocol2PacketHandler(); + Protocol2PacketHandler(); - UINT16_T UpdateCRC(UINT16_T crc_accum, UINT8_T *data_blk_ptr, UINT16_T data_blk_size); - void AddStuffing(UINT8_T *packet); - void RemoveStuffing(UINT8_T *packet); + uint16_t updateCRC(uint16_t crc_accum, uint8_t *data_blk_ptr, uint16_t data_blk_size); + void addStuffing(uint8_t *packet); + void removeStuffing(uint8_t *packet); -public: - static Protocol2PacketHandler *GetInstance() { return unique_instance_; } + public: + static Protocol2PacketHandler *getInstance() { return unique_instance_; } - virtual ~Protocol2PacketHandler() { } + virtual ~Protocol2PacketHandler() { } - float GetProtocolVersion() { return 2.0; } + float getProtocolVersion() { return 2.0; } - void PrintTxRxResult(int result); - void PrintRxPacketError(UINT8_T error); + 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); + 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); - int Ping (PortHandler *port, UINT8_T id, UINT8_T *error = 0); - int Ping (PortHandler *port, UINT8_T id, UINT16_T *model_number, UINT8_T *error = 0); + int ping (PortHandler *port, uint8_t id, uint8_t *error = 0); + int ping (PortHandler *port, uint8_t id, uint16_t *model_number, uint8_t *error = 0); - // BroadcastPing - int BroadcastPing (PortHandler *port, std::vector &id_list); + // broadcastPing + int broadcastPing (PortHandler *port, std::vector &id_list); - int Action (PortHandler *port, UINT8_T id); - int Reboot (PortHandler *port, UINT8_T id, UINT8_T *error = 0); - int FactoryReset (PortHandler *port, UINT8_T id, UINT8_T option, UINT8_T *error = 0); + int action (PortHandler *port, uint8_t id); + int reboot (PortHandler *port, uint8_t id, uint8_t *error = 0); + int factoryReset (PortHandler *port, uint8_t id, uint8_t option, uint8_t *error = 0); - int ReadTx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length); - int ReadRx (PortHandler *port, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); - int ReadTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); + int readTx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length); + int readRx (PortHandler *port, uint16_t length, uint8_t *data, uint8_t *error = 0); + int readTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - int Read1ByteTx (PortHandler *port, UINT8_T id, UINT16_T address); - int Read1ByteRx (PortHandler *port, UINT8_T *data, UINT8_T *error = 0); - int Read1ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T *data, UINT8_T *error = 0); + int read1ByteTx (PortHandler *port, uint8_t id, uint16_t address); + int read1ByteRx (PortHandler *port, uint8_t *data, uint8_t *error = 0); + int read1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error = 0); - int Read2ByteTx (PortHandler *port, UINT8_T id, UINT16_T address); - int Read2ByteRx (PortHandler *port, UINT16_T *data, UINT8_T *error = 0); - int Read2ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T *data, UINT8_T *error = 0); + int read2ByteTx (PortHandler *port, uint8_t id, uint16_t address); + int read2ByteRx (PortHandler *port, uint16_t *data, uint8_t *error = 0); + int read2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error = 0); - int Read4ByteTx (PortHandler *port, UINT8_T id, UINT16_T address); - int Read4ByteRx (PortHandler *port, UINT32_T *data, UINT8_T *error = 0); - int Read4ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T *data, UINT8_T *error = 0); + int read4ByteTx (PortHandler *port, uint8_t id, uint16_t address); + int read4ByteRx (PortHandler *port, uint32_t *data, uint8_t *error = 0); + int read4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error = 0); - int WriteTxOnly (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data); - int WriteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); + int writeTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data); + int writeTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - int Write1ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data); - int Write1ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data, UINT8_T *error = 0); + int write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data); + int write1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error = 0); - int Write2ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data); - int Write2ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data, UINT8_T *error = 0); + int write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data); + int write2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error = 0); - int Write4ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data); - int Write4ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data, UINT8_T *error = 0); + int write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data); + int write4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error = 0); - int RegWriteTxOnly (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data); - int RegWriteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); + int regWriteTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data); + int regWriteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - int SyncReadTx (PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length); - // SyncReadRx -> GroupSyncRead class - // SyncReadTxRx -> GroupSyncRead class + int syncReadTx (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length); + // SyncReadRx -> GroupSyncRead class + // SyncReadTxRx -> GroupSyncRead class - // param : ID1 DATA0 DATA1 ... DATAn ID2 DATA0 DATA1 ... DATAn ID3 DATA0 DATA1 ... DATAn - int SyncWriteTxOnly (PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length); + // param : ID1 DATA0 DATA1 ... DATAn ID2 DATA0 DATA1 ... DATAn ID3 DATA0 DATA1 ... DATAn + int syncWriteTxOnly (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length); - // param : ID1 ADDR_L1 ADDR_H1 LEN_L1 LEN_H1 ID2 ADDR_L2 ADDR_H2 LEN_L2 LEN_H2 ... - int BulkReadTx (PortHandler *port, UINT8_T *param, UINT16_T param_length); - // BulkReadRx -> GroupBulkRead class - // BulkReadTxRx -> GroupBulkRead class + // param : ID1 ADDR_L1 ADDR_H1 LEN_L1 LEN_H1 ID2 ADDR_L2 ADDR_H2 LEN_L2 LEN_H2 ... + int bulkReadTx (PortHandler *port, uint8_t *param, uint16_t param_length); + // BulkReadRx -> GroupBulkRead class + // BulkReadTxRx -> GroupBulkRead class - // param : ID1 START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H DATA0 DATA1 ... DATAn ID2 START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H DATA0 DATA1 ... DATAn - int BulkWriteTxOnly (PortHandler *port, UINT8_T *param, UINT16_T param_length); + // param : ID1 START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H DATA0 DATA1 ... DATAn ID2 START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H DATA0 DATA1 ... DATAn + int bulkWriteTxOnly (PortHandler *port, uint8_t *param, uint16_t param_length); }; } diff --git a/dynamixel_sdk/include/dynamixel_sdk_linux/port_handler_linux.h b/dynamixel_sdk/include/dynamixel_sdk_linux/port_handler_linux.h index 6ff516e..3a9ed24 100644 --- a/dynamixel_sdk/include/dynamixel_sdk_linux/port_handler_linux.h +++ b/dynamixel_sdk/include/dynamixel_sdk_linux/port_handler_linux.h @@ -1,59 +1,90 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* * Redistributions of source code must retain the above copyright notice, this +* list of conditions and the following disclaimer. +* +* * Redistributions in binary form must reproduce the above copyright notice, +* this list of conditions and the following disclaimer in the documentation +* and/or other materials provided with the distribution. +* +* * Neither the name of ROBOTIS nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*******************************************************************************/ + +/* Author: zerom, Leon Ryu Woon Jung */ + /* - * PortHandlerLinux.h + * port_handler_linux.h * * Created on: 2016. 1. 26. - * Author: zerom, leon */ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ #define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ -#include "dynamixel_sdk/PortHandler.h" +#include "dynamixel_sdk/port_handler.h" -namespace ROBOTIS +namespace dynamixel { class PortHandlerLinux : public PortHandler { -private: - int socket_fd_; - int baudrate_; - char port_name_[30]; + private: + int socket_fd_; + int baudrate_; + char port_name_[30]; - double packet_start_time_; - double packet_timeout_; - double tx_time_per_byte; + double packet_start_time_; + double packet_timeout_; + double tx_time_per_byte; - bool SetupPort(const int cflag_baud); - bool SetCustomBaudrate(int speed); - int GetCFlagBaud(const int baudrate); + bool setupPort(const int cflag_baud); + bool setCustomBaudrate(int speed); + int getCFlagBaud(const int baudrate); - double GetCurrentTime(); - double GetTimeSinceStart(); + double getCurrentTime(); + double getTimeSinceStart(); -public: - PortHandlerLinux(const char *port_name); - virtual ~PortHandlerLinux() { ClosePort(); } + public: + PortHandlerLinux(const char *port_name); + virtual ~PortHandlerLinux() { closePort(); } - bool OpenPort(); - void ClosePort(); - void ClearPort(); + bool openPort(); + void closePort(); + void clearPort(); - void SetPortName(const char *port_name); - char *GetPortName(); + void setPortName(const char *port_name); + char *getPortName(); - bool SetBaudRate(const int baudrate); - int GetBaudRate(); + bool setBaudRate(const int baudrate); + int getBaudRate(); - int GetBytesAvailable(); + int getBytesAvailable(); - int ReadPort(UINT8_T *packet, int length); - int WritePort(UINT8_T *packet, int length); + int readPort(uint8_t *packet, int length); + int writePort(uint8_t *packet, int length); - void SetPacketTimeout(UINT16_T packet_length); - void SetPacketTimeout(double msec); - bool IsPacketTimeout(); + void setPacketTimeout(uint16_t packet_length); + void setPacketTimeout(double msec); + bool isPacketTimeout(); }; } diff --git a/dynamixel_sdk/src/dynamixel_sdk/group_bulk_read.cpp b/dynamixel_sdk/src/dynamixel_sdk/group_bulk_read.cpp index f248ee8..7ae113b 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/group_bulk_read.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/group_bulk_read.cpp @@ -1,8 +1,39 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* * Redistributions of source code must retain the above copyright notice, this +* list of conditions and the following disclaimer. +* +* * Redistributions in binary form must reproduce the above copyright notice, +* this list of conditions and the following disclaimer in the documentation +* and/or other materials provided with the distribution. +* +* * Neither the name of ROBOTIS nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*******************************************************************************/ + +/* Author: zerom, Leon Ryu Woon Jung */ + /* - * GroupBulkRead.cpp + * group_bulk_read.cpp * * Created on: 2016. 1. 28. - * Author: zerom, leon */ #if defined(_WIN32) || defined(_WIN64) #define WINDLLEXPORT @@ -10,190 +41,197 @@ #include #include -#include "dynamixel_sdk/GroupBulkRead.h" +#include "dynamixel_sdk/group_bulk_read.h" -using namespace ROBOTIS; +using namespace dynamixel; GroupBulkRead::GroupBulkRead(PortHandler *port, PacketHandler *ph) - : port_(port), - ph_(ph), - last_result_(false), - is_param_changed_(false), - param_(0) + : port_(port), + ph_(ph), + last_result_(false), + is_param_changed_(false), + param_(0) { - ClearParam(); + clearParam(); } -void GroupBulkRead::MakeParam() +void GroupBulkRead::makeParam() { - if(id_list_.size() == 0) - return; - - if(param_ != 0) - delete[] param_; - param_ = 0; - - if(ph_->GetProtocolVersion() == 1.0) - param_ = new UINT8_T[id_list_.size() * 3]; // ID(1) + ADDR(1) + LENGTH(1) + if (id_list_.size() == 0) + return; + + if (param_ != 0) + delete[] param_; + param_ = 0; + + if (ph_->getProtocolVersion() == 1.0) + { + param_ = new uint8_t[id_list_.size() * 3]; // ID(1) + ADDR(1) + LENGTH(1) + } + else // 2.0 + { + param_ = new uint8_t[id_list_.size() * 5]; // ID(1) + ADDR(2) + LENGTH(2) + } + + int idx = 0; + for (unsigned int i = 0; i < id_list_.size(); i++) + { + uint8_t id = id_list_[i]; + if (ph_->getProtocolVersion() == 1.0) + { + param_[idx++] = (uint8_t)length_list_[id]; // LEN + param_[idx++] = id; // ID + param_[idx++] = (uint8_t)address_list_[id]; // ADDR + } else // 2.0 - param_ = new UINT8_T[id_list_.size() * 5]; // ID(1) + ADDR(2) + LENGTH(2) - - int _idx = 0; - for(unsigned int _i = 0; _i < id_list_.size(); _i++) { - UINT8_T _id = id_list_[_i]; - if(ph_->GetProtocolVersion() == 1.0) - { - param_[_idx++] = (UINT8_T)length_list_[_id]; // LEN - param_[_idx++] = _id; // ID - param_[_idx++] = (UINT8_T)address_list_[_id]; // ADDR - } - else // 2.0 - { - param_[_idx++] = _id; // ID - param_[_idx++] = DXL_LOBYTE(address_list_[_id]); // ADDR_L - param_[_idx++] = DXL_HIBYTE(address_list_[_id]); // ADDR_H - param_[_idx++] = DXL_LOBYTE(length_list_[_id]); // LEN_L - param_[_idx++] = DXL_HIBYTE(length_list_[_id]); // LEN_H - } + param_[idx++] = id; // ID + param_[idx++] = DXL_LOBYTE(address_list_[id]); // ADDR_L + param_[idx++] = DXL_HIBYTE(address_list_[id]); // ADDR_H + param_[idx++] = DXL_LOBYTE(length_list_[id]); // LEN_L + param_[idx++] = DXL_HIBYTE(length_list_[id]); // LEN_H } + } } -bool GroupBulkRead::AddParam(UINT8_T id, UINT16_T start_address, UINT16_T data_length) +bool GroupBulkRead::addParam(uint8_t id, uint16_t start_address, uint16_t data_length) { - if(std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist - return false; + if (std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist + return false; - id_list_.push_back(id); - length_list_[id] = data_length; - address_list_[id] = start_address; - data_list_[id] = new UINT8_T[data_length]; + id_list_.push_back(id); + length_list_[id] = data_length; + address_list_[id] = start_address; + data_list_[id] = new uint8_t[data_length]; - is_param_changed_ = true; - return true; + is_param_changed_ = true; + return true; } -void GroupBulkRead::RemoveParam(UINT8_T id) +void GroupBulkRead::removeParam(uint8_t id) { - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if(it == id_list_.end()) // NOT exist - return; + std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); + if (it == id_list_.end()) // NOT exist + return; - id_list_.erase(it); - address_list_.erase(id); - length_list_.erase(id); - delete[] data_list_[id]; - data_list_.erase(id); + id_list_.erase(it); + address_list_.erase(id); + length_list_.erase(id); + delete[] data_list_[id]; + data_list_.erase(id); - is_param_changed_ = true; + is_param_changed_ = true; } -void GroupBulkRead::ClearParam() +void GroupBulkRead::clearParam() { - if(id_list_.size() == 0) - return; - - for(unsigned int _i = 0; _i < id_list_.size(); _i++) - delete[] data_list_[id_list_[_i]]; - - id_list_.clear(); - address_list_.clear(); - length_list_.clear(); - data_list_.clear(); - if(param_ != 0) - delete[] param_; - param_ = 0; + if (id_list_.size() == 0) + return; + + for (unsigned int i = 0; i < id_list_.size(); i++) + delete[] data_list_[id_list_[i]]; + + id_list_.clear(); + address_list_.clear(); + length_list_.clear(); + data_list_.clear(); + if (param_ != 0) + delete[] param_; + param_ = 0; } -int GroupBulkRead::TxPacket() +int GroupBulkRead::txPacket() { - if(id_list_.size() == 0) - return COMM_NOT_AVAILABLE; - - if(is_param_changed_ == true) - MakeParam(); - - if(ph_->GetProtocolVersion() == 1.0) - return ph_->BulkReadTx(port_, param_, id_list_.size() * 3); - else // 2.0 - return ph_->BulkReadTx(port_, param_, id_list_.size() * 5); + if (id_list_.size() == 0) + return COMM_NOT_AVAILABLE; + + if (is_param_changed_ == true) + makeParam(); + + if (ph_->getProtocolVersion() == 1.0) + { + return ph_->bulkReadTx(port_, param_, id_list_.size() * 3); + } + else // 2.0 + { + return ph_->bulkReadTx(port_, param_, id_list_.size() * 5); + } } -int GroupBulkRead::RxPacket() +int GroupBulkRead::rxPacket() { - int _cnt = id_list_.size(); - int _result = COMM_RX_FAIL; + int cnt = id_list_.size(); + int result = COMM_RX_FAIL; - last_result_ = false; + last_result_ = false; - if(_cnt == 0) - return COMM_NOT_AVAILABLE; + if (cnt == 0) + return COMM_NOT_AVAILABLE; - for(int _i = 0; _i < _cnt; _i++) + for (int i = 0; i < cnt; i++) + { + uint8_t id = id_list_[i]; + + result = ph_->readRx(port_, length_list_[id], data_list_[id]); + if (result != COMM_SUCCESS) { - UINT8_T _id = id_list_[_i]; - - _result = ph_->ReadRx(port_, length_list_[_id], data_list_[_id]); - if(_result != COMM_SUCCESS) - { - fprintf(stderr, "[GroupBulkRead::RxPacket] ID %d result : %d !!!!!!!!!!\n", _id, _result); - return _result; - } + fprintf(stderr, "[GroupBulkRead::rxPacket] ID %d result : %d !!!!!!!!!!\n", id, result); + return result; } + } - if(_result == COMM_SUCCESS) - last_result_ = true; + if (result == COMM_SUCCESS) + last_result_ = true; - return _result; + return result; } -int GroupBulkRead::TxRxPacket() +int GroupBulkRead::txRxPacket() { - int _result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - _result = TxPacket(); - if(_result != COMM_SUCCESS) - return _result; + result = txPacket(); + if (result != COMM_SUCCESS) + return result; - return RxPacket(); + return rxPacket(); } -bool GroupBulkRead::IsAvailable(UINT8_T id, UINT16_T address, UINT16_T data_length) +bool GroupBulkRead::isAvailable(uint8_t id, uint16_t address, uint16_t data_length) { - UINT16_T _start_addr, _data_length; + uint16_t start_addr; - if(last_result_ == false || data_list_.find(id) == data_list_.end()) - return false; + if (last_result_ == false || data_list_.find(id) == data_list_.end()) + return false; - _start_addr = address_list_[id]; - _data_length = length_list_[id]; + start_addr = address_list_[id]; - if(address < _start_addr || _start_addr + _data_length - data_length < address) - return false; + if (address < start_addr || start_addr + length_list_[id] - data_length < address) + return false; - return true; + return true; } -UINT32_T GroupBulkRead::GetData(UINT8_T id, UINT16_T address, UINT16_T data_length) +uint32_t GroupBulkRead::getData(uint8_t id, uint16_t address, uint16_t data_length) { - if(IsAvailable(id, address, data_length) == false) - return 0; + if (isAvailable(id, address, data_length) == false) + return 0; - UINT16_T _start_addr = address_list_[id]; + uint16_t start_addr = address_list_[id]; - switch(data_length) - { + switch(data_length) + { case 1: - return data_list_[id][address - _start_addr]; + return data_list_[id][address - start_addr]; case 2: - return DXL_MAKEWORD(data_list_[id][address - _start_addr], data_list_[id][address - _start_addr + 1]); + return DXL_MAKEWORD(data_list_[id][address - start_addr], data_list_[id][address - start_addr + 1]); case 4: - return DXL_MAKEDWORD(DXL_MAKEWORD(data_list_[id][address - _start_addr + 0], data_list_[id][address - _start_addr + 1]), - DXL_MAKEWORD(data_list_[id][address - _start_addr + 2], data_list_[id][address - _start_addr + 3])); + return DXL_MAKEDWORD(DXL_MAKEWORD(data_list_[id][address - start_addr + 0], data_list_[id][address - start_addr + 1]), + DXL_MAKEWORD(data_list_[id][address - start_addr + 2], data_list_[id][address - start_addr + 3])); default: - return 0; - } + return 0; + } } diff --git a/dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp b/dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp index 973793b..7b84151 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp @@ -1,137 +1,168 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* * Redistributions of source code must retain the above copyright notice, this +* list of conditions and the following disclaimer. +* +* * Redistributions in binary form must reproduce the above copyright notice, +* this list of conditions and the following disclaimer in the documentation +* and/or other materials provided with the distribution. +* +* * Neither the name of ROBOTIS nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*******************************************************************************/ + +/* Author: zerom, Leon Ryu Woon Jung */ + /* - * GroupBulkWrite.cpp + * group_bulk_write.cpp * * Created on: 2016. 2. 2. - * Author: zerom, leon */ #if defined(_WIN32) || defined(_WIN64) #define WINDLLEXPORT #endif #include -#include "dynamixel_sdk/GroupBulkWrite.h" +#include "dynamixel_sdk/group_bulk_write.h" -using namespace ROBOTIS; +using namespace dynamixel; GroupBulkWrite::GroupBulkWrite(PortHandler *port, PacketHandler *ph) - : port_(port), - ph_(ph), - is_param_changed_(false), - param_(0), - param_length_(0) + : port_(port), + ph_(ph), + is_param_changed_(false), + param_(0), + param_length_(0) { - ClearParam(); + clearParam(); } -void GroupBulkWrite::MakeParam() +void GroupBulkWrite::makeParam() { - if(ph_->GetProtocolVersion() == 1.0 || id_list_.size() == 0) - return; - - if(param_ != 0) - delete[] param_; - param_ = 0; - - param_length_ = 0; - for(unsigned int _i = 0; _i < id_list_.size(); _i++) - param_length_ += 1 + 2 + 2 + length_list_[id_list_[_i]]; - - param_ = new UINT8_T[param_length_]; - - int _idx = 0; - for(unsigned int _i = 0; _i < id_list_.size(); _i++) - { - UINT8_T _id = id_list_[_i]; - if(data_list_[_id] == 0) - return; - - param_[_idx++] = _id; - param_[_idx++] = DXL_LOBYTE(address_list_[_id]); - param_[_idx++] = DXL_HIBYTE(address_list_[_id]); - param_[_idx++] = DXL_LOBYTE(length_list_[_id]); - param_[_idx++] = DXL_HIBYTE(length_list_[_id]); - for(int _c = 0; _c < length_list_[_id]; _c++) - param_[_idx++] = (data_list_[_id])[_c]; - } + if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) + return; + + if (param_ != 0) + delete[] param_; + param_ = 0; + + param_length_ = 0; + for (unsigned int i = 0; i < id_list_.size(); i++) + param_length_ += 1 + 2 + 2 + length_list_[id_list_[i]]; + + param_ = new uint8_t[param_length_]; + + int idx = 0; + for (unsigned int i = 0; i < id_list_.size(); i++) + { + uint8_t id = id_list_[i]; + if (data_list_[id] == 0) + return; + + param_[idx++] = id; + param_[idx++] = DXL_LOBYTE(address_list_[id]); + param_[idx++] = DXL_HIBYTE(address_list_[id]); + param_[idx++] = DXL_LOBYTE(length_list_[id]); + param_[idx++] = DXL_HIBYTE(length_list_[id]); + for (int c = 0; c < length_list_[id]; c++) + param_[idx++] = (data_list_[id])[c]; + } } -bool GroupBulkWrite::AddParam(UINT8_T id, UINT16_T start_address, UINT16_T data_length, UINT8_T *data) +bool GroupBulkWrite::addParam(uint8_t id, uint16_t start_address, uint16_t data_length, uint8_t *data) { - if(ph_->GetProtocolVersion() == 1.0) - return false; + if (ph_->getProtocolVersion() == 1.0) + return false; - if(std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist - return false; + if (std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist + return false; - id_list_.push_back(id); - address_list_[id] = start_address; - length_list_[id] = data_length; - data_list_[id] = new UINT8_T[data_length]; - for(int _c = 0; _c < data_length; _c++) - data_list_[id][_c] = data[_c]; + id_list_.push_back(id); + address_list_[id] = start_address; + length_list_[id] = data_length; + data_list_[id] = new uint8_t[data_length]; + for (int c = 0; c < data_length; c++) + data_list_[id][c] = data[c]; - is_param_changed_ = true; - return true; + is_param_changed_ = true; + return true; } -void GroupBulkWrite::RemoveParam(UINT8_T id) +void GroupBulkWrite::removeParam(uint8_t id) { - if(ph_->GetProtocolVersion() == 1.0) - return; + if (ph_->getProtocolVersion() == 1.0) + return; - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if(it == id_list_.end()) // NOT exist - return; + std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); + if (it == id_list_.end()) // NOT exist + return; - id_list_.erase(it); - address_list_.erase(id); - length_list_.erase(id); - delete[] data_list_[id]; - data_list_.erase(id); + id_list_.erase(it); + address_list_.erase(id); + length_list_.erase(id); + delete[] data_list_[id]; + data_list_.erase(id); - is_param_changed_ = true; + is_param_changed_ = true; } -bool GroupBulkWrite::ChangeParam(UINT8_T id, UINT16_T start_address, UINT16_T data_length, UINT8_T *data) +bool GroupBulkWrite::changeParam(uint8_t id, uint16_t start_address, uint16_t data_length, uint8_t *data) { - if(ph_->GetProtocolVersion() == 1.0) - return false; - - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if(it == id_list_.end()) // NOT exist - return false; - - address_list_[id] = start_address; - length_list_[id] = data_length; - delete[] data_list_[id]; - data_list_[id] = new UINT8_T[data_length]; - for(int _c = 0; _c < data_length; _c++) - data_list_[id][_c] = data[_c]; - - is_param_changed_ = true; - return true; + if (ph_->getProtocolVersion() == 1.0) + return false; + + std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); + if (it == id_list_.end()) // NOT exist + return false; + + address_list_[id] = start_address; + length_list_[id] = data_length; + delete[] data_list_[id]; + data_list_[id] = new uint8_t[data_length]; + for (int c = 0; c < data_length; c++) + data_list_[id][c] = data[c]; + + is_param_changed_ = true; + return true; } -void GroupBulkWrite::ClearParam() +void GroupBulkWrite::clearParam() { - if(ph_->GetProtocolVersion() == 1.0 || id_list_.size() == 0) - return; - - for(unsigned int _i = 0; _i < id_list_.size(); _i++) - delete[] data_list_[id_list_[_i]]; - - id_list_.clear(); - address_list_.clear(); - length_list_.clear(); - data_list_.clear(); - if(param_ != 0) - delete[] param_; - param_ = 0; + if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) + return; + + for (unsigned int i = 0; i < id_list_.size(); i++) + delete[] data_list_[id_list_[i]]; + + id_list_.clear(); + address_list_.clear(); + length_list_.clear(); + data_list_.clear(); + if (param_ != 0) + delete[] param_; + param_ = 0; } -int GroupBulkWrite::TxPacket() +int GroupBulkWrite::txPacket() { - if(ph_->GetProtocolVersion() == 1.0 || id_list_.size() == 0) - return COMM_NOT_AVAILABLE; + if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) + return COMM_NOT_AVAILABLE; - if(is_param_changed_ == true) - MakeParam(); + if (is_param_changed_ == true) + makeParam(); - return ph_->BulkWriteTxOnly(port_, param_, param_length_); + return ph_->bulkWriteTxOnly(port_, param_, param_length_); } diff --git a/dynamixel_sdk/src/dynamixel_sdk/group_sync_read.cpp b/dynamixel_sdk/src/dynamixel_sdk/group_sync_read.cpp index fede283..13818cc 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/group_sync_read.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/group_sync_read.cpp @@ -1,172 +1,203 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* * Redistributions of source code must retain the above copyright notice, this +* list of conditions and the following disclaimer. +* +* * Redistributions in binary form must reproduce the above copyright notice, +* this list of conditions and the following disclaimer in the documentation +* and/or other materials provided with the distribution. +* +* * Neither the name of ROBOTIS nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*******************************************************************************/ + +/* Author: zerom, Leon Ryu Woon Jung */ + /* - * GroupSyncRead.cpp + * group_sync_read.cpp * * Created on: 2016. 2. 2. - * Author: zerom, leon */ #if defined(_WIN32) || defined(_WIN64) #define WINDLLEXPORT #endif #include -#include "dynamixel_sdk/GroupSyncRead.h" - -using namespace ROBOTIS; - -GroupSyncRead::GroupSyncRead(PortHandler *port, PacketHandler *ph, UINT16_T start_address, UINT16_T data_length) - : port_(port), - ph_(ph), - last_result_(false), - is_param_changed_(false), - param_(0), - start_address_(start_address), - data_length_(data_length) +#include "dynamixel_sdk/group_sync_read.h" + +using namespace dynamixel; + +GroupSyncRead::GroupSyncRead(PortHandler *port, PacketHandler *ph, uint16_t start_address, uint16_t data_length) + : port_(port), + ph_(ph), + last_result_(false), + is_param_changed_(false), + param_(0), + start_address_(start_address), + data_length_(data_length) { - ClearParam(); + clearParam(); } -void GroupSyncRead::MakeParam() +void GroupSyncRead::makeParam() { - if(ph_->GetProtocolVersion() == 1.0 || id_list_.size() == 0) - return; + if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) + return; - if(param_ != 0) - delete[] param_; - param_ = 0; + if (param_ != 0) + delete[] param_; + param_ = 0; - param_ = new UINT8_T[id_list_.size() * 1]; // ID(1) + param_ = new uint8_t[id_list_.size() * 1]; // ID(1) - int _idx = 0; - for(unsigned int _i = 0; _i < id_list_.size(); _i++) - param_[_idx++] = id_list_[_i]; + int idx = 0; + for (unsigned int i = 0; i < id_list_.size(); i++) + param_[idx++] = id_list_[i]; } -bool GroupSyncRead::AddParam(UINT8_T id) +bool GroupSyncRead::addParam(uint8_t id) { - if(ph_->GetProtocolVersion() == 1.0) - return false; + if (ph_->getProtocolVersion() == 1.0) + return false; - if(std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist - return false; + if (std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist + return false; - id_list_.push_back(id); - data_list_[id] = new UINT8_T[data_length_]; + id_list_.push_back(id); + data_list_[id] = new uint8_t[data_length_]; - is_param_changed_ = true; - return true; + is_param_changed_ = true; + return true; } -void GroupSyncRead::RemoveParam(UINT8_T id) +void GroupSyncRead::removeParam(uint8_t id) { - if(ph_->GetProtocolVersion() == 1.0) - return; + if (ph_->getProtocolVersion() == 1.0) + return; - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if(it == id_list_.end()) // NOT exist - return; + std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); + if (it == id_list_.end()) // NOT exist + return; - id_list_.erase(it); - delete[] data_list_[id]; - data_list_.erase(id); + id_list_.erase(it); + delete[] data_list_[id]; + data_list_.erase(id); - is_param_changed_ = true; + is_param_changed_ = true; } -void GroupSyncRead::ClearParam() +void GroupSyncRead::clearParam() { - if(ph_->GetProtocolVersion() == 1.0 || id_list_.size() == 0) - return; + if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) + return; - for(unsigned int _i = 0; _i < id_list_.size(); _i++) - delete[] data_list_[id_list_[_i]]; + for (unsigned int i = 0; i < id_list_.size(); i++) + delete[] data_list_[id_list_[i]]; - id_list_.clear(); - data_list_.clear(); - if(param_ != 0) - delete[] param_; - param_ = 0; + id_list_.clear(); + data_list_.clear(); + if (param_ != 0) + delete[] param_; + param_ = 0; } -int GroupSyncRead::TxPacket() +int GroupSyncRead::txPacket() { - if(ph_->GetProtocolVersion() == 1.0 || id_list_.size() == 0) - return COMM_NOT_AVAILABLE; + if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) + return COMM_NOT_AVAILABLE; - if(is_param_changed_ == true) - MakeParam(); + if (is_param_changed_ == true) + makeParam(); - return ph_->SyncReadTx(port_, start_address_, data_length_, param_, (UINT16_T)id_list_.size() * 1); + return ph_->syncReadTx(port_, start_address_, data_length_, param_, (uint16_t)id_list_.size() * 1); } -int GroupSyncRead::RxPacket() +int GroupSyncRead::rxPacket() { - last_result_ = false; + last_result_ = false; - if(ph_->GetProtocolVersion() == 1.0) - return COMM_NOT_AVAILABLE; + if (ph_->getProtocolVersion() == 1.0) + return COMM_NOT_AVAILABLE; - int _cnt = id_list_.size(); - int _result = COMM_RX_FAIL; + int cnt = id_list_.size(); + int result = COMM_RX_FAIL; - if(_cnt == 0) - return COMM_NOT_AVAILABLE; + if (cnt == 0) + return COMM_NOT_AVAILABLE; - for(int _i = 0; _i < _cnt; _i++) - { - UINT8_T _id = id_list_[_i]; + for (int i = 0; i < cnt; i++) + { + uint8_t id = id_list_[i]; - _result = ph_->ReadRx(port_, data_length_, data_list_[_id]); - if(_result != COMM_SUCCESS) - return _result; - } + result = ph_->readRx(port_, data_length_, data_list_[id]); + if (result != COMM_SUCCESS) + return result; + } - if(_result == COMM_SUCCESS) - last_result_ = true; + if (result == COMM_SUCCESS) + last_result_ = true; - return _result; + return result; } -int GroupSyncRead::TxRxPacket() +int GroupSyncRead::txRxPacket() { - if(ph_->GetProtocolVersion() == 1.0) - return COMM_NOT_AVAILABLE; + if (ph_->getProtocolVersion() == 1.0) + return COMM_NOT_AVAILABLE; - int _result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - _result = TxPacket(); - if(_result != COMM_SUCCESS) - return _result; + result = txPacket(); + if (result != COMM_SUCCESS) + return result; - return RxPacket(); + return rxPacket(); } -bool GroupSyncRead::IsAvailable(UINT8_T id, UINT16_T address, UINT16_T data_length) +bool GroupSyncRead::isAvailable(uint8_t id, uint16_t address, uint16_t data_length) { - if(ph_->GetProtocolVersion() == 1.0 || last_result_ == false || data_list_.find(id) == data_list_.end()) - return false; + if (ph_->getProtocolVersion() == 1.0 || last_result_ == false || data_list_.find(id) == data_list_.end()) + return false; - if(address < start_address_ || start_address_ + data_length_ - data_length < address) - return false; + if (address < start_address_ || start_address_ + data_length_ - data_length < address) + return false; - return true; + return true; } -UINT32_T GroupSyncRead::GetData(UINT8_T id, UINT16_T address, UINT16_T data_length) +uint32_t GroupSyncRead::getData(uint8_t id, uint16_t address, uint16_t data_length) { - if(IsAvailable(id, address, data_length) == false) - return 0; + if (isAvailable(id, address, data_length) == false) + return 0; - switch(data_length) - { + switch(data_length) + { case 1: - return data_list_[id][address - start_address_]; + return data_list_[id][address - start_address_]; case 2: - return DXL_MAKEWORD(data_list_[id][address - start_address_], data_list_[id][address - start_address_ + 1]); + return DXL_MAKEWORD(data_list_[id][address - start_address_], data_list_[id][address - start_address_ + 1]); case 4: - return DXL_MAKEDWORD(DXL_MAKEWORD(data_list_[id][address - start_address_ + 0], data_list_[id][address - start_address_ + 1]), - DXL_MAKEWORD(data_list_[id][address - start_address_ + 2], data_list_[id][address - start_address_ + 3])); + return DXL_MAKEDWORD(DXL_MAKEWORD(data_list_[id][address - start_address_ + 0], data_list_[id][address - start_address_ + 1]), + DXL_MAKEWORD(data_list_[id][address - start_address_ + 2], data_list_[id][address - start_address_ + 3])); default: - return 0; - } + return 0; + } } diff --git a/dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp b/dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp index 8a61435..94dea6f 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp @@ -1,117 +1,147 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* * Redistributions of source code must retain the above copyright notice, this +* list of conditions and the following disclaimer. +* +* * Redistributions in binary form must reproduce the above copyright notice, +* this list of conditions and the following disclaimer in the documentation +* and/or other materials provided with the distribution. +* +* * Neither the name of ROBOTIS nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*******************************************************************************/ + +/* Author: zerom, Leon Ryu Woon Jung */ + /* - * GroupSyncWrite.cpp + * group_sync_write.cpp * * Created on: 2016. 1. 28. - * Author: zerom, leon */ #if defined(_WIN32) || defined(_WIN64) #define WINDLLEXPORT #endif #include -#include "dynamixel_sdk/GroupSyncWrite.h" +#include "dynamixel_sdk/group_sync_write.h" -using namespace ROBOTIS; +using namespace dynamixel; -GroupSyncWrite::GroupSyncWrite(PortHandler *port, PacketHandler *ph, UINT16_T start_address, UINT16_T data_length) - : port_(port), - ph_(ph), - is_param_changed_(false), - param_(0), - start_address_(start_address), - data_length_(data_length) +GroupSyncWrite::GroupSyncWrite(PortHandler *port, PacketHandler *ph, uint16_t start_address, uint16_t data_length) + : port_(port), + ph_(ph), + is_param_changed_(false), + param_(0), + start_address_(start_address), + data_length_(data_length) { - ClearParam(); + clearParam(); } -void GroupSyncWrite::MakeParam() +void GroupSyncWrite::makeParam() { - if(id_list_.size() == 0) - return; - - if(param_ != 0) - delete[] param_; - param_ = 0; - - param_ = new UINT8_T[id_list_.size() * (1 + data_length_)]; // ID(1) + DATA(data_length) - - int _idx = 0; - for(unsigned int _i = 0; _i < id_list_.size(); _i++) - { - UINT8_T _id = id_list_[_i]; - if(data_list_[_id] == 0) - return; - - param_[_idx++] = _id; - for(int _c = 0; _c < data_length_; _c++) - param_[_idx++] = (data_list_[_id])[_c]; - } + if (id_list_.size() == 0) return; + + if (param_ != 0) + delete[] param_; + param_ = 0; + + param_ = new uint8_t[id_list_.size() * (1 + data_length_)]; // ID(1) + DATA(data_length) + + int idx = 0; + for (unsigned int i = 0; i < id_list_.size(); i++) + { + uint8_t id = id_list_[i]; + if (data_list_[id] == 0) + return; + + param_[idx++] = id; + for (int c = 0; c < data_length_; c++) + param_[idx++] = (data_list_[id])[c]; + } } -bool GroupSyncWrite::AddParam(UINT8_T id, UINT8_T *data) +bool GroupSyncWrite::addParam(uint8_t id, uint8_t *data) { - if(std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist - return false; + if (std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist + return false; - id_list_.push_back(id); - data_list_[id] = new UINT8_T[data_length_]; - for(int _c = 0; _c < data_length_; _c++) - data_list_[id][_c] = data[_c]; + id_list_.push_back(id); + data_list_[id] = new uint8_t[data_length_]; + for (int c = 0; c < data_length_; c++) + data_list_[id][c] = data[c]; - is_param_changed_ = true; - return true; + is_param_changed_ = true; + return true; } -void GroupSyncWrite::RemoveParam(UINT8_T id) +void GroupSyncWrite::removeParam(uint8_t id) { - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if(it == id_list_.end()) // NOT exist - return; + std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); + if (it == id_list_.end()) // NOT exist + return; - id_list_.erase(it); - delete[] data_list_[id]; - data_list_.erase(id); + id_list_.erase(it); + delete[] data_list_[id]; + data_list_.erase(id); - is_param_changed_ = true; + is_param_changed_ = true; } -bool GroupSyncWrite::ChangeParam(UINT8_T id, UINT8_T *data) +bool GroupSyncWrite::changeParam(uint8_t id, uint8_t *data) { - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if(it == id_list_.end()) // NOT exist - return false; + std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); + if (it == id_list_.end()) // NOT exist + return false; - delete[] data_list_[id]; - data_list_[id] = new UINT8_T[data_length_]; - for(int _c = 0; _c < data_length_; _c++) - data_list_[id][_c] = data[_c]; + delete[] data_list_[id]; + data_list_[id] = new uint8_t[data_length_]; + for (int c = 0; c < data_length_; c++) + data_list_[id][c] = data[c]; - is_param_changed_ = true; - return true; + is_param_changed_ = true; + return true; } -void GroupSyncWrite::ClearParam() +void GroupSyncWrite::clearParam() { - if(id_list_.size() == 0) - return; + if (id_list_.size() == 0) + return; - for(unsigned int _i = 0; _i < id_list_.size(); _i++) - delete[] data_list_[id_list_[_i]]; + for (unsigned int i = 0; i < id_list_.size(); i++) + delete[] data_list_[id_list_[i]]; - id_list_.clear(); - data_list_.clear(); - if(param_ != 0) - delete[] param_; - param_ = 0; + id_list_.clear(); + data_list_.clear(); + if (param_ != 0) + delete[] param_; + param_ = 0; } -int GroupSyncWrite::TxPacket() +int GroupSyncWrite::txPacket() { - if(id_list_.size() == 0) - return COMM_NOT_AVAILABLE; + if (id_list_.size() == 0) + return COMM_NOT_AVAILABLE; - if(is_param_changed_ == true) - MakeParam(); + if (is_param_changed_ == true) + makeParam(); - return ph_->SyncWriteTxOnly(port_, start_address_, data_length_, param_, id_list_.size() * (1 + data_length_)); + return ph_->syncWriteTxOnly(port_, start_address_, data_length_, param_, id_list_.size() * (1 + data_length_)); } diff --git a/dynamixel_sdk/src/dynamixel_sdk/packet_handler.cpp b/dynamixel_sdk/src/dynamixel_sdk/packet_handler.cpp index 8ad1c48..04fdc20 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/packet_handler.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/packet_handler.cpp @@ -1,25 +1,60 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* * Redistributions of source code must retain the above copyright notice, this +* list of conditions and the following disclaimer. +* +* * Redistributions in binary form must reproduce the above copyright notice, +* this list of conditions and the following disclaimer in the documentation +* and/or other materials provided with the distribution. +* +* * Neither the name of ROBOTIS nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*******************************************************************************/ + +/* Author: zerom, Leon Ryu Woon Jung */ + /* - * PacketHandler.cpp + * packet_handler.cpp * * Created on: 2016. 1. 26. - * Author: zerom, leon */ #if defined(_WIN32) || defined(_WIN64) #define WINDLLEXPORT #endif -#include "dynamixel_sdk/PacketHandler.h" -#include "dynamixel_sdk/Protocol1PacketHandler.h" -#include "dynamixel_sdk/Protocol2PacketHandler.h" +#include "dynamixel_sdk/packet_handler.h" +#include "dynamixel_sdk/protocol1_packet_handler.h" +#include "dynamixel_sdk/protocol2_packet_handler.h" -using namespace ROBOTIS; +using namespace dynamixel; -PacketHandler *PacketHandler::GetPacketHandler(float protocol_version) +PacketHandler *PacketHandler::getPacketHandler(float protocol_version) { - if(protocol_version == 1.0) - return (PacketHandler *)(Protocol1PacketHandler::GetInstance()); - else if(protocol_version == 2.0) - return (PacketHandler *)(Protocol2PacketHandler::GetInstance()); + if (protocol_version == 1.0) + { + return (PacketHandler *)(Protocol1PacketHandler::getInstance()); + } + else if (protocol_version == 2.0) + { + return (PacketHandler *)(Protocol2PacketHandler::getInstance()); + } - return (PacketHandler *)(Protocol2PacketHandler::GetInstance()); + return (PacketHandler *)(Protocol2PacketHandler::getInstance()); } diff --git a/dynamixel_sdk/src/dynamixel_sdk/port_handler.cpp b/dynamixel_sdk/src/dynamixel_sdk/port_handler.cpp index ff4355c..14c0c28 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/port_handler.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/port_handler.cpp @@ -1,32 +1,63 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* * Redistributions of source code must retain the above copyright notice, this +* list of conditions and the following disclaimer. +* +* * Redistributions in binary form must reproduce the above copyright notice, +* this list of conditions and the following disclaimer in the documentation +* and/or other materials provided with the distribution. +* +* * Neither the name of ROBOTIS nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*******************************************************************************/ + +/* Author: zerom, Leon Ryu Woon Jung */ + /* - * PortHandler.cpp + * port_handler.cpp * * Created on: 2016. 2. 5. - * Author: zerom, leon */ #if defined(_WIN32) || defined(_WIN64) #define WINDLLEXPORT #endif -#include "dynamixel_sdk/PortHandler.h" +#include "dynamixel_sdk/port_handler.h" #ifdef __linux__ - #include "dynamixel_sdk_linux/PortHandlerLinux.h" + #include "dynamixel_sdk_linux/port_handler_linux.h" #endif #if defined(_WIN32) || defined(_WIN64) - #include "dynamixel_sdk_windows/PortHandlerWindows.h" + #include "dynamixel_sdk_windows/port_handler_windows.h" #endif -using namespace ROBOTIS; +using namespace dynamixel; -PortHandler *PortHandler::GetPortHandler(const char *port_name) +PortHandler *PortHandler::getPortHandler(const char *port_name) { #ifdef __linux__ - return (PortHandler *)(new PortHandlerLinux(port_name)); + return (PortHandler *)(new PortHandlerLinux(port_name)); #endif #if defined(_WIN32) || defined(_WIN64) - return (PortHandler *)(new PortHandlerWindows(port_name)); + return (PortHandler *)(new PortHandlerWindows(port_name)); #endif } diff --git a/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp b/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp index 66877e7..5129164 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp @@ -1,8 +1,39 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* * Redistributions of source code must retain the above copyright notice, this +* list of conditions and the following disclaimer. +* +* * Redistributions in binary form must reproduce the above copyright notice, +* this list of conditions and the following disclaimer in the documentation +* and/or other materials provided with the distribution. +* +* * Neither the name of ROBOTIS nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*******************************************************************************/ + +/* Author: zerom, Leon Ryu Woon Jung */ + /* - * Protocol1PacketHandler.cpp + * protocol1_packet_handler.cpp * * Created on: 2016. 1. 26. - * Author: zerom, leon */ #if defined(_WIN32) || defined(_WIN64) #define WINDLLEXPORT @@ -10,7 +41,7 @@ #include #include -#include "dynamixel_sdk/Protocol1PacketHandler.h" +#include "dynamixel_sdk/protocol1_packet_handler.h" #define TXPACKET_MAX_LEN (250) #define RXPACKET_MAX_LEN (250) @@ -33,638 +64,664 @@ #define ERRBIT_OVERLOAD 32 // The current load cannot be controlled by the set torque. #define ERRBIT_INSTRUCTION 64 // Undefined instruction or delivering the action command without the reg_write command. -using namespace ROBOTIS; +using namespace dynamixel; Protocol1PacketHandler *Protocol1PacketHandler::unique_instance_ = new Protocol1PacketHandler(); Protocol1PacketHandler::Protocol1PacketHandler() { } -void Protocol1PacketHandler::PrintTxRxResult(int result) +void Protocol1PacketHandler::printTxRxResult(int result) { - switch(result) - { - case COMM_SUCCESS: - printf("[TxRxResult] Communication success.\n"); - break; + 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_PORT_BUSY: + printf("[TxRxResult] Port is in use!\n"); + break; - case COMM_TX_FAIL: - printf("[TxRxResult] Failed transmit instruction packet!\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_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_TX_ERROR: + printf("[TxRxResult] Incorrect instruction packet!\n"); + break; - case COMM_RX_WAITING: - printf("[TxRxResult] Now recieving status 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_TIMEOUT: + printf("[TxRxResult] There is no status packet!\n"); + break; - case COMM_RX_CORRUPT: - printf("[TxRxResult] Incorrect 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; + case COMM_NOT_AVAILABLE: + printf("[TxRxResult] Protocol does not support This function!\n"); + break; - default: - break; - } + default: + break; + } } -void Protocol1PacketHandler::PrintRxPacketError(UINT8_T error) +void Protocol1PacketHandler::printRxPacketError(uint8_t error) { - if(error & ERRBIT_VOLTAGE) - printf("[RxPacketError] Input voltage error!\n"); + if (error & ERRBIT_VOLTAGE) + printf("[RxPacketError] Input voltage error!\n"); - if(error & ERRBIT_ANGLE) - printf("[RxPacketError] Angle limit error!\n"); + if (error & ERRBIT_ANGLE) + printf("[RxPacketError] Angle limit error!\n"); - if(error & ERRBIT_OVERHEAT) - printf("[RxPacketError] Overheat 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_RANGE) + printf("[RxPacketError] Out of range error!\n"); - if(error & ERRBIT_CHECKSUM) - printf("[RxPacketError] Checksum error!\n"); + if (error & ERRBIT_CHECKSUM) + printf("[RxPacketError] Checksum error!\n"); - if(error & ERRBIT_OVERLOAD) - printf("[RxPacketError] Overload error!\n"); + if (error & ERRBIT_OVERLOAD) + printf("[RxPacketError] Overload error!\n"); - if(error & ERRBIT_INSTRUCTION) - printf("[RxPacketError] Instruction code error!\n"); + if (error & ERRBIT_INSTRUCTION) + printf("[RxPacketError] Instruction code error!\n"); } -int Protocol1PacketHandler::TxPacket(PortHandler *port, UINT8_T *txpacket) +int Protocol1PacketHandler::txPacket(PortHandler *port, uint8_t *txpacket) { - UINT8_T _checksum = 0; - UINT8_T _total_packet_length = txpacket[PKT_LENGTH] + 4; // 4: HEADER0 HEADER1 ID LENGTH - UINT8_T _written_packet_length = 0; + uint8_t checksum = 0; + uint8_t total_packet_length = txpacket[PKT_LENGTH] + 4; // 4: HEADER0 HEADER1 ID LENGTH + uint8_t written_packet_length = 0; - if(port->is_using) - return COMM_PORT_BUSY; - port->is_using = true; + if (port->is_using_) + return COMM_PORT_BUSY; + port->is_using_ = true; - // check max packet length - if(_total_packet_length > TXPACKET_MAX_LEN) - { - port->is_using = false; - return COMM_TX_ERROR; - } + // check max packet length + if (total_packet_length > TXPACKET_MAX_LEN) + { + port->is_using_ = false; + return COMM_TX_ERROR; + } - // make packet header - txpacket[PKT_HEADER0] = 0xFF; - txpacket[PKT_HEADER1] = 0xFF; + // make packet header + txpacket[PKT_HEADER0] = 0xFF; + txpacket[PKT_HEADER1] = 0xFF; - // add a checksum to the packet - for(int _idx = 2; _idx < _total_packet_length - 1; _idx++) // except header, checksum - _checksum += txpacket[_idx]; - txpacket[_total_packet_length - 1] = ~_checksum; + // add a checksum to the packet + for (int idx = 2; idx < total_packet_length - 1; idx++) // except header, checksum + checksum += txpacket[idx]; + txpacket[total_packet_length - 1] = ~checksum; - // tx packet - port->ClearPort(); - _written_packet_length = port->WritePort(txpacket, _total_packet_length); - if(_total_packet_length != _written_packet_length) - { - port->is_using = false; - return COMM_TX_FAIL; - } + // tx packet + port->clearPort(); + written_packet_length = port->writePort(txpacket, total_packet_length); + if (total_packet_length != written_packet_length) + { + port->is_using_ = false; + return COMM_TX_FAIL; + } - return COMM_SUCCESS; + return COMM_SUCCESS; } -int Protocol1PacketHandler::RxPacket(PortHandler *port, UINT8_T *rxpacket) +int Protocol1PacketHandler::rxPacket(PortHandler *port, uint8_t *rxpacket) { - int _result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - UINT8_T _checksum = 0; - UINT8_T _rx_length = 0; - UINT8_T _wait_length = 6; // minimum length ( HEADER0 HEADER1 ID LENGTH ERROR CHKSUM ) + uint8_t checksum = 0; + uint8_t rx_length = 0; + uint8_t wait_length = 6; // minimum length (HEADER0 HEADER1 ID LENGTH ERROR CHKSUM) - while(true) + while(true) + { + rx_length += port->readPort(&rxpacket[rx_length], wait_length - rx_length); + if (rx_length >= wait_length) { - _rx_length += port->ReadPort(&rxpacket[_rx_length], _wait_length - _rx_length); - if(_rx_length >= _wait_length) + uint8_t idx = 0; + + // find packet header + for (idx = 0; idx < (rx_length - 1); idx++) + { + if (rxpacket[idx] == 0xFF && rxpacket[idx+1] == 0xFF) + break; + } + + if (idx == 0) // found at the beginning of the packet + { + if (rxpacket[PKT_ID] > 0xFD || // unavailable ID + rxpacket[PKT_LENGTH] > RXPACKET_MAX_LEN || // unavailable Length + rxpacket[PKT_ERROR] >= 0x64) // unavailable Error { - UINT8_T _idx = 0; + // remove the first byte in the packet + for (uint8_t s = 0; s < rx_length - 1; s++) + rxpacket[s] = rxpacket[1 + s]; + //memcpy(&rxpacket[0], &rxpacket[idx], rx_length - idx); + rx_length -= 1; + continue; + } - // find packet header - for(_idx = 0; _idx < (_rx_length - 1); _idx++) - { - if(rxpacket[_idx] == 0xFF && rxpacket[_idx+1] == 0xFF) - break; - } + // re-calculate the exact length of the rx packet + if (wait_length != rxpacket[PKT_LENGTH] + PKT_LENGTH + 1) + { + wait_length = rxpacket[PKT_LENGTH] + PKT_LENGTH + 1; + continue; + } - if(_idx == 0) // found at the beginning of the packet + if (rx_length < wait_length) + { + // check timeout + if (port->isPacketTimeout() == true) + { + if (rx_length == 0) { - if(rxpacket[PKT_ID] > 0xFD || // unavailable ID - rxpacket[PKT_LENGTH] > RXPACKET_MAX_LEN || // unavailable Length - rxpacket[PKT_ERROR] >= 0x64) // unavailable Error - { - // remove the first byte in the packet - for(UINT8_T _s = 0; _s < _rx_length - 1; _s++) - rxpacket[_s] = rxpacket[1 + _s]; - //memcpy(&rxpacket[0], &rxpacket[_idx], _rx_length - _idx); - _rx_length -= 1; - continue; - } - - // re-calculate the exact length of the rx packet - if(_wait_length != rxpacket[PKT_LENGTH] + PKT_LENGTH + 1) - { - _wait_length = rxpacket[PKT_LENGTH] + PKT_LENGTH + 1; - continue; - } - - if(_rx_length < _wait_length) - { - // check timeout - if(port->IsPacketTimeout() == true) - { - if(_rx_length == 0) - _result = COMM_RX_TIMEOUT; - else - _result = COMM_RX_CORRUPT; - break; - } - else - continue; - } - - // calculate checksum - for(int _i = 2; _i < _wait_length - 1; _i++) // except header, checksum - _checksum += rxpacket[_i]; - _checksum = ~_checksum; - - // verify checksum - if(rxpacket[_wait_length - 1] == _checksum) - _result = COMM_SUCCESS; - else - _result = COMM_RX_CORRUPT; - break; + result = COMM_RX_TIMEOUT; } else { - // remove unnecessary packets - for(UINT8_T _s = 0; _s < _rx_length - _idx; _s++) - rxpacket[_s] = rxpacket[_idx + _s]; - //memcpy(&rxpacket[0], &rxpacket[_idx], _rx_length - _idx); - _rx_length -= _idx; + result = COMM_RX_CORRUPT; } + break; + } + else + { + continue; + } + } + + // calculate checksum + for (int i = 2; i < wait_length - 1; i++) // except header, checksum + checksum += rxpacket[i]; + checksum = ~checksum; + + // verify checksum + if (rxpacket[wait_length - 1] == checksum) + { + result = COMM_SUCCESS; } else { - // check timeout - if(port->IsPacketTimeout() == true) - { - if(_rx_length == 0) - _result = COMM_RX_TIMEOUT; - else - _result = COMM_RX_CORRUPT; - break; - } + result = COMM_RX_CORRUPT; + } + break; + } + else + { + // remove unnecessary packets + for (uint8_t s = 0; s < rx_length - idx; s++) + rxpacket[s] = rxpacket[idx + s]; + //memcpy(&rxpacket[0], &rxpacket[idx], rx_length - idx); + rx_length -= idx; + } + } + else + { + // check timeout + if (port->isPacketTimeout() == true) + { + if (rx_length == 0) + { + result = COMM_RX_TIMEOUT; } + else + { + result = COMM_RX_CORRUPT; + } + break; + } } - port->is_using = false; + } + port->is_using_ = false; - return _result; + return result; } // NOT for BulkRead instruction -int Protocol1PacketHandler::TxRxPacket(PortHandler *port, UINT8_T *txpacket, UINT8_T *rxpacket, UINT8_T *error) +int Protocol1PacketHandler::txRxPacket(PortHandler *port, uint8_t *txpacket, uint8_t *rxpacket, uint8_t *error) { - int _result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; // tx packet - _result = TxPacket(port, txpacket); - if(_result != COMM_SUCCESS) - return _result; + result = txPacket(port, txpacket); + if (result != COMM_SUCCESS) + return result; // (ID == Broadcast ID && NOT BulkRead) == no need to wait for status packet - // (Instruction == Action) == no need to wait for status packet - if((txpacket[PKT_ID] == BROADCAST_ID && txpacket[PKT_INSTRUCTION] != INST_BULK_READ) || + // (Instruction == action) == no need to wait for status packet + if ((txpacket[PKT_ID] == BROADCAST_ID && txpacket[PKT_INSTRUCTION] != INST_BULK_READ) || (txpacket[PKT_INSTRUCTION] == INST_ACTION)) { - port->is_using = false; - return _result; + port->is_using_ = false; + return result; } // set packet timeout - if(txpacket[PKT_INSTRUCTION] == INST_READ) - port->SetPacketTimeout((UINT16_T)(txpacket[PKT_PARAMETER0+1] + 6)); + if (txpacket[PKT_INSTRUCTION] == INST_READ) + { + port->setPacketTimeout((uint16_t)(txpacket[PKT_PARAMETER0+1] + 6)); + } else - port->SetPacketTimeout((UINT16_T)6); + { + port->setPacketTimeout((uint16_t)6); + } // rx packet - _result = RxPacket(port, rxpacket); + result = rxPacket(port, rxpacket); // check txpacket ID == rxpacket ID - if(txpacket[PKT_ID] != rxpacket[PKT_ID]) - _result = RxPacket(port, rxpacket); + if (txpacket[PKT_ID] != rxpacket[PKT_ID]) + result = rxPacket(port, rxpacket); - if(_result == COMM_SUCCESS && txpacket[PKT_ID] != BROADCAST_ID) + if (result == COMM_SUCCESS && txpacket[PKT_ID] != BROADCAST_ID) { - if(error != 0) - *error = (UINT8_T)rxpacket[PKT_ERROR]; + if (error != 0) + *error = (uint8_t)rxpacket[PKT_ERROR]; } - - return _result; + return result; } -int Protocol1PacketHandler::Ping(PortHandler *port, UINT8_T id, UINT8_T *error) +int Protocol1PacketHandler::ping(PortHandler *port, uint8_t id, uint8_t *error) { - return Ping(port, id, 0, error); + return ping(port, id, 0, error); } -int Protocol1PacketHandler::Ping(PortHandler *port, UINT8_T id, UINT16_T *model_number, UINT8_T *error) +int Protocol1PacketHandler::ping(PortHandler *port, uint8_t id, uint16_t *model_number, uint8_t *error) { - int _result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - UINT8_T txpacket[6] = {0}; - UINT8_T rxpacket[6] = {0}; + uint8_t txpacket[6] = {0}; + uint8_t rxpacket[6] = {0}; - if(id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; + if (id >= BROADCAST_ID) + return COMM_NOT_AVAILABLE; - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = 2; - txpacket[PKT_INSTRUCTION] = INST_PING; + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = 2; + txpacket[PKT_INSTRUCTION] = INST_PING; - _result = TxRxPacket(port, txpacket, rxpacket, error); - if(_result == COMM_SUCCESS && model_number != 0) - { - UINT8_T _data[2] = {0}; - _result = ReadTxRx(port, id, 0, 2, _data); // Address 0 : Model Number - if(_result == COMM_SUCCESS) - *model_number = DXL_MAKEWORD(_data[0], _data[1]); - } + result = txRxPacket(port, txpacket, rxpacket, error); + if (result == COMM_SUCCESS && model_number != 0) + { + uint8_t data_read[2] = {0}; + result = readTxRx(port, id, 0, 2, data_read); // Address 0 : Model Number + if (result == COMM_SUCCESS) *model_number = DXL_MAKEWORD(data_read[0], data_read[1]); + } - return _result; + return result; } -int Protocol1PacketHandler::BroadcastPing(PortHandler *port, std::vector &id_list) +int Protocol1PacketHandler::broadcastPing(PortHandler *port, std::vector &id_list) { - return COMM_NOT_AVAILABLE; + return COMM_NOT_AVAILABLE; } -int Protocol1PacketHandler::Action(PortHandler *port, UINT8_T id) +int Protocol1PacketHandler::action(PortHandler *port, uint8_t id) { - UINT8_T txpacket[6] = {0}; + uint8_t txpacket[6] = {0}; - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = 2; - txpacket[PKT_INSTRUCTION] = INST_ACTION; + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = 2; + txpacket[PKT_INSTRUCTION] = INST_ACTION; - return TxRxPacket(port, txpacket, 0); + return txRxPacket(port, txpacket, 0); } -int Protocol1PacketHandler::Reboot(PortHandler *port, UINT8_T id, UINT8_T *error) +int Protocol1PacketHandler::reboot(PortHandler *port, uint8_t id, uint8_t *error) { - return COMM_NOT_AVAILABLE; + return COMM_NOT_AVAILABLE; } -int Protocol1PacketHandler::FactoryReset(PortHandler *port, UINT8_T id, UINT8_T option, UINT8_T *error) +int Protocol1PacketHandler::factoryReset(PortHandler *port, uint8_t id, uint8_t option, uint8_t *error) { - UINT8_T txpacket[6] = {0}; - UINT8_T rxpacket[6] = {0}; + uint8_t txpacket[6] = {0}; + uint8_t rxpacket[6] = {0}; - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = 2; - txpacket[PKT_INSTRUCTION] = INST_FACTORY_RESET; + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = 2; + txpacket[PKT_INSTRUCTION] = INST_FACTORY_RESET; - return TxRxPacket(port, txpacket, rxpacket, error); + return txRxPacket(port, txpacket, rxpacket, error); } -int Protocol1PacketHandler::ReadTx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length) +int Protocol1PacketHandler::readTx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length) { - int _result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - UINT8_T txpacket[8] = {0}; + uint8_t txpacket[8] = {0}; - if(id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; + if (id >= BROADCAST_ID) + return COMM_NOT_AVAILABLE; - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = 4; - txpacket[PKT_INSTRUCTION] = INST_READ; - txpacket[PKT_PARAMETER0+0] = (UINT8_T)address; - txpacket[PKT_PARAMETER0+1] = (UINT8_T)length; + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = 4; + txpacket[PKT_INSTRUCTION] = INST_READ; + txpacket[PKT_PARAMETER0+0] = (uint8_t)address; + txpacket[PKT_PARAMETER0+1] = (uint8_t)length; - _result = TxPacket(port, txpacket); + result = txPacket(port, txpacket); - // set packet timeout - if(_result == COMM_SUCCESS) - port->SetPacketTimeout((UINT16_T)(length+6)); + // set packet timeout + if (result == COMM_SUCCESS) + port->setPacketTimeout((uint16_t)(length+6)); - return _result; + return result; } -int Protocol1PacketHandler::ReadRx(PortHandler *port, UINT16_T length, UINT8_T *data, UINT8_T *error) +int Protocol1PacketHandler::readRx(PortHandler *port, uint16_t length, uint8_t *data, uint8_t *error) { - int _result = COMM_TX_FAIL; - UINT8_T *rxpacket = (UINT8_T *)malloc(RXPACKET_MAX_LEN);//(length+6); - //UINT8_T *rxpacket = new UINT8_T[length+6]; + int result = COMM_TX_FAIL; + uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN);//(length+6); + //uint8_t *rxpacket = new uint8_t[length+6]; - _result = RxPacket(port, rxpacket); - if(_result == COMM_SUCCESS) + result = rxPacket(port, rxpacket); + if (result == COMM_SUCCESS) + { + if (error != 0) + { + *error = (uint8_t)rxpacket[PKT_ERROR]; + } + for (uint8_t s = 0; s < length; s++) { - if(error != 0) - *error = (UINT8_T)rxpacket[PKT_ERROR]; - for(UINT8_T _s = 0; _s < length; _s++) - data[_s] = rxpacket[PKT_PARAMETER0 + _s]; - //memcpy(data, &rxpacket[PKT_PARAMETER0], length); + data[s] = rxpacket[PKT_PARAMETER0 + s]; } + //memcpy(data, &rxpacket[PKT_PARAMETER0], length); + } - free(rxpacket); - //delete[] rxpacket; - return _result; + free(rxpacket); + //delete[] rxpacket; + return result; } -int Protocol1PacketHandler::ReadTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) +int Protocol1PacketHandler::readTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) { - int _result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - UINT8_T txpacket[8] = {0}; - UINT8_T *rxpacket = (UINT8_T *)malloc(RXPACKET_MAX_LEN);//(length+6); + uint8_t txpacket[8] = {0}; + uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN);//(length+6); - if(id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; + if (id >= BROADCAST_ID) + return COMM_NOT_AVAILABLE; - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = 4; - txpacket[PKT_INSTRUCTION] = INST_READ; - txpacket[PKT_PARAMETER0+0] = (UINT8_T)address; - txpacket[PKT_PARAMETER0+1] = (UINT8_T)length; + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = 4; + txpacket[PKT_INSTRUCTION] = INST_READ; + txpacket[PKT_PARAMETER0+0] = (uint8_t)address; + txpacket[PKT_PARAMETER0+1] = (uint8_t)length; - _result = TxRxPacket(port, txpacket, rxpacket, error); - if(_result == COMM_SUCCESS) + result = txRxPacket(port, txpacket, rxpacket, error); + if (result == COMM_SUCCESS) + { + if (error != 0) + { + *error = (uint8_t)rxpacket[PKT_ERROR]; + } + for (uint8_t s = 0; s < length; s++) { - if(error != 0) - *error = (UINT8_T)rxpacket[PKT_ERROR]; - for(UINT8_T _s = 0; _s < length; _s++) - data[_s] = rxpacket[PKT_PARAMETER0 + _s]; - //memcpy(data, &rxpacket[PKT_PARAMETER0], length); + data[s] = rxpacket[PKT_PARAMETER0 + s]; } + //memcpy(data, &rxpacket[PKT_PARAMETER0], length); + } - free(rxpacket); - //delete[] rxpacket; - return _result; + free(rxpacket); + //delete[] rxpacket; + return result; } -int Protocol1PacketHandler::Read1ByteTx(PortHandler *port, UINT8_T id, UINT16_T address) +int Protocol1PacketHandler::read1ByteTx(PortHandler *port, uint8_t id, uint16_t address) { - return ReadTx(port, id, address, 1); + return readTx(port, id, address, 1); } -int Protocol1PacketHandler::Read1ByteRx(PortHandler *port, UINT8_T *data, UINT8_T *error) +int Protocol1PacketHandler::read1ByteRx(PortHandler *port, uint8_t *data, uint8_t *error) { - UINT8_T _data[1] = {0}; - int _result = ReadRx(port, 1, _data, error); - if(_result == COMM_SUCCESS) - *data = _data[0]; - return _result; + uint8_t data_read[1] = {0}; + int result = readRx(port, 1, data_read, error); + if (result == COMM_SUCCESS) + *data = data_read[0]; + return result; } -int Protocol1PacketHandler::Read1ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T *data, UINT8_T *error) +int Protocol1PacketHandler::read1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error) { - UINT8_T _data[1] = {0}; - int _result = ReadTxRx(port, id, address, 1, _data, error); - if(_result == COMM_SUCCESS) - *data = _data[0]; - return _result; + uint8_t data_read[1] = {0}; + int result = readTxRx(port, id, address, 1, data_read, error); + if (result == COMM_SUCCESS) + *data = data_read[0]; + return result; } -int Protocol1PacketHandler::Read2ByteTx(PortHandler *port, UINT8_T id, UINT16_T address) +int Protocol1PacketHandler::read2ByteTx(PortHandler *port, uint8_t id, uint16_t address) { - return ReadTx(port, id, address, 2); + return readTx(port, id, address, 2); } -int Protocol1PacketHandler::Read2ByteRx(PortHandler *port, UINT16_T *data, UINT8_T *error) +int Protocol1PacketHandler::read2ByteRx(PortHandler *port, uint16_t *data, uint8_t *error) { - UINT8_T _data[2] = {0}; - int _result = ReadRx(port, 2, _data, error); - if(_result == COMM_SUCCESS) - *data = DXL_MAKEWORD(_data[0], _data[1]); - return _result; + uint8_t data_read[2] = {0}; + int result = readRx(port, 2, data_read, error); + if (result == COMM_SUCCESS) + *data = DXL_MAKEWORD(data_read[0], data_read[1]); + return result; } -int Protocol1PacketHandler::Read2ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T *data, UINT8_T *error) +int Protocol1PacketHandler::read2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error) { - UINT8_T _data[2] = {0}; - int _result = ReadTxRx(port, id, address, 2, _data, error); - if(_result == COMM_SUCCESS) - *data = DXL_MAKEWORD(_data[0], _data[1]); - return _result; + uint8_t data_read[2] = {0}; + int result = readTxRx(port, id, address, 2, data_read, error); + if (result == COMM_SUCCESS) + *data = DXL_MAKEWORD(data_read[0], data_read[1]); + return result; } -int Protocol1PacketHandler::Read4ByteTx(PortHandler *port, UINT8_T id, UINT16_T address) +int Protocol1PacketHandler::read4ByteTx(PortHandler *port, uint8_t id, uint16_t address) { - return COMM_NOT_AVAILABLE; + return COMM_NOT_AVAILABLE; } -int Protocol1PacketHandler::Read4ByteRx(PortHandler *port, UINT32_T *data, UINT8_T *error) +int Protocol1PacketHandler::read4ByteRx(PortHandler *port, uint32_t *data, uint8_t *error) { - return COMM_NOT_AVAILABLE; + return COMM_NOT_AVAILABLE; } -int Protocol1PacketHandler::Read4ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T *data, UINT8_T *error) +int Protocol1PacketHandler::read4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error) { - return COMM_NOT_AVAILABLE; + return COMM_NOT_AVAILABLE; } -int Protocol1PacketHandler::WriteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data) +int Protocol1PacketHandler::writeTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) { - int _result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - UINT8_T *txpacket = (UINT8_T *)malloc(length+7); - //UINT8_T *txpacket = new UINT8_T[length+7]; + uint8_t *txpacket = (uint8_t *)malloc(length+7); + //uint8_t *txpacket = new uint8_t[length+7]; - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = length+3; - txpacket[PKT_INSTRUCTION] = INST_WRITE; - txpacket[PKT_PARAMETER0] = (UINT8_T)address; + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = length+3; + txpacket[PKT_INSTRUCTION] = INST_WRITE; + txpacket[PKT_PARAMETER0] = (uint8_t)address; - for(UINT8_T _s = 0; _s < length; _s++) - txpacket[PKT_PARAMETER0+1+_s] = data[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); + for (uint8_t s = 0; s < length; s++) + txpacket[PKT_PARAMETER0+1+s] = data[s]; + //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); - _result = TxPacket(port, txpacket); - port->is_using = false; + result = txPacket(port, txpacket); + port->is_using_ = false; - free(txpacket); - //delete[] txpacket; - return _result; + free(txpacket); + //delete[] txpacket; + return result; } -int Protocol1PacketHandler::WriteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) +int Protocol1PacketHandler::writeTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) { - int _result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - UINT8_T *txpacket = (UINT8_T *)malloc(length+6); - //UINT8_T *txpacket = new UINT8_T[length+6]; - UINT8_T rxpacket[6] = {0}; + uint8_t *txpacket = (uint8_t *)malloc(length+7); //#6->7 + //uint8_t *txpacket = new uint8_t[length+7]; + uint8_t rxpacket[6] = {0}; - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = length+3; - txpacket[PKT_INSTRUCTION] = INST_WRITE; - txpacket[PKT_PARAMETER0] = (UINT8_T)address; + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = length+3; + txpacket[PKT_INSTRUCTION] = INST_WRITE; + txpacket[PKT_PARAMETER0] = (uint8_t)address; - for(UINT8_T _s = 0; _s < length; _s++) - txpacket[PKT_PARAMETER0+1+_s] = data[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); + for (uint8_t s = 0; s < length; s++) + txpacket[PKT_PARAMETER0+1+s] = data[s]; + //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); - _result = TxRxPacket(port, txpacket, rxpacket, error); + result = txRxPacket(port, txpacket, rxpacket, error); - free(txpacket); - //delete[] txpacket; - return _result; + free(txpacket); + //delete[] txpacket; + return result; } -int Protocol1PacketHandler::Write1ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data) +int Protocol1PacketHandler::write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data) { - UINT8_T _data[1] = { data }; - return WriteTxOnly(port, id, address, 1, _data); + uint8_t data_write[1] = { data }; + return writeTxOnly(port, id, address, 1, data_write); } -int Protocol1PacketHandler::Write1ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data, UINT8_T *error) +int Protocol1PacketHandler::write1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error) { - UINT8_T _data[1] = { data }; - return WriteTxRx(port, id, address, 1, _data, error); + uint8_t data_write[1] = { data }; + return writeTxRx(port, id, address, 1, data_write, error); } -int Protocol1PacketHandler::Write2ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data) +int Protocol1PacketHandler::write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data) { - UINT8_T _data[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; - return WriteTxOnly(port, id, address, 2, _data); + uint8_t data_write[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; + return writeTxOnly(port, id, address, 2, data_write); } -int Protocol1PacketHandler::Write2ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data, UINT8_T *error) +int Protocol1PacketHandler::write2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error) { - UINT8_T _data[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; - return WriteTxRx(port, id, address, 2, _data, error); + uint8_t data_write[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; + return writeTxRx(port, id, address, 2, data_write, error); } -int Protocol1PacketHandler::Write4ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data) +int Protocol1PacketHandler::write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data) { - return COMM_NOT_AVAILABLE; + return COMM_NOT_AVAILABLE; } -int Protocol1PacketHandler::Write4ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data, UINT8_T *error) +int Protocol1PacketHandler::write4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error) { - return COMM_NOT_AVAILABLE; + return COMM_NOT_AVAILABLE; } -int Protocol1PacketHandler::RegWriteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data) +int Protocol1PacketHandler::regWriteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) { - int _result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - UINT8_T *txpacket = (UINT8_T *)malloc(length+6); - //UINT8_T *txpacket = new UINT8_T[length+6]; + uint8_t *txpacket = (uint8_t *)malloc(length+6); + //uint8_t *txpacket = new uint8_t[length+6]; - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = length+3; - txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; - txpacket[PKT_PARAMETER0] = (UINT8_T)address; + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = length+3; + txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; + txpacket[PKT_PARAMETER0] = (uint8_t)address; - for(UINT8_T _s = 0; _s < length; _s++) - txpacket[PKT_PARAMETER0+1+_s] = data[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); + for (uint8_t s = 0; s < length; s++) + txpacket[PKT_PARAMETER0+1+s] = data[s]; + //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); - _result = TxPacket(port, txpacket); - port->is_using = false; + result = txPacket(port, txpacket); + port->is_using_ = false; - free(txpacket); - //delete[] txpacket; - return _result; + free(txpacket); + //delete[] txpacket; + return result; } -int Protocol1PacketHandler::RegWriteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) +int Protocol1PacketHandler::regWriteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) { - int _result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - UINT8_T *txpacket = (UINT8_T *)malloc(length+6); - //UINT8_T *txpacket = new UINT8_T[length+6]; - UINT8_T rxpacket[6] = {0}; + uint8_t *txpacket = (uint8_t *)malloc(length+6); + //uint8_t *txpacket = new uint8_t[length+6]; + uint8_t rxpacket[6] = {0}; - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = length+3; - txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; - txpacket[PKT_PARAMETER0] = (UINT8_T)address; + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = length+3; + txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; + txpacket[PKT_PARAMETER0] = (uint8_t)address; - for(UINT8_T _s = 0; _s < length; _s++) - txpacket[PKT_PARAMETER0+1+_s] = data[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); + for (uint8_t s = 0; s < length; s++) + txpacket[PKT_PARAMETER0+1+s] = data[s]; + //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); - _result = TxRxPacket(port, txpacket, rxpacket, error); + result = txRxPacket(port, txpacket, rxpacket, error); - free(txpacket); - //delete[] txpacket; - return _result; + free(txpacket); + //delete[] txpacket; + return result; } -int Protocol1PacketHandler::SyncReadTx(PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length) +int Protocol1PacketHandler::syncReadTx(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) { - return COMM_NOT_AVAILABLE; + return COMM_NOT_AVAILABLE; } -int Protocol1PacketHandler::SyncWriteTxOnly(PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length) +int Protocol1PacketHandler::syncWriteTxOnly(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) { - int _result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - UINT8_T *txpacket = (UINT8_T *)malloc(param_length+8); // 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN ... CHKSUM - //UINT8_T *txpacket = new UINT8_T[param_length + 8]; // 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN ... CHKSUM + uint8_t *txpacket = (uint8_t *)malloc(param_length+8); + // 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN ... CHKSUM + //uint8_t *txpacket = new uint8_t[param_length + 8]; - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH] = param_length + 4; // 4: INST START_ADDR DATA_LEN ... CHKSUM - txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE; - txpacket[PKT_PARAMETER0+0] = start_address; - txpacket[PKT_PARAMETER0+1] = data_length; + txpacket[PKT_ID] = BROADCAST_ID; + txpacket[PKT_LENGTH] = param_length + 4; // 4: INST START_ADDR DATA_LEN ... CHKSUM + txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE; + txpacket[PKT_PARAMETER0+0] = start_address; + txpacket[PKT_PARAMETER0+1] = data_length; - for(UINT8_T _s = 0; _s < param_length; _s++) - txpacket[PKT_PARAMETER0+2+_s] = param[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+2], param, param_length); + for (uint8_t s = 0; s < param_length; s++) + txpacket[PKT_PARAMETER0+2+s] = param[s]; + //memcpy(&txpacket[PKT_PARAMETER0+2], param, param_length); - _result = TxRxPacket(port, txpacket, 0, 0); + result = txRxPacket(port, txpacket, 0, 0); - free(txpacket); - //delete[] txpacket; - return _result; + free(txpacket); + //delete[] txpacket; + return result; } -int Protocol1PacketHandler::BulkReadTx(PortHandler *port, UINT8_T *param, UINT16_T param_length) +int Protocol1PacketHandler::bulkReadTx(PortHandler *port, uint8_t *param, uint16_t param_length) { - int _result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - UINT8_T *txpacket = (UINT8_T *)malloc(param_length+7); // 7: HEADER0 HEADER1 ID LEN INST 0x00 ... CHKSUM - //UINT8_T *txpacket = new UINT8_T[param_length + 7]; // 7: HEADER0 HEADER1 ID LEN INST 0x00 ... CHKSUM + uint8_t *txpacket = (uint8_t *)malloc(param_length+7); + // 7: HEADER0 HEADER1 ID LEN INST 0x00 ... CHKSUM + //uint8_t *txpacket = new uint8_t[param_length + 7]; - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH] = param_length + 3; // 3: INST 0x00 ... CHKSUM - txpacket[PKT_INSTRUCTION] = INST_BULK_READ; - txpacket[PKT_PARAMETER0+0] = 0x00; + txpacket[PKT_ID] = BROADCAST_ID; + txpacket[PKT_LENGTH] = param_length + 3; // 3: INST 0x00 ... CHKSUM + txpacket[PKT_INSTRUCTION] = INST_BULK_READ; + txpacket[PKT_PARAMETER0+0] = 0x00; - for(UINT8_T _s = 0; _s < param_length; _s++) - txpacket[PKT_PARAMETER0+1+_s] = param[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+1], param, param_length); + for (uint8_t s = 0; s < param_length; s++) + txpacket[PKT_PARAMETER0+1+s] = param[s]; + //memcpy(&txpacket[PKT_PARAMETER0+1], param, param_length); - _result = TxPacket(port, txpacket); - if(_result == COMM_SUCCESS) - { - int _wait_length = 0; - for(int _i = 0; _i < param_length; _i += 3) - _wait_length += param[_i] + 7; - port->SetPacketTimeout((UINT16_T)_wait_length); - } + result = txPacket(port, txpacket); + if (result == COMM_SUCCESS) + { + int wait_length = 0; + for (int i = 0; i < param_length; i += 3) + wait_length += param[i] + 7; + port->setPacketTimeout((uint16_t)wait_length); + } - free(txpacket); - //delete[] txpacket; - return _result; + free(txpacket); + //delete[] txpacket; + return result; } -int Protocol1PacketHandler::BulkWriteTxOnly(PortHandler *port, UINT8_T *param, UINT16_T param_length) +int Protocol1PacketHandler::bulkWriteTxOnly(PortHandler *port, uint8_t *param, uint16_t param_length) { - return COMM_NOT_AVAILABLE; + return COMM_NOT_AVAILABLE; } diff --git a/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp b/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp index 771fee7..f814229 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp @@ -1,8 +1,39 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* * Redistributions of source code must retain the above copyright notice, this +* list of conditions and the following disclaimer. +* +* * Redistributions in binary form must reproduce the above copyright notice, +* this list of conditions and the following disclaimer in the documentation +* and/or other materials provided with the distribution. +* +* * Neither the name of ROBOTIS nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*******************************************************************************/ + +/* Author: zerom, Leon Ryu Woon Jung */ + /* - * Protocol2PacketHandler.cpp + * protocol2_packet_handler.cpp * * Created on: 2016. 1. 26. - * Author: zerom, leon */ #if defined(_WIN32) || defined(_WIN64) @@ -12,7 +43,7 @@ #include #include #include -#include "dynamixel_sdk/Protocol2PacketHandler.h" +#include "dynamixel_sdk/protocol2_packet_handler.h" #define TXPACKET_MAX_LEN (4*1024) #define RXPACKET_MAX_LEN (4*1024) @@ -40,946 +71,966 @@ #define ERRBIT_ALERT 128 //When the device has a problem, this bit is set to 1. Check "Device Status Check" value. -using namespace ROBOTIS; +using namespace dynamixel; Protocol2PacketHandler *Protocol2PacketHandler::unique_instance_ = new Protocol2PacketHandler(); Protocol2PacketHandler::Protocol2PacketHandler() { } -void Protocol2PacketHandler::PrintTxRxResult(int result) +void Protocol2PacketHandler::printTxRxResult(int result) { - switch(result) - { + switch(result) + { case COMM_SUCCESS: - printf("[TxRxResult] Communication success.\n"); - break; + printf("[TxRxResult] Communication success.\n"); + break; case COMM_PORT_BUSY: - printf("[TxRxResult] Port is in use!\n"); - break; + printf("[TxRxResult] Port is in use!\n"); + break; case COMM_TX_FAIL: - printf("[TxRxResult] Failed transmit instruction packet!\n"); - break; + printf("[TxRxResult] Failed transmit instruction packet!\n"); + break; case COMM_RX_FAIL: - printf("[TxRxResult] Failed get status packet from device!\n"); - break; + printf("[TxRxResult] Failed get status packet from device!\n"); + break; case COMM_TX_ERROR: - printf("[TxRxResult] Incorrect instruction packet!\n"); - break; + printf("[TxRxResult] Incorrect instruction packet!\n"); + break; case COMM_RX_WAITING: - printf("[TxRxResult] Now recieving status packet!\n"); - break; + printf("[TxRxResult] Now recieving status packet!\n"); + break; case COMM_RX_TIMEOUT: - printf("[TxRxResult] There is no status packet!\n"); - break; + printf("[TxRxResult] There is no status packet!\n"); + break; case COMM_RX_CORRUPT: - printf("[TxRxResult] Incorrect status packet!\n"); - break; + printf("[TxRxResult] Incorrect status packet!\n"); + break; case COMM_NOT_AVAILABLE: - printf("[TxRxResult] Protocol does not support This function!\n"); - break; + printf("[TxRxResult] Protocol does not support This function!\n"); + break; default: - break; - } + break; + } } -void Protocol2PacketHandler::PrintRxPacketError(UINT8_T error) +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"); + if (error & ERRBIT_ALERT) + printf("[RxPacketError] Hardware error occurred. Check the error at Control Table (Hardware Error Status)!\n"); - int _error = error & ~ERRBIT_ALERT; + int not_alert_error = error & ~ERRBIT_ALERT; - switch(_error) - { + switch(not_alert_error) + { case 0: - break; + break; case ERRNUM_RESULT_FAIL: - printf("[RxPacketError] Failed to process the instruction packet!\n"); - break; + printf("[RxPacketError] Failed to process the instruction packet!\n"); + break; case ERRNUM_INSTRUCTION: - printf("[RxPacketError] Undefined instruction or incorrect instruction!\n"); - break; + printf("[RxPacketError] Undefined instruction or incorrect instruction!\n"); + break; case ERRNUM_CRC: - printf("[RxPacketError] CRC doesn't match!\n"); - break; + printf("[RxPacketError] CRC doesn't match!\n"); + break; case ERRNUM_DATA_RANGE: - printf("[RxPacketError] The data value is out of range!\n"); - break; + 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; + 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; + 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; + printf("[RxPacketError] Writing or Reading is not available to target address!\n"); + break; default: - printf("[RxPacketError] Unknown error code!\n"); - break; - } + 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; - UINT16_T crc_table[256] = {0x0000, - 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011, - 0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, - 0x0022, 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, - 0x8077, 0x0072, 0x0050, 0x8055, 0x805F, 0x005A, 0x804B, - 0x004E, 0x0044, 0x8041, 0x80C3, 0x00C6, 0x00CC, 0x80C9, - 0x00D8, 0x80DD, 0x80D7, 0x00D2, 0x00F0, 0x80F5, 0x80FF, - 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1, 0x00A0, 0x80A5, - 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1, 0x8093, - 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082, - 0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, - 0x0192, 0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, - 0x01A4, 0x81A1, 0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, - 0x01FE, 0x01F4, 0x81F1, 0x81D3, 0x01D6, 0x01DC, 0x81D9, - 0x01C8, 0x81CD, 0x81C7, 0x01C2, 0x0140, 0x8145, 0x814F, - 0x014A, 0x815B, 0x015E, 0x0154, 0x8151, 0x8173, 0x0176, - 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162, 0x8123, - 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132, - 0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, - 0x8101, 0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, - 0x8317, 0x0312, 0x0330, 0x8335, 0x833F, 0x033A, 0x832B, - 0x032E, 0x0324, 0x8321, 0x0360, 0x8365, 0x836F, 0x036A, - 0x837B, 0x037E, 0x0374, 0x8371, 0x8353, 0x0356, 0x035C, - 0x8359, 0x0348, 0x834D, 0x8347, 0x0342, 0x03C0, 0x83C5, - 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1, 0x83F3, - 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2, - 0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, - 0x03B2, 0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, - 0x0384, 0x8381, 0x0280, 0x8285, 0x828F, 0x028A, 0x829B, - 0x029E, 0x0294, 0x8291, 0x82B3, 0x02B6, 0x02BC, 0x82B9, - 0x02A8, 0x82AD, 0x82A7, 0x02A2, 0x82E3, 0x02E6, 0x02EC, - 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2, 0x02D0, 0x82D5, - 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1, 0x8243, - 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252, - 0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, - 0x8261, 0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, - 0x0234, 0x8231, 0x8213, 0x0216, 0x021C, 0x8219, 0x0208, - 0x820D, 0x8207, 0x0202 }; - - for(j = 0; j < data_blk_size; j++) - { - i = ((UINT16_T)(crc_accum >> 8) ^ *data_blk_ptr++) & 0xFF; - crc_accum = (crc_accum << 8) ^ crc_table[i]; - } - - return crc_accum; +unsigned short Protocol2PacketHandler::updateCRC(uint16_t crc_accum, uint8_t *data_blk_ptr, uint16_t data_blk_size) +{ + uint16_t i; + uint16_t crc_table[256] = {0x0000, + 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011, + 0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, + 0x0022, 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, + 0x8077, 0x0072, 0x0050, 0x8055, 0x805F, 0x005A, 0x804B, + 0x004E, 0x0044, 0x8041, 0x80C3, 0x00C6, 0x00CC, 0x80C9, + 0x00D8, 0x80DD, 0x80D7, 0x00D2, 0x00F0, 0x80F5, 0x80FF, + 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1, 0x00A0, 0x80A5, + 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1, 0x8093, + 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082, + 0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, + 0x0192, 0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, + 0x01A4, 0x81A1, 0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, + 0x01FE, 0x01F4, 0x81F1, 0x81D3, 0x01D6, 0x01DC, 0x81D9, + 0x01C8, 0x81CD, 0x81C7, 0x01C2, 0x0140, 0x8145, 0x814F, + 0x014A, 0x815B, 0x015E, 0x0154, 0x8151, 0x8173, 0x0176, + 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162, 0x8123, + 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132, + 0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, + 0x8101, 0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, + 0x8317, 0x0312, 0x0330, 0x8335, 0x833F, 0x033A, 0x832B, + 0x032E, 0x0324, 0x8321, 0x0360, 0x8365, 0x836F, 0x036A, + 0x837B, 0x037E, 0x0374, 0x8371, 0x8353, 0x0356, 0x035C, + 0x8359, 0x0348, 0x834D, 0x8347, 0x0342, 0x03C0, 0x83C5, + 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1, 0x83F3, + 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2, + 0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, + 0x03B2, 0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, + 0x0384, 0x8381, 0x0280, 0x8285, 0x828F, 0x028A, 0x829B, + 0x029E, 0x0294, 0x8291, 0x82B3, 0x02B6, 0x02BC, 0x82B9, + 0x02A8, 0x82AD, 0x82A7, 0x02A2, 0x82E3, 0x02E6, 0x02EC, + 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2, 0x02D0, 0x82D5, + 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1, 0x8243, + 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252, + 0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, + 0x8261, 0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, + 0x0234, 0x8231, 0x8213, 0x0216, 0x021C, 0x8219, 0x0208, + 0x820D, 0x8207, 0x0202 }; + + for (uint16_t j = 0; j < data_blk_size; j++) + { + i = ((uint16_t)(crc_accum >> 8) ^ *data_blk_ptr++) & 0xFF; + crc_accum = (crc_accum << 8) ^ crc_table[i]; + } + + return crc_accum; } -void Protocol2PacketHandler::AddStuffing(UINT8_T *packet) +void Protocol2PacketHandler::addStuffing(uint8_t *packet) { - int i = 0, index = 0; - int packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]); - int packet_length_out = packet_length_in; - UINT8_T temp[TXPACKET_MAX_LEN] = {0}; - - for(UINT8_T _s = PKT_HEADER0; _s <= PKT_LENGTH_H; _s++) - temp[_s] = packet[_s]; // FF FF FD XX ID LEN_L LEN_H - //memcpy(temp, packet, PKT_LENGTH_H+1); - index = PKT_INSTRUCTION; - for( i = 0; i < packet_length_in - 2; i++) // except CRC - { - temp[index++] = packet[i+PKT_INSTRUCTION]; - if(packet[i+PKT_INSTRUCTION] == 0xFD && packet[i+PKT_INSTRUCTION-1] == 0xFF && packet[i+PKT_INSTRUCTION-2] == 0xFF) - { // FF FF FD - temp[index++] = 0xFD; - packet_length_out++; - } + int i = 0, index = 0; + int packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]); + int packet_length_out = packet_length_in; + uint8_t temp[TXPACKET_MAX_LEN] = {0}; + + for (uint8_t s = PKT_HEADER0; s <= PKT_LENGTH_H; s++) + temp[s] = packet[s]; // FF FF FD XX ID LEN_L LEN_H + //memcpy(temp, packet, PKT_LENGTH_H+1); + index = PKT_INSTRUCTION; + for (i = 0; i < packet_length_in - 2; i++) // except CRC + { + temp[index++] = packet[i+PKT_INSTRUCTION]; + if (packet[i+PKT_INSTRUCTION] == 0xFD && packet[i+PKT_INSTRUCTION-1] == 0xFF && packet[i+PKT_INSTRUCTION-2] == 0xFF) + { // FF FF FD + temp[index++] = 0xFD; + packet_length_out++; } - temp[index++] = packet[PKT_INSTRUCTION+packet_length_in-2]; - temp[index++] = packet[PKT_INSTRUCTION+packet_length_in-1]; + } + temp[index++] = packet[PKT_INSTRUCTION+packet_length_in-2]; + temp[index++] = packet[PKT_INSTRUCTION+packet_length_in-1]; - ////////////////////////// - if(packet_length_in != packet_length_out) - packet = (UINT8_T *)realloc(packet, index * sizeof(UINT8_T)); + ////////////////////////// + if (packet_length_in != packet_length_out) + packet = (uint8_t *)realloc(packet, index * sizeof(uint8_t)); - /////////////////////////// + /////////////////////////// - for(UINT8_T _s = 0; _s < index; _s++) - packet[_s] = temp[_s]; - //memcpy(packet, temp, index); - packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out); - packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out); + for (uint8_t s = 0; s < index; s++) + packet[s] = temp[s]; + //memcpy(packet, temp, index); + packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out); + packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out); } -void Protocol2PacketHandler::RemoveStuffing(UINT8_T *packet) +void Protocol2PacketHandler::removeStuffing(uint8_t *packet) { - int i = 0, index = 0; - int packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]); - int packet_length_out = packet_length_in; - - index = PKT_INSTRUCTION; - for( i = 0; i < packet_length_in - 2; i++) // except CRC - { - if(packet[i+PKT_INSTRUCTION] == 0xFD && packet[i+PKT_INSTRUCTION+1] == 0xFD && packet[i+PKT_INSTRUCTION-1] == 0xFF && packet[i+PKT_INSTRUCTION-2] == 0xFF) - { // FF FF FD FD - packet_length_out--; - i++; - } - packet[index++] = packet[i+PKT_INSTRUCTION]; + int i = 0, index = 0; + int packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]); + int packet_length_out = packet_length_in; + + index = PKT_INSTRUCTION; + for (i = 0; i < packet_length_in - 2; i++) // except CRC + { + if (packet[i+PKT_INSTRUCTION] == 0xFD && packet[i+PKT_INSTRUCTION+1] == 0xFD && packet[i+PKT_INSTRUCTION-1] == 0xFF && packet[i+PKT_INSTRUCTION-2] == 0xFF) + { // FF FF FD FD + packet_length_out--; + i++; } - packet[index++] = packet[PKT_INSTRUCTION+packet_length_in-2]; - packet[index++] = packet[PKT_INSTRUCTION+packet_length_in-1]; + packet[index++] = packet[i+PKT_INSTRUCTION]; + } + packet[index++] = packet[PKT_INSTRUCTION+packet_length_in-2]; + packet[index++] = packet[PKT_INSTRUCTION+packet_length_in-1]; - packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out); - packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out); + packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out); + packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out); } -int Protocol2PacketHandler::TxPacket(PortHandler *port, UINT8_T *txpacket) +int Protocol2PacketHandler::txPacket(PortHandler *port, uint8_t *txpacket) { - UINT16_T _total_packet_length = 0; - UINT16_T _written_packet_length = 0; - - if(port->is_using) - return COMM_PORT_BUSY; - port->is_using = true; - - // byte stuffing for header - AddStuffing(txpacket); - - // check max packet length - _total_packet_length = DXL_MAKEWORD(txpacket[PKT_LENGTH_L], txpacket[PKT_LENGTH_H]) + 7; - // 7: HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H - if(_total_packet_length > TXPACKET_MAX_LEN) - { - port->is_using = false; - return COMM_TX_ERROR; - } - - // make packet header - txpacket[PKT_HEADER0] = 0xFF; - txpacket[PKT_HEADER1] = 0xFF; - txpacket[PKT_HEADER2] = 0xFD; - txpacket[PKT_RESERVED] = 0x00; - - // add CRC16 - UINT16_T crc = UpdateCRC(0, txpacket, _total_packet_length - 2); // 2: CRC16 - txpacket[_total_packet_length - 2] = DXL_LOBYTE(crc); - txpacket[_total_packet_length - 1] = DXL_HIBYTE(crc); - - // tx packet - port->ClearPort(); - _written_packet_length = port->WritePort(txpacket, _total_packet_length); - if(_total_packet_length != _written_packet_length) - { - port->is_using = false; - return COMM_TX_FAIL; - } - - return COMM_SUCCESS; + uint16_t total_packet_length = 0; + uint16_t written_packet_length = 0; + + if (port->is_using_) + return COMM_PORT_BUSY; + port->is_using_ = true; + + // byte stuffing for header + addStuffing(txpacket); + + // check max packet length + total_packet_length = DXL_MAKEWORD(txpacket[PKT_LENGTH_L], txpacket[PKT_LENGTH_H]) + 7; + // 7: HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H + if (total_packet_length > TXPACKET_MAX_LEN) + { + port->is_using_ = false; + return COMM_TX_ERROR; + } + + // make packet header + txpacket[PKT_HEADER0] = 0xFF; + txpacket[PKT_HEADER1] = 0xFF; + txpacket[PKT_HEADER2] = 0xFD; + txpacket[PKT_RESERVED] = 0x00; + + // add CRC16 + uint16_t crc = updateCRC(0, txpacket, total_packet_length - 2); // 2: CRC16 + txpacket[total_packet_length - 2] = DXL_LOBYTE(crc); + txpacket[total_packet_length - 1] = DXL_HIBYTE(crc); + + // tx packet + port->clearPort(); + written_packet_length = port->writePort(txpacket, total_packet_length); + if (total_packet_length != written_packet_length) + { + port->is_using_ = false; + return COMM_TX_FAIL; + } + + return COMM_SUCCESS; } -int Protocol2PacketHandler::RxPacket(PortHandler *port, UINT8_T *rxpacket) +int Protocol2PacketHandler::rxPacket(PortHandler *port, uint8_t *rxpacket) { - int _result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - UINT16_T _rx_length = 0; - UINT16_T _wait_length = 11; - // minimum length ( HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H INST ERROR CRC16_L CRC16_H ) + uint16_t rx_length = 0; + uint16_t wait_length = 11; // minimum length (HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H INST ERROR CRC16_L CRC16_H) - while(true) + while(true) + { + rx_length += port->readPort(&rxpacket[rx_length], wait_length - rx_length); + if (rx_length >= wait_length) { - _rx_length += port->ReadPort(&rxpacket[_rx_length], _wait_length - _rx_length); - if(_rx_length >= _wait_length) + uint16_t idx = 0; + + // find packet header + for (idx = 0; idx < (rx_length - 3); idx++) + { + if ((rxpacket[idx] == 0xFF) && (rxpacket[idx+1] == 0xFF) && (rxpacket[idx+2] == 0xFD) && (rxpacket[idx+3] != 0xFD)) + break; + } + + if (idx == 0) // found at the beginning of the packet + { + if (rxpacket[PKT_RESERVED] != 0x00 || + rxpacket[PKT_ID] > 0xFC || + DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) > RXPACKET_MAX_LEN || + rxpacket[PKT_INSTRUCTION] != 0x55) { - UINT16_T _idx = 0; + // remove the first byte in the packet + for (uint8_t s = 0; s < rx_length - 1; s++) + rxpacket[s] = rxpacket[1 + s]; + //memcpy(&rxpacket[0], &rxpacket[idx], rx_length - idx); + rx_length -= 1; + continue; + } - // find packet header - for(_idx = 0; _idx < (_rx_length - 3); _idx++) - { - if((rxpacket[_idx] == 0xFF) && (rxpacket[_idx+1] == 0xFF) && (rxpacket[_idx+2] == 0xFD) && (rxpacket[_idx+3] != 0xFD)) - break; - } + // re-calculate the exact length of the rx packet + if (wait_length != DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1) + { + wait_length = DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1; + continue; + } - if(_idx == 0) // found at the beginning of the packet + if (rx_length < wait_length) + { + // check timeout + if (port->isPacketTimeout() == true) + { + if (rx_length == 0) { - if(rxpacket[PKT_RESERVED] != 0x00 || - rxpacket[PKT_ID] > 0xFC || - DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) > RXPACKET_MAX_LEN || - rxpacket[PKT_INSTRUCTION] != 0x55) - { - // remove the first byte in the packet - for(UINT8_T _s = 0; _s < _rx_length - 1; _s++) - rxpacket[_s] = rxpacket[1 + _s]; - //memcpy(&rxpacket[0], &rxpacket[_idx], _rx_length - _idx); - _rx_length -= 1; - continue; - } - - // re-calculate the exact length of the rx packet - if(_wait_length != DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1) - { - _wait_length = DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1; - continue; - } - - if(_rx_length < _wait_length) - { - // check timeout - if(port->IsPacketTimeout() == true) - { - if(_rx_length == 0) - _result = COMM_RX_TIMEOUT; - else - _result = COMM_RX_CORRUPT; - break; - } - else - continue; - } - - // verify CRC16 - UINT16_T crc = DXL_MAKEWORD(rxpacket[_wait_length-2], rxpacket[_wait_length-1]); - if(UpdateCRC(0, rxpacket, _wait_length - 2) == crc) - _result = COMM_SUCCESS; - else - _result = COMM_RX_CORRUPT; - break; + result = COMM_RX_TIMEOUT; } else { - // remove unnecessary packets - for(UINT8_T _s = 0; _s < _rx_length - _idx; _s++) - rxpacket[_s] = rxpacket[_idx + _s]; - //memcpy(&rxpacket[0], &rxpacket[_idx], _rx_length - _idx); - _rx_length -= _idx; + result = COMM_RX_CORRUPT; } + break; + } + else + { + continue; + } + } + + // verify CRC16 + uint16_t crc = DXL_MAKEWORD(rxpacket[wait_length-2], rxpacket[wait_length-1]); + if (updateCRC(0, rxpacket, wait_length - 2) == crc) + { + result = COMM_SUCCESS; } else { - // check timeout - if(port->IsPacketTimeout() == true) - { - if(_rx_length == 0) - _result = COMM_RX_TIMEOUT; - else - _result = COMM_RX_CORRUPT; - break; - } + result = COMM_RX_CORRUPT; + } + break; + } + else + { + // remove unnecessary packets + for (uint8_t s = 0; s < rx_length - idx; s++) + rxpacket[s] = rxpacket[idx + s]; + //memcpy(&rxpacket[0], &rxpacket[idx], rx_length - idx); + rx_length -= idx; + } + } + else + { + // check timeout + if (port->isPacketTimeout() == true) + { + if (rx_length == 0) + { + result = COMM_RX_TIMEOUT; + } + else + { + result = COMM_RX_CORRUPT; } + break; + } } - port->is_using = false; + } + port->is_using_ = false; - if(_result == COMM_SUCCESS) - RemoveStuffing(rxpacket); + if (result == COMM_SUCCESS) + removeStuffing(rxpacket); - return _result; + return result; } // NOT for BulkRead / SyncRead instruction -int Protocol2PacketHandler::TxRxPacket(PortHandler *port, UINT8_T *txpacket, UINT8_T *rxpacket, UINT8_T *error) +int Protocol2PacketHandler::txRxPacket(PortHandler *port, uint8_t *txpacket, uint8_t *rxpacket, uint8_t *error) { - int _result = COMM_TX_FAIL; - - // tx packet - _result = TxPacket(port, txpacket); - if(_result != COMM_SUCCESS) - return _result; - - // (ID == Broadcast ID && NOT BulkRead) == no need to wait for status packet - // (Instruction == Action) == no need to wait for status packet - if((txpacket[PKT_ID] == BROADCAST_ID && txpacket[PKT_INSTRUCTION] != INST_BULK_READ) || - (txpacket[PKT_ID] == BROADCAST_ID && txpacket[PKT_INSTRUCTION] != INST_SYNC_READ) || - (txpacket[PKT_INSTRUCTION] == INST_ACTION)) - { - port->is_using = false; - return _result; - } - - // set packet timeout - if(txpacket[PKT_INSTRUCTION] == INST_READ) - port->SetPacketTimeout((UINT16_T)(DXL_MAKEWORD(txpacket[PKT_PARAMETER0+2], txpacket[PKT_PARAMETER0+3]) + 11)); - else - port->SetPacketTimeout((UINT16_T)11); // HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H INST ERROR CRC16_L CRC16_H - - // rx packet - _result = RxPacket(port, rxpacket); - // check txpacket ID == rxpacket ID - if(txpacket[PKT_ID] != rxpacket[PKT_ID]) - _result = RxPacket(port, rxpacket); - - if(_result == COMM_SUCCESS && txpacket[PKT_ID] != BROADCAST_ID) - { - if(error != 0) - *error = (UINT8_T)rxpacket[PKT_ERROR]; - } - - return _result; + int result = COMM_TX_FAIL; + + // tx packet + result = txPacket(port, txpacket); + if (result != COMM_SUCCESS) + return result; + + // (ID == Broadcast ID && NOT BulkRead) == no need to wait for status packet + // (Instruction == action) == no need to wait for status packet + if ((txpacket[PKT_ID] == BROADCAST_ID && txpacket[PKT_INSTRUCTION] != INST_BULK_READ) || + (txpacket[PKT_ID] == BROADCAST_ID && txpacket[PKT_INSTRUCTION] != INST_SYNC_READ) || + (txpacket[PKT_INSTRUCTION] == INST_ACTION)) + { + port->is_using_ = false; + return result; + } + + // set packet timeout + if (txpacket[PKT_INSTRUCTION] == INST_READ) + { + port->setPacketTimeout((uint16_t)(DXL_MAKEWORD(txpacket[PKT_PARAMETER0+2], txpacket[PKT_PARAMETER0+3]) + 11)); + } + else + { + port->setPacketTimeout((uint16_t)11); + // HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H INST ERROR CRC16_L CRC16_H + } + + // rx packet + result = rxPacket(port, rxpacket); + // check txpacket ID == rxpacket ID + if (txpacket[PKT_ID] != rxpacket[PKT_ID]) + result = rxPacket(port, rxpacket); + + if (result == COMM_SUCCESS && txpacket[PKT_ID] != BROADCAST_ID) + { + if (error != 0) + *error = (uint8_t)rxpacket[PKT_ERROR]; + } + + return result; } -int Protocol2PacketHandler::Ping(PortHandler *port, UINT8_T id, UINT8_T *error) +int Protocol2PacketHandler::ping(PortHandler *port, uint8_t id, uint8_t *error) { - return Ping(port, id, 0, error); + return ping(port, id, 0, error); } -int Protocol2PacketHandler::Ping(PortHandler *port, UINT8_T id, UINT16_T *model_number, UINT8_T *error) +int Protocol2PacketHandler::ping(PortHandler *port, uint8_t id, uint16_t *model_number, uint8_t *error) { - int _result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - UINT8_T txpacket[10] = {0}; - UINT8_T rxpacket[14] = {0}; + uint8_t txpacket[10] = {0}; + uint8_t rxpacket[14] = {0}; - if(id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; + if (id >= BROADCAST_ID) + return COMM_NOT_AVAILABLE; - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 3; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_PING; + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = 3; + txpacket[PKT_LENGTH_H] = 0; + txpacket[PKT_INSTRUCTION] = INST_PING; - _result = TxRxPacket(port, txpacket, rxpacket, error); - if(_result == COMM_SUCCESS && model_number != 0) - *model_number = DXL_MAKEWORD(rxpacket[PKT_PARAMETER0+1], rxpacket[PKT_PARAMETER0+2]); + result = txRxPacket(port, txpacket, rxpacket, error); + if (result == COMM_SUCCESS && model_number != 0) + *model_number = DXL_MAKEWORD(rxpacket[PKT_PARAMETER0+1], rxpacket[PKT_PARAMETER0+2]); - return _result; + return result; } -int Protocol2PacketHandler::BroadcastPing(PortHandler *port, std::vector &id_list) +int Protocol2PacketHandler::broadcastPing(PortHandler *port, std::vector &id_list) { - const int STATUS_LENGTH = 14; - int _result = COMM_TX_FAIL; + const int STATUS_LENGTH = 14; + int result = COMM_TX_FAIL; - id_list.clear(); + id_list.clear(); - UINT16_T _rx_length = 0; - UINT16_T _wait_length = STATUS_LENGTH * MAX_ID; - - UINT8_T txpacket[10] = {0}; - UINT8_T rxpacket[STATUS_LENGTH * MAX_ID] = {0}; - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH_L] = 3; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_PING; - - _result = TxPacket(port, txpacket); - if(_result != COMM_SUCCESS) - { - port->is_using = false; - return _result; - } - - // set rx timeout - port->SetPacketTimeout((UINT16_T)(_wait_length * 30)); - - while(1) - { - _rx_length += port->ReadPort(&rxpacket[_rx_length], _wait_length - _rx_length); - if(port->IsPacketTimeout() == true)// || _rx_length >= _wait_length) - break; - } + uint16_t rx_length = 0; + uint16_t wait_length = STATUS_LENGTH * MAX_ID; - port->is_using = false; + uint8_t txpacket[10] = {0}; + uint8_t rxpacket[STATUS_LENGTH * MAX_ID] = {0}; - if(_rx_length == 0) - return COMM_RX_TIMEOUT; + txpacket[PKT_ID] = BROADCAST_ID; + txpacket[PKT_LENGTH_L] = 3; + txpacket[PKT_LENGTH_H] = 0; + txpacket[PKT_INSTRUCTION] = INST_PING; - while(1) - { - if(_rx_length < STATUS_LENGTH) - return COMM_RX_CORRUPT; + result = txPacket(port, txpacket); + if (result != COMM_SUCCESS) + { + port->is_using_ = false; + return result; + } - UINT16_T _idx = 0; + // set rx timeout + port->setPacketTimeout((uint16_t)(wait_length * 30)); - // find packet header - for(_idx = 0; _idx < (_rx_length - 2); _idx++) - { - if(rxpacket[_idx] == 0xFF && rxpacket[_idx+1] == 0xFF && rxpacket[_idx+2] == 0xFD) - break; - } + while(1) + { + rx_length += port->readPort(&rxpacket[rx_length], wait_length - rx_length); + if (port->isPacketTimeout() == true)// || rx_length >= wait_length) + break; + } - if(_idx == 0) // found at the beginning of the packet - { - // verify CRC16 - UINT16_T crc = DXL_MAKEWORD(rxpacket[STATUS_LENGTH-2], rxpacket[STATUS_LENGTH-1]); + port->is_using_ = false; - if(UpdateCRC(0, rxpacket, STATUS_LENGTH - 2) == crc) - { - _result = COMM_SUCCESS; + if (rx_length == 0) + return COMM_RX_TIMEOUT; - id_list.push_back(rxpacket[PKT_ID]); + while(1) + { + if (rx_length < STATUS_LENGTH) + return COMM_RX_CORRUPT; - for(UINT8_T _s = 0; _s < _rx_length - STATUS_LENGTH; _s++) - rxpacket[_s] = rxpacket[STATUS_LENGTH + _s]; - _rx_length -= STATUS_LENGTH; + uint16_t idx = 0; - if(_rx_length == 0) - return _result; - } - else - { - _result = COMM_RX_CORRUPT; + // find packet header + for (idx = 0; idx < (rx_length - 2); idx++) + { + if (rxpacket[idx] == 0xFF && rxpacket[idx+1] == 0xFF && rxpacket[idx+2] == 0xFD) + break; + } - // remove header (0xFF 0xFF 0xFD) - for(UINT8_T _s = 0; _s < _rx_length - 3; _s++) - rxpacket[_s] = rxpacket[3 + _s]; - _rx_length -= 3; - } - } - else - { - // remove unnecessary packets - for(UINT8_T _s = 0; _s < _rx_length - _idx; _s++) - rxpacket[_s] = rxpacket[_idx + _s]; - _rx_length -= _idx; - } + if (idx == 0) // found at the beginning of the packet + { + // verify CRC16 + uint16_t crc = DXL_MAKEWORD(rxpacket[STATUS_LENGTH-2], rxpacket[STATUS_LENGTH-1]); + + if (updateCRC(0, rxpacket, STATUS_LENGTH - 2) == crc) + { + result = COMM_SUCCESS; + + id_list.push_back(rxpacket[PKT_ID]); + + for (uint8_t s = 0; s < rx_length - STATUS_LENGTH; s++) + rxpacket[s] = rxpacket[STATUS_LENGTH + s]; + rx_length -= STATUS_LENGTH; + + if (rx_length == 0) + return result; + } + else + { + result = COMM_RX_CORRUPT; + + // remove header (0xFF 0xFF 0xFD) + for (uint8_t s = 0; s < rx_length - 3; s++) + rxpacket[s] = rxpacket[3 + s]; + rx_length -= 3; + } + } + else + { + // remove unnecessary packets + for (uint8_t s = 0; s < rx_length - idx; s++) + rxpacket[s] = rxpacket[idx + s]; + rx_length -= idx; } + } - return _result; + return result; } -int Protocol2PacketHandler::Action(PortHandler *port, UINT8_T id) +int Protocol2PacketHandler::action(PortHandler *port, uint8_t id) { - UINT8_T txpacket[10] = {0}; + uint8_t txpacket[10] = {0}; - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 3; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_ACTION; + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = 3; + txpacket[PKT_LENGTH_H] = 0; + txpacket[PKT_INSTRUCTION] = INST_ACTION; - return TxRxPacket(port, txpacket, 0); + return txRxPacket(port, txpacket, 0); } -int Protocol2PacketHandler::Reboot(PortHandler *port, UINT8_T id, UINT8_T *error) +int Protocol2PacketHandler::reboot(PortHandler *port, uint8_t id, uint8_t *error) { - UINT8_T txpacket[10] = {0}; - UINT8_T rxpacket[11] = {0}; + uint8_t txpacket[10] = {0}; + uint8_t rxpacket[11] = {0}; - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 3; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_REBOOT; + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = 3; + txpacket[PKT_LENGTH_H] = 0; + txpacket[PKT_INSTRUCTION] = INST_REBOOT; - return TxRxPacket(port, txpacket, rxpacket, error); + return txRxPacket(port, txpacket, rxpacket, error); } -int Protocol2PacketHandler::FactoryReset(PortHandler *port, UINT8_T id, UINT8_T option, UINT8_T *error) +int Protocol2PacketHandler::factoryReset(PortHandler *port, uint8_t id, uint8_t option, uint8_t *error) { - UINT8_T txpacket[11] = {0}; - UINT8_T rxpacket[11] = {0}; + uint8_t txpacket[11] = {0}; + uint8_t rxpacket[11] = {0}; - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 4; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_FACTORY_RESET; - txpacket[PKT_PARAMETER0] = option; + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = 4; + txpacket[PKT_LENGTH_H] = 0; + txpacket[PKT_INSTRUCTION] = INST_FACTORY_RESET; + txpacket[PKT_PARAMETER0] = option; - return TxRxPacket(port, txpacket, rxpacket, error); + return txRxPacket(port, txpacket, rxpacket, error); } -int Protocol2PacketHandler::ReadTx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length) +int Protocol2PacketHandler::readTx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length) { - int _result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - UINT8_T txpacket[14] = {0}; + uint8_t txpacket[14] = {0}; - if(id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; + if (id >= BROADCAST_ID) + return COMM_NOT_AVAILABLE; - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 7; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_READ; - txpacket[PKT_PARAMETER0+0] = (UINT8_T)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (UINT8_T)DXL_HIBYTE(address); - txpacket[PKT_PARAMETER0+2] = (UINT8_T)DXL_LOBYTE(length); - txpacket[PKT_PARAMETER0+3] = (UINT8_T)DXL_HIBYTE(length); + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = 7; + txpacket[PKT_LENGTH_H] = 0; + txpacket[PKT_INSTRUCTION] = INST_READ; + txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); + txpacket[PKT_PARAMETER0+2] = (uint8_t)DXL_LOBYTE(length); + txpacket[PKT_PARAMETER0+3] = (uint8_t)DXL_HIBYTE(length); - _result = TxPacket(port, txpacket); + result = txPacket(port, txpacket); - // set packet timeout - if(_result == COMM_SUCCESS) - port->SetPacketTimeout((UINT16_T)(length + 11)); + // set packet timeout + if (result == COMM_SUCCESS) + port->setPacketTimeout((uint16_t)(length + 11)); - return _result; + return result; } -int Protocol2PacketHandler::ReadRx(PortHandler *port, UINT16_T length, UINT8_T *data, UINT8_T *error) +int Protocol2PacketHandler::readRx(PortHandler *port, uint16_t length, uint8_t *data, uint8_t *error) { - int _result = COMM_TX_FAIL; - UINT8_T *rxpacket = (UINT8_T *)malloc(RXPACKET_MAX_LEN);//(length + 11 + (length/3)); // (length/3): consider stuffing - //UINT8_T *rxpacket = new UINT8_T[length + 11 + (length/3)]; // (length/3): consider stuffing - - _result = RxPacket(port, rxpacket); - if(_result == COMM_SUCCESS) - { - if(error != 0) - *error = (UINT8_T)rxpacket[PKT_ERROR]; - for(UINT8_T _s = 0; _s < length; _s++) - data[_s] = rxpacket[PKT_PARAMETER0 + 1 + _s]; - //memcpy(data, &rxpacket[PKT_PARAMETER0+1], length); - } - - free(rxpacket); - //delete[] rxpacket; - return _result; + int result = COMM_TX_FAIL; + uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN); + //(length + 11 + (length/3)); // (length/3): consider stuffing + //uint8_t *rxpacket = new uint8_t[length + 11 + (length/3)]; // (length/3): consider stuffing + + result = rxPacket(port, rxpacket); + if (result == COMM_SUCCESS) + { + if (error != 0) + *error = (uint8_t)rxpacket[PKT_ERROR]; + for (uint8_t s = 0; s < length; s++) + data[s] = rxpacket[PKT_PARAMETER0 + 1 + s]; + //memcpy(data, &rxpacket[PKT_PARAMETER0+1], length); + } + + free(rxpacket); + //delete[] rxpacket; + return result; } -int Protocol2PacketHandler::ReadTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) +int Protocol2PacketHandler::readTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) { - int _result = COMM_TX_FAIL; - - UINT8_T txpacket[14] = {0}; - UINT8_T *rxpacket = (UINT8_T *)malloc(RXPACKET_MAX_LEN);//(length + 11 + (length/3)); // (length/3): consider stuffing - - if(id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 7; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_READ; - txpacket[PKT_PARAMETER0+0] = (UINT8_T)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (UINT8_T)DXL_HIBYTE(address); - txpacket[PKT_PARAMETER0+2] = (UINT8_T)DXL_LOBYTE(length); - txpacket[PKT_PARAMETER0+3] = (UINT8_T)DXL_HIBYTE(length); - - _result = TxRxPacket(port, txpacket, rxpacket, error); - if(_result == COMM_SUCCESS) - { - if(error != 0) - *error = (UINT8_T)rxpacket[PKT_ERROR]; - for(UINT8_T _s = 0; _s < length; _s++) - data[_s] = rxpacket[PKT_PARAMETER0 + 1 + _s]; - //memcpy(data, &rxpacket[PKT_PARAMETER0+1], length); - } - - free(rxpacket); - //delete[] rxpacket; - return _result; + int result = COMM_TX_FAIL; + + uint8_t txpacket[14] = {0}; + uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN); + //(length + 11 + (length/3)); // (length/3): consider stuffing + + if (id >= BROADCAST_ID) + return COMM_NOT_AVAILABLE; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = 7; + txpacket[PKT_LENGTH_H] = 0; + txpacket[PKT_INSTRUCTION] = INST_READ; + txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); + txpacket[PKT_PARAMETER0+2] = (uint8_t)DXL_LOBYTE(length); + txpacket[PKT_PARAMETER0+3] = (uint8_t)DXL_HIBYTE(length); + + result = txRxPacket(port, txpacket, rxpacket, error); + if (result == COMM_SUCCESS) + { + if (error != 0) + *error = (uint8_t)rxpacket[PKT_ERROR]; + for (uint8_t s = 0; s < length; s++) + data[s] = rxpacket[PKT_PARAMETER0 + 1 + s]; + //memcpy(data, &rxpacket[PKT_PARAMETER0+1], length); + } + + free(rxpacket); + //delete[] rxpacket; + return result; } -int Protocol2PacketHandler::Read1ByteTx(PortHandler *port, UINT8_T id, UINT16_T address) +int Protocol2PacketHandler::read1ByteTx(PortHandler *port, uint8_t id, uint16_t address) { - return ReadTx(port, id, address, 1); + return readTx(port, id, address, 1); } -int Protocol2PacketHandler::Read1ByteRx(PortHandler *port, UINT8_T *data, UINT8_T *error) +int Protocol2PacketHandler::read1ByteRx(PortHandler *port, uint8_t *data, uint8_t *error) { - UINT8_T _data[1] = {0}; - int _result = ReadRx(port, 1, _data, error); - if(_result == COMM_SUCCESS) - *data = _data[0]; - return _result; + uint8_t data_read[1] = {0}; + int result = readRx(port, 1, data_read, error); + if (result == COMM_SUCCESS) + *data = data_read[0]; + return result; } -int Protocol2PacketHandler::Read1ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T *data, UINT8_T *error) +int Protocol2PacketHandler::read1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error) { - UINT8_T _data[1] = {0}; - int _result = ReadTxRx(port, id, address, 1, _data, error); - if(_result == COMM_SUCCESS) - *data = _data[0]; - return _result; + uint8_t data_read[1] = {0}; + int result = readTxRx(port, id, address, 1, data_read, error); + if (result == COMM_SUCCESS) + *data = data_read[0]; + return result; } -int Protocol2PacketHandler::Read2ByteTx(PortHandler *port, UINT8_T id, UINT16_T address) +int Protocol2PacketHandler::read2ByteTx(PortHandler *port, uint8_t id, uint16_t address) { - return ReadTx(port, id, address, 2); + return readTx(port, id, address, 2); } -int Protocol2PacketHandler::Read2ByteRx(PortHandler *port, UINT16_T *data, UINT8_T *error) +int Protocol2PacketHandler::read2ByteRx(PortHandler *port, uint16_t *data, uint8_t *error) { - UINT8_T _data[2] = {0}; - int _result = ReadRx(port, 2, _data, error); - if(_result == COMM_SUCCESS) - *data = DXL_MAKEWORD(_data[0], _data[1]); - return _result; + uint8_t data_read[2] = {0}; + int result = readRx(port, 2, data_read, error); + if (result == COMM_SUCCESS) + *data = DXL_MAKEWORD(data_read[0], data_read[1]); + return result; } -int Protocol2PacketHandler::Read2ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T *data, UINT8_T *error) +int Protocol2PacketHandler::read2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error) { - UINT8_T _data[2] = {0}; - int _result = ReadTxRx(port, id, address, 2, _data, error); - if(_result == COMM_SUCCESS) - *data = DXL_MAKEWORD(_data[0], _data[1]); - return _result; + uint8_t data_read[2] = {0}; + int result = readTxRx(port, id, address, 2, data_read, error); + if (result == COMM_SUCCESS) + *data = DXL_MAKEWORD(data_read[0], data_read[1]); + return result; } -int Protocol2PacketHandler::Read4ByteTx(PortHandler *port, UINT8_T id, UINT16_T address) +int Protocol2PacketHandler::read4ByteTx(PortHandler *port, uint8_t id, uint16_t address) { - return ReadTx(port, id, address, 4); + return readTx(port, id, address, 4); } -int Protocol2PacketHandler::Read4ByteRx(PortHandler *port, UINT32_T *data, UINT8_T *error) +int Protocol2PacketHandler::read4ByteRx(PortHandler *port, uint32_t *data, uint8_t *error) { - UINT8_T _data[4] = {0}; - int _result = ReadRx(port, 4, _data, error); - if(_result == COMM_SUCCESS) - *data = DXL_MAKEDWORD(DXL_MAKEWORD(_data[0], _data[1]), DXL_MAKEWORD(_data[2], _data[3])); - return _result; + uint8_t data_read[4] = {0}; + int result = readRx(port, 4, data_read, error); + if (result == COMM_SUCCESS) + *data = DXL_MAKEDWORD(DXL_MAKEWORD(data_read[0], data_read[1]), DXL_MAKEWORD(data_read[2], data_read[3])); + return result; } -int Protocol2PacketHandler::Read4ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T *data, UINT8_T *error) +int Protocol2PacketHandler::read4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error) { - UINT8_T _data[4] = {0}; - int _result = ReadTxRx(port, id, address, 4, _data, error); - if(_result == COMM_SUCCESS) - *data = DXL_MAKEDWORD(DXL_MAKEWORD(_data[0], _data[1]), DXL_MAKEWORD(_data[2], _data[3])); - return _result; + uint8_t data_read[4] = {0}; + int result = readTxRx(port, id, address, 4, data_read, error); + if (result == COMM_SUCCESS) + *data = DXL_MAKEDWORD(DXL_MAKEWORD(data_read[0], data_read[1]), DXL_MAKEWORD(data_read[2], data_read[3])); + return result; } -int Protocol2PacketHandler::WriteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data) +int Protocol2PacketHandler::writeTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) { - int _result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - UINT8_T *txpacket = (UINT8_T *)malloc(length+12); - //UINT8_T *txpacket = new UINT8_T[length+12]; + uint8_t *txpacket = (uint8_t *)malloc(length+12); + //uint8_t *txpacket = new uint8_t[length+12]; - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); - txpacket[PKT_INSTRUCTION] = INST_WRITE; - txpacket[PKT_PARAMETER0+0] = (UINT8_T)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (UINT8_T)DXL_HIBYTE(address); + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); + txpacket[PKT_INSTRUCTION] = INST_WRITE; + txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); - for(UINT8_T _s = 0; _s < length; _s++) - txpacket[PKT_PARAMETER0+2+_s] = data[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); + for (uint8_t s = 0; s < length; s++) + txpacket[PKT_PARAMETER0+2+s] = data[s]; + //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); - _result = TxPacket(port, txpacket); - port->is_using = false; + result = txPacket(port, txpacket); + port->is_using_ = false; - free(txpacket); - //delete[] txpacket; - return _result; + free(txpacket); + //delete[] txpacket; + return result; } -int Protocol2PacketHandler::WriteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) +int Protocol2PacketHandler::writeTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) { - int _result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - UINT8_T *txpacket = (UINT8_T *)malloc(length + 12); - //UINT8_T *txpacket = new UINT8_T[length+12]; - UINT8_T rxpacket[11] = {0}; + uint8_t *txpacket = (uint8_t *)malloc(length + 12); + //uint8_t *txpacket = new uint8_t[length+12]; + uint8_t rxpacket[11] = {0}; - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); - txpacket[PKT_INSTRUCTION] = INST_WRITE; - txpacket[PKT_PARAMETER0+0] = (UINT8_T)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (UINT8_T)DXL_HIBYTE(address); + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); + txpacket[PKT_INSTRUCTION] = INST_WRITE; + txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); - for(UINT8_T _s = 0; _s < length; _s++) - txpacket[PKT_PARAMETER0+2+_s] = data[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); + for (uint8_t s = 0; s < length; s++) + txpacket[PKT_PARAMETER0+2+s] = data[s]; + //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); - _result = TxRxPacket(port, txpacket, rxpacket, error); + result = txRxPacket(port, txpacket, rxpacket, error); - free(txpacket); - //delete[] txpacket; - return _result; + free(txpacket); + //delete[] txpacket; + return result; } -int Protocol2PacketHandler::Write1ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data) +int Protocol2PacketHandler::write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data) { - UINT8_T _data[1] = { data }; - return WriteTxOnly(port, id, address, 1, _data); + uint8_t data_write[1] = { data }; + return writeTxOnly(port, id, address, 1, data_write); } -int Protocol2PacketHandler::Write1ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data, UINT8_T *error) +int Protocol2PacketHandler::write1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error) { - UINT8_T _data[1] = { data }; - return WriteTxRx(port, id, address, 1, _data, error); + uint8_t data_write[1] = { data }; + return writeTxRx(port, id, address, 1, data_write, error); } -int Protocol2PacketHandler::Write2ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data) +int Protocol2PacketHandler::write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data) { - UINT8_T _data[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; - return WriteTxOnly(port, id, address, 2, _data); + uint8_t data_write[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; + return writeTxOnly(port, id, address, 2, data_write); } -int Protocol2PacketHandler::Write2ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data, UINT8_T *error) +int Protocol2PacketHandler::write2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error) { - UINT8_T _data[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; - return WriteTxRx(port, id, address, 2, _data, error); + uint8_t data_write[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; + return writeTxRx(port, id, address, 2, data_write, error); } -int Protocol2PacketHandler::Write4ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data) +int Protocol2PacketHandler::write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data) { - UINT8_T _data[4] = { DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data)) }; - return WriteTxOnly(port, id, address, 4, _data); + uint8_t data_write[4] = { DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data)) }; + return writeTxOnly(port, id, address, 4, data_write); } -int Protocol2PacketHandler::Write4ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data, UINT8_T *error) +int Protocol2PacketHandler::write4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error) { - UINT8_T _data[4] = { DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data)) }; - return WriteTxRx(port, id, address, 4, _data, error); + uint8_t data_write[4] = { DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data)) }; + return writeTxRx(port, id, address, 4, data_write, error); } -int Protocol2PacketHandler::RegWriteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data) +int Protocol2PacketHandler::regWriteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) { - int _result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - UINT8_T *txpacket = (UINT8_T *)malloc(length + 12); - //UINT8_T *txpacket = new UINT8_T[length+12]; + uint8_t *txpacket = (uint8_t *)malloc(length + 12); + //uint8_t *txpacket = new uint8_t[length+12]; - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); - txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; - txpacket[PKT_PARAMETER0+0] = (UINT8_T)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (UINT8_T)DXL_HIBYTE(address); + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); + txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; + txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); - for(UINT8_T _s = 0; _s < length; _s++) - txpacket[PKT_PARAMETER0+2+_s] = data[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); + for (uint8_t s = 0; s < length; s++) + txpacket[PKT_PARAMETER0+2+s] = data[s]; + //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); - _result = TxPacket(port, txpacket); - port->is_using = false; + result = txPacket(port, txpacket); + port->is_using_ = false; - free(txpacket); - //delete[] txpacket; - return _result; + free(txpacket); + //delete[] txpacket; + return result; } -int Protocol2PacketHandler::RegWriteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) +int Protocol2PacketHandler::regWriteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) { - int _result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - UINT8_T *txpacket = (UINT8_T *)malloc(length + 12); - //UINT8_T *txpacket = new UINT8_T[length+12]; - UINT8_T rxpacket[11] = {0}; + uint8_t *txpacket = (uint8_t *)malloc(length + 12); + //uint8_t *txpacket = new uint8_t[length+12]; + uint8_t rxpacket[11] = {0}; - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); - txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; - txpacket[PKT_PARAMETER0+0] = (UINT8_T)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (UINT8_T)DXL_HIBYTE(address); + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); + txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; + txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); - for(UINT8_T _s = 0; _s < length; _s++) - txpacket[PKT_PARAMETER0+2+_s] = data[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); + for (uint8_t s = 0; s < length; s++) + txpacket[PKT_PARAMETER0+2+s] = data[s]; + //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); - _result = TxRxPacket(port, txpacket, rxpacket, error); + result = txRxPacket(port, txpacket, rxpacket, error); - free(txpacket); - //delete[] txpacket; - return _result; + free(txpacket); + //delete[] txpacket; + return result; } -int Protocol2PacketHandler::SyncReadTx(PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length) +int Protocol2PacketHandler::syncReadTx(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) { - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(param_length + 14); - // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - txpacket[PKT_INSTRUCTION] = INST_SYNC_READ; - txpacket[PKT_PARAMETER0+0] = DXL_LOBYTE(start_address); - txpacket[PKT_PARAMETER0+1] = DXL_HIBYTE(start_address); - txpacket[PKT_PARAMETER0+2] = DXL_LOBYTE(data_length); - txpacket[PKT_PARAMETER0+3] = DXL_HIBYTE(data_length); - - for(UINT8_T _s = 0; _s < param_length; _s++) - txpacket[PKT_PARAMETER0+4+_s] = param[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+4], param, param_length); - - _result = TxPacket(port, txpacket); - if(_result == COMM_SUCCESS) - port->SetPacketTimeout((UINT16_T)((11 + data_length) * param_length)); - - free(txpacket); - return _result; + int result = COMM_TX_FAIL; + + uint8_t *txpacket = (uint8_t *)malloc(param_length + 14); + // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + + txpacket[PKT_ID] = BROADCAST_ID; + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + txpacket[PKT_INSTRUCTION] = INST_SYNC_READ; + txpacket[PKT_PARAMETER0+0] = DXL_LOBYTE(start_address); + txpacket[PKT_PARAMETER0+1] = DXL_HIBYTE(start_address); + txpacket[PKT_PARAMETER0+2] = DXL_LOBYTE(data_length); + txpacket[PKT_PARAMETER0+3] = DXL_HIBYTE(data_length); + + for (uint8_t s = 0; s < param_length; s++) + txpacket[PKT_PARAMETER0+4+s] = param[s]; + //memcpy(&txpacket[PKT_PARAMETER0+4], param, param_length); + + result = txPacket(port, txpacket); + if (result == COMM_SUCCESS) + port->setPacketTimeout((uint16_t)((11 + data_length) * param_length)); + + free(txpacket); + return result; } -int Protocol2PacketHandler::SyncWriteTxOnly(PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length) +int Protocol2PacketHandler::syncWriteTxOnly(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) { - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(param_length + 14); - //UINT8_T *txpacket = new UINT8_T[param_length + 14]; - // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE; - txpacket[PKT_PARAMETER0+0] = DXL_LOBYTE(start_address); - txpacket[PKT_PARAMETER0+1] = DXL_HIBYTE(start_address); - txpacket[PKT_PARAMETER0+2] = DXL_LOBYTE(data_length); - txpacket[PKT_PARAMETER0+3] = DXL_HIBYTE(data_length); - - for(UINT8_T _s = 0; _s < param_length; _s++) - txpacket[PKT_PARAMETER0+4+_s] = param[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+4], param, param_length); - - _result = TxRxPacket(port, txpacket, 0, 0); - - free(txpacket); - //delete[] txpacket; - return _result; + int result = COMM_TX_FAIL; + + uint8_t *txpacket = (uint8_t *)malloc(param_length + 14); + //uint8_t *txpacket = new uint8_t[param_length + 14]; + // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + + txpacket[PKT_ID] = BROADCAST_ID; + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE; + txpacket[PKT_PARAMETER0+0] = DXL_LOBYTE(start_address); + txpacket[PKT_PARAMETER0+1] = DXL_HIBYTE(start_address); + txpacket[PKT_PARAMETER0+2] = DXL_LOBYTE(data_length); + txpacket[PKT_PARAMETER0+3] = DXL_HIBYTE(data_length); + + for (uint8_t s = 0; s < param_length; s++) + txpacket[PKT_PARAMETER0+4+s] = param[s]; + //memcpy(&txpacket[PKT_PARAMETER0+4], param, param_length); + + result = txRxPacket(port, txpacket, 0, 0); + + free(txpacket); + //delete[] txpacket; + return result; } -int Protocol2PacketHandler::BulkReadTx(PortHandler *port, UINT8_T *param, UINT16_T param_length) +int Protocol2PacketHandler::bulkReadTx(PortHandler *port, uint8_t *param, uint16_t param_length) { - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(param_length + 10); - //UINT8_T *txpacket = new UINT8_T[param_length + 10]; - // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H - txpacket[PKT_INSTRUCTION] = INST_BULK_READ; - - for(UINT8_T _s = 0; _s < param_length; _s++) - txpacket[PKT_PARAMETER0+_s] = param[_s]; - //memcpy(&txpacket[PKT_PARAMETER0], param, param_length); - - _result = TxPacket(port, txpacket); - if(_result == COMM_SUCCESS) - { - int _wait_length = 0; - for(int _i = 0; _i < param_length; _i += 5) - _wait_length += DXL_MAKEWORD(param[_i+3], param[_i+4]) + 10; - port->SetPacketTimeout((UINT16_T)_wait_length); - } - - free(txpacket); - //delete[] txpacket; - return _result; + int result = COMM_TX_FAIL; + + uint8_t *txpacket = (uint8_t *)malloc(param_length + 10); + //uint8_t *txpacket = new uint8_t[param_length + 10]; + // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H + + txpacket[PKT_ID] = BROADCAST_ID; + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H + txpacket[PKT_INSTRUCTION] = INST_BULK_READ; + + for (uint8_t s = 0; s < param_length; s++) + txpacket[PKT_PARAMETER0+s] = param[s]; + //memcpy(&txpacket[PKT_PARAMETER0], param, param_length); + + result = txPacket(port, txpacket); + if (result == COMM_SUCCESS) + { + int wait_length = 0; + for (int i = 0; i < param_length; i += 5) + wait_length += DXL_MAKEWORD(param[i+3], param[i+4]) + 10; + port->setPacketTimeout((uint16_t)wait_length); + } + + free(txpacket); + //delete[] txpacket; + return result; } -int Protocol2PacketHandler::BulkWriteTxOnly(PortHandler *port, UINT8_T *param, UINT16_T param_length) +int Protocol2PacketHandler::bulkWriteTxOnly(PortHandler *port, uint8_t *param, uint16_t param_length) { - int _result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - UINT8_T *txpacket = (UINT8_T *)malloc(param_length + 10); - //UINT8_T *txpacket = new UINT8_T[param_length + 10]; - // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H + uint8_t *txpacket = (uint8_t *)malloc(param_length + 10); + //uint8_t *txpacket = new uint8_t[param_length + 10]; + // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H - txpacket[PKT_INSTRUCTION] = INST_BULK_WRITE; + txpacket[PKT_ID] = BROADCAST_ID; + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H + txpacket[PKT_INSTRUCTION] = INST_BULK_WRITE; - for(UINT8_T _s = 0; _s < param_length; _s++) - txpacket[PKT_PARAMETER0+_s] = param[_s]; - //memcpy(&txpacket[PKT_PARAMETER0], param, param_length); + for (uint8_t s = 0; s < param_length; s++) + txpacket[PKT_PARAMETER0+s] = param[s]; + //memcpy(&txpacket[PKT_PARAMETER0], param, param_length); - _result = TxRxPacket(port, txpacket, 0, 0); + result = txRxPacket(port, txpacket, 0, 0); - free(txpacket); - //delete[] txpacket; - return _result; + free(txpacket); + //delete[] txpacket; + return result; } diff --git a/dynamixel_sdk/src/dynamixel_sdk_linux/port_handler_linux.cpp b/dynamixel_sdk/src/dynamixel_sdk_linux/port_handler_linux.cpp index 96c3f75..0cc11e0 100644 --- a/dynamixel_sdk/src/dynamixel_sdk_linux/port_handler_linux.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk_linux/port_handler_linux.cpp @@ -1,8 +1,39 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* * Redistributions of source code must retain the above copyright notice, this +* list of conditions and the following disclaimer. +* +* * Redistributions in binary form must reproduce the above copyright notice, +* this list of conditions and the following disclaimer in the documentation +* and/or other materials provided with the distribution. +* +* * Neither the name of ROBOTIS nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*******************************************************************************/ + +/* Author: zerom, Leon Ryu Woon Jung */ + /* - * PortHandlerLinux.cpp + * port_handler_linux.cpp * * Created on: 2016. 1. 26. - * Author: zerom, leon */ #include @@ -15,231 +46,231 @@ #include #include -#include "dynamixel_sdk_linux/PortHandlerLinux.h" +#include "dynamixel_sdk_linux/port_handler_linux.h" #define LATENCY_TIMER 4 // msec (USB latency timer) -using namespace ROBOTIS; +using namespace dynamixel; PortHandlerLinux::PortHandlerLinux(const char *port_name) - : socket_fd_(-1), - baudrate_(DEFAULT_BAUDRATE), - packet_start_time_(0.0), - packet_timeout_(0.0), - tx_time_per_byte(0.0) + : socket_fd_(-1), + baudrate_(DEFAULT_BAUDRATE_), + packet_start_time_(0.0), + packet_timeout_(0.0), + tx_time_per_byte(0.0) { - is_using = false; - SetPortName(port_name); + is_using_ = false; + setPortName(port_name); } -bool PortHandlerLinux::OpenPort() +bool PortHandlerLinux::openPort() { - return SetBaudRate(baudrate_); + return setBaudRate(baudrate_); } -void PortHandlerLinux::ClosePort() +void PortHandlerLinux::closePort() { - if(socket_fd_ != -1) - close(socket_fd_); - socket_fd_ = -1; + if(socket_fd_ != -1) + close(socket_fd_); + socket_fd_ = -1; } -void PortHandlerLinux::ClearPort() +void PortHandlerLinux::clearPort() { - tcflush(socket_fd_, TCIOFLUSH); + tcflush(socket_fd_, TCIOFLUSH); } -void PortHandlerLinux::SetPortName(const char *port_name) +void PortHandlerLinux::setPortName(const char *port_name) { - strcpy(port_name_, port_name); + strcpy(port_name_, port_name); } -char *PortHandlerLinux::GetPortName() +char *PortHandlerLinux::getPortName() { - return port_name_; + return port_name_; } // TODO: baud number ?? -bool PortHandlerLinux::SetBaudRate(const int baudrate) +bool PortHandlerLinux::setBaudRate(const int baudrate) { - int _baud = GetCFlagBaud(baudrate); - - ClosePort(); - - if(_baud <= 0) // custom baudrate - { - SetupPort(B38400); - baudrate_ = baudrate; - return SetCustomBaudrate(baudrate); - } - else - { - baudrate_ = baudrate; - return SetupPort(_baud); - } + int baud = getCFlagBaud(baudrate); + + closePort(); + + if(baud <= 0) // custom baudrate + { + setupPort(B38400); + baudrate_ = baudrate; + return setCustomBaudrate(baudrate); + } + else + { + baudrate_ = baudrate; + return setupPort(baud); + } } -int PortHandlerLinux::GetBaudRate() +int PortHandlerLinux::getBaudRate() { - return baudrate_; + return baudrate_; } -int PortHandlerLinux::GetBytesAvailable() +int PortHandlerLinux::getBytesAvailable() { - int _bytes_available; - ioctl(socket_fd_, FIONREAD, &_bytes_available); - return _bytes_available; + int bytes_available; + ioctl(socket_fd_, FIONREAD, &bytes_available); + return bytes_available; } -int PortHandlerLinux::ReadPort(UINT8_T *packet, int length) +int PortHandlerLinux::readPort(uint8_t *packet, int length) { - return read(socket_fd_, packet, length); + return read(socket_fd_, packet, length); } -int PortHandlerLinux::WritePort(UINT8_T *packet, int length) +int PortHandlerLinux::writePort(uint8_t *packet, int length) { - return write(socket_fd_, packet, length); + return write(socket_fd_, packet, length); } -void PortHandlerLinux::SetPacketTimeout(UINT16_T packet_length) +void PortHandlerLinux::setPacketTimeout(uint16_t packet_length) { - packet_start_time_ = GetCurrentTime(); - packet_timeout_ = (tx_time_per_byte * (double)packet_length) + (LATENCY_TIMER * 2.0) + 2.0; + packet_start_time_ = getCurrentTime(); + packet_timeout_ = (tx_time_per_byte * (double)packet_length) + (LATENCY_TIMER * 2.0) + 2.0; } -void PortHandlerLinux::SetPacketTimeout(double msec) +void PortHandlerLinux::setPacketTimeout(double msec) { - packet_start_time_ = GetCurrentTime(); - packet_timeout_ = msec; + packet_start_time_ = getCurrentTime(); + packet_timeout_ = msec; } -bool PortHandlerLinux::IsPacketTimeout() +bool PortHandlerLinux::isPacketTimeout() { - if(GetTimeSinceStart() > packet_timeout_) - { - packet_timeout_ = 0; - return true; - } - return false; + if(getTimeSinceStart() > packet_timeout_) + { + packet_timeout_ = 0; + return true; + } + return false; } -double PortHandlerLinux::GetCurrentTime() +double PortHandlerLinux::getCurrentTime() { - struct timespec _tv; - clock_gettime( CLOCK_REALTIME, &_tv); - return ((double)_tv.tv_sec*1000.0 + (double)_tv.tv_nsec*0.001*0.001); + struct timespec tv; + clock_gettime( CLOCK_REALTIME, &tv); + return ((double)tv.tv_sec*1000.0 + (double)tv.tv_nsec*0.001*0.001); } -double PortHandlerLinux::GetTimeSinceStart() +double PortHandlerLinux::getTimeSinceStart() { - double _time; + double time; - _time = GetCurrentTime() - packet_start_time_; - if(_time < 0.0) - packet_start_time_ = GetCurrentTime(); + time = getCurrentTime() - packet_start_time_; + if(time < 0.0) + packet_start_time_ = getCurrentTime(); - return _time; + return time; } -bool PortHandlerLinux::SetupPort(int cflag_baud) +bool PortHandlerLinux::setupPort(int cflag_baud) { - struct termios newtio; + struct termios newtio; - socket_fd_ = open(port_name_, O_RDWR|O_NOCTTY|O_NONBLOCK); - if(socket_fd_ < 0) - { - printf("[PortHandlerLinux::SetupPort] Error opening serial port!\n"); - return false; - } + socket_fd_ = open(port_name_, O_RDWR|O_NOCTTY|O_NONBLOCK); + if(socket_fd_ < 0) + { + printf("[PortHandlerLinux::SetupPort] Error opening serial port!\n"); + return false; + } - bzero(&newtio, sizeof(newtio)); // clear struct for new port settings + bzero(&newtio, sizeof(newtio)); // clear struct for new port settings - newtio.c_cflag = cflag_baud | CS8 | CLOCAL | CREAD; - newtio.c_iflag = IGNPAR; - newtio.c_oflag = 0; - newtio.c_lflag = 0; - newtio.c_cc[VTIME] = 0; - newtio.c_cc[VMIN] = 0; + newtio.c_cflag = cflag_baud | CS8 | CLOCAL | CREAD; + newtio.c_iflag = IGNPAR; + newtio.c_oflag = 0; + newtio.c_lflag = 0; + newtio.c_cc[VTIME] = 0; + newtio.c_cc[VMIN] = 0; - // clean the buffer and activate the settings for the port - tcflush(socket_fd_, TCIFLUSH); - tcsetattr(socket_fd_, TCSANOW, &newtio); + // clean the buffer and activate the settings for the port + tcflush(socket_fd_, TCIFLUSH); + tcsetattr(socket_fd_, TCSANOW, &newtio); - tx_time_per_byte = (1000.0 / (double)baudrate_) * 10.0; - return true; + tx_time_per_byte = (1000.0 / (double)baudrate_) * 10.0; + return true; } -bool PortHandlerLinux::SetCustomBaudrate(int speed) +bool PortHandlerLinux::setCustomBaudrate(int speed) { - // try to set a custom divisor - struct serial_struct ss; - if(ioctl(socket_fd_, TIOCGSERIAL, &ss) != 0) - { - printf("[PortHandlerLinux::SetCustomBaudrate] TIOCGSERIAL failed!\n"); - return false; - } + // try to set a custom divisor + struct serial_struct ss; + if(ioctl(socket_fd_, TIOCGSERIAL, &ss) != 0) + { + printf("[PortHandlerLinux::SetCustomBaudrate] TIOCGSERIAL failed!\n"); + return false; + } - ss.flags = (ss.flags & ~ASYNC_SPD_MASK) | ASYNC_SPD_CUST; - ss.custom_divisor = (ss.baud_base + (speed / 2)) / speed; - int closest_speed = ss.baud_base / ss.custom_divisor; + ss.flags = (ss.flags & ~ASYNC_SPD_MASK) | ASYNC_SPD_CUST; + ss.custom_divisor = (ss.baud_base + (speed / 2)) / speed; + int closest_speed = ss.baud_base / ss.custom_divisor; - if(closest_speed < speed * 98 / 100 || closest_speed > speed * 102 / 100) - { - printf("[PortHandlerLinux::SetCustomBaudrate] Cannot set speed to %d, closest is %d \n", speed, closest_speed); - return false; - } + if(closest_speed < speed * 98 / 100 || closest_speed > speed * 102 / 100) + { + printf("[PortHandlerLinux::SetCustomBaudrate] Cannot set speed to %d, closest is %d \n", speed, closest_speed); + return false; + } - if(ioctl(socket_fd_, TIOCSSERIAL, &ss) < 0) - { - printf("[PortHandlerLinux::SetCustomBaudrate] TIOCSSERIAL failed!\n"); - return false; - } + if(ioctl(socket_fd_, TIOCSSERIAL, &ss) < 0) + { + printf("[PortHandlerLinux::SetCustomBaudrate] TIOCSSERIAL failed!\n"); + return false; + } - tx_time_per_byte = (1000.0 / (double)speed) * 10.0; - return true; + tx_time_per_byte = (1000.0 / (double)speed) * 10.0; + return true; } -int PortHandlerLinux::GetCFlagBaud(int baudrate) +int PortHandlerLinux::getCFlagBaud(int baudrate) { - switch(baudrate) - { + switch(baudrate) + { case 9600: - return B9600; + return B9600; case 19200: - return B19200; + return B19200; case 38400: - return B38400; + return B38400; case 57600: - return B57600; + return B57600; case 115200: - return B115200; + return B115200; case 230400: - return B230400; + return B230400; case 460800: - return B460800; + return B460800; case 500000: - return B500000; + return B500000; case 576000: - return B576000; + return B576000; case 921600: - return B921600; + return B921600; case 1000000: - return B1000000; + return B1000000; case 1152000: - return B1152000; + return B1152000; case 1500000: - return B1500000; + return B1500000; case 2000000: - return B2000000; + return B2000000; case 2500000: - return B2500000; + return B2500000; case 3000000: - return B3000000; + return B3000000; case 3500000: - return B3500000; + return B3500000; case 4000000: - return B4000000; + return B4000000; default: - return -1; - } + return -1; + } } diff --git a/robotis_controller/CMakeLists.txt b/robotis_controller/CMakeLists.txt index 5865116..42486cc 100644 --- a/robotis_controller/CMakeLists.txt +++ b/robotis_controller/CMakeLists.txt @@ -13,15 +13,6 @@ find_package(catkin REQUIRED COMPONENTS dynamixel_sdk ) -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( INCLUDE_DIRS include LIBRARIES robotis_controller @@ -29,24 +20,15 @@ catkin_package( # DEPENDS system_lib ) -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) include_directories( include ${catkin_INCLUDE_DIRS} ) -## Declare a cpp library add_library(robotis_controller - src/robotis_controller/RobotisController.cpp + src/robotis_controller/robotis_controller.cpp ) -## Specify libraries to link a library or executable target against target_link_libraries(robotis_controller yaml-cpp ${catkin_LIBRARIES} diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h index d974a3e..e2c8b64 100644 --- a/robotis_controller/include/robotis_controller/robotis_controller.h +++ b/robotis_controller/include/robotis_controller/robotis_controller.h @@ -1,12 +1,42 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + /* - * RobotisController.h + * robotis_controller.h * * Created on: 2016. 1. 15. * Author: zerom */ -#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_ROBOTIS_CONTROLLER_ROBOTISCONTROLLER_H_ -#define ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_ROBOTIS_CONTROLLER_ROBOTISCONTROLLER_H_ +#ifndef ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ +#define ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ #include @@ -16,132 +46,131 @@ #include #include -#include "robotis_device/Robot.h" -#include "robotis_framework_common/MotionModule.h" -#include "robotis_framework_common/SensorModule.h" - #include "robotis_controller_msgs/SyncWriteItem.h" #include "robotis_controller_msgs/JointCtrlModule.h" #include "robotis_controller_msgs/GetJointModule.h" -#include "dynamixel_sdk/GroupBulkRead.h" -#include "dynamixel_sdk/GroupSyncWrite.h" +#include "robotis_device/robot.h" +#include "robotis_framework_common/motion_module.h" +#include "robotis_framework_common/sensor_module.h" +#include "dynamixel_sdk/group_bulk_read.h" +#include "dynamixel_sdk/group_sync_write.h" -namespace ROBOTIS +namespace robotis_framework { -enum CONTROLLER_MODE +enum ControllerMode { - MOTION_MODULE_MODE, - DIRECT_CONTROL_MODE + MotionModuleMode, + DirectControlMode }; class RobotisController : public Singleton { private: - boost::thread queue_thread_; - boost::thread gazebo_thread_; - boost::thread set_module_thread_; - boost::mutex queue_mutex_; + boost::thread queue_thread_; + boost::thread gazebo_thread_; + boost::thread set_module_thread_; + boost::mutex queue_mutex_; - bool init_pose_loaded_; - bool is_timer_running_; - bool stop_timer_; - pthread_t timer_thread_; - CONTROLLER_MODE controller_mode_; + bool init_pose_loaded_; + bool is_timer_running_; + bool stop_timer_; + pthread_t timer_thread_; + ControllerMode controller_mode_; - std::list motion_modules_; - std::list sensor_modules_; - std::vector direct_sync_write_; + std::list motion_modules_; + std::list sensor_modules_; + std::vector direct_sync_write_; - std::map sensor_result_; + std::map sensor_result_; - void QueueThread(); - void GazeboThread(); - void SetCtrlModuleThread(std::string ctrl_module); + void gazeboTimerThread(); + void msgQueueThread(); + void setCtrlModuleThread(std::string ctrl_module); - bool CheckTimerStop(); - void InitSyncWrite(); + bool isTimerStopped(); + void initializeSyncWrite(); public: - static const int CONTROL_CYCLE_MSEC = 8; // 8 msec + static const int CONTROL_CYCLE_MSEC = 8; // 8 msec - bool DEBUG_PRINT; - Robot *robot; + bool DEBUG_PRINT; + Robot *robot_; - bool gazebo_mode; - std::string gazebo_robot_name; + bool gazebo_mode_; + std::string gazebo_robot_name_; - /* bulk read */ - std::map port_to_bulk_read; + /* bulk read */ + std::map port_to_bulk_read_; - /* sync write */ - std::map port_to_sync_write_position; - std::map port_to_sync_write_velocity; - std::map port_to_sync_write_torque; - std::map port_to_sync_write_position_p_gain; + /* sync write */ + std::map port_to_sync_write_position_; + std::map port_to_sync_write_velocity_; + std::map port_to_sync_write_torque_; + std::map port_to_sync_write_position_p_gain_; - /* publisher */ - ros::Publisher goal_joint_state_pub; - ros::Publisher present_joint_state_pub; - ros::Publisher current_module_pub; + /* publisher */ + ros::Publisher goal_joint_state_pub_; + ros::Publisher present_joint_state_pub_; + ros::Publisher current_module_pub_; - std::map gazebo_joint_pub; + std::map gazebo_joint_pub_; - static void *ThreadProc(void *param); + static void *timerThread(void *param); - RobotisController(); + RobotisController(); - bool Initialize(const std::string robot_file_path, const std::string init_file_path); - void InitDevice(const std::string init_file_path); - void Process(); + bool initialize(const std::string robot_file_path, const std::string init_file_path); + void initializeDevice(const std::string init_file_path); + void process(); - void AddMotionModule(MotionModule *module); - void RemoveMotionModule(MotionModule *module); - void AddSensorModule(SensorModule *module); - void RemoveSensorModule(SensorModule *module); + void addMotionModule(MotionModule *module); + void removeMotionModule(MotionModule *module); + void addSensorModule(SensorModule *module); + void removeSensorModule(SensorModule *module); - void StartTimer(); - void StopTimer(); - bool IsTimerRunning(); + void startTimer(); + void stopTimer(); + bool isTimerRunning(); - void LoadOffset(const std::string path); + void loadOffset(const std::string path); - /* ROS Topic Callback Functions */ - void SyncWriteItemCallback(const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg); - void SetControllerModeCallback(const std_msgs::String::ConstPtr &msg); - void SetJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); - void SetJointCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg); - void SetCtrlModuleCallback(const std_msgs::String::ConstPtr &msg); - bool GetCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res); + /* ROS Topic Callback Functions */ + void syncWriteItemCallback(const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg); + void setControllerModeCallback(const std_msgs::String::ConstPtr &msg); + void setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); + void setJointCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg); + void setCtrlModuleCallback(const std_msgs::String::ConstPtr &msg); + bool getCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res); - void GazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); + void gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); - int Ping (const std::string joint_name, UINT8_T *error = 0); - int Ping (const std::string joint_name, UINT16_T* model_number, UINT8_T *error = 0); + int ping (const std::string joint_name, uint8_t *error = 0); + int ping (const std::string joint_name, uint16_t* model_number, uint8_t *error = 0); - int Action (const std::string joint_name); - int Reboot (const std::string joint_name, UINT8_T *error = 0); - int FactoryReset(const std::string joint_name, UINT8_T option = 0, UINT8_T *error = 0); + int action (const std::string joint_name); + int reboot (const std::string joint_name, uint8_t *error = 0); + int factoryReset(const std::string joint_name, uint8_t option = 0, uint8_t *error = 0); - int Read (const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); - int ReadCtrlItem(const std::string joint_name, const std::string item_name, UINT32_T *data, UINT8_T *error = 0); + int read (const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); + int readCtrlItem(const std::string joint_name, const std::string item_name, uint32_t *data, uint8_t *error = 0); - int Read1Byte (const std::string joint_name, UINT16_T address, UINT8_T *data, UINT8_T *error = 0); - int Read2Byte (const std::string joint_name, UINT16_T address, UINT16_T *data, UINT8_T *error = 0); - int Read4Byte (const std::string joint_name, UINT16_T address, UINT32_T *data, UINT8_T *error = 0); + int read1Byte (const std::string joint_name, uint16_t address, uint8_t *data, uint8_t *error = 0); + int read2Byte (const std::string joint_name, uint16_t address, uint16_t *data, uint8_t *error = 0); + int read4Byte (const std::string joint_name, uint16_t address, uint32_t *data, uint8_t *error = 0); - int Write (const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); - int WriteCtrlItem(const std::string joint_name, const std::string item_name, UINT32_T data, UINT8_T *error = 0); + int write (const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); + int writeCtrlItem(const std::string joint_name, const std::string item_name, uint32_t data, uint8_t *error = 0); - int Write1Byte (const std::string joint_name, UINT16_T address, UINT8_T data, UINT8_T *error = 0); - int Write2Byte (const std::string joint_name, UINT16_T address, UINT16_T data, UINT8_T *error = 0); - int Write4Byte (const std::string joint_name, UINT16_T address, UINT32_T data, UINT8_T *error = 0); + int write1Byte (const std::string joint_name, uint16_t address, uint8_t data, uint8_t *error = 0); + int write2Byte (const std::string joint_name, uint16_t address, uint16_t data, uint8_t *error = 0); + int write4Byte (const std::string joint_name, uint16_t address, uint32_t data, uint8_t *error = 0); - int RegWrite (const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); + int regWrite (const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); }; } -#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_ROBOTIS_CONTROLLER_ROBOTISCONTROLLER_H_ */ +#endif /* ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ */ diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index 8a25f7e..6dcf6d2 100644 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -6,16 +6,12 @@ ROBOTIS - BSD - - ROBOTIS - catkin roscpp diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 97d283a..8d8275d 100644 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -1,5 +1,35 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + /* - * RobotisController.cpp + * robotis_controller.cpp * * Created on: 2016. 1. 15. * Author: zerom @@ -8,1781 +38,1853 @@ #include #include -#include "robotis_controller/RobotisController.h" +#include "robotis_controller/robotis_controller.h" -using namespace ROBOTIS; +using namespace robotis_framework; RobotisController::RobotisController() - : is_timer_running_(false), - stop_timer_(false), - init_pose_loaded_(false), - timer_thread_(0), - controller_mode_(MOTION_MODULE_MODE), - DEBUG_PRINT(false), - robot(0), - gazebo_mode(false), - gazebo_robot_name("robotis") + : is_timer_running_(false), + stop_timer_(false), + init_pose_loaded_(false), + timer_thread_(0), + controller_mode_(MotionModuleMode), + DEBUG_PRINT(false), + robot_(0), + gazebo_mode_(false), + gazebo_robot_name_("robotis") { - direct_sync_write_.clear(); + direct_sync_write_.clear(); } -void RobotisController::InitSyncWrite() +void RobotisController::initializeSyncWrite() { - if(gazebo_mode == true) - return; - - ROS_INFO("FIRST BULKREAD"); - // bulkread twice - for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) - _it->second->TxRxPacket(); - for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) - { - int _error_cnt = 0; - int _result = COMM_SUCCESS; - do { - if(++_error_cnt > 10) - { - ROS_ERROR("[RobotisController] bulk read fail!!"); - exit(-1); - } - usleep(10*1000); - _result = _it->second->TxRxPacket(); - } while (_result != COMM_SUCCESS); - } - init_pose_loaded_ = true; - ROS_INFO("FIRST BULKREAD END"); - - // clear syncwrite param setting - for(std::map::iterator _it = port_to_sync_write_position.begin(); _it != port_to_sync_write_position.end(); _it++) - { - if(_it->second != NULL) - _it->second->ClearParam(); - } - for(std::map::iterator _it = port_to_sync_write_position_p_gain.begin(); _it != port_to_sync_write_position_p_gain.end(); _it++) + if (gazebo_mode_ == true) + return; + + ROS_INFO("FIRST BULKREAD"); + // bulkread twice + for (std::map::iterator it = port_to_bulk_read_.begin(); + it != port_to_bulk_read_.end(); it++) + { + it->second->txRxPacket(); + } + for (std::map::iterator it = port_to_bulk_read_.begin(); + it != port_to_bulk_read_.end(); it++) + { + int error_count = 0; + int result = COMM_SUCCESS; + do { - if(_it->second != NULL) - _it->second->ClearParam(); - } - for(std::map::iterator _it = port_to_sync_write_velocity.begin(); _it != port_to_sync_write_velocity.end(); _it++) - { - if(_it->second != NULL) - _it->second->ClearParam(); - } - for(std::map::iterator _it = port_to_sync_write_torque.begin(); _it != port_to_sync_write_torque.end(); _it++) + if (++error_count > 10) + { + ROS_ERROR("[RobotisController] bulk read fail!!"); + exit(-1); + } + usleep(10 * 1000); + result = it->second->txRxPacket(); + } while (result != COMM_SUCCESS); + } + init_pose_loaded_ = true; + ROS_INFO("FIRST BULKREAD END"); + + // clear syncwrite param setting + for (std::map::iterator it = port_to_sync_write_position_.begin(); + it != port_to_sync_write_position_.end(); it++) + { + if (it->second != NULL) + it->second->clearParam(); + } + for (std::map::iterator it = port_to_sync_write_position_p_gain_.begin(); + it != port_to_sync_write_position_p_gain_.end(); it++) + { + if (it->second != NULL) + it->second->clearParam(); + } + for (std::map::iterator it = port_to_sync_write_velocity_.begin(); + it != port_to_sync_write_velocity_.end(); it++) + { + if (it->second != NULL) + it->second->clearParam(); + } + for (std::map::iterator it = port_to_sync_write_torque_.begin(); + it != port_to_sync_write_torque_.end(); it++) + { + if (it->second != NULL) + it->second->clearParam(); + } + + // set init syncwrite param(from data of bulkread) + for (std::map::iterator it = robot_->dxls_.begin(); it != robot_->dxls_.end(); it++) + { + std::string joint_name = it->first; + Dynamixel *dxl = it->second; + + for (int i = 0; i < dxl->bulk_read_items_.size(); i++) { - if(_it->second != NULL) - _it->second->ClearParam(); - } - - // set init syncwrite param(from data of bulkread) - for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) - { - std::string _joint_name = _it->first; - Dynamixel *_dxl = _it->second; - - for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++) + uint32_t read_data = 0; + uint8_t sync_write_data[4]; + + if (port_to_bulk_read_[dxl->port_name_]->isAvailable(dxl->id_, + dxl->bulk_read_items_[i]->address_, + dxl->bulk_read_items_[i]->data_length_) == true) + { + read_data = port_to_bulk_read_[dxl->port_name_]->getData(dxl->id_, + dxl->bulk_read_items_[i]->address_, + dxl->bulk_read_items_[i]->data_length_); + + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(read_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(read_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(read_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(read_data)); + + if ((dxl->present_position_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->present_position_item_->item_name_)) { - UINT32_T _read_data = 0; - UINT8_T _sync_write_data[4]; - - if(port_to_bulk_read[_dxl->port_name]->IsAvailable(_dxl->id, - _dxl->bulk_read_items[_i]->address, - _dxl->bulk_read_items[_i]->data_length) == true) - { - _read_data = port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, - _dxl->bulk_read_items[_i]->address, - _dxl->bulk_read_items[_i]->data_length); - - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_read_data)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_read_data)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_read_data)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_read_data)); - - if(_dxl->present_position_item != 0 && _dxl->bulk_read_items[_i]->item_name == _dxl->present_position_item->item_name) - { - _dxl->dxl_state->present_position = _dxl->ConvertValue2Radian(_read_data) - _dxl->dxl_state->position_offset; // remove offset - _dxl->dxl_state->goal_position = _dxl->dxl_state->present_position; + dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(read_data) - dxl->dxl_state_->position_offset_; // remove offset + dxl->dxl_state_->goal_position_ = dxl->dxl_state_->present_position_; - port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); - } - else if(_dxl->position_p_gain_item != 0 && _dxl->bulk_read_items[_i]->item_name == _dxl->position_p_gain_item->item_name) - { - _dxl->dxl_state->position_p_gain = _read_data; - } - else if(_dxl->present_velocity_item != 0 && _dxl->bulk_read_items[_i]->item_name == _dxl->present_velocity_item->item_name) - { - _dxl->dxl_state->present_velocity = _dxl->ConvertValue2Velocity(_read_data); - _dxl->dxl_state->goal_velocity = _dxl->dxl_state->present_velocity; - } - else if(_dxl->present_current_item != 0 && _dxl->bulk_read_items[_i]->item_name == _dxl->present_current_item->item_name) - { - _dxl->dxl_state->present_current = _dxl->ConvertValue2Current(_read_data); - _dxl->dxl_state->goal_current = _dxl->dxl_state->present_current; - } - } + port_to_sync_write_position_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + } + else if ((dxl->position_p_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->position_p_gain_item_->item_name_)) + { + dxl->dxl_state_->position_p_gain_ = read_data; } + else if ((dxl->present_velocity_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->present_velocity_item_->item_name_)) + { + dxl->dxl_state_->present_velocity_ = dxl->convertValue2Velocity(read_data); + dxl->dxl_state_->goal_velocity_ = dxl->dxl_state_->present_velocity_; + } + else if ((dxl->present_current_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->present_current_item_->item_name_)) + { + dxl->dxl_state_->present_current_ = dxl->convertValue2Current(read_data); + dxl->dxl_state_->goal_current_ = dxl->dxl_state_->present_current_; + } + } } + } } -bool RobotisController::Initialize(const std::string robot_file_path, const std::string init_file_path) +bool RobotisController::initialize(const std::string robot_file_path, const std::string init_file_path) { - std::string _dev_desc_dir_path = ros::package::getPath("robotis_device") + "/devices"; + std::string dev_desc_dir_path = ros::package::getPath("robotis_device") + "/devices"; + + // load robot info : port , device + robot_ = new Robot(robot_file_path, dev_desc_dir_path); - // load robot info : port , device - robot = new Robot(robot_file_path, _dev_desc_dir_path); + if (gazebo_mode_ == true) + { + queue_thread_ = boost::thread(boost::bind(&RobotisController::msgQueueThread, this)); + return true; + } - if(gazebo_mode == true) + for (std::map::iterator it = robot_->ports_.begin(); + it != robot_->ports_.end(); it++) + { + std::string port_name = it->first; + dynamixel::PortHandler *port = it->second; + dynamixel::PacketHandler *default_pkt_handler = dynamixel::PacketHandler::getPacketHandler(2.0); + + if (port->setBaudRate(port->getBaudRate()) == false) { - queue_thread_ = boost::thread(boost::bind(&RobotisController::QueueThread, this)); - return true; + ROS_ERROR("PORT [%s] SETUP ERROR! (baudrate: %d)", port_name.c_str(), port->getBaudRate()); + exit(-1); } - for(std::map::iterator _it = robot->ports.begin(); _it != robot->ports.end(); _it++) + // get the default device info of the port + std::string default_device_name = robot_->port_default_device_[port_name]; + std::map::iterator dxl_it = robot_->dxls_.find(default_device_name); + std::map::iterator sensor_it = robot_->sensors_.find(default_device_name); + if (dxl_it != robot_->dxls_.end()) + { + Dynamixel *default_device = dxl_it->second; + default_pkt_handler = dynamixel::PacketHandler::getPacketHandler(default_device->protocol_version_); + + if (default_device->goal_position_item_ != 0) + { + port_to_sync_write_position_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->goal_position_item_->address_, + default_device->goal_position_item_->data_length_); + } + + if (default_device->position_p_gain_item_ != 0) + { + port_to_sync_write_position_p_gain_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->position_p_gain_item_->address_, + default_device->position_p_gain_item_->data_length_); + } + + if (default_device->goal_velocity_item_ != 0) + { + port_to_sync_write_velocity_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->goal_velocity_item_->address_, + default_device->goal_velocity_item_->data_length_); + } + + if (default_device->goal_current_item_ != 0) + { + port_to_sync_write_torque_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->goal_current_item_->address_, + default_device->goal_current_item_->data_length_); + } + } + else if (sensor_it != robot_->sensors_.end()) { - std::string _port_name = _it->first; - PortHandler *_port = _it->second; + Sensor *_default_device = sensor_it->second; + default_pkt_handler = dynamixel::PacketHandler::getPacketHandler(_default_device->protocol_version_); + } - PacketHandler *_default_pkt_handler = PacketHandler::GetPacketHandler(2.0); + port_to_bulk_read_[port_name] = new dynamixel::GroupBulkRead(port, default_pkt_handler); + } - if(_port->SetBaudRate(_port->GetBaudRate()) == false) - { - ROS_ERROR("PORT [%s] SETUP ERROR! (baudrate: %d)", _port_name.c_str(), _port->GetBaudRate()); - exit(-1); - } + // (for loop) check all dxls are connected. + for (std::map::iterator it = robot_->dxls_.begin(); it != robot_->dxls_.end(); it++) + { + std::string joint_name = it->first; + Dynamixel *dxl = it->second; - // get the default device info of the port - std::string _default_device_name = robot->port_default_device[_port_name]; - std::map::iterator _dxl_it = robot->dxls.find(_default_device_name); - std::map::iterator _sensor_it = robot->sensors.find(_default_device_name); - if(_dxl_it != robot->dxls.end()) - { - Dynamixel *_default_device = _dxl_it->second; - _default_pkt_handler = PacketHandler::GetPacketHandler(_default_device->protocol_version); + if (ping(joint_name) != 0) + { + usleep(10 * 1000); + if (ping(joint_name) != 0) + ROS_ERROR("JOINT[%s] does NOT respond!!", joint_name.c_str()); + } + } - if(_default_device->goal_position_item != 0) - { - port_to_sync_write_position[_port_name] = new GroupSyncWrite(_port, - _default_pkt_handler, - _default_device->goal_position_item->address, - _default_device->goal_position_item->data_length); - } + initializeDevice(init_file_path); - if(_default_device->position_p_gain_item != 0) - { - port_to_sync_write_position_p_gain[_port_name] = new GroupSyncWrite(_port, - _default_pkt_handler, - _default_device->position_p_gain_item->address, - _default_device->position_p_gain_item->data_length); - } + // [ BulkRead ] StartAddress : Present Position , Length : 10 ( Position/Velocity/Current ) + for (std::map::iterator it = robot_->dxls_.begin(); it != robot_->dxls_.end(); it++) + { + std::string joint_name = it->first; + Dynamixel *dxl = it->second; - if(_default_device->goal_velocity_item != 0) - { - port_to_sync_write_velocity[_port_name] = new GroupSyncWrite(_port, - _default_pkt_handler, - _default_device->goal_velocity_item->address, - _default_device->goal_velocity_item->data_length); - } + if (dxl == NULL) + continue; - if(_default_device->goal_current_item != 0) - { - port_to_sync_write_torque[_port_name] = new GroupSyncWrite(_port, - _default_pkt_handler, - _default_device->goal_current_item->address, - _default_device->goal_current_item->data_length); - } - } - else if(_sensor_it != robot->sensors.end()) - { - Sensor *_default_device = _sensor_it->second; - _default_pkt_handler = PacketHandler::GetPacketHandler(_default_device->protocol_version); - } + int bulkread_start_addr = 0; + int bulkread_data_length = 0; - port_to_bulk_read[_port_name] = new GroupBulkRead(_port, _default_pkt_handler); - } +// // bulk read default : present position +// if(dxl->present_position_item != 0) +// { +// bulkread_start_addr = dxl->present_position_item->address; +// bulkread_data_length = dxl->present_position_item->data_length; +// } - // (for loop) check all dxls are connected. - for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) + // calculate bulk read start address & data length + std::map::iterator indirect_addr_it = dxl->ctrl_table_.find(INDIRECT_ADDRESS_1); + if (indirect_addr_it != dxl->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist { - std::string _joint_name = _it->first; - Dynamixel *_dxl = _it->second; - - if(Ping(_joint_name) != 0) + if (dxl->bulk_read_items_.size() != 0) + { + bulkread_start_addr = dxl->bulk_read_items_[0]->address_; + bulkread_data_length = 0; + + // set indirect address + int indirect_addr = indirect_addr_it->second->address_; + for (int i = 0; i < dxl->bulk_read_items_.size(); i++) { - usleep(10*1000); - if(Ping(_joint_name) != 0) - ROS_ERROR("JOINT[%s] does NOT respond!!", _joint_name.c_str()); + int addr_leng = dxl->bulk_read_items_[i]->data_length_; + + bulkread_data_length += addr_leng; + for (int l = 0; l < addr_leng; l++) + { +// ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", joint_name.c_str(), indirect_addr, dxl->ctrl_table[dxl->bulk_read_items[i]->item_name]->address + _l); + write2Byte(joint_name, indirect_addr, dxl->ctrl_table_[dxl->bulk_read_items_[i]->item_name_]->address_ + l); + indirect_addr += 2; + } } + } } - - InitDevice(init_file_path); - - // [ BulkRead ] StartAddress : Present Position , Length : 10 ( Position/Velocity/Current ) - for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) + else // INDIRECT_ADDRESS_1 NOT exist { - std::string _joint_name = _it->first; - Dynamixel *_dxl = _it->second; + if (dxl->bulk_read_items_.size() != 0) + { + bulkread_start_addr = dxl->bulk_read_items_[0]->address_; + bulkread_data_length = 0; - if(_dxl == NULL) - continue; + ControlTableItem *last_item = dxl->bulk_read_items_[0]; - int _bulkread_start_addr = 0; - int _bulkread_data_length = 0; - -// // bulk read default : present position -// if(_dxl->present_position_item != 0) -// { -// _bulkread_start_addr = _dxl->present_position_item->address; -// _bulkread_data_length = _dxl->present_position_item->data_length; -// } - - // calculate bulk read start address & data length - std::map::iterator _indirect_addr_it = _dxl->ctrl_table.find(INDIRECT_ADDRESS_1); - if(_indirect_addr_it != _dxl->ctrl_table.end()) // INDIRECT_ADDRESS_1 exist + for (int i = 0; i < dxl->bulk_read_items_.size(); i++) { - if(_dxl->bulk_read_items.size() != 0) - { - _bulkread_start_addr = _dxl->bulk_read_items[0]->address; - _bulkread_data_length = 0; - - // set indirect address - int _indirect_addr = _indirect_addr_it->second->address; - for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++) - { - int _addr_leng = _dxl->bulk_read_items[_i]->data_length; - - _bulkread_data_length += _addr_leng; - for(int _l = 0; _l < _addr_leng; _l++) - { -// ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", _joint_name.c_str(), _indirect_addr, _dxl->ctrl_table[_dxl->bulk_read_items[_i]->item_name]->address + _l); - Write2Byte(_joint_name, _indirect_addr, _dxl->ctrl_table[_dxl->bulk_read_items[_i]->item_name]->address + _l); - _indirect_addr += 2; - } - } - } + int addr = dxl->bulk_read_items_[i]->address_; + if (addr < bulkread_start_addr) + bulkread_start_addr = addr; + else if (last_item->address_ < addr) + last_item = dxl->bulk_read_items_[i]; } - else // INDIRECT_ADDRESS_1 NOT exist - { - if(_dxl->bulk_read_items.size() != 0) - { - _bulkread_start_addr = _dxl->bulk_read_items[0]->address; - _bulkread_data_length = 0; - - ControlTableItem *_last_item = _dxl->bulk_read_items[0]; - - for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++) - { - int _addr = _dxl->bulk_read_items[_i]->address; - if(_addr < _bulkread_start_addr) - _bulkread_start_addr = _addr; - else if(_last_item->address < _addr) - _last_item = _dxl->bulk_read_items[_i]; - } - _bulkread_data_length = _last_item->address - _bulkread_start_addr + _last_item->data_length; - } - } + bulkread_data_length = last_item->address_ - bulkread_start_addr + last_item->data_length_; + } + } -// ROS_WARN("[%12s] start_addr: %d, data_length: %d", _joint_name.c_str(), _bulkread_start_addr, _bulkread_data_length); - if(_bulkread_start_addr != 0) - port_to_bulk_read[_dxl->port_name]->AddParam(_dxl->id, _bulkread_start_addr, _bulkread_data_length); +// ROS_WARN("[%12s] start_addr: %d, data_length: %d", joint_name.c_str(), bulkread_start_addr, bulkread_data_length); + if (bulkread_start_addr != 0) + port_to_bulk_read_[dxl->port_name_]->addParam(dxl->id_, bulkread_start_addr, bulkread_data_length); - // Torque ON - if(WriteCtrlItem(_joint_name, _dxl->torque_enable_item->item_name, 1) != COMM_SUCCESS) - WriteCtrlItem(_joint_name, _dxl->torque_enable_item->item_name, 1); - } + // Torque ON + if (writeCtrlItem(joint_name, dxl->torque_enable_item_->item_name_, 1) != COMM_SUCCESS) + writeCtrlItem(joint_name, dxl->torque_enable_item_->item_name_, 1); + } - for(std::map::iterator _it = robot->sensors.begin(); _it != robot->sensors.end(); _it++) - { - std::string _sensor_name = _it->first; - Sensor *_sensor = _it->second; + for (std::map::iterator it = robot_->sensors_.begin(); it != robot_->sensors_.end(); it++) + { + std::string sensor_name = it->first; + Sensor *sensor = it->second; - if(_sensor == NULL) - continue; + if (sensor == NULL) + continue; - int _bulkread_start_addr = 0; - int _bulkread_data_length = 0; + int bulkread_start_addr = 0; + int bulkread_data_length = 0; - // calculate bulk read start address & data length - std::map::iterator _indirect_addr_it = _sensor->ctrl_table.find(INDIRECT_ADDRESS_1); - if(_indirect_addr_it != _sensor->ctrl_table.end()) // INDIRECT_ADDRESS_1 exist + // calculate bulk read start address & data length + std::map::iterator indirect_addr_it = sensor->ctrl_table_.find(INDIRECT_ADDRESS_1); + if (indirect_addr_it != sensor->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist + { + if (sensor->bulk_read_items_.size() != 0) + { + bulkread_start_addr = sensor->bulk_read_items_[0]->address_; + bulkread_data_length = 0; + + // set indirect address + int indirect_addr = indirect_addr_it->second->address_; + for (int i = 0; i < sensor->bulk_read_items_.size(); i++) { - if(_sensor->bulk_read_items.size() != 0) - { - _bulkread_start_addr = _sensor->bulk_read_items[0]->address; - _bulkread_data_length = 0; - - // set indirect address - int _indirect_addr = _indirect_addr_it->second->address; - for(int _i = 0; _i < _sensor->bulk_read_items.size(); _i++) - { - int _addr_leng = _sensor->bulk_read_items[_i]->data_length; - - _bulkread_data_length += _addr_leng; - for(int _l = 0; _l < _addr_leng; _l++) - { -// ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", _sensor_name.c_str(), _indirect_addr, _sensor->ctrl_table[_sensor->bulk_read_items[_i]->item_name]->address + _l); - Write2Byte(_sensor_name, _indirect_addr, _sensor->ctrl_table[_sensor->bulk_read_items[_i]->item_name]->address + _l); - _indirect_addr += 2; - } - } - } + int addr_leng = sensor->bulk_read_items_[i]->data_length_; + + bulkread_data_length += addr_leng; + for (int l = 0; l < addr_leng; l++) + { +// ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", sensor_name.c_str(), indirect_addr, sensor->ctrl_table[sensor->bulk_read_items[i]->item_name]->address + _l); + write2Byte(sensor_name, + indirect_addr, + sensor->ctrl_table_[sensor->bulk_read_items_[i]->item_name_]->address_ + l); + indirect_addr += 2; + } } - else // INDIRECT_ADDRESS_1 NOT exist - { - if(_sensor->bulk_read_items.size() != 0) - { - _bulkread_start_addr = _sensor->bulk_read_items[0]->address; - _bulkread_data_length = 0; - - ControlTableItem *_last_item = _sensor->bulk_read_items[0]; + } + } + else // INDIRECT_ADDRESS_1 NOT exist + { + if (sensor->bulk_read_items_.size() != 0) + { + bulkread_start_addr = sensor->bulk_read_items_[0]->address_; + bulkread_data_length = 0; - for(int _i = 0; _i < _sensor->bulk_read_items.size(); _i++) - { - int _addr = _sensor->bulk_read_items[_i]->address; - if(_addr < _bulkread_start_addr) - _bulkread_start_addr = _addr; - else if(_last_item->address < _addr) - _last_item = _sensor->bulk_read_items[_i]; - } + ControlTableItem *last_item = sensor->bulk_read_items_[0]; - _bulkread_data_length = _last_item->address - _bulkread_start_addr + _last_item->data_length; - } + for (int i = 0; i < sensor->bulk_read_items_.size(); i++) + { + int addr = sensor->bulk_read_items_[i]->address_; + if (addr < bulkread_start_addr) + bulkread_start_addr = addr; + else if (last_item->address_ < addr) + last_item = sensor->bulk_read_items_[i]; } - //ROS_WARN("[%12s] start_addr: %d, data_length: %d", _sensor_name.c_str(), _bulkread_start_addr, _bulkread_data_length); - if(_bulkread_start_addr != 0) - port_to_bulk_read[_sensor->port_name]->AddParam(_sensor->id, _bulkread_start_addr, _bulkread_data_length); + bulkread_data_length = last_item->address_ - bulkread_start_addr + last_item->data_length_; + } } - queue_thread_ = boost::thread(boost::bind(&RobotisController::QueueThread, this)); - return true; + //ROS_WARN("[%12s] start_addr: %d, data_length: %d", sensor_name.c_str(), bulkread_start_addr, bulkread_data_length); + if (bulkread_start_addr != 0) + port_to_bulk_read_[sensor->port_name_]->addParam(sensor->id_, bulkread_start_addr, bulkread_data_length); + } + + queue_thread_ = boost::thread(boost::bind(&RobotisController::msgQueueThread, this)); + return true; } -void RobotisController::InitDevice(const std::string init_file_path) +void RobotisController::initializeDevice(const std::string init_file_path) { - // device initialize - if(DEBUG_PRINT) ROS_WARN("INIT FILE LOAD"); + // device initialize + if (DEBUG_PRINT) + ROS_WARN("INIT FILE LOAD"); - YAML::Node _doc; - try{ - _doc = YAML::LoadFile(init_file_path.c_str()); + YAML::Node doc; + try + { + doc = YAML::LoadFile(init_file_path.c_str()); - for(YAML::const_iterator _it_doc = _doc.begin(); _it_doc != _doc.end(); _it_doc++) - { - std::string _joint_name = _it_doc->first.as(); + for (YAML::const_iterator it_doc = doc.begin(); it_doc != doc.end(); it_doc++) + { + std::string joint_name = it_doc->first.as(); - YAML::Node _joint_node = _doc[_joint_name]; - if(_joint_node.size() == 0) - continue; + YAML::Node joint_node = doc[joint_name]; + if (joint_node.size() == 0) + continue; - Dynamixel *_dxl = NULL; - std::map::iterator _dxl_it = robot->dxls.find(_joint_name); - if(_dxl_it != robot->dxls.end()) - _dxl = _dxl_it->second; + Dynamixel *dxl = NULL; + std::map::iterator dxl_it = robot_->dxls_.find(joint_name); + if (dxl_it != robot_->dxls_.end()) + dxl = dxl_it->second; - if(_dxl == NULL) - { - ROS_WARN("Joint [%s] does not found.", _joint_name.c_str()); - continue; - } - if(DEBUG_PRINT) ROS_INFO("JOINT_NAME: %s", _joint_name.c_str()); + if (dxl == NULL) + { + ROS_WARN("Joint [%s] does not found.", joint_name.c_str()); + continue; + } + if (DEBUG_PRINT) + ROS_INFO("JOINT_NAME: %s", joint_name.c_str()); - for(YAML::const_iterator _it_joint = _joint_node.begin(); _it_joint != _joint_node.end(); _it_joint++) - { - std::string _item_name = _it_joint->first.as(); + for (YAML::const_iterator it_joint = joint_node.begin(); it_joint != joint_node.end(); it_joint++) + { + std::string item_name = it_joint->first.as(); - if(DEBUG_PRINT) ROS_INFO(" ITEM_NAME: %s", _item_name.c_str()); + if (DEBUG_PRINT) + ROS_INFO(" ITEM_NAME: %s", item_name.c_str()); - UINT32_T _value = _it_joint->second.as(); + uint32_t value = it_joint->second.as(); - ControlTableItem *_item = _dxl->ctrl_table[_item_name]; - if(_item == NULL) - { - ROS_WARN("Control Item [%s] does not found.", _item_name.c_str()); - continue; - } + ControlTableItem *item = dxl->ctrl_table_[item_name]; + if (item == NULL) + { + ROS_WARN("Control Item [%s] does not found.", item_name.c_str()); + continue; + } - if(_item->memory_type == EEPROM) - { - UINT8_T _data8 = 0; - UINT16_T _data16 = 0; - UINT32_T _data32 = 0; - - switch(_item->data_length) - { - case 1: - Read1Byte(_joint_name, _item->address, &_data8); - if(_data8 == _value) - continue; - break; - case 2: - Read2Byte(_joint_name, _item->address, &_data16); - if(_data16 == _value) - continue; - break; - case 4: - Read4Byte(_joint_name, _item->address, &_data32); - if(_data32 == _value) - continue; - break; - default: - break; - } - } + if (item->memory_type_ == EEPROM) + { + uint8_t data8 = 0; + uint16_t data16 = 0; + uint32_t data32 = 0; + + switch (item->data_length_) + { + case 1: + read1Byte(joint_name, item->address_, &data8); + if (data8 == value) + continue; + break; + case 2: + read2Byte(joint_name, item->address_, &data16); + if (data16 == value) + continue; + break; + case 4: + read4Byte(joint_name, item->address_, &data32); + if (data32 == value) + continue; + break; + default: + break; + } + } - switch(_item->data_length) - { - case 1: - Write1Byte(_joint_name, _item->address, (UINT8_T)_value); - break; - case 2: - Write2Byte(_joint_name, _item->address, (UINT16_T)_value); - break; - case 4: - Write4Byte(_joint_name, _item->address, _value); - break; - default: - break; - } + switch (item->data_length_) + { + case 1: + write1Byte(joint_name, item->address_, (uint8_t) value); + break; + case 2: + write2Byte(joint_name, item->address_, (uint16_t) value); + break; + case 4: + write4Byte(joint_name, item->address_, value); + break; + default: + break; + } - if(_item->memory_type == EEPROM) - { - // Write to EEPROM -> delay is required (max delay: 55 msec per byte) - usleep(_item->data_length * 55 * 1000); - } - } + if (item->memory_type_ == EEPROM) + { + // Write to EEPROM -> delay is required (max delay: 55 msec per byte) + usleep(item->data_length_ * 55 * 1000); } - } catch(const std::exception& e) { - ROS_INFO("Dynamixel Init file not found."); + } } + } catch (const std::exception& e) + { + ROS_INFO("Dynamixel Init file not found."); + } } -void RobotisController::GazeboThread() +void RobotisController::gazeboTimerThread() { - ros::Rate gazebo_rate(1000 / CONTROL_CYCLE_MSEC); + ros::Rate gazebo_rate(1000 / CONTROL_CYCLE_MSEC); + + while (!stop_timer_) + { + if (init_pose_loaded_ == true) + process(); + gazebo_rate.sleep(); + } +} - while(!stop_timer_) +void RobotisController::msgQueueThread() +{ + ros::NodeHandle ros_node; + ros::CallbackQueue callback_queue; + + ros_node.setCallbackQueue(&callback_queue); + + /* subscriber */ + ros::Subscriber sync_write_item_sub = ros_node.subscribe("/robotis/sync_write_item", 10, + &RobotisController::syncWriteItemCallback, this); + ros::Subscriber joint_ctrl_modules_sub = ros_node.subscribe("/robotis/set_joint_ctrl_modules", 10, + &RobotisController::setJointCtrlModuleCallback, this); + ros::Subscriber enable_ctrl_module_sub = ros_node.subscribe("/robotis/enable_ctrl_module", 10, + &RobotisController::setCtrlModuleCallback, this); + ros::Subscriber control_mode_sub = ros_node.subscribe("/robotis/set_control_mode", 10, + &RobotisController::setControllerModeCallback, this); + ros::Subscriber joint_states_sub = ros_node.subscribe("/robotis/set_joint_states", 10, + &RobotisController::setJointStatesCallback, this); + + ros::Subscriber gazebo_joint_states_sub; + if (gazebo_mode_ == true) + gazebo_joint_states_sub = ros_node.subscribe("/" + gazebo_robot_name_ + "/joint_states", 10, + &RobotisController::gazeboJointStatesCallback, this); + + /* publisher */ + goal_joint_state_pub_ = ros_node.advertise("/robotis/goal_joint_states", 10); + present_joint_state_pub_ = ros_node.advertise("/robotis/present_joint_states", 10); + current_module_pub_ = ros_node.advertise( + "/robotis/present_joint_ctrl_modules", 10); + + if (gazebo_mode_ == true) + { + for (std::map::iterator it = robot_->dxls_.begin(); it != robot_->dxls_.end(); it++) { - if(init_pose_loaded_ == true) - Process(); - gazebo_rate.sleep(); + gazebo_joint_pub_[it->first] = ros_node.advertise( + "/" + gazebo_robot_name_ + "/" + it->first + "_pos/command", 1); } + } + + /* service */ + ros::ServiceServer joint_module_server = ros_node.advertiseService("/robotis/get_present_joint_ctrl_modules", + &RobotisController::getCtrlModuleCallback, this); + + while (ros_node.ok()) + { + callback_queue.callAvailable(); + usleep(100); + } } -void RobotisController::QueueThread() +void *RobotisController::timerThread(void *param) { - ros::NodeHandle _ros_node; - ros::CallbackQueue _callback_queue; + RobotisController *controller = (RobotisController *) param; + static struct timespec next_time; + static struct timespec curr_time; - _ros_node.setCallbackQueue(&_callback_queue); + ROS_INFO("controller::thread_proc"); - /* subscriber */ - ros::Subscriber _sync_write_item_sub = _ros_node.subscribe("/robotis/sync_write_item", 10, &RobotisController::SyncWriteItemCallback, this); - ros::Subscriber _joint_ctrl_modules_sub = _ros_node.subscribe("/robotis/set_joint_ctrl_modules", 10, &RobotisController::SetJointCtrlModuleCallback, this); - ros::Subscriber _enable_ctrl_module_sub = _ros_node.subscribe("/robotis/enable_ctrl_module", 10, &RobotisController::SetCtrlModuleCallback, this); - ros::Subscriber _control_mode_sub = _ros_node.subscribe("/robotis/set_control_mode", 10, &RobotisController::SetControllerModeCallback, this); - ros::Subscriber _joint_states_sub = _ros_node.subscribe("/robotis/set_joint_states", 10, &RobotisController::SetJointStatesCallback, this); + clock_gettime(CLOCK_MONOTONIC, &next_time); - ros::Subscriber _gazebo_joint_states_sub; - if(gazebo_mode == true) - _gazebo_joint_states_sub = _ros_node.subscribe("/" + gazebo_robot_name + "/joint_states", 10, &RobotisController::GazeboJointStatesCallback, this); + while (!controller->stop_timer_) + { + next_time.tv_sec += (next_time.tv_nsec + CONTROL_CYCLE_MSEC * 1000000) / 1000000000; + next_time.tv_nsec = (next_time.tv_nsec + CONTROL_CYCLE_MSEC * 1000000) % 1000000000; - /* publisher */ - goal_joint_state_pub = _ros_node.advertise("/robotis/goal_joint_states", 10); - present_joint_state_pub = _ros_node.advertise("/robotis/present_joint_states", 10); - current_module_pub = _ros_node.advertise("/robotis/present_joint_ctrl_modules", 10); + controller->process(); - if(gazebo_mode == true) + clock_gettime(CLOCK_MONOTONIC, &curr_time); + long delta_nsec = (next_time.tv_sec - curr_time.tv_sec) * 1000000000 + (next_time.tv_nsec - curr_time.tv_nsec); + if (delta_nsec < -100000) { - for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) - gazebo_joint_pub[_it->first] = _ros_node.advertise("/" + gazebo_robot_name + "/" + _it->first + "_pos/command", 1); + if (controller->DEBUG_PRINT == true) + { + ROS_WARN("[RobotisController::ThreadProc] NEXT TIME < CURR TIME.. (%f)[%ld.%09ld / %ld.%09ld]", + delta_nsec / 1000000.0, (long )next_time.tv_sec, (long )next_time.tv_nsec, + (long )curr_time.tv_sec, (long )curr_time.tv_nsec); + } + + // next_time = curr_time + 3 msec + next_time.tv_sec = curr_time.tv_sec + (curr_time.tv_nsec + 3000000) / 1000000000; + next_time.tv_nsec = (curr_time.tv_nsec + 3000000) % 1000000000; } - /* service */ - ros::ServiceServer _joint_module_server = _ros_node.advertiseService("/robotis/get_present_joint_ctrl_modules", &RobotisController::GetCtrlModuleCallback, this); - - while(_ros_node.ok()) - { - _callback_queue.callAvailable(); - usleep(100); - } + clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_time, NULL); + } + return 0; } -void *RobotisController::ThreadProc(void *param) +void RobotisController::startTimer() { - RobotisController *controller = (RobotisController *)param; - static struct timespec next_time; - static struct timespec curr_time; - - ROS_INFO("controller::thread_proc"); + if (this->is_timer_running_ == true) + return; + + if (this->gazebo_mode_ == true) + { + // create and start the thread + gazebo_thread_ = boost::thread(boost::bind(&RobotisController::gazeboTimerThread, this)); + } + else + { + initializeSyncWrite(); + + for (std::map::iterator it = port_to_bulk_read_.begin(); + it != port_to_bulk_read_.end(); it++) + { + it->second->txPacket(); + } - clock_gettime(CLOCK_MONOTONIC, &next_time); + int error; + struct sched_param param; + pthread_attr_t attr; - while(!controller->stop_timer_) - { - next_time.tv_sec += (next_time.tv_nsec + CONTROL_CYCLE_MSEC * 1000000) / 1000000000; - next_time.tv_nsec = (next_time.tv_nsec + CONTROL_CYCLE_MSEC * 1000000) % 1000000000; + pthread_attr_init(&attr); - controller->Process(); + error = pthread_attr_setschedpolicy(&attr, SCHED_RR); + if (error != 0) + ROS_ERROR("pthread_attr_setschedpolicy error = %d\n", error); + error = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED); + if (error != 0) + ROS_ERROR("pthread_attr_setinheritsched error = %d\n", error); - clock_gettime(CLOCK_MONOTONIC, &curr_time); - long delta_nsec = (next_time.tv_sec - curr_time.tv_sec) * 1000000000 + (next_time.tv_nsec - curr_time.tv_nsec); - if(delta_nsec < -100000 ) - { - if(controller->DEBUG_PRINT == true) - ROS_WARN("[RobotisController::ThreadProc] NEXT TIME < CURR TIME.. (%f)[%ld.%09ld / %ld.%09ld]", delta_nsec/1000000.0, (long)next_time.tv_sec, (long)next_time.tv_nsec, (long)curr_time.tv_sec, (long)curr_time.tv_nsec); - // next_time = curr_time + 3 msec - next_time.tv_sec = curr_time.tv_sec + (curr_time.tv_nsec + 3000000) / 1000000000; - next_time.tv_nsec = (curr_time.tv_nsec + 3000000) % 1000000000; - } + memset(¶m, 0, sizeof(param)); + param.sched_priority = 31; // RT + error = pthread_attr_setschedparam(&attr, ¶m); + if (error != 0) + ROS_ERROR("pthread_attr_setschedparam error = %d\n", error); - clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_time, NULL); + // create and start the thread + if ((error = pthread_create(&this->timer_thread_, &attr, this->timerThread, this)) != 0) + { + ROS_ERROR("timer thread create fail!!"); + exit(-1); } - return 0; + } + + this->is_timer_running_ = true; } -void RobotisController::StartTimer() +void RobotisController::stopTimer() { - if(this->is_timer_running_ == true) - return; + int error = 0; + + // set the flag to stop the thread + if (this->is_timer_running_) + { + this->stop_timer_ = true; - if(this->gazebo_mode == true) + if (this->gazebo_mode_ == false) { - // create and start the thread - gazebo_thread_ = boost::thread(boost::bind(&RobotisController::GazeboThread, this)); + // wait until the thread is stopped. + if ((error = pthread_join(this->timer_thread_, NULL)) != 0) + exit(-1); + + for (std::map::iterator it = port_to_bulk_read_.begin(); + it != port_to_bulk_read_.end(); it++) + { + if (it->second != NULL) + it->second->rxPacket(); + } + + for (std::map::iterator it = port_to_sync_write_position_.begin(); + it != port_to_sync_write_position_.end(); it++) + { + if (it->second != NULL) + it->second->clearParam(); + } + for (std::map::iterator it = port_to_sync_write_position_p_gain_.begin(); + it != port_to_sync_write_position_p_gain_.end(); it++) + { + if (it->second != NULL) + it->second->clearParam(); + } + for (std::map::iterator it = port_to_sync_write_velocity_.begin(); + it != port_to_sync_write_velocity_.end(); it++) + { + if (it->second != NULL) + it->second->clearParam(); + } + for (std::map::iterator it = port_to_sync_write_torque_.begin(); + it != port_to_sync_write_torque_.end(); it++) + { + if (it->second != NULL) + it->second->clearParam(); + } } else { - InitSyncWrite(); - - for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) - _it->second->TxPacket(); - - int error; - struct sched_param param; - pthread_attr_t attr; - - pthread_attr_init(&attr); - - error = pthread_attr_setschedpolicy(&attr, SCHED_RR); - if(error != 0) - ROS_ERROR("pthread_attr_setschedpolicy error = %d\n",error); - error = pthread_attr_setinheritsched(&attr,PTHREAD_EXPLICIT_SCHED); - if(error != 0) - ROS_ERROR("pthread_attr_setinheritsched error = %d\n",error); - - memset(¶m, 0, sizeof(param)); - param.sched_priority = 31;// RT - error = pthread_attr_setschedparam(&attr, ¶m); - if(error != 0) - ROS_ERROR("pthread_attr_setschedparam error = %d\n",error); - - // create and start the thread - if((error = pthread_create(&this->timer_thread_, &attr, this->ThreadProc, this))!= 0) { - ROS_ERROR("timer thread create fail!!"); - exit(-1); - } + // wait until the thread is stopped. + gazebo_thread_.join(); } - this->is_timer_running_ = true; -} - -void RobotisController::StopTimer() -{ - int error = 0; - - // set the flag to stop the thread - if(this->is_timer_running_) - { - this->stop_timer_ = true; - - if(this->gazebo_mode == false) - { - // wait until the thread is stopped. - if((error = pthread_join(this->timer_thread_, NULL)) != 0) - exit(-1); - - for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) - { - if(_it->second != NULL) - _it->second->RxPacket(); - } - - for(std::map::iterator _it = port_to_sync_write_position.begin(); _it != port_to_sync_write_position.end(); _it++) - { - if(_it->second != NULL) - _it->second->ClearParam(); - } - for(std::map::iterator _it = port_to_sync_write_position_p_gain.begin(); _it != port_to_sync_write_position_p_gain.end(); _it++) - { - if(_it->second != NULL) - _it->second->ClearParam(); - } - for(std::map::iterator _it = port_to_sync_write_velocity.begin(); _it != port_to_sync_write_velocity.end(); _it++) - { - if(_it->second != NULL) - _it->second->ClearParam(); - } - for(std::map::iterator _it = port_to_sync_write_torque.begin(); _it != port_to_sync_write_torque.end(); _it++) - { - if(_it->second != NULL) - _it->second->ClearParam(); - } - } - else - { - // wait until the thread is stopped. - gazebo_thread_.join(); - } - - this->stop_timer_ = false; - this->is_timer_running_ = false; - } + this->stop_timer_ = false; + this->is_timer_running_ = false; + } } -bool RobotisController::IsTimerRunning() +bool RobotisController::isTimerRunning() { - return this->is_timer_running_; + return this->is_timer_running_; } -void RobotisController::LoadOffset(const std::string path) +void RobotisController::loadOffset(const std::string path) { - YAML::Node _doc; - try{ - _doc = YAML::LoadFile(path.c_str()); - } catch(const std::exception& e) { - ROS_ERROR("Fail to load offset yaml."); - return; - } - - YAML::Node _offset_node = _doc["offset"]; - if(_offset_node.size() == 0) - return; - - ROS_INFO("Load offsets..."); - for(YAML::const_iterator _it = _offset_node.begin(); _it != _offset_node.end(); _it++) - { - std::string _joint_name = _it->first.as(); - double _offset = _it->second.as(); - - std::map::iterator _dxl_it = robot->dxls.find(_joint_name); - if(_dxl_it != robot->dxls.end()) - _dxl_it->second->dxl_state->position_offset = _offset; - } + YAML::Node doc; + try + { + doc = YAML::LoadFile(path.c_str()); + } catch (const std::exception& e) + { + ROS_ERROR("Fail to load offset yaml."); + return; + } + + YAML::Node offset_node = doc["offset"]; + if (offset_node.size() == 0) + return; + + ROS_INFO("Load offsets..."); + for (YAML::const_iterator it = offset_node.begin(); it != offset_node.end(); it++) + { + std::string joint_name = it->first.as(); + double offset = it->second.as(); + + std::map::iterator dxl_it = robot_->dxls_.find(joint_name); + if (dxl_it != robot_->dxls_.end()) + dxl_it->second->dxl_state_->position_offset_ = offset; + } } -void RobotisController::Process() +void RobotisController::process() { - // avoid duplicated function call - static bool _is_process_running = false; - if(_is_process_running == true) - return; - _is_process_running = true; + // avoid duplicated function call + static bool is_process_running = false; + if (is_process_running == true) + return; + is_process_running = true; - // ROS_INFO("Controller::Process()"); - bool _do_sync_write = false; + // ROS_INFO("Controller::Process()"); + bool do_sync_write = false; - ros::Time _start_time; - ros::Duration _time_duration; + ros::Time start_time; + ros::Duration time_duration; - if(DEBUG_PRINT) - _start_time = ros::Time::now(); + if (DEBUG_PRINT) + start_time = ros::Time::now(); - sensor_msgs::JointState _goal_state; - sensor_msgs::JointState _present_state; + sensor_msgs::JointState goal_state; + sensor_msgs::JointState present_state; - _present_state.header.stamp = ros::Time::now(); - _goal_state.header.stamp = _present_state.header.stamp; + present_state.header.stamp = ros::Time::now(); + goal_state.header.stamp = present_state.header.stamp; + // BulkRead Rx + // -> save to Robot->dxls[]->dynamixel_state.present_position + if (gazebo_mode_ == false) + { // BulkRead Rx - // -> save to Robot->dxls[]->dynamixel_state.present_position - if(gazebo_mode == false) + for (std::map::iterator it = port_to_bulk_read_.begin(); + it != port_to_bulk_read_.end(); it++) { - // BulkRead Rx - for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) - { - robot->ports[_it->first]->SetPacketTimeout(0.0); - _it->second->RxPacket(); - } + robot_->ports_[it->first]->setPacketTimeout(0.0); + it->second->rxPacket(); + } - if(robot->dxls.size() > 0) + if (robot_->dxls_.size() > 0) + { + for (std::map::iterator dxl_it = robot_->dxls_.begin(); + dxl_it != robot_->dxls_.end(); dxl_it++) + { + Dynamixel *dxl = dxl_it->second; + std::string port_name = dxl_it->second->port_name_; + std::string joint_name = dxl_it->first; + + if (dxl->bulk_read_items_.size() > 0) { - for(std::map::iterator dxl_it = robot->dxls.begin(); dxl_it != robot->dxls.end(); dxl_it++) + uint32_t data = 0; + for (int i = 0; i < dxl->bulk_read_items_.size(); i++) + { + ControlTableItem *item = dxl->bulk_read_items_[i]; + if (port_to_bulk_read_[port_name]->isAvailable(dxl->id_, item->address_, item->data_length_) == true) { - Dynamixel *_dxl = dxl_it->second; - std::string _port_name = dxl_it->second->port_name; - std::string _joint_name = dxl_it->first; - - if(_dxl->bulk_read_items.size() > 0) - { - UINT32_T _data = 0; - for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++) - { - ControlTableItem *_item = _dxl->bulk_read_items[_i]; - if(port_to_bulk_read[_port_name]->IsAvailable(_dxl->id, _item->address, _item->data_length) == true) - { - _data = port_to_bulk_read[_port_name]->GetData(_dxl->id, _item->address, _item->data_length); - - // change dxl_state - if(_dxl->present_position_item != 0 && _item->item_name == _dxl->present_position_item->item_name) - _dxl->dxl_state->present_position = _dxl->ConvertValue2Radian(_data) - _dxl->dxl_state->position_offset; // remove offset - else if(_dxl->present_velocity_item != 0 && _item->item_name == _dxl->present_velocity_item->item_name) - _dxl->dxl_state->present_velocity = _dxl->ConvertValue2Velocity(_data); - else if(_dxl->present_current_item != 0 && _item->item_name == _dxl->present_current_item->item_name) - _dxl->dxl_state->present_current = _dxl->ConvertValue2Current(_data); - else if(_dxl->goal_position_item != 0 && _item->item_name == _dxl->goal_position_item->item_name) - _dxl->dxl_state->goal_position = _dxl->ConvertValue2Radian(_data) - _dxl->dxl_state->position_offset; // remove offset - else if(_dxl->goal_velocity_item != 0 && _item->item_name == _dxl->goal_velocity_item->item_name) - _dxl->dxl_state->goal_velocity = _dxl->ConvertValue2Velocity(_data); - else if(_dxl->goal_current_item != 0 && _item->item_name == _dxl->goal_current_item->item_name) - _dxl->dxl_state->goal_current = _dxl->ConvertValue2Current(_data); - else - _dxl->dxl_state->bulk_read_table[_item->item_name] = _data; - } - } - - // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp - _dxl->dxl_state->update_time_stamp = TimeStamp(_present_state.header.stamp.sec, _present_state.header.stamp.nsec); - } + data = port_to_bulk_read_[port_name]->getData(dxl->id_, item->address_, item->data_length_); + + // change dxl_state + if (dxl->present_position_item_ != 0 && item->item_name_ == dxl->present_position_item_->item_name_) + dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset + else if (dxl->present_velocity_item_ != 0 && item->item_name_ == dxl->present_velocity_item_->item_name_) + dxl->dxl_state_->present_velocity_ = dxl->convertValue2Velocity(data); + else if (dxl->present_current_item_ != 0 && item->item_name_ == dxl->present_current_item_->item_name_) + dxl->dxl_state_->present_current_ = dxl->convertValue2Current(data); + else if (dxl->goal_position_item_ != 0 && item->item_name_ == dxl->goal_position_item_->item_name_) + dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset + else if (dxl->goal_velocity_item_ != 0 && item->item_name_ == dxl->goal_velocity_item_->item_name_) + dxl->dxl_state_->goal_velocity_ = dxl->convertValue2Velocity(data); + else if (dxl->goal_current_item_ != 0 && item->item_name_ == dxl->goal_current_item_->item_name_) + dxl->dxl_state_->goal_current_ = dxl->convertValue2Current(data); + else + dxl->dxl_state_->bulk_read_table_[item->item_name_] = data; } + } + + // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp + dxl->dxl_state_->update_time_stamp_ = TimeStamp(present_state.header.stamp.sec, present_state.header.stamp.nsec); } + } + } - if(robot->sensors.size() > 0) + if (robot_->sensors_.size() > 0) + { + for (std::map::iterator sensor_it = robot_->sensors_.begin(); + sensor_it != robot_->sensors_.end(); sensor_it++) + { + Sensor *sensor = sensor_it->second; + std::string port_name = sensor_it->second->port_name_; + std::string sensor_name = sensor_it->first; + + if (sensor->bulk_read_items_.size() > 0) { - for(std::map::iterator sensor_it = robot->sensors.begin(); sensor_it != robot->sensors.end(); sensor_it++) + uint32_t data = 0; + for (int i = 0; i < sensor->bulk_read_items_.size(); i++) + { + ControlTableItem *item = sensor->bulk_read_items_[i]; + if (port_to_bulk_read_[port_name]->isAvailable(sensor->id_, item->address_, item->data_length_) == true) { - Sensor *_sensor = sensor_it->second; - std::string _port_name = sensor_it->second->port_name; - std::string _sensor_name = sensor_it->first; + data = port_to_bulk_read_[port_name]->getData(sensor->id_, item->address_, item->data_length_); - if(_sensor->bulk_read_items.size() > 0) - { - UINT32_T _data = 0; - for(int _i = 0; _i < _sensor->bulk_read_items.size(); _i++) - { - ControlTableItem *_item = _sensor->bulk_read_items[_i]; - if(port_to_bulk_read[_port_name]->IsAvailable(_sensor->id, _item->address, _item->data_length) == true) - { - _data = port_to_bulk_read[_port_name]->GetData(_sensor->id, _item->address, _item->data_length); - - // change sensor_state - _sensor->sensor_state->bulk_read_table[_item->item_name] = _data; - } - } - - // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp - _sensor->sensor_state->update_time_stamp = TimeStamp(_present_state.header.stamp.sec, _present_state.header.stamp.nsec); - } + // change sensor_state + sensor->sensor_state_->bulk_read_table_[item->item_name_] = data; } + } + + // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp + sensor->sensor_state_->update_time_stamp_ = TimeStamp(present_state.header.stamp.sec, present_state.header.stamp.nsec); } + } } - - if(DEBUG_PRINT) + } + + if (DEBUG_PRINT) + { + time_duration = ros::Time::now() - start_time; + ROS_INFO("(%2.6f) BulkRead Rx & update state", time_duration.nsec * 0.000001); + } + + // Call SensorModule Process() + // -> for loop : call SensorModule list -> Process() + if (sensor_modules_.size() > 0) + { + for (std::list::iterator module_it = sensor_modules_.begin(); module_it != sensor_modules_.end(); module_it++) { - _time_duration = ros::Time::now() - _start_time; - ROS_INFO("(%2.6f) BulkRead Rx & update state", _time_duration.nsec * 0.000001); - } + (*module_it)->process(robot_->dxls_, robot_->sensors_); - // Call SensorModule Process() - // -> for loop : call SensorModule list -> Process() - if(sensor_modules_.size() > 0) + for (std::map::iterator it = (*module_it)->result_.begin(); it != (*module_it)->result_.end(); it++) + sensor_result_[it->first] = it->second; + } + } + + if (DEBUG_PRINT) + { + time_duration = ros::Time::now() - start_time; + ROS_INFO("(%2.6f) SensorModule Process() & save result", time_duration.nsec * 0.000001); + } + + if (controller_mode_ == MotionModuleMode) + { + // Call MotionModule Process() + // -> for loop : call MotionModule list -> Process() + if (motion_modules_.size() > 0) { - for(std::list::iterator _module_it = sensor_modules_.begin(); _module_it != sensor_modules_.end(); _module_it++) - { - (*_module_it)->Process(robot->dxls, robot->sensors); + queue_mutex_.lock(); - for(std::map::iterator _it = (*_module_it)->result.begin(); _it != (*_module_it)->result.end(); _it++) - sensor_result_[_it->first] = _it->second; - } - } + for (std::list::iterator module_it = motion_modules_.begin(); module_it != motion_modules_.end(); module_it++) + { + if ((*module_it)->getModuleEnable() == false) + continue; - if(DEBUG_PRINT) - { - _time_duration = ros::Time::now() - _start_time; - ROS_INFO("(%2.6f) SensorModule Process() & save result", _time_duration.nsec * 0.000001); - } + (*module_it)->process(robot_->dxls_, sensor_result_); - if(controller_mode_ == MOTION_MODULE_MODE) - { - // Call MotionModule Process() - // -> for loop : call MotionModule list -> Process() - if(motion_modules_.size() > 0) + // for loop : joint list + for (std::map::iterator dxl_it = robot_->dxls_.begin(); dxl_it != robot_->dxls_.end(); dxl_it++) { - queue_mutex_.lock(); + std::string joint_name = dxl_it->first; + Dynamixel *dxl = dxl_it->second; + DynamixelState *dxl_state = dxl_it->second->dxl_state_; + + if (dxl->ctrl_module_name_ == (*module_it)->getModuleName()) + { + do_sync_write = true; + DynamixelState *result_state = (*module_it)->result_[joint_name]; - for(std::list::iterator module_it = motion_modules_.begin(); module_it != motion_modules_.end(); module_it++) + if (result_state == NULL) { - if((*module_it)->GetModuleEnable() == false) - continue; + ROS_INFO("[%s] %s", (*module_it)->getModuleName().c_str(), joint_name.c_str()); + continue; + } - (*module_it)->Process(robot->dxls, sensor_result_); + // TODO: check update time stamp ? - // for loop : joint list - for(std::map::iterator dxl_it = robot->dxls.begin(); dxl_it != robot->dxls.end(); dxl_it++) + if ((*module_it)->getControlMode() == PositionControl) + { +// if(result_state->goal_position == 0 && dxl->id == 3) +// ROS_INFO("[MODULE:%s][ID:%2d] goal position = %f", (*module_it)->GetModuleName().c_str(), dxl->id, dxl_state->goal_position); + dxl_state->goal_position_ = result_state->goal_position_; + + if (gazebo_mode_ == false) + { + // add offset + uint32_t pos_data = dxl->convertRadian2Value(dxl_state->goal_position_ + dxl_state->position_offset_); + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos_data)); + + if (abs(pos_data) > 151800) { - std::string _joint_name = dxl_it->first; - Dynamixel *_dxl = dxl_it->second; - DynamixelState *_dxl_state = dxl_it->second->dxl_state; - - if(_dxl->ctrl_module_name == (*module_it)->GetModuleName()) - { - _do_sync_write = true; - DynamixelState *_result_state = (*module_it)->result[_joint_name]; - - if(_result_state == NULL) { - ROS_INFO("[%s] %s", (*module_it)->GetModuleName().c_str(), _joint_name.c_str()); - continue; - } - - // TODO: check update time stamp ? - - if((*module_it)->GetControlMode() == POSITION_CONTROL) - { -// if(_result_state->goal_position == 0 && _dxl->id == 3) -// ROS_INFO("[MODULE:%s][ID:%2d] goal position = %f", (*module_it)->GetModuleName().c_str(), _dxl->id, _dxl_state->goal_position); - _dxl_state->goal_position = _result_state->goal_position; - - if(gazebo_mode == false) - { - // add offset - UINT32_T _pos_data = _dxl->ConvertRadian2Value(_dxl_state->goal_position + _dxl_state->position_offset); - UINT8_T _sync_write_data[4] = {0}; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data)); - - if(abs(_pos_data) > 151800) - printf("goal_pos : %f | position_offset : %f | pos_data : %d\n", _dxl_state->goal_position , _dxl_state->position_offset, _pos_data); - - if(port_to_sync_write_position[_dxl->port_name] != NULL) - port_to_sync_write_position[_dxl->port_name]->ChangeParam(_dxl->id, _sync_write_data); - - // if position p gain value is changed -> sync write - if(_dxl_state->position_p_gain != _result_state->position_p_gain) - { - _dxl_state->position_p_gain = _result_state->position_p_gain; - - UINT8_T _sync_write_p_gain[4] = {0}; - _sync_write_p_gain[0] = DXL_LOBYTE(DXL_LOWORD(_dxl_state->position_p_gain)); - _sync_write_p_gain[1] = DXL_HIBYTE(DXL_LOWORD(_dxl_state->position_p_gain)); - _sync_write_p_gain[2] = DXL_LOBYTE(DXL_HIWORD(_dxl_state->position_p_gain)); - _sync_write_p_gain[3] = DXL_HIBYTE(DXL_HIWORD(_dxl_state->position_p_gain)); - - if(port_to_sync_write_position_p_gain[_dxl->port_name] != NULL) - port_to_sync_write_position_p_gain[_dxl->port_name]->AddParam(_dxl->id, _sync_write_p_gain); - } - } - } - else if((*module_it)->GetControlMode() == VELOCITY_CONTROL) - { - _dxl_state->goal_velocity = _result_state->goal_velocity; - - if(gazebo_mode == false) - { - UINT32_T _vel_data = _dxl->ConvertVelocity2Value(_dxl_state->goal_velocity); - UINT8_T _sync_write_data[4] = {0}; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_vel_data)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_vel_data)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_vel_data)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_vel_data)); - - if(port_to_sync_write_velocity[_dxl->port_name] != NULL) - port_to_sync_write_velocity[_dxl->port_name]->ChangeParam(_dxl->id, _sync_write_data); - } - } - else if((*module_it)->GetControlMode() == CURRENT_CONTROL) - { - _dxl_state->goal_current = _result_state->goal_current; - - if(gazebo_mode == false) - { - UINT32_T _torq_data = _dxl->ConvertCurrent2Value(_dxl_state->goal_current); - UINT8_T _sync_write_data[2]; - _sync_write_data[0] = DXL_LOBYTE(_torq_data); - _sync_write_data[1] = DXL_HIBYTE(_torq_data); - - if(port_to_sync_write_torque[_dxl->port_name] != NULL) - port_to_sync_write_torque[_dxl->port_name]->ChangeParam(_dxl->id, _sync_write_data); - } - } - } + printf("goal_pos : %f | position_offset : %f | pos_data : %d\n", + dxl_state->goal_position_, dxl_state->position_offset_, pos_data); } - } - queue_mutex_.unlock(); - } - - if(DEBUG_PRINT) - { - _time_duration = ros::Time::now() - _start_time; - ROS_INFO("(%2.6f) MotionModule Process() & save result", _time_duration.nsec * 0.000001); - } + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data); - // SyncWrite - if(gazebo_mode == false && _do_sync_write) - { - if(port_to_sync_write_position_p_gain.size() > 0) - { - for(std::map::iterator _it = port_to_sync_write_position_p_gain.begin(); _it != port_to_sync_write_position_p_gain.end(); _it++) + // if position p gain value is changed -> sync write + if (dxl_state->position_p_gain_ != result_state->position_p_gain_) { - _it->second->TxPacket(); - _it->second->ClearParam(); + dxl_state->position_p_gain_ = result_state->position_p_gain_; + uint8_t sync_write_p_gain[4] = { 0 }; + sync_write_p_gain[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->position_p_gain_)); + sync_write_p_gain[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->position_p_gain_)); + sync_write_p_gain[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->position_p_gain_)); + sync_write_p_gain[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->position_p_gain_)); + + if (port_to_sync_write_position_p_gain_[dxl->port_name_] != NULL) + port_to_sync_write_position_p_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_p_gain); } + } } - for(std::map::iterator _it = port_to_sync_write_position.begin(); _it != port_to_sync_write_position.end(); _it++) - if(_it->second != NULL) _it->second->TxPacket(); - for(std::map::iterator _it = port_to_sync_write_velocity.begin(); _it != port_to_sync_write_velocity.end(); _it++) - if(_it->second != NULL) _it->second->TxPacket(); - for(std::map::iterator _it = port_to_sync_write_torque.begin(); _it != port_to_sync_write_torque.end(); _it++) - if(_it->second != NULL) _it->second->TxPacket(); - } - else if(gazebo_mode == true) - { - std_msgs::Float64 _joint_msg; - - for(std::map::iterator dxl_it = robot->dxls.begin(); dxl_it != robot->dxls.end(); dxl_it++) + else if ((*module_it)->getControlMode() == VelocityControl) { - _joint_msg.data = dxl_it->second->dxl_state->goal_position; - gazebo_joint_pub[dxl_it->first].publish(_joint_msg); + dxl_state->goal_velocity_ = result_state->goal_velocity_; + + if (gazebo_mode_ == false) + { + uint32_t vel_data = dxl->convertVelocity2Value(dxl_state->goal_velocity_); + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(vel_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(vel_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(vel_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(vel_data)); + + if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data); + } } - } - } - else if(controller_mode_ == DIRECT_CONTROL_MODE) - { - queue_mutex_.lock(); - - for(std::map::iterator _it = port_to_sync_write_position.begin(); _it != port_to_sync_write_position.end(); _it++) - { - _it->second->TxPacket(); - _it->second->ClearParam(); - } - - if(direct_sync_write_.size() > 0) - { - for(int _i = 0; _i < direct_sync_write_.size(); _i++) + else if ((*module_it)->getControlMode() == CurrentControl) { - direct_sync_write_[_i]->TxPacket(); - direct_sync_write_[_i]->ClearParam(); + dxl_state->goal_current_ = result_state->goal_current_; + + if (gazebo_mode_ == false) + { + uint32_t curr_data = dxl->convertCurrent2Value(dxl_state->goal_current_); + uint8_t sync_write_data[2] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(curr_data); + sync_write_data[1] = DXL_HIBYTE(curr_data); + + if (port_to_sync_write_torque_[dxl->port_name_] != NULL) + port_to_sync_write_torque_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data); + } } - direct_sync_write_.clear(); + } } + } - queue_mutex_.unlock(); + queue_mutex_.unlock(); } - // TODO: User Read/Write - - // BulkRead Tx - if(gazebo_mode == false) + if (DEBUG_PRINT) { - for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) - _it->second->TxPacket(); + time_duration = ros::Time::now() - start_time; + ROS_INFO("(%2.6f) MotionModule Process() & save result", time_duration.nsec * 0.000001); } - if(DEBUG_PRINT) + // SyncWrite + if (gazebo_mode_ == false && do_sync_write) { - _time_duration = ros::Time::now() - _start_time; - ROS_INFO("(%2.6f) SyncWrite & BulkRead Tx", _time_duration.nsec * 0.000001); + if (port_to_sync_write_position_p_gain_.size() > 0) + { + for (std::map::iterator it = port_to_sync_write_position_p_gain_.begin(); + it != port_to_sync_write_position_p_gain_.end(); it++) + { + it->second->txPacket(); + it->second->clearParam(); + } + } + for (std::map::iterator it = port_to_sync_write_position_.begin(); + it != port_to_sync_write_position_.end(); it++) + { + if (it->second != NULL) + it->second->txPacket(); + } + for (std::map::iterator it = port_to_sync_write_velocity_.begin(); + it != port_to_sync_write_velocity_.end(); it++) + { + if (it->second != NULL) + it->second->txPacket(); + } + for (std::map::iterator it = port_to_sync_write_torque_.begin(); + it != port_to_sync_write_torque_.end(); it++) + { + if (it->second != NULL) + it->second->txPacket(); + } } - - // publish present & goal position - for(std::map::iterator dxl_it = robot->dxls.begin(); dxl_it != robot->dxls.end(); dxl_it++) + else if (gazebo_mode_ == true) { - std::string _joint_name = dxl_it->first; - Dynamixel *_dxl = dxl_it->second; - - _present_state.name.push_back(_joint_name); - _present_state.position.push_back(_dxl->dxl_state->present_position); - _present_state.velocity.push_back(_dxl->dxl_state->present_velocity); - _present_state.effort.push_back(_dxl->dxl_state->present_current); - - _goal_state.name.push_back(_joint_name); - _goal_state.position.push_back(_dxl->dxl_state->goal_position); - _goal_state.velocity.push_back(_dxl->dxl_state->goal_velocity); - _goal_state.effort.push_back(_dxl->dxl_state->goal_current); + std_msgs::Float64 joint_msg; + + for (std::map::iterator dxl_it = robot_->dxls_.begin(); dxl_it != robot_->dxls_.end(); + dxl_it++) + { + joint_msg.data = dxl_it->second->dxl_state_->goal_position_; + gazebo_joint_pub_[dxl_it->first].publish(joint_msg); + } } + } + else if (controller_mode_ == DirectControlMode) + { + queue_mutex_.lock(); - // -> publish present joint_states & goal joint states topic - present_joint_state_pub.publish(_present_state); - goal_joint_state_pub.publish(_goal_state); + for (std::map::iterator it = port_to_sync_write_position_.begin(); + it != port_to_sync_write_position_.end(); it++) + { + it->second->txPacket(); + it->second->clearParam(); + } - if(DEBUG_PRINT) + if (direct_sync_write_.size() > 0) { - _time_duration = ros::Time::now() - _start_time; - ROS_WARN("(%2.6f) Process() DONE", _time_duration.nsec * 0.000001); + for (int i = 0; i < direct_sync_write_.size(); i++) + { + direct_sync_write_[i]->txPacket(); + direct_sync_write_[i]->clearParam(); + } + direct_sync_write_.clear(); } - _is_process_running = false; + queue_mutex_.unlock(); + } + + // TODO: User Read/Write + + // BulkRead Tx + if (gazebo_mode_ == false) + { + for (std::map::iterator it = port_to_bulk_read_.begin(); it != port_to_bulk_read_.end(); it++) + it->second->txPacket(); + } + + if (DEBUG_PRINT) + { + time_duration = ros::Time::now() - start_time; + ROS_INFO("(%2.6f) SyncWrite & BulkRead Tx", time_duration.nsec * 0.000001); + } + + // publish present & goal position + for (std::map::iterator dxl_it = robot_->dxls_.begin(); dxl_it != robot_->dxls_.end(); dxl_it++) + { + std::string joint_name = dxl_it->first; + Dynamixel *dxl = dxl_it->second; + + present_state.name.push_back(joint_name); + present_state.position.push_back(dxl->dxl_state_->present_position_); + present_state.velocity.push_back(dxl->dxl_state_->present_velocity_); + present_state.effort.push_back(dxl->dxl_state_->present_current_); + + goal_state.name.push_back(joint_name); + goal_state.position.push_back(dxl->dxl_state_->goal_position_); + goal_state.velocity.push_back(dxl->dxl_state_->goal_velocity_); + goal_state.effort.push_back(dxl->dxl_state_->goal_current_); + } + + // -> publish present joint_states & goal joint states topic + present_joint_state_pub_.publish(present_state); + goal_joint_state_pub_.publish(goal_state); + + if (DEBUG_PRINT) + { + time_duration = ros::Time::now() - start_time; + ROS_WARN("(%2.6f) Process() DONE", time_duration.nsec * 0.000001); + } + + is_process_running = false; } -void RobotisController::AddMotionModule(MotionModule *module) +void RobotisController::addMotionModule(MotionModule *module) { - // check whether the module name already exists - for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) + // check whether the module name already exists + for (std::list::iterator m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + { + if ((*m_it)->getModuleName() == module->getModuleName()) { - if((*_m_it)->GetModuleName() == module->GetModuleName()) - { - ROS_ERROR("Motion Module Name [%s] already exist !!", module->GetModuleName().c_str()); - return; - } + ROS_ERROR("Motion Module Name [%s] already exist !!", module->getModuleName().c_str()); + return; } + } - module->Initialize(CONTROL_CYCLE_MSEC, robot); - motion_modules_.push_back(module); - motion_modules_.unique(); + module->initialize(CONTROL_CYCLE_MSEC, robot_); + motion_modules_.push_back(module); + motion_modules_.unique(); } -void RobotisController::RemoveMotionModule(MotionModule *module) +void RobotisController::removeMotionModule(MotionModule *module) { - motion_modules_.remove(module); + motion_modules_.remove(module); } -void RobotisController::AddSensorModule(SensorModule *module) +void RobotisController::addSensorModule(SensorModule *module) { - // check whether the module name already exists - for(std::list::iterator _m_it = sensor_modules_.begin(); _m_it != sensor_modules_.end(); _m_it++) + // check whether the module name already exists + for (std::list::iterator m_it = sensor_modules_.begin(); m_it != sensor_modules_.end(); m_it++) + { + if ((*m_it)->getModuleName() == module->getModuleName()) { - if((*_m_it)->GetModuleName() == module->GetModuleName()) - { - ROS_ERROR("Sensor Module Name [%s] already exist !!", module->GetModuleName().c_str()); - return; - } + ROS_ERROR("Sensor Module Name [%s] already exist !!", module->getModuleName().c_str()); + return; } + } - module->Initialize(CONTROL_CYCLE_MSEC, robot); - sensor_modules_.push_back(module); - sensor_modules_.unique(); + module->initialize(CONTROL_CYCLE_MSEC, robot_); + sensor_modules_.push_back(module); + sensor_modules_.unique(); } -void RobotisController::RemoveSensorModule(SensorModule *module) +void RobotisController::removeSensorModule(SensorModule *module) { - sensor_modules_.remove(module); + sensor_modules_.remove(module); } -void RobotisController::SyncWriteItemCallback(const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg) +void RobotisController::syncWriteItemCallback(const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg) { - for(int _i = 0; _i < msg->joint_name.size(); _i++) - { - Dynamixel *_dxl = robot->dxls[msg->joint_name[_i]]; - ControlTableItem *_item = _dxl->ctrl_table[msg->item_name]; - - PortHandler *_port = robot->ports[_dxl->port_name]; - PacketHandler *_packet_handler= PacketHandler::GetPacketHandler(_dxl->protocol_version); + for (int i = 0; i < msg->joint_name.size(); i++) + { + Dynamixel *dxl = robot_->dxls_[msg->joint_name[i]]; + ControlTableItem *item = dxl->ctrl_table_[msg->item_name]; - if(_item->access_type == READ) - continue; + dynamixel::PortHandler *port = robot_->ports_[dxl->port_name_]; + dynamixel::PacketHandler *packet_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); - int _idx = 0; - if(direct_sync_write_.size() == 0) - { - direct_sync_write_.push_back(new GroupSyncWrite(_port, _packet_handler, _item->address, _item->data_length)); - _idx = 0; - } - else - { - for(_idx = 0; _idx < direct_sync_write_.size(); _idx++) - { - if(direct_sync_write_[_idx]->GetPortHandler() == _port && - direct_sync_write_[_idx]->GetPacketHandler() == _packet_handler) - break; - } + if (item->access_type_ == Read) + continue; - if(_idx == direct_sync_write_.size()) - direct_sync_write_.push_back(new GroupSyncWrite(_port, _packet_handler, _item->address, _item->data_length)); - } + int idx = 0; + if (direct_sync_write_.size() == 0) + { + direct_sync_write_.push_back(new dynamixel::GroupSyncWrite(port, packet_handler, item->address_, item->data_length_)); + idx = 0; + } + else + { + for (idx = 0; idx < direct_sync_write_.size(); idx++) + { + if (direct_sync_write_[idx]->getPortHandler() == port && direct_sync_write_[idx]->getPacketHandler() == packet_handler) + break; + } + + if (idx == direct_sync_write_.size()) + direct_sync_write_.push_back(new dynamixel::GroupSyncWrite(port, packet_handler, item->address_, item->data_length_)); + } - UINT8_T *_data = new UINT8_T[_item->data_length]; - if(_item->data_length == 1) - _data[0] = (UINT8_T)msg->value[_i]; - else if(_item->data_length == 2) - { - _data[0] = DXL_LOBYTE((UINT16_T)msg->value[_i]); - _data[1] = DXL_HIBYTE((UINT16_T)msg->value[_i]); - } - else if(_item->data_length == 4) - { - _data[0] = DXL_LOBYTE(DXL_LOWORD((UINT32_T)msg->value[_i])); - _data[1] = DXL_HIBYTE(DXL_LOWORD((UINT32_T)msg->value[_i])); - _data[2] = DXL_LOBYTE(DXL_HIWORD((UINT32_T)msg->value[_i])); - _data[3] = DXL_HIBYTE(DXL_HIWORD((UINT32_T)msg->value[_i])); - } - direct_sync_write_[_idx]->AddParam(_dxl->id, _data); - delete[] _data; + uint8_t *data = new uint8_t[item->data_length_]; + if (item->data_length_ == 1) + data[0] = (uint8_t) msg->value[i]; + else if (item->data_length_ == 2) + { + data[0] = DXL_LOBYTE((uint16_t )msg->value[i]); + data[1] = DXL_HIBYTE((uint16_t )msg->value[i]); } + else if (item->data_length_ == 4) + { + data[0] = DXL_LOBYTE(DXL_LOWORD((uint32_t)msg->value[i])); + data[1] = DXL_HIBYTE(DXL_LOWORD((uint32_t)msg->value[i])); + data[2] = DXL_LOBYTE(DXL_HIWORD((uint32_t)msg->value[i])); + data[3] = DXL_HIBYTE(DXL_HIWORD((uint32_t)msg->value[i])); + } + direct_sync_write_[idx]->addParam(dxl->id_, data); + delete[] data; + } } -void RobotisController::SetControllerModeCallback(const std_msgs::String::ConstPtr &msg) +void RobotisController::setControllerModeCallback(const std_msgs::String::ConstPtr &msg) { - if(msg->data == "DIRECT_CONTROL_MODE") - controller_mode_ = DIRECT_CONTROL_MODE; - else if(msg->data == "MOTION_MODULE_MODE") - controller_mode_ = MOTION_MODULE_MODE; + if (msg->data == "DirectControlMode") + controller_mode_ = DirectControlMode; + else if (msg->data == "MotionModuleMode") + controller_mode_ = MotionModuleMode; } -void RobotisController::SetJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) +void RobotisController::setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) { - if(controller_mode_ != DIRECT_CONTROL_MODE) - return; + if (controller_mode_ != DirectControlMode) + return; - queue_mutex_.lock(); + queue_mutex_.lock(); - for(int _i = 0; _i < msg->name.size(); _i++) - { - INT32_T _pos = 0; + for (int i = 0; i < msg->name.size(); i++) + { + int32_t pos = 0; - Dynamixel *_dxl = robot->dxls[msg->name[_i]]; - if(_dxl == NULL) - continue; + Dynamixel *dxl = robot_->dxls_[msg->name[i]]; + if (dxl == NULL) + continue; - _dxl->dxl_state->goal_position = msg->position[_i]; - _pos = _dxl->ConvertRadian2Value((double)msg->position[_i]); + dxl->dxl_state_->goal_position_ = msg->position[i]; + pos = dxl->convertRadian2Value((double) msg->position[i]); - UINT8_T _sync_write_data[4]; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos)); + uint8_t sync_write_data[4]; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos)); - if(port_to_sync_write_position[_dxl->port_name] != NULL) - port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); - } + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + } - queue_mutex_.unlock(); + queue_mutex_.unlock(); } -void RobotisController::SetCtrlModuleCallback(const std_msgs::String::ConstPtr &msg) +void RobotisController::setCtrlModuleCallback(const std_msgs::String::ConstPtr &msg) { - std::string _module_name_to_set = msg->data; + std::string _module_name_to_set = msg->data; - set_module_thread_ = boost::thread(boost::bind(&RobotisController::SetCtrlModuleThread, this, _module_name_to_set)); + set_module_thread_ = boost::thread(boost::bind(&RobotisController::setCtrlModuleThread, this, _module_name_to_set)); } -void RobotisController::SetJointCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg) +void RobotisController::setJointCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg) { - if(msg->joint_name.size() != msg->module_name.size()) - return; + if (msg->joint_name.size() != msg->module_name.size()) + return; - queue_mutex_.lock(); + queue_mutex_.lock(); - for(unsigned int idx = 0; idx < msg->joint_name.size(); idx++) - { - Dynamixel *_dxl = NULL; - std::map::iterator _dxl_it = robot->dxls.find((std::string)(msg->joint_name[idx])); - if(_dxl_it != robot->dxls.end()) - _dxl = _dxl_it->second; - else - continue; - - // none - if(msg->module_name[idx] == "" || msg->module_name[idx] == "none") - { - _dxl->ctrl_module_name = msg->module_name[idx]; - continue; - } + for (unsigned int idx = 0; idx < msg->joint_name.size(); idx++) + { + Dynamixel *dxl = NULL; + std::map::iterator dxl_it = robot_->dxls_.find((std::string) (msg->joint_name[idx])); + if (dxl_it != robot_->dxls_.end()) + dxl = dxl_it->second; + else + continue; - // check whether the module is using this joint - for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) - { - if((*_m_it)->GetModuleName() == msg->module_name[idx]) - { - if((*_m_it)->result.find(msg->joint_name[idx]) != (*_m_it)->result.end()) - { - _dxl->ctrl_module_name = msg->module_name[idx]; - break; - } - } - } + // none + if (msg->module_name[idx] == "" || msg->module_name[idx] == "none") + { + dxl->ctrl_module_name_ = msg->module_name[idx]; + continue; } - for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) + // check whether the module is using this joint + for (std::list::iterator m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) { - // set all modules -> disable - (*_m_it)->SetModuleEnable(false); - - // set all used modules -> enable - for(std::map::iterator _d_it = robot->dxls.begin(); _d_it != robot->dxls.end(); _d_it++) + if ((*m_it)->getModuleName() == msg->module_name[idx]) + { + if ((*m_it)->result_.find(msg->joint_name[idx]) != (*m_it)->result_.end()) { - if(_d_it->second->ctrl_module_name == (*_m_it)->GetModuleName()) - { - (*_m_it)->SetModuleEnable(true); - break; - } + dxl->ctrl_module_name_ = msg->module_name[idx]; + break; } + } } + } - // TODO: set indirect address - // -> check module's control_mode + for (std::list::iterator m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + { + // set all modules -> disable + (*m_it)->setModuleEnable(false); - queue_mutex_.unlock(); - - robotis_controller_msgs::JointCtrlModule _current_module_msg; - for(std::map::iterator _dxl_iter = robot->dxls.begin(); _dxl_iter != robot->dxls.end(); ++_dxl_iter) + // set all used modules -> enable + for (std::map::iterator d_it = robot_->dxls_.begin(); d_it != robot_->dxls_.end(); d_it++) { - _current_module_msg.joint_name.push_back(_dxl_iter->first); - _current_module_msg.module_name.push_back(_dxl_iter->second->ctrl_module_name); + if (d_it->second->ctrl_module_name_ == (*m_it)->getModuleName()) + { + (*m_it)->setModuleEnable(true); + break; + } } + } + + // TODO: set indirect address + // -> check module's control_mode + + queue_mutex_.unlock(); + + robotis_controller_msgs::JointCtrlModule current_module_msg; + for (std::map::iterator dxl_iter = robot_->dxls_.begin(); dxl_iter != robot_->dxls_.end(); ++dxl_iter) + { + current_module_msg.joint_name.push_back(dxl_iter->first); + current_module_msg.module_name.push_back(dxl_iter->second->ctrl_module_name_); + } - if(_current_module_msg.joint_name.size() == _current_module_msg.module_name.size()) - current_module_pub.publish(_current_module_msg); + if (current_module_msg.joint_name.size() == current_module_msg.module_name.size()) + current_module_pub_.publish(current_module_msg); } -bool RobotisController::GetCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res) +bool RobotisController::getCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req, + robotis_controller_msgs::GetJointModule::Response &res) { - for(unsigned int idx = 0; idx < req.joint_name.size(); idx++) + for (unsigned int idx = 0; idx < req.joint_name.size(); idx++) + { + std::map::iterator d_it = robot_->dxls_.find((std::string) (req.joint_name[idx])); + if (d_it != robot_->dxls_.end()) { - std::map::iterator _d_it = robot->dxls.find((std::string)(req.joint_name[idx])); - if(_d_it != robot->dxls.end()) - { - res.joint_name.push_back(req.joint_name[idx]); - res.module_name.push_back(_d_it->second->ctrl_module_name); - } + res.joint_name.push_back(req.joint_name[idx]); + res.module_name.push_back(d_it->second->ctrl_module_name_); } + } - if(res.joint_name.size() == 0) return false; + if (res.joint_name.size() == 0) + return false; - return true; + return true; } -void RobotisController::SetCtrlModuleThread(std::string ctrl_module) +void RobotisController::setCtrlModuleThread(std::string ctrl_module) { - // stop module - std::list _stop_modules; + // stop module + std::list stop_modules; - if(ctrl_module == "" || ctrl_module == "none") + if (ctrl_module == "" || ctrl_module == "none") + { + // enqueue all modules in order to stop + for (std::list::iterator m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) { - // enqueue all modules in order to stop - for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) - { - if((*_m_it)->GetModuleEnable() == true) _stop_modules.push_back(*_m_it); - } + if ((*m_it)->getModuleEnable() == true) + stop_modules.push_back(*m_it); } - else + } + else + { + for (std::list::iterator m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) { - for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) + // if it exist + if ((*m_it)->getModuleName() == ctrl_module) + { + // enqueue the module which lost control of joint in order to stop + for (std::map::iterator result_it = (*m_it)->result_.begin(); + result_it != (*m_it)->result_.end(); result_it++) { - // if it exist - if((*_m_it)->GetModuleName() == ctrl_module) + std::map::iterator d_it = robot_->dxls_.find(result_it->first); + + if (d_it != robot_->dxls_.end()) + { + // enqueue + if (d_it->second->ctrl_module_name_ != ctrl_module) { - // enqueue the module which lost control of joint in order to stop - for(std::map::iterator _result_it = (*_m_it)->result.begin(); _result_it != (*_m_it)->result.end(); _result_it++) + for (std::list::iterator stop_m_it = motion_modules_.begin(); + stop_m_it != motion_modules_.end(); stop_m_it++) + { + if (((*stop_m_it)->getModuleName() == d_it->second->ctrl_module_name_) && + ((*stop_m_it)->getModuleEnable() == true)) { - std::map::iterator _d_it = robot->dxls.find(_result_it->first); - - if(_d_it != robot->dxls.end()) - { - // enqueue - if(_d_it->second->ctrl_module_name != ctrl_module) - { - for(std::list::iterator _stop_m_it = motion_modules_.begin(); _stop_m_it != motion_modules_.end(); _stop_m_it++) - { - if((*_stop_m_it)->GetModuleName() == _d_it->second->ctrl_module_name && (*_stop_m_it)->GetModuleEnable() == true) - _stop_modules.push_back(*_stop_m_it); - } - } - } + stop_modules.push_back(*stop_m_it); } - - break; + } } + } } - } - // stop the module - _stop_modules.unique(); - for(std::list::iterator _stop_m_it = _stop_modules.begin(); _stop_m_it != _stop_modules.end(); _stop_m_it++) - { - (*_stop_m_it)->Stop(); + break; + } } - - // wait to stop - for(std::list::iterator _stop_m_it = _stop_modules.begin(); _stop_m_it != _stop_modules.end(); _stop_m_it++) + } + + // stop the module + stop_modules.unique(); + for (std::list::iterator stop_m_it = stop_modules.begin(); stop_m_it != stop_modules.end(); stop_m_it++) + { + (*stop_m_it)->stop(); + } + + // wait to stop + for (std::list::iterator stop_m_it = stop_modules.begin(); stop_m_it != stop_modules.end(); stop_m_it++) + { + while ((*stop_m_it)->isRunning()) + usleep(CONTROL_CYCLE_MSEC * 1000); + } + + // set ctrl module + queue_mutex_.lock(); + + if (DEBUG_PRINT) + ROS_INFO_STREAM("set module : " << ctrl_module); + + // none + if ((ctrl_module == "") || (ctrl_module == "none")) + { + // set all modules -> disable + for (std::list::iterator m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) { - while((*_stop_m_it)->IsRunning()) - usleep(CONTROL_CYCLE_MSEC * 1000); + (*m_it)->setModuleEnable(false); } - // set ctrl module - queue_mutex_.lock(); - - if(DEBUG_PRINT) ROS_INFO_STREAM("set module : " << ctrl_module); - - // none - if(ctrl_module == "" || ctrl_module == "none") + // set dxl's control module to "none" + for (std::map::iterator d_it = robot_->dxls_.begin(); d_it != robot_->dxls_.end(); d_it++) { - // set all modules -> disable - for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) - { - (*_m_it)->SetModuleEnable(false); - } - - // set dxl's control module to "none" - for(std::map::iterator _d_it = robot->dxls.begin(); _d_it != robot->dxls.end(); _d_it++) - { - Dynamixel *_dxl = _d_it->second; - _dxl->ctrl_module_name = "none"; - - if(gazebo_mode == true) - continue; - - UINT32_T _pos_data = _dxl->ConvertRadian2Value(_dxl->dxl_state->goal_position + _dxl->dxl_state->position_offset); - UINT8_T _sync_write_data[4]; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data)); - - if(port_to_sync_write_position[_dxl->port_name] != NULL) - port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); - - if(port_to_sync_write_torque[_dxl->port_name] != NULL) - port_to_sync_write_torque[_dxl->port_name]->RemoveParam(_dxl->id); - if(port_to_sync_write_velocity[_dxl->port_name] != NULL) - port_to_sync_write_velocity[_dxl->port_name]->RemoveParam(_dxl->id); - } + Dynamixel *dxl = d_it->second; + dxl->ctrl_module_name_ = "none"; + + if (gazebo_mode_ == true) + continue; + + uint32_t pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_); + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos_data)); + + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + + if (port_to_sync_write_torque_[dxl->port_name_] != NULL) + port_to_sync_write_torque_[dxl->port_name_]->removeParam(dxl->id_); + if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_[dxl->port_name_]->removeParam(dxl->id_); } - else + } + else + { + // check whether the module exist + for (std::list::iterator m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) { - // check whether the module exist - for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) + // if it exist + if ((*m_it)->getModuleName() == ctrl_module) + { + ControlMode mode = (*m_it)->getControlMode(); + for (std::map::iterator result_it = (*m_it)->result_.begin(); + result_it != (*m_it)->result_.end(); result_it++) { - // if it exist - if((*_m_it)->GetModuleName() == ctrl_module) - { - CONTROL_MODE _mode = (*_m_it)->GetControlMode(); - for(std::map::iterator _result_it = (*_m_it)->result.begin(); _result_it != (*_m_it)->result.end(); _result_it++) - { - std::map::iterator _d_it = robot->dxls.find(_result_it->first); - if(_d_it != robot->dxls.end()) - { - Dynamixel *_dxl = _d_it->second; - _dxl->ctrl_module_name = ctrl_module; - - if(gazebo_mode == true) - continue; - - if(_mode == POSITION_CONTROL) - { - UINT32_T _pos_data = _dxl->ConvertRadian2Value(_dxl->dxl_state->goal_position + _dxl->dxl_state->position_offset); - UINT8_T _sync_write_data[4]; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data)); - - if(port_to_sync_write_position[_dxl->port_name] != NULL) - port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); - - if(port_to_sync_write_torque[_dxl->port_name] != NULL) - port_to_sync_write_torque[_dxl->port_name]->RemoveParam(_dxl->id); - if(port_to_sync_write_velocity[_dxl->port_name] != NULL) - port_to_sync_write_velocity[_dxl->port_name]->RemoveParam(_dxl->id); - } - else if(_mode == VELOCITY_CONTROL) - { - UINT32_T _vel_data = _dxl->ConvertVelocity2Value(_dxl->dxl_state->goal_velocity); - UINT8_T _sync_write_data[4]; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_vel_data)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_vel_data)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_vel_data)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_vel_data)); - - if(port_to_sync_write_velocity[_dxl->port_name] != NULL) - port_to_sync_write_velocity[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); - - if(port_to_sync_write_torque[_dxl->port_name] != NULL) - port_to_sync_write_torque[_dxl->port_name]->RemoveParam(_dxl->id); - if(port_to_sync_write_position[_dxl->port_name] != NULL) - port_to_sync_write_position[_dxl->port_name]->RemoveParam(_dxl->id); - } - else if(_mode == CURRENT_CONTROL) - { - UINT32_T _curr_data = _dxl->ConvertCurrent2Value(_dxl->dxl_state->goal_current); - UINT8_T _sync_write_data[4]; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_curr_data)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_curr_data)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_curr_data)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_curr_data)); - - if(port_to_sync_write_torque[_dxl->port_name] != NULL) - port_to_sync_write_torque[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); - - if(port_to_sync_write_velocity[_dxl->port_name] != NULL) - port_to_sync_write_velocity[_dxl->port_name]->RemoveParam(_dxl->id); - if(port_to_sync_write_position[_dxl->port_name] != NULL) - port_to_sync_write_position[_dxl->port_name]->RemoveParam(_dxl->id); - } - } - } - - break; - } - } - } + std::map::iterator d_it = robot_->dxls_.find(result_it->first); + if (d_it != robot_->dxls_.end()) + { + Dynamixel *dxl = d_it->second; + dxl->ctrl_module_name_ = ctrl_module; - for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) - { - // set all modules -> disable - (*_m_it)->SetModuleEnable(false); + if (gazebo_mode_ == true) + continue; - // set all used modules -> enable - for(std::map::iterator _d_it = robot->dxls.begin(); _d_it != robot->dxls.end(); _d_it++) - { - if(_d_it->second->ctrl_module_name == (*_m_it)->GetModuleName()) + if (mode == PositionControl) + { + uint32_t pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_); + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos_data)); + + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + + if (port_to_sync_write_torque_[dxl->port_name_] != NULL) + port_to_sync_write_torque_[dxl->port_name_]->removeParam(dxl->id_); + if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_[dxl->port_name_]->removeParam(dxl->id_); + } + else if (mode == VelocityControl) { - (*_m_it)->SetModuleEnable(true); - break; + uint32_t vel_data = dxl->convertVelocity2Value(dxl->dxl_state_->goal_velocity_); + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(vel_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(vel_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(vel_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(vel_data)); + + if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + + if (port_to_sync_write_torque_[dxl->port_name_] != NULL) + port_to_sync_write_torque_[dxl->port_name_]->removeParam(dxl->id_); + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->removeParam(dxl->id_); } + else if (mode == CurrentControl) + { + uint32_t curr_data = dxl->convertCurrent2Value(dxl->dxl_state_->goal_current_); + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(curr_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(curr_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(curr_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(curr_data)); + + if (port_to_sync_write_torque_[dxl->port_name_] != NULL) + port_to_sync_write_torque_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + + if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_[dxl->port_name_]->removeParam(dxl->id_); + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->removeParam(dxl->id_); + } + } } - } - // TODO: set indirect address - // -> check module's control_mode + break; + } + } + } - queue_mutex_.unlock(); + for (std::list::iterator m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + { + // set all modules -> disable + (*m_it)->setModuleEnable(false); - // publish current module - robotis_controller_msgs::JointCtrlModule _current_module_msg; - for(std::map::iterator _dxl_iter = robot->dxls.begin(); _dxl_iter != robot->dxls.end(); ++_dxl_iter) + // set all used modules -> enable + for (std::map::iterator d_it = robot_->dxls_.begin(); d_it != robot_->dxls_.end(); d_it++) { - _current_module_msg.joint_name.push_back(_dxl_iter->first); - _current_module_msg.module_name.push_back(_dxl_iter->second->ctrl_module_name); + if (d_it->second->ctrl_module_name_ == (*m_it)->getModuleName()) + { + (*m_it)->setModuleEnable(true); + break; + } } + } + + // TODO: set indirect address + // -> check module's control_mode - if(_current_module_msg.joint_name.size() == _current_module_msg.module_name.size()) - current_module_pub.publish(_current_module_msg); + queue_mutex_.unlock(); + + // publish current module + robotis_controller_msgs::JointCtrlModule current_module_msg; + for (std::map::iterator dxl_iter = robot_->dxls_.begin(); dxl_iter != robot_->dxls_.end(); ++dxl_iter) + { + current_module_msg.joint_name.push_back(dxl_iter->first); + current_module_msg.module_name.push_back(dxl_iter->second->ctrl_module_name_); + } + + if (current_module_msg.joint_name.size() == current_module_msg.module_name.size()) + current_module_pub_.publish(current_module_msg); } -void RobotisController::GazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) +void RobotisController::gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) { - queue_mutex_.lock(); + queue_mutex_.lock(); - for(unsigned int _i = 0; _i < msg->name.size(); _i++) + for (unsigned int i = 0; i < msg->name.size(); i++) + { + std::map::iterator d_it = robot_->dxls_.find((std::string) msg->name[i]); + if (d_it != robot_->dxls_.end()) { - std::map::iterator _d_it = robot->dxls.find((std::string)msg->name[_i]); - if(_d_it != robot->dxls.end()) - { - _d_it->second->dxl_state->present_position = msg->position[_i]; - _d_it->second->dxl_state->present_velocity = msg->velocity[_i]; - _d_it->second->dxl_state->present_current = msg->effort[_i]; - } + d_it->second->dxl_state_->present_position_ = msg->position[i]; + d_it->second->dxl_state_->present_velocity_ = msg->velocity[i]; + d_it->second->dxl_state_->present_current_ = msg->effort[i]; } + } - if(init_pose_loaded_ == false) - { - for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) - _it->second->dxl_state->goal_position = _it->second->dxl_state->present_position; - init_pose_loaded_ = true; - } + if (init_pose_loaded_ == false) + { + for (std::map::iterator it = robot_->dxls_.begin(); it != robot_->dxls_.end(); it++) + it->second->dxl_state_->goal_position_ = it->second->dxl_state_->present_position_; + init_pose_loaded_ = true; + } - queue_mutex_.unlock(); + queue_mutex_.unlock(); } -bool RobotisController::CheckTimerStop() +bool RobotisController::isTimerStopped() { - if(this->is_timer_running_) - { - if(DEBUG_PRINT == true) - ROS_WARN("Process Timer is running.. STOP the timer first."); - return false; - } - return true; + if (this->is_timer_running_) + { + if (DEBUG_PRINT == true) + ROS_WARN("Process Timer is running.. STOP the timer first."); + return false; + } + return true; } -int RobotisController::Ping(const std::string joint_name, UINT8_T *error) +int RobotisController::ping(const std::string joint_name, uint8_t *error) { - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - return _pkt_handler->Ping(_port_handler, _dxl->id, error); + return ping(joint_name, 0, error); } -int RobotisController::Ping(const std::string joint_name, UINT16_T* model_number, UINT8_T *error) +int RobotisController::ping(const std::string joint_name, uint16_t* model_number, uint8_t *error) { - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; + if (isTimerStopped() == false) + return COMM_PORT_BUSY; - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - return _pkt_handler->Ping(_port_handler, _dxl->id, model_number, error); + return pkt_handler->ping(port_handler, dxl->id_, model_number, error); } -int RobotisController::Action(const std::string joint_name) +int RobotisController::action(const std::string joint_name) { - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; + if (isTimerStopped() == false) + return COMM_PORT_BUSY; - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - return _pkt_handler->Action(_port_handler, _dxl->id); + return pkt_handler->action(port_handler, dxl->id_); } -int RobotisController::Reboot(const std::string joint_name, UINT8_T *error) +int RobotisController::reboot(const std::string joint_name, uint8_t *error) { - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; + if (isTimerStopped() == false) + return COMM_PORT_BUSY; - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - return _pkt_handler->Reboot(_port_handler, _dxl->id, error); + return pkt_handler->reboot(port_handler, dxl->id_, error); } -int RobotisController::FactoryReset(const std::string joint_name, UINT8_T option, UINT8_T *error) +int RobotisController::factoryReset(const std::string joint_name, uint8_t option, uint8_t *error) { - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; + if (isTimerStopped() == false) + return COMM_PORT_BUSY; - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - return _pkt_handler->FactoryReset(_port_handler, _dxl->id, option, error); + return pkt_handler->factoryReset(port_handler, dxl->id_, option, error); } -int RobotisController::Read(const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) +int RobotisController::read(const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) { - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; + if (isTimerStopped() == false) + return COMM_PORT_BUSY; - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - return _pkt_handler->ReadTxRx(_port_handler, _dxl->id, address, length, data, error); + return pkt_handler->readTxRx(port_handler, dxl->id_, address, length, data, error); } -int RobotisController::ReadCtrlItem(const std::string joint_name, const std::string item_name, UINT32_T *data, UINT8_T *error) +int RobotisController::readCtrlItem(const std::string joint_name, const std::string item_name, uint32_t *data, uint8_t *error) { - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - ControlTableItem*_item = _dxl->ctrl_table[item_name]; - if(_item == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - int _result = COMM_NOT_AVAILABLE; - switch(_item->data_length) - { - case 1: - { - UINT8_T _data = 0; - _result = _pkt_handler->Read1ByteTxRx(_port_handler, _dxl->id, _item->address, &_data, error); - if(_result == COMM_SUCCESS) - *data = _data; - break; - } - case 2: - { - UINT16_T _data = 0; - _result = _pkt_handler->Read2ByteTxRx(_port_handler, _dxl->id, _item->address, &_data, error); - if(_result == COMM_SUCCESS) - *data = _data; - break; - } - case 4: - { - UINT32_T _data = 0; - _result = _pkt_handler->Read4ByteTxRx(_port_handler, _dxl->id, _item->address, &_data, error); - if(_result == COMM_SUCCESS) - *data = _data; - break; - } - default: - break; - } - return _result; + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + ControlTableItem *item = dxl->ctrl_table_[item_name]; + if (item == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + int result = COMM_NOT_AVAILABLE; + switch (item->data_length_) + { + case 1: + { + uint8_t read_data = 0; + result = pkt_handler->read1ByteTxRx(port_handler, dxl->id_, item->address_, &read_data, error); + if (result == COMM_SUCCESS) + *data = read_data; + break; + } + case 2: + { + uint16_t read_data = 0; + result = pkt_handler->read2ByteTxRx(port_handler, dxl->id_, item->address_, &read_data, error); + if (result == COMM_SUCCESS) + *data = read_data; + break; + } + case 4: + { + uint32_t read_data = 0; + result = pkt_handler->read4ByteTxRx(port_handler, dxl->id_, item->address_, &read_data, error); + if (result == COMM_SUCCESS) + *data = read_data; + break; + } + default: + break; + } + return result; } -int RobotisController::Read1Byte(const std::string joint_name, UINT16_T address, UINT8_T *data, UINT8_T *error) +int RobotisController::read1Byte(const std::string joint_name, uint16_t address, uint8_t *data, uint8_t *error) { - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; + if (isTimerStopped() == false) + return COMM_PORT_BUSY; - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - return _pkt_handler->Read1ByteTxRx(_port_handler, _dxl->id, address, data, error); + return pkt_handler->read1ByteTxRx(port_handler, dxl->id_, address, data, error); } -int RobotisController::Read2Byte(const std::string joint_name, UINT16_T address, UINT16_T *data, UINT8_T *error) +int RobotisController::read2Byte(const std::string joint_name, uint16_t address, uint16_t *data, uint8_t *error) { - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; + if (isTimerStopped() == false) + return COMM_PORT_BUSY; - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - return _pkt_handler->Read2ByteTxRx(_port_handler, _dxl->id, address, data, error); + return pkt_handler->read2ByteTxRx(port_handler, dxl->id_, address, data, error); } -int RobotisController::Read4Byte(const std::string joint_name, UINT16_T address, UINT32_T *data, UINT8_T *error) +int RobotisController::read4Byte(const std::string joint_name, uint16_t address, uint32_t *data, uint8_t *error) { - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; + if (isTimerStopped() == false) + return COMM_PORT_BUSY; - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - return _pkt_handler->Read4ByteTxRx(_port_handler, _dxl->id, address, data, error); + return pkt_handler->read4ByteTxRx(port_handler, dxl->id_, address, data, error); } -int RobotisController::Write(const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) +int RobotisController::write(const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) { - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; + if (isTimerStopped() == false) + return COMM_PORT_BUSY; - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - return _pkt_handler->WriteTxRx(_port_handler, _dxl->id, address, length, data, error); + return pkt_handler->writeTxRx(port_handler, dxl->id_, address, length, data, error); } -int RobotisController::WriteCtrlItem(const std::string joint_name, const std::string item_name, UINT32_T data, UINT8_T *error) +int RobotisController::writeCtrlItem(const std::string joint_name, const std::string item_name, uint32_t data, uint8_t *error) { - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - ControlTableItem*_item = _dxl->ctrl_table[item_name]; - if(_item == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - int _result = COMM_NOT_AVAILABLE; - UINT8_T *_data = new UINT8_T[_item->data_length]; - if(_item->data_length == 1) - { - _data[0] = (UINT8_T)data; - _result = _pkt_handler->Write1ByteTxRx(_port_handler, _dxl->id, _item->address, data, error); - } - else if(_item->data_length == 2) - { - _data[0] = DXL_LOBYTE((UINT16_T)data); - _data[1] = DXL_HIBYTE((UINT16_T)data); - _result = _pkt_handler->Write2ByteTxRx(_port_handler, _dxl->id, _item->address, data, error); - } - else if(_item->data_length == 4) - { - _data[0] = DXL_LOBYTE(DXL_LOWORD((UINT32_T)data)); - _data[1] = DXL_HIBYTE(DXL_LOWORD((UINT32_T)data)); - _data[2] = DXL_LOBYTE(DXL_HIWORD((UINT32_T)data)); - _data[3] = DXL_HIBYTE(DXL_HIWORD((UINT32_T)data)); - _result = _pkt_handler->Write4ByteTxRx(_port_handler, _dxl->id, _item->address, data, error); - } - delete[] _data; - return _result; + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + ControlTableItem *item = dxl->ctrl_table_[item_name]; + if (item == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + int result = COMM_NOT_AVAILABLE; + uint8_t *write_data = new uint8_t[item->data_length_]; + if (item->data_length_ == 1) + { + write_data[0] = (uint8_t) data; + result = pkt_handler->write1ByteTxRx(port_handler, dxl->id_, item->address_, data, error); + } + else if (item->data_length_ == 2) + { + write_data[0] = DXL_LOBYTE((uint16_t )data); + write_data[1] = DXL_HIBYTE((uint16_t )data); + result = pkt_handler->write2ByteTxRx(port_handler, dxl->id_, item->address_, data, error); + } + else if (item->data_length_ == 4) + { + write_data[0] = DXL_LOBYTE(DXL_LOWORD((uint32_t)data)); + write_data[1] = DXL_HIBYTE(DXL_LOWORD((uint32_t)data)); + write_data[2] = DXL_LOBYTE(DXL_HIWORD((uint32_t)data)); + write_data[3] = DXL_HIBYTE(DXL_HIWORD((uint32_t)data)); + result = pkt_handler->write4ByteTxRx(port_handler, dxl->id_, item->address_, data, error); + } + delete[] write_data; + return result; } -int RobotisController::Write1Byte(const std::string joint_name, UINT16_T address, UINT8_T data, UINT8_T *error) +int RobotisController::write1Byte(const std::string joint_name, uint16_t address, uint8_t data, uint8_t *error) { - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; + if (isTimerStopped() == false) + return COMM_PORT_BUSY; - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - return _pkt_handler->Write1ByteTxRx(_port_handler, _dxl->id, address, data, error); + return pkt_handler->write1ByteTxRx(port_handler, dxl->id_, address, data, error); } -int RobotisController::Write2Byte(const std::string joint_name, UINT16_T address, UINT16_T data, UINT8_T *error) +int RobotisController::write2Byte(const std::string joint_name, uint16_t address, uint16_t data, uint8_t *error) { - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; + if (isTimerStopped() == false) + return COMM_PORT_BUSY; - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - return _pkt_handler->Write2ByteTxRx(_port_handler, _dxl->id, address, data, error); + return pkt_handler->write2ByteTxRx(port_handler, dxl->id_, address, data, error); } -int RobotisController::Write4Byte(const std::string joint_name, UINT16_T address, UINT32_T data, UINT8_T *error) +int RobotisController::write4Byte(const std::string joint_name, uint16_t address, uint32_t data, uint8_t *error) { - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; + if (isTimerStopped() == false) + return COMM_PORT_BUSY; - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - return _pkt_handler->Write4ByteTxRx(_port_handler, _dxl->id, address, data, error); + return pkt_handler->write4ByteTxRx(port_handler, dxl->id_, address, data, error); } -int RobotisController::RegWrite(const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) +int RobotisController::regWrite(const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, + uint8_t *error) { - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; + if (isTimerStopped() == false) + return COMM_PORT_BUSY; - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - return _pkt_handler->RegWriteTxRx(_port_handler, _dxl->id, address, length, data, error); + return pkt_handler->regWriteTxRx(port_handler, dxl->id_, address, length, data, error); } diff --git a/robotis_controller_msgs/package.xml b/robotis_controller_msgs/package.xml index 3ce5db7..8d54012 100644 --- a/robotis_controller_msgs/package.xml +++ b/robotis_controller_msgs/package.xml @@ -9,7 +9,7 @@ - robotis --> + ROBOTIS catkin @@ -21,10 +21,4 @@ std_msgs message_runtime - - - - - - \ No newline at end of file diff --git a/robotis_device/CMakeLists.txt b/robotis_device/CMakeLists.txt index 8bd7be5..6c53ff0 100644 --- a/robotis_device/CMakeLists.txt +++ b/robotis_device/CMakeLists.txt @@ -8,15 +8,6 @@ find_package(catkin REQUIRED COMPONENTS robotis_framework_common ) -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( INCLUDE_DIRS include LIBRARIES robotis_device @@ -24,38 +15,19 @@ catkin_package( # DEPENDS system_lib ) -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) include_directories( include ${catkin_INCLUDE_DIRS} ) -## Declare a C++ library add_library(robotis_device - src/robotis_device/Robot.cpp - src/robotis_device/Sensor.cpp - src/robotis_device/Dynamixel.cpp + src/robotis_device/robot.cpp + src/robotis_device/sensor.cpp + src/robotis_device/dynamixel.cpp ) -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure add_dependencies(robotis_device ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -## Declare a C++ executable -# add_executable(robotis_device_node src/robotis_device_node.cpp) - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(robotis_device_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against target_link_libraries(robotis_device ${catkin_LIBRARIES} ) diff --git a/robotis_device/include/robotis_device/control_table_item.h b/robotis_device/include/robotis_device/control_table_item.h index e7b75fd..28a840a 100644 --- a/robotis_device/include/robotis_device/control_table_item.h +++ b/robotis_device/include/robotis_device/control_table_item.h @@ -1,54 +1,84 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + /* - * ControlTableItem.h + * control_table_item.h * * Created on: 2015. 12. 16. * Author: zerom */ -#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_CONTROLTABLEITEM_H_ -#define ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_CONTROLTABLEITEM_H_ +#ifndef ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ +#define ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ -#include +#include -namespace ROBOTIS +namespace robotis_framework { -enum ACCESS_TYPE { - READ, - READ_WRITE +enum AccessType { + Read, + ReadWrite }; -enum MEMORY_TYPE { - EEPROM, - RAM +enum MemoryType { + EEPROM, + RAM }; class ControlTableItem { public: - std::string item_name; - UINT16_T address; - ACCESS_TYPE access_type; - MEMORY_TYPE memory_type; - UINT8_T data_length; - INT32_T data_min_value; - INT32_T data_max_value; - bool is_signed; - - ControlTableItem() - : item_name(""), - address(0), - access_type(READ), - memory_type(RAM), - data_length(0), - data_min_value(0), - data_max_value(0), - is_signed(false) - { } + std::string item_name_; + uint16_t address_; + AccessType access_type_; + MemoryType memory_type_; + uint8_t data_length_; + int32_t data_min_value_; + int32_t data_max_value_; + bool is_signed_; + + ControlTableItem() + : item_name_(""), + address_(0), + access_type_(Read), + memory_type_(RAM), + data_length_(0), + data_min_value_(0), + data_max_value_(0), + is_signed_(false) + { } }; } -#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_CONTROLTABLEITEM_H_ */ +#endif /* ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ */ diff --git a/robotis_device/include/robotis_device/device.h b/robotis_device/include/robotis_device/device.h index d9fd0bf..0a5556e 100644 --- a/robotis_device/include/robotis_device/device.h +++ b/robotis_device/include/robotis_device/device.h @@ -1,37 +1,68 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + /* - * Device.h + * device.h * * Created on: 2016. 5. 12. * Author: zerom */ -#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DEVICE_H_ -#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DEVICE_H_ +#ifndef ROBOTIS_DEVICE_DEVICE_H_ +#define ROBOTIS_DEVICE_DEVICE_H_ #include #include #include -#include "ControlTableItem.h" -namespace ROBOTIS +#include "control_table_item.h" + +namespace robotis_framework { class Device { public: - UINT8_T id; - float protocol_version; - std::string model_name; - std::string port_name; + uint8_t id_; + float protocol_version_; + std::string model_name_; + std::string port_name_; - std::map ctrl_table; - std::vector bulk_read_items; + std::map ctrl_table_; + std::vector bulk_read_items_; - virtual ~Device() { } + virtual ~Device() { } }; } -#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DEVICE_H_ */ +#endif /* ROBOTIS_DEVICE_DEVICE_H_ */ diff --git a/robotis_device/include/robotis_device/dynamixel.h b/robotis_device/include/robotis_device/dynamixel.h index a0a9beb..916f4c5 100644 --- a/robotis_device/include/robotis_device/dynamixel.h +++ b/robotis_device/include/robotis_device/dynamixel.h @@ -1,61 +1,92 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + /* - * Dynamixel.h + * dynamixel.h * * Created on: 2015. 12. 8. * Author: zerom */ -#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXEL_H_ -#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXEL_H_ +#ifndef ROBOTIS_DEVICE_DYNAMIXEL_H_ +#define ROBOTIS_DEVICE_DYNAMIXEL_H_ #include #include #include -#include "Device.h" -#include "DynamixelState.h" -#include "ControlTableItem.h" -namespace ROBOTIS +#include "control_table_item.h" +#include "device.h" +#include "dynamixel_state.h" + +namespace robotis_framework { class Dynamixel : public Device { public: - std::string ctrl_module_name; - DynamixelState *dxl_state; + std::string ctrl_module_name_; + DynamixelState *dxl_state_; - double current_ratio; - double velocity_ratio; + double current_ratio_; + double velocity_ratio_; - INT32_T value_of_0_radian_position; - INT32_T value_of_min_radian_position; - INT32_T value_of_max_radian_position; - double min_radian; - double max_radian; + int32_t value_of_0_radian_position_; + int32_t value_of_min_radian_position_; + int32_t value_of_max_radian_position_; + double min_radian_; + double max_radian_; - ControlTableItem *torque_enable_item; - ControlTableItem *present_position_item; - ControlTableItem *present_velocity_item; - ControlTableItem *present_current_item; - ControlTableItem *goal_position_item; - ControlTableItem *goal_velocity_item; - ControlTableItem *goal_current_item; - ControlTableItem *position_p_gain_item; + ControlTableItem *torque_enable_item_; + ControlTableItem *present_position_item_; + ControlTableItem *present_velocity_item_; + ControlTableItem *present_current_item_; + ControlTableItem *goal_position_item_; + ControlTableItem *goal_velocity_item_; + ControlTableItem *goal_current_item_; + ControlTableItem *position_p_gain_item_; - Dynamixel(int id, std::string model_name, float protocol_version); + Dynamixel(int id, std::string model_name, float protocol_version); - double ConvertValue2Radian(INT32_T value); - INT32_T ConvertRadian2Value(double radian); + double convertValue2Radian(int32_t value); + int32_t convertRadian2Value(double radian); - double ConvertValue2Velocity(INT32_T value); - INT32_T ConvertVelocity2Value(double velocity); + double convertValue2Velocity(int32_t value); + int32_t convertVelocity2Value(double velocity); - double ConvertValue2Current(INT16_T value); - INT16_T ConvertCurrent2Value(double torque); + double convertValue2Current(int16_t value); + int16_t convertCurrent2Value(double current); }; } -#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXEL_H_ */ +#endif /* ROBOTIS_DEVICE_DYNAMIXEL_H_ */ diff --git a/robotis_device/include/robotis_device/dynamixel_state.h b/robotis_device/include/robotis_device/dynamixel_state.h index e3768d4..b5f4d4d 100644 --- a/robotis_device/include/robotis_device/dynamixel_state.h +++ b/robotis_device/include/robotis_device/dynamixel_state.h @@ -1,57 +1,86 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + /* - * DynamixelState.h + * dynamixel_state.h * * Created on: 2015. 12. 8. * Author: zerom */ -#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXELSTATE_H_ -#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXELSTATE_H_ +#ifndef ROBOTIS_DEVICE_DYNAMIXEL_STATE_H_ +#define ROBOTIS_DEVICE_DYNAMIXEL_STATE_H_ #include -#include "robotis_device/TimeStamp.h" -#include "robotis_framework_common/RobotisDef.h" +#include "time_stamp.h" #define INDIRECT_DATA_1 "indirect_data_1" #define INDIRECT_ADDRESS_1 "indirect_address_1" -namespace ROBOTIS +namespace robotis_framework { class DynamixelState { public: - TimeStamp update_time_stamp; - - double present_position; - double present_velocity; - double present_current; - double goal_position; - double goal_velocity; - double goal_current; - double position_p_gain; - - std::map bulk_read_table; - - double position_offset; - - DynamixelState() - : update_time_stamp(0, 0), - present_position(0.0), - present_velocity(0.0), - present_current(0.0), - goal_position(0.0), - goal_velocity(0.0), - goal_current(0.0), - position_p_gain(0.0), - position_offset(0.0) - { - bulk_read_table.clear(); - } + TimeStamp update_time_stamp_; + + double present_position_; + double present_velocity_; + double present_current_; + double goal_position_; + double goal_velocity_; + double goal_current_; + double position_p_gain_; + + std::map bulk_read_table_; + + double position_offset_; + + DynamixelState() + : update_time_stamp_(0, 0), + present_position_(0.0), + present_velocity_(0.0), + present_current_(0.0), + goal_position_(0.0), + goal_velocity_(0.0), + goal_current_(0.0), + position_p_gain_(0.0), + position_offset_(0.0) + { + bulk_read_table_.clear(); + } }; } -#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXELSTATE_H_ */ +#endif /* ROBOTIS_DEVICE_DYNAMIXEL_STATE_H_ */ diff --git a/robotis_device/include/robotis_device/robot.h b/robotis_device/include/robotis_device/robot.h index 2473e7c..4dfe5bc 100644 --- a/robotis_device/include/robotis_device/robot.h +++ b/robotis_device/include/robotis_device/robot.h @@ -1,38 +1,78 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + /* - * Robot.h + * robot.h * * Created on: 2015. 12. 11. * Author: zerom */ -#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_ROBOT_H_ -#define ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_ROBOT_H_ +#ifndef ROBOTIS_DEVICE_ROBOT_H_ +#define ROBOTIS_DEVICE_ROBOT_H_ #include -#include "Sensor.h" -#include "Dynamixel.h" -#include "dynamixel_sdk/PortHandler.h" -namespace ROBOTIS +#include "sensor.h" +#include "dynamixel.h" +#include "dynamixel_sdk/port_handler.h" + +#define DYNAMIXEL "dynamixel" +#define SENSOR "sensor" + +#define SESSION_PORT_INFO "port info" +#define SESSION_DEVICE_INFO "device info" + +#define SESSION_TYPE_INFO "type info" +#define SESSION_CONTROL_TABLE "control table" + +namespace robotis_framework { class Robot { public: - std::map ports; // string: port name - std::map port_default_device; // port name, default device name + std::map ports_; // string: port name + std::map port_default_device_; // port name, default device name - std::map dxls; // string: joint name - std::map sensors; // string: sensor name + std::map dxls_; // string: joint name + std::map sensors_; // string: sensor name - Robot(std::string robot_file_path, std::string dev_desc_dir_path); + Robot(std::string robot_file_path, std::string dev_desc_dir_path); - Sensor *getSensor(std::string path, int id, std::string port, float protocol_version); - Dynamixel *getDynamixel(std::string path, int id, std::string port, float protocol_version); + Sensor *getSensor(std::string path, int id, std::string port, float protocol_version); + Dynamixel *getDynamixel(std::string path, int id, std::string port, float protocol_version); }; } -#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_ROBOT_H_ */ +#endif /* ROBOTIS_DEVICE_ROBOT_H_ */ diff --git a/robotis_device/include/robotis_device/sensor.h b/robotis_device/include/robotis_device/sensor.h index 54df44f..2d1b757 100644 --- a/robotis_device/include/robotis_device/sensor.h +++ b/robotis_device/include/robotis_device/sensor.h @@ -1,32 +1,63 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + /* - * Sensor.h + * sensor.h * * Created on: 2015. 12. 16. * Author: zerom */ -#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSOR_H_ -#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSOR_H_ +#ifndef ROBOTIS_DEVICE_SENSOR_H_ +#define ROBOTIS_DEVICE_SENSOR_H_ #include #include #include -#include "Device.h" -#include "SensorState.h" -#include "ControlTableItem.h" -namespace ROBOTIS +#include "device.h" +#include "sensor_state.h" +#include "control_table_item.h" + +namespace robotis_framework { class Sensor : public Device { public: - SensorState *sensor_state; + SensorState *sensor_state_; - Sensor(int id, std::string model_name, float protocol_version); + Sensor(int id, std::string model_name, float protocol_version); }; } -#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSOR_H_ */ +#endif /* ROBOTIS_DEVICE_SENSOR_H_ */ diff --git a/robotis_device/include/robotis_device/sensor_state.h b/robotis_device/include/robotis_device/sensor_state.h index 4137b14..f40afa4 100644 --- a/robotis_device/include/robotis_device/sensor_state.h +++ b/robotis_device/include/robotis_device/sensor_state.h @@ -1,35 +1,64 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + /* - * SensorState.h + * sensor_state.h * * Created on: 2016. 5. 16. * Author: zerom */ -#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSORSTATE_H_ -#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSORSTATE_H_ +#ifndef ROBOTIS_DEVICE_SENSOR_STATE_H_ +#define ROBOTIS_DEVICE_SENSOR_STATE_H_ -#include "robotis_device/TimeStamp.h" -#include "robotis_framework_common/RobotisDef.h" +#include "time_stamp.h" -namespace ROBOTIS +namespace robotis_framework { class SensorState { public: - TimeStamp update_time_stamp; + TimeStamp update_time_stamp_; - std::map bulk_read_table; + std::map bulk_read_table_; - SensorState() - : update_time_stamp(0, 0) - { - bulk_read_table.clear(); - } + SensorState() + : update_time_stamp_(0, 0) + { + bulk_read_table_.clear(); + } }; } -#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSORSTATE_H_ */ +#endif /* ROBOTIS_DEVICE_SENSOR_STATE_H_ */ diff --git a/robotis_device/include/robotis_device/time_stamp.h b/robotis_device/include/robotis_device/time_stamp.h index 17b749a..b0de1e5 100644 --- a/robotis_device/include/robotis_device/time_stamp.h +++ b/robotis_device/include/robotis_device/time_stamp.h @@ -1,25 +1,59 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + /* - * TimeStamp.h + * time_stamp.h * * Created on: 2016. 5. 16. * Author: zerom */ -#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_TIMESTAMP_H_ -#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_TIMESTAMP_H_ +#ifndef ROBOTIS_DEVICE_TIME_STAMP_H_ +#define ROBOTIS_DEVICE_TIME_STAMP_H_ -namespace ROBOTIS +namespace robotis_framework { class TimeStamp { public: - long sec; - long nsec; - TimeStamp(long sec, long nsec) : sec(sec), nsec(nsec) { } + long sec_; + long nsec_; + + TimeStamp(long sec, long nsec) + : sec_(sec), + nsec_(nsec) + { } }; } -#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_TIMESTAMP_H_ */ +#endif /* ROBOTIS_DEVICE_TIME_STAMP_H_ */ diff --git a/robotis_device/package.xml b/robotis_device/package.xml index be72665..8e494db 100644 --- a/robotis_device/package.xml +++ b/robotis_device/package.xml @@ -6,16 +6,12 @@ robotis - BSD - - robotis - catkin roscpp @@ -26,9 +22,4 @@ roscpp rospy dynamixel_sdk - - - - - \ No newline at end of file diff --git a/robotis_device/src/robotis_device/dynamixel.cpp b/robotis_device/src/robotis_device/dynamixel.cpp index 55108ca..c90e4eb 100644 --- a/robotis_device/src/robotis_device/dynamixel.cpp +++ b/robotis_device/src/robotis_device/dynamixel.cpp @@ -1,111 +1,147 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + /* - * Dynamixel.cpp + * dynamixel.cpp * * Created on: 2015. 12. 8. * Author: zerom */ -#include "robotis_device/Dynamixel.h" +#include "robotis_device/dynamixel.h" -using namespace ROBOTIS; +using namespace robotis_framework; Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version) - : ctrl_module_name("none"), - current_ratio(1.0), - velocity_ratio(1.0), - value_of_0_radian_position(0), - value_of_min_radian_position(0), - value_of_max_radian_position(0), - min_radian(-3.14159265), - max_radian(3.14159265), - torque_enable_item(0), - present_position_item(0), - present_velocity_item(0), - present_current_item(0), - goal_position_item(0), - goal_velocity_item(0), - goal_current_item(0), - position_p_gain_item(0) + : ctrl_module_name_("none"), + current_ratio_(1.0), + velocity_ratio_(1.0), + value_of_0_radian_position_(0), + value_of_min_radian_position_(0), + value_of_max_radian_position_(0), + min_radian_(-3.14159265), + max_radian_(3.14159265), + torque_enable_item_(0), + present_position_item_(0), + present_velocity_item_(0), + present_current_item_(0), + goal_position_item_(0), + goal_velocity_item_(0), + goal_current_item_(0), + position_p_gain_item_(0) { - this->id = id; - this->model_name = model_name; - this->port_name = ""; - this->protocol_version = protocol_version; + this->id_ = id; + this->model_name_ = model_name; + this->port_name_ = ""; + this->protocol_version_ = protocol_version; - ctrl_table.clear(); - dxl_state = new DynamixelState(); + ctrl_table_.clear(); + dxl_state_ = new DynamixelState(); - bulk_read_items.clear(); + bulk_read_items_.clear(); } -double Dynamixel::ConvertValue2Radian(INT32_T value) +double Dynamixel::convertValue2Radian(int32_t value) { - double _radian = 0.0; - if(value > value_of_0_radian_position) - { - if(max_radian <= 0) - return max_radian; - _radian = (double)(value - value_of_0_radian_position) * max_radian / (double)(value_of_max_radian_position - value_of_0_radian_position); - } - else if(value < value_of_0_radian_position) - { - if(min_radian >= 0) - return min_radian; - _radian = (double)(value - value_of_0_radian_position) * min_radian / (double)(value_of_min_radian_position - value_of_0_radian_position); - } - - if(_radian > max_radian) - return max_radian; - else if(_radian < min_radian) - return min_radian; - - return _radian; + double radian = 0.0; + if (value > value_of_0_radian_position_) + { + if (max_radian_ <= 0) + return max_radian_; + + radian = (double) (value - value_of_0_radian_position_) * max_radian_ + / (double) (value_of_max_radian_position_ - value_of_0_radian_position_); + } + else if (value < value_of_0_radian_position_) + { + if (min_radian_ >= 0) + return min_radian_; + + radian = (double) (value - value_of_0_radian_position_) * min_radian_ + / (double) (value_of_min_radian_position_ - value_of_0_radian_position_); + } + + if (radian > max_radian_) + return max_radian_; + else if (radian < min_radian_) + return min_radian_; + + return radian; } -INT32_T Dynamixel::ConvertRadian2Value(double radian) +int32_t Dynamixel::convertRadian2Value(double radian) { - //return radian * value_of_max_radian_position / max_radian; - - INT32_T _value = 0; - if(radian > 0) - { - if(value_of_max_radian_position <= value_of_0_radian_position) - return value_of_max_radian_position; - _value = (radian * (value_of_max_radian_position - value_of_0_radian_position) / max_radian) + value_of_0_radian_position; - } - else if(radian < 0) - { - if(value_of_min_radian_position >= value_of_0_radian_position) - return value_of_min_radian_position; - _value = (radian * (value_of_min_radian_position - value_of_0_radian_position) / min_radian) + value_of_0_radian_position; - } - else - _value = value_of_0_radian_position; - - if(_value > value_of_max_radian_position) - return value_of_max_radian_position; - else if(_value < value_of_min_radian_position) - return value_of_min_radian_position; - - return _value; + int32_t value = 0; + if (radian > 0) + { + if (value_of_max_radian_position_ <= value_of_0_radian_position_) + return value_of_max_radian_position_; + + value = (radian * (value_of_max_radian_position_ - value_of_0_radian_position_) / max_radian_) + + value_of_0_radian_position_; + } + else if (radian < 0) + { + if (value_of_min_radian_position_ >= value_of_0_radian_position_) + return value_of_min_radian_position_; + + value = (radian * (value_of_min_radian_position_ - value_of_0_radian_position_) / min_radian_) + + value_of_0_radian_position_; + } + else + value = value_of_0_radian_position_; + + if (value > value_of_max_radian_position_) + return value_of_max_radian_position_; + else if (value < value_of_min_radian_position_) + return value_of_min_radian_position_; + + return value; } -double Dynamixel::ConvertValue2Velocity(INT32_T value) +double Dynamixel::convertValue2Velocity(int32_t value) { - return (double)value * velocity_ratio; + return (double) value * velocity_ratio_; } -INT32_T Dynamixel::ConvertVelocity2Value(double velocity) +int32_t Dynamixel::convertVelocity2Value(double velocity) { - return (INT32_T)(velocity / velocity_ratio);; + return (int32_t) (velocity / velocity_ratio_);; } -double Dynamixel::ConvertValue2Current(INT16_T value) +double Dynamixel::convertValue2Current(int16_t value) { - return (double)value * current_ratio; + return (double) value * current_ratio_; } -INT16_T Dynamixel::ConvertCurrent2Value(double torque) +int16_t Dynamixel::convertCurrent2Value(double current) { - return (INT16_T)(torque / current_ratio); + return (int16_t) (current / current_ratio_); } diff --git a/robotis_device/src/robotis_device/robot.cpp b/robotis_device/src/robotis_device/robot.cpp index ff4d86b..1ae3fb9 100644 --- a/robotis_device/src/robotis_device/robot.cpp +++ b/robotis_device/src/robotis_device/robot.cpp @@ -1,5 +1,35 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + /* - * Robot.cpp + * robot.cpp * * Created on: 2015. 12. 11. * Author: zerom @@ -8,404 +38,420 @@ #include #include #include -#include "../include/robotis_device/Robot.h" -using namespace ROBOTIS; +#include "robotis_device/robot.h" -static inline std::string <rim(std::string &s) { - s.erase(s.begin(), std::find_if(s.begin(), s.end(), std::not1(std::ptr_fun(std::isspace)))); - return s; +using namespace robotis_framework; + +static inline std::string <rim(std::string &s) +{ + s.erase(s.begin(), std::find_if(s.begin(), s.end(), std::not1(std::ptr_fun(std::isspace)))); + return s; } -static inline std::string &rtrim(std::string &s) { - s.erase(std::find_if(s.rbegin(), s.rend(), std::not1(std::ptr_fun(std::isspace))).base(), s.end()); - return s; +static inline std::string &rtrim(std::string &s) +{ + s.erase(std::find_if(s.rbegin(), s.rend(), std::not1(std::ptr_fun(std::isspace))).base(), s.end()); + return s; } -static inline std::string &trim(std::string &s) { - return ltrim(rtrim(s)); +static inline std::string &trim(std::string &s) +{ + return ltrim(rtrim(s)); } -static inline std::vector split(const std::string &text, char sep) { - std::vector tokens; - std::size_t start = 0, end = 0; - while((end = text.find(sep, start)) != (std::string::npos)) { - tokens.push_back(text.substr(start, end - start)); - trim(tokens.back()); - start = end + 1; - } - tokens.push_back(text.substr(start)); +static inline std::vector split(const std::string &text, char sep) +{ + std::vector tokens; + std::size_t start = 0, end = 0; + + while ((end = text.find(sep, start)) != (std::string::npos)) + { + tokens.push_back(text.substr(start, end - start)); trim(tokens.back()); - return tokens; + start = end + 1; + } + tokens.push_back(text.substr(start)); + trim(tokens.back()); + + return tokens; } Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) { - if(dev_desc_dir_path.compare(dev_desc_dir_path.size()-1, 1, "/") != 0) - dev_desc_dir_path += "/"; - - std::ifstream file(robot_file_path.c_str()); - if(file.is_open()) + if (dev_desc_dir_path.compare(dev_desc_dir_path.size() - 1, 1, "/") != 0) + dev_desc_dir_path += "/"; + + std::ifstream file(robot_file_path.c_str()); + if (file.is_open()) + { + std::string session = ""; + std::string input_str; + while (!file.eof()) { - std::string session = ""; - std::string input_str; - while(!file.eof()) + std::getline(file, input_str); + + // remove comment ( # ) + std::size_t pos = input_str.find("#"); + if (pos != std::string::npos) + input_str = input_str.substr(0, pos); + + // trim + input_str = trim(input_str); + + // find session + if (!input_str.compare(0, 1, "[") && !input_str.compare(input_str.size() - 1, 1, "]")) + { + input_str = input_str.substr(1, input_str.size() - 2); + std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower); + session = trim(input_str); + continue; + } + + if (session == SESSION_PORT_INFO) + { + std::vector tokens = split(input_str, '|'); + if (tokens.size() != 3) + continue; + + std::cout << tokens[0] << " added. (baudrate: " << tokens[1] << ")" << std::endl; + + ports_[tokens[0]] = dynamixel::PortHandler::getPortHandler(tokens[0].c_str()); + ports_[tokens[0]]->setBaudRate(std::atoi(tokens[1].c_str())); + port_default_device_[tokens[0]] = tokens[2]; + } + else if (session == SESSION_DEVICE_INFO) + { + std::vector tokens = split(input_str, '|'); + if (tokens.size() != 7) + continue; + + if (tokens[0] == DYNAMIXEL) { - std::getline(file, input_str); - - // remove comment ( # ) - std::size_t pos = input_str.find("#"); - if(pos != std::string::npos) - input_str = input_str.substr(0, pos); - - // trim - input_str = trim(input_str); - - // find session - if(!input_str.compare(0, 1, "[") && !input_str.compare(input_str.size()-1, 1, "]")) + std::string file_path = dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device"; + int id = std::atoi(tokens[2].c_str()); + std::string port = tokens[1]; + float protocol = std::atof(tokens[4].c_str()); + std::string dev_name = tokens[5]; + + dxls_[dev_name] = getDynamixel(file_path, id, port, protocol); + + Dynamixel *dxl = dxls_[dev_name]; + std::vector sub_tokens = split(tokens[6], ','); + if (sub_tokens.size() > 0) + { + std::map::iterator indirect_it = dxl->ctrl_table_.find(INDIRECT_ADDRESS_1); + if (indirect_it != dxl->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist { - input_str = input_str.substr(1, input_str.size()-2); - std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower); - session = trim(input_str); - continue; + uint16_t indirect_data_addr = dxl->ctrl_table_[INDIRECT_DATA_1]->address_; + for (int _i = 0; _i < sub_tokens.size(); _i++) + { + dxl->bulk_read_items_.push_back(new ControlTableItem()); + ControlTableItem *dest_item = dxl->bulk_read_items_[_i]; + ControlTableItem *src_item = dxl->ctrl_table_[sub_tokens[_i]]; + + dest_item->item_name_ = src_item->item_name_; + dest_item->address_ = indirect_data_addr; + dest_item->access_type_ = src_item->access_type_; + dest_item->memory_type_ = src_item->memory_type_; + dest_item->data_length_ = src_item->data_length_; + dest_item->data_min_value_ = src_item->data_min_value_; + dest_item->data_max_value_ = src_item->data_max_value_; + dest_item->is_signed_ = src_item->is_signed_; + + indirect_data_addr += dest_item->data_length_; + } } - - if(session == "port info") + else // INDIRECT_ADDRESS_1 not exist { - std::vector tokens = split(input_str, '|'); - if(tokens.size() != 3) - continue; - - std::cout << tokens[0] << " added. (baudrate: " << tokens[1] << ")" << std::endl; - - ports[tokens[0]] = (PortHandler*)PortHandler::GetPortHandler(tokens[0].c_str()); - ports[tokens[0]]->SetBaudRate(std::atoi(tokens[1].c_str())); - port_default_device[tokens[0]] = tokens[2]; + for (int i = 0; i < sub_tokens.size(); i++) + { + if (dxl->ctrl_table_[sub_tokens[i]] != NULL) + dxl->bulk_read_items_.push_back(dxl->ctrl_table_[sub_tokens[i]]); + } } - else if(session == "device info") + } + } + else if (tokens[0] == SENSOR) + { + std::string file_path = dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device"; + int id = std::atoi(tokens[2].c_str()); + std::string port = tokens[1]; + float protocol = std::atof(tokens[4].c_str()); + std::string dev_name = tokens[5]; + + sensors_[dev_name] = getSensor(file_path, id, port, protocol); + + Sensor *sensor = sensors_[dev_name]; + std::vector sub_tokens = split(tokens[6], ','); + if (sub_tokens.size() > 0) + { + std::map::iterator indirect_it = sensor->ctrl_table_.find(INDIRECT_ADDRESS_1); + if (indirect_it != sensor->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist { - std::vector tokens = split(input_str, '|'); - if(tokens.size() != 7) - continue; - - if(tokens[0] == "dynamixel") - { - std::string _file_path = dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device"; - int _id = std::atoi(tokens[2].c_str()); - std::string _port = tokens[1]; - float _protocol = std::atof(tokens[4].c_str()); - std::string _dev_name = tokens[5]; - - dxls[_dev_name] = getDynamixel(_file_path, _id, _port, _protocol); - - Dynamixel *_dxl = dxls[_dev_name]; - std::vector sub_tokens = split(tokens[6], ','); - if(sub_tokens.size() > 0) - { - std::map::iterator _indirect_it = _dxl->ctrl_table.find(INDIRECT_ADDRESS_1); - if(_indirect_it != _dxl->ctrl_table.end()) // INDIRECT_ADDRESS_1 exist - { - UINT16_T _indirect_data_addr = _dxl->ctrl_table[INDIRECT_DATA_1]->address; - for(int _i = 0; _i < sub_tokens.size(); _i++) - { - _dxl->bulk_read_items.push_back(new ControlTableItem()); - ControlTableItem *_dest_item = _dxl->bulk_read_items[_i]; - ControlTableItem *_src_item = _dxl->ctrl_table[sub_tokens[_i]]; - - _dest_item->item_name = _src_item->item_name; - _dest_item->address = _indirect_data_addr; - _dest_item->access_type = _src_item->access_type; - _dest_item->memory_type = _src_item->memory_type; - _dest_item->data_length = _src_item->data_length; - _dest_item->data_min_value = _src_item->data_min_value; - _dest_item->data_max_value = _src_item->data_max_value; - _dest_item->is_signed = _src_item->is_signed; - - _indirect_data_addr += _dest_item->data_length; - } - } - else // INDIRECT_ADDRESS_1 not exist - { - for(int _i = 0; _i < sub_tokens.size(); _i++) - { - if(_dxl->ctrl_table[sub_tokens[_i]] != NULL) - _dxl->bulk_read_items.push_back(_dxl->ctrl_table[sub_tokens[_i]]); - } - } - } - } - else if(tokens[0] == "sensor") - { - std::string _file_path = dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device"; - int _id = std::atoi(tokens[2].c_str()); - std::string _port = tokens[1]; - float _protocol = std::atof(tokens[4].c_str()); - std::string _dev_name = tokens[5]; - - sensors[_dev_name] = getSensor(_file_path, _id, _port, _protocol); - - Sensor *_sensor = sensors[_dev_name]; - std::vector sub_tokens = split(tokens[6], ','); - if(sub_tokens.size() > 0) - { - std::map::iterator _indirect_it = _sensor->ctrl_table.find(INDIRECT_ADDRESS_1); - if(_indirect_it != _sensor->ctrl_table.end()) // INDIRECT_ADDRESS_1 exist - { - UINT16_T _indirect_data_addr = _sensor->ctrl_table[INDIRECT_DATA_1]->address; - for(int _i = 0; _i < sub_tokens.size(); _i++) - { - _sensor->bulk_read_items.push_back(new ControlTableItem()); - ControlTableItem *_dest_item = _sensor->bulk_read_items[_i]; - ControlTableItem *_src_item = _sensor->ctrl_table[sub_tokens[_i]]; - - _dest_item->item_name = _src_item->item_name; - _dest_item->address = _indirect_data_addr; - _dest_item->access_type = _src_item->access_type; - _dest_item->memory_type = _src_item->memory_type; - _dest_item->data_length = _src_item->data_length; - _dest_item->data_min_value = _src_item->data_min_value; - _dest_item->data_max_value = _src_item->data_max_value; - _dest_item->is_signed = _src_item->is_signed; - - _indirect_data_addr += _dest_item->data_length; - } - } - else // INDIRECT_ADDRESS_1 exist - { - for(int _i = 0; _i < sub_tokens.size(); _i++) - _sensor->bulk_read_items.push_back(_sensor->ctrl_table[sub_tokens[_i]]); - } - } - } + uint16_t indirect_data_addr = sensor->ctrl_table_[INDIRECT_DATA_1]->address_; + for (int i = 0; i < sub_tokens.size(); i++) + { + sensor->bulk_read_items_.push_back(new ControlTableItem()); + ControlTableItem *dest_item = sensor->bulk_read_items_[i]; + ControlTableItem *src_item = sensor->ctrl_table_[sub_tokens[i]]; + + dest_item->item_name_ = src_item->item_name_; + dest_item->address_ = indirect_data_addr; + dest_item->access_type_ = src_item->access_type_; + dest_item->memory_type_ = src_item->memory_type_; + dest_item->data_length_ = src_item->data_length_; + dest_item->data_min_value_ = src_item->data_min_value_; + dest_item->data_max_value_ = src_item->data_max_value_; + dest_item->is_signed_ = src_item->is_signed_; + + indirect_data_addr += dest_item->data_length_; + } } + else // INDIRECT_ADDRESS_1 exist + { + for (int i = 0; i < sub_tokens.size(); i++) + sensor->bulk_read_items_.push_back(sensor->ctrl_table_[sub_tokens[i]]); + } + } } - file.close(); - } - else - { - std::cout << "Unable to open file : " + robot_file_path << std::endl; + } } + file.close(); + } + else + { + std::cout << "Unable to open file : " + robot_file_path << std::endl; + } } Sensor *Robot::getSensor(std::string path, int id, std::string port, float protocol_version) { - Sensor *_sensor; - - // load file model_name.device - std::ifstream file(path.c_str()); - if(file.is_open()) - { - std::string _session = ""; - std::string _input_str; - - while(!file.eof()) - { - std::getline(file, _input_str); + Sensor *sensor; - // remove comment ( # ) - std::size_t pos = _input_str.find("#"); - if(pos != std::string::npos) - _input_str = _input_str.substr(0, pos); + // load file model_name.device + std::ifstream file(path.c_str()); + if (file.is_open()) + { + std::string session = ""; + std::string input_str; - // trim - _input_str = trim(_input_str); - if(_input_str == "") - continue; - - // find _session - if(!_input_str.compare(0, 1, "[") && !_input_str.compare(_input_str.size()-1, 1, "]")) - { - _input_str = _input_str.substr(1, _input_str.size()-2); - std::transform(_input_str.begin(), _input_str.end(), _input_str.begin(), ::tolower); - _session = trim(_input_str); - continue; - } - - if(_session == "device info") - { - std::vector tokens = split(_input_str, '='); - if(tokens.size() != 2) - continue; - - if(tokens[0] == "model_name") - _sensor = new Sensor(id, tokens[1], protocol_version); - } - else if(_session == "control table") - { - std::vector tokens = split(_input_str, '|'); - if(tokens.size() != 8) - continue; - - ControlTableItem *item = new ControlTableItem(); - item->item_name = tokens[1]; - item->address = std::atoi(tokens[0].c_str()); - item->data_length = std::atoi(tokens[2].c_str()); - if(tokens[3] == "R") - item->access_type = READ; - else if(tokens[3] == "RW") - item->access_type = READ_WRITE; - if(tokens[4] == "EEPROM") - item->memory_type = EEPROM; - else if(tokens[4] == "RAM") - item->memory_type = RAM; - item->data_min_value = std::atol(tokens[5].c_str()); - item->data_max_value = std::atol(tokens[6].c_str()); - if(tokens[7] == "Y") - item->is_signed = true; - else if(tokens[7] == "N") - item->is_signed = false; - _sensor->ctrl_table[tokens[1]] = item; - } - } - _sensor->port_name = port; - - fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), _sensor->id, _sensor->model_name.c_str()); - //std::cout << "[ID:" << (int)(_sensor->id) << "] " << _sensor->model_name << " added. (" << port << ")" << std::endl; - file.close(); + while (!file.eof()) + { + std::getline(file, input_str); + + // remove comment ( # ) + std::size_t pos = input_str.find("#"); + if (pos != std::string::npos) + input_str = input_str.substr(0, pos); + + // trim + input_str = trim(input_str); + if (input_str == "") + continue; + + // find _session + if (!input_str.compare(0, 1, "[") && !input_str.compare(input_str.size() - 1, 1, "]")) + { + input_str = input_str.substr(1, input_str.size() - 2); + std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower); + session = trim(input_str); + continue; + } + + if (session == SESSION_DEVICE_INFO) + { + std::vector tokens = split(input_str, '='); + if (tokens.size() != 2) + continue; + + if (tokens[0] == "model_name") + sensor = new Sensor(id, tokens[1], protocol_version); + } + else if (session == SESSION_CONTROL_TABLE) + { + std::vector tokens = split(input_str, '|'); + if (tokens.size() != 8) + continue; + + ControlTableItem *item = new ControlTableItem(); + item->item_name_ = tokens[1]; + item->address_ = std::atoi(tokens[0].c_str()); + item->data_length_ = std::atoi(tokens[2].c_str()); + + if (tokens[3] == "R") + item->access_type_ = Read; + else if (tokens[3] == "RW") + item->access_type_ = ReadWrite; + + if (tokens[4] == "EEPROM") + item->memory_type_ = EEPROM; + else if (tokens[4] == "RAM") + item->memory_type_ = RAM; + + item->data_min_value_ = std::atol(tokens[5].c_str()); + item->data_max_value_ = std::atol(tokens[6].c_str()); + + if (tokens[7] == "Y") + item->is_signed_ = true; + else if (tokens[7] == "N") + item->is_signed_ = false; + sensor->ctrl_table_[tokens[1]] = item; + } } - else - std::cout << "Unable to open file : " + path << std::endl; + sensor->port_name_ = port; - return _sensor; + fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), sensor->id_, sensor->model_name_.c_str()); + //std::cout << "[ID:" << (int)(_sensor->id) << "] " << _sensor->model_name << " added. (" << port << ")" << std::endl; + file.close(); + } + else + std::cout << "Unable to open file : " + path << std::endl; + + return sensor; } Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float protocol_version) { - Dynamixel *_dxl; - - // load file model_name.device - std::ifstream file(path.c_str()); - if(file.is_open()) + Dynamixel *dxl; + + // load file model_name.device + std::ifstream file(path.c_str()); + if (file.is_open()) + { + std::string session = ""; + std::string input_str; + + std::string torque_enable_item_name = ""; + std::string present_position_item_name = ""; + std::string present_velocity_item_name = ""; + std::string present_current_item_name = ""; + std::string goal_position_item_name = ""; + std::string goal_velocity_item_name = ""; + std::string goal_current_item_name = ""; + + while (!file.eof()) { - std::string _session = ""; - std::string _input_str; - - std::string _torque_enable_item_name = ""; - std::string _present_position_item_name = ""; - std::string _present_velocity_item_name = ""; - std::string _present_current_item_name = ""; - std::string _goal_position_item_name = ""; - std::string _goal_velocity_item_name = ""; - std::string _goal_current_item_name = ""; - - while(!file.eof()) - { - std::getline(file, _input_str); - - // remove comment ( # ) - std::size_t pos = _input_str.find("#"); - if(pos != std::string::npos) - _input_str = _input_str.substr(0, pos); - - // trim - _input_str = trim(_input_str); - if(_input_str == "") - continue; - - // find _session - if(!_input_str.compare(0, 1, "[") && !_input_str.compare(_input_str.size()-1, 1, "]")) - { - _input_str = _input_str.substr(1, _input_str.size()-2); - std::transform(_input_str.begin(), _input_str.end(), _input_str.begin(), ::tolower); - _session = trim(_input_str); - continue; - } - - if(_session == "device info") - { - std::vector tokens = split(_input_str, '='); - if(tokens.size() != 2) - continue; - - if(tokens[0] == "model_name") - _dxl = new Dynamixel(id, tokens[1], protocol_version); - } - else if(_session == "type info") - { - std::vector tokens = split(_input_str, '='); - if(tokens.size() != 2) - continue; - - if(tokens[0] == "current_ratio") - _dxl->current_ratio = std::atof(tokens[1].c_str()); - else if(tokens[0] == "velocity_ratio") - _dxl->velocity_ratio = std::atof(tokens[1].c_str()); - - else if(tokens[0] == "value_of_0_radian_position") - _dxl->value_of_0_radian_position = std::atoi(tokens[1].c_str()); - else if(tokens[0] == "value_of_min_radian_position") - _dxl->value_of_min_radian_position = std::atoi(tokens[1].c_str()); - else if(tokens[0] == "value_of_max_radian_position") - _dxl->value_of_max_radian_position = std::atoi(tokens[1].c_str()); - else if(tokens[0] == "min_radian") - _dxl->min_radian = std::atof(tokens[1].c_str()); - else if(tokens[0] == "max_radian") - _dxl->max_radian = std::atof(tokens[1].c_str()); - - else if(tokens[0] == "torque_enable_item_name") - _torque_enable_item_name = tokens[1]; - else if(tokens[0] == "present_position_item_name") - _present_position_item_name = tokens[1]; - else if(tokens[0] == "present_velocity_item_name") - _present_velocity_item_name = tokens[1]; - else if(tokens[0] == "present_current_item_name") - _present_current_item_name = tokens[1]; - else if(tokens[0] == "goal_position_item_name") - _goal_position_item_name = tokens[1]; - else if(tokens[0] == "goal_velocity_item_name") - _goal_velocity_item_name = tokens[1]; - else if(tokens[0] == "goal_current_item_name") - _goal_current_item_name = tokens[1]; - } - else if(_session == "control table") - { - std::vector tokens = split(_input_str, '|'); - if(tokens.size() != 8) - continue; - - ControlTableItem *item = new ControlTableItem(); - item->item_name = tokens[1]; - item->address = std::atoi(tokens[0].c_str()); - item->data_length = std::atoi(tokens[2].c_str()); - if(tokens[3] == "R") - item->access_type = READ; - else if(tokens[3] == "RW") - item->access_type = READ_WRITE; - if(tokens[4] == "EEPROM") - item->memory_type = EEPROM; - else if(tokens[4] == "RAM") - item->memory_type = RAM; - item->data_min_value = std::atol(tokens[5].c_str()); - item->data_max_value = std::atol(tokens[6].c_str()); - if(tokens[7] == "Y") - item->is_signed = true; - else if(tokens[7] == "N") - item->is_signed = false; - _dxl->ctrl_table[tokens[1]] = item; - } - } - _dxl->port_name = port; - - if(_dxl->ctrl_table[_torque_enable_item_name] != NULL) - _dxl->torque_enable_item = _dxl->ctrl_table[_torque_enable_item_name]; - if(_dxl->ctrl_table[_present_position_item_name] != NULL) - _dxl->present_position_item = _dxl->ctrl_table[_present_position_item_name]; - if(_dxl->ctrl_table[_present_velocity_item_name] != NULL) - _dxl->present_velocity_item = _dxl->ctrl_table[_present_velocity_item_name]; - if(_dxl->ctrl_table[_present_current_item_name] != NULL) - _dxl->present_current_item = _dxl->ctrl_table[_present_current_item_name]; - if(_dxl->ctrl_table[_goal_position_item_name] != NULL) - _dxl->goal_position_item = _dxl->ctrl_table[_goal_position_item_name]; - if(_dxl->ctrl_table[_goal_velocity_item_name] != NULL) - _dxl->goal_velocity_item = _dxl->ctrl_table[_goal_velocity_item_name]; - if(_dxl->ctrl_table[_goal_current_item_name] != NULL) - _dxl->goal_current_item = _dxl->ctrl_table[_goal_current_item_name]; - - fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), _dxl->id, _dxl->model_name.c_str()); - //std::cout << "[ID:" << (int)(_dxl->id) << "] " << _dxl->model_name << " added. (" << port << ")" << std::endl; - file.close(); + std::getline(file, input_str); + + // remove comment ( # ) + std::size_t pos = input_str.find("#"); + if (pos != std::string::npos) + input_str = input_str.substr(0, pos); + + // trim + input_str = trim(input_str); + if (input_str == "") + continue; + + // find session + if (!input_str.compare(0, 1, "[") && !input_str.compare(input_str.size() - 1, 1, "]")) + { + input_str = input_str.substr(1, input_str.size() - 2); + std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower); + session = trim(input_str); + continue; + } + + if (session == SESSION_DEVICE_INFO) + { + std::vector tokens = split(input_str, '='); + if (tokens.size() != 2) + continue; + + if (tokens[0] == "model_name") + dxl = new Dynamixel(id, tokens[1], protocol_version); + } + else if (session == SESSION_TYPE_INFO) + { + std::vector tokens = split(input_str, '='); + if (tokens.size() != 2) + continue; + + if (tokens[0] == "current_ratio") + dxl->current_ratio_ = std::atof(tokens[1].c_str()); + else if (tokens[0] == "velocity_ratio") + dxl->velocity_ratio_ = std::atof(tokens[1].c_str()); + + else if (tokens[0] == "value_of_0_radian_position") + dxl->value_of_0_radian_position_ = std::atoi(tokens[1].c_str()); + else if (tokens[0] == "value_of_min_radian_position") + dxl->value_of_min_radian_position_ = std::atoi(tokens[1].c_str()); + else if (tokens[0] == "value_of_max_radian_position") + dxl->value_of_max_radian_position_ = std::atoi(tokens[1].c_str()); + else if (tokens[0] == "min_radian") + dxl->min_radian_ = std::atof(tokens[1].c_str()); + else if (tokens[0] == "max_radian") + dxl->max_radian_ = std::atof(tokens[1].c_str()); + + else if (tokens[0] == "torque_enable_item_name") + torque_enable_item_name = tokens[1]; + else if (tokens[0] == "present_position_item_name") + present_position_item_name = tokens[1]; + else if (tokens[0] == "present_velocity_item_name") + present_velocity_item_name = tokens[1]; + else if (tokens[0] == "present_current_item_name") + present_current_item_name = tokens[1]; + else if (tokens[0] == "goal_position_item_name") + goal_position_item_name = tokens[1]; + else if (tokens[0] == "goal_velocity_item_name") + goal_velocity_item_name = tokens[1]; + else if (tokens[0] == "goal_current_item_name") + goal_current_item_name = tokens[1]; + } + else if (session == SESSION_CONTROL_TABLE) + { + std::vector tokens = split(input_str, '|'); + if (tokens.size() != 8) + continue; + + ControlTableItem *item = new ControlTableItem(); + item->item_name_ = tokens[1]; + item->address_ = std::atoi(tokens[0].c_str()); + item->data_length_ = std::atoi(tokens[2].c_str()); + + if (tokens[3] == "R") + item->access_type_ = Read; + else if (tokens[3] == "RW") + item->access_type_ = ReadWrite; + + if (tokens[4] == "EEPROM") + item->memory_type_ = EEPROM; + else if (tokens[4] == "RAM") + item->memory_type_ = RAM; + + item->data_min_value_ = std::atol(tokens[5].c_str()); + item->data_max_value_ = std::atol(tokens[6].c_str()); + + if (tokens[7] == "Y") + item->is_signed_ = true; + else if (tokens[7] == "N") + item->is_signed_ = false; + dxl->ctrl_table_[tokens[1]] = item; + } } - else - std::cout << "Unable to open file : " + path << std::endl; - - return _dxl; + dxl->port_name_ = port; + + if (dxl->ctrl_table_[torque_enable_item_name] != NULL) + dxl->torque_enable_item_ = dxl->ctrl_table_[torque_enable_item_name]; + if (dxl->ctrl_table_[present_position_item_name] != NULL) + dxl->present_position_item_ = dxl->ctrl_table_[present_position_item_name]; + if (dxl->ctrl_table_[present_velocity_item_name] != NULL) + dxl->present_velocity_item_ = dxl->ctrl_table_[present_velocity_item_name]; + if (dxl->ctrl_table_[present_current_item_name] != NULL) + dxl->present_current_item_ = dxl->ctrl_table_[present_current_item_name]; + if (dxl->ctrl_table_[goal_position_item_name] != NULL) + dxl->goal_position_item_ = dxl->ctrl_table_[goal_position_item_name]; + if (dxl->ctrl_table_[goal_velocity_item_name] != NULL) + dxl->goal_velocity_item_ = dxl->ctrl_table_[goal_velocity_item_name]; + if (dxl->ctrl_table_[goal_current_item_name] != NULL) + dxl->goal_current_item_ = dxl->ctrl_table_[goal_current_item_name]; + + fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), dxl->id_, dxl->model_name_.c_str()); + //std::cout << "[ID:" << (int)(_dxl->id) << "] " << _dxl->model_name << " added. (" << port << ")" << std::endl; + file.close(); + } + else + std::cout << "Unable to open file : " + path << std::endl; + + return dxl; } diff --git a/robotis_device/src/robotis_device/sensor.cpp b/robotis_device/src/robotis_device/sensor.cpp index db95494..537989d 100644 --- a/robotis_device/src/robotis_device/sensor.cpp +++ b/robotis_device/src/robotis_device/sensor.cpp @@ -1,23 +1,53 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + /* - * Sensor.cpp + * sensor.cpp * * Created on: 2016. 5. 11. * Author: zerom */ -#include "robotis_device/Sensor.h" +#include "robotis_device/sensor.h" -using namespace ROBOTIS; +using namespace robotis_framework; Sensor::Sensor(int id, std::string model_name, float protocol_version) { - this->id = id; - this->model_name = model_name; - this->port_name = ""; - this->protocol_version = protocol_version; - ctrl_table.clear(); + this->id_ = id; + this->model_name_ = model_name; + this->port_name_ = ""; + this->protocol_version_ = protocol_version; + ctrl_table_.clear(); - sensor_state = new SensorState(); + sensor_state_ = new SensorState(); - bulk_read_items.clear(); + bulk_read_items_.clear(); } diff --git a/robotis_framework_common/CMakeLists.txt b/robotis_framework_common/CMakeLists.txt index 17cd2cc..51f0f3f 100644 --- a/robotis_framework_common/CMakeLists.txt +++ b/robotis_framework_common/CMakeLists.txt @@ -5,15 +5,6 @@ find_package(catkin REQUIRED COMPONENTS roscpp ) -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( INCLUDE_DIRS include # LIBRARIES robotis_framework_common @@ -21,36 +12,7 @@ catkin_package( # DEPENDS system_lib ) -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) include_directories( ${catkin_INCLUDE_DIRS} ) -## Declare a C++ library -# add_library(robotis_framework_common -# src/${PROJECT_NAME}/robotis_framework_common.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(robotis_framework_common ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -# add_executable(robotis_framework_common_node src/robotis_framework_common_node.cpp) - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(robotis_framework_common_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(robotis_framework_common_node -# ${catkin_LIBRARIES} -# ) - diff --git a/robotis_framework_common/include/robotis_framework_common/motion_module.h b/robotis_framework_common/include/robotis_framework_common/motion_module.h index 0d0fb0f..c602f34 100644 --- a/robotis_framework_common/include/robotis_framework_common/motion_module.h +++ b/robotis_framework_common/include/robotis_framework_common/motion_module.h @@ -1,71 +1,101 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + /* - * MotionModule.h + * motion_module.h * * Created on: 2016. 1. 15. * Author: zerom */ -#ifndef ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_MOTIONMODULE_H_ -#define ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_MOTIONMODULE_H_ +#ifndef ROBOTIS_FRAMEWORK_COMMON_MOTION_MODULE_H_ +#define ROBOTIS_FRAMEWORK_COMMON_MOTION_MODULE_H_ #include #include -#include "robotis_device/Robot.h" -#include "robotis_device/Dynamixel.h" -#include "robotis_framework_common/Singleton.h" +#include "singleton.h" +#include "robotis_device/robot.h" +#include "robotis_device/dynamixel.h" -namespace ROBOTIS +namespace robotis_framework { -enum CONTROL_MODE +enum ControlMode { - POSITION_CONTROL, - VELOCITY_CONTROL, - CURRENT_CONTROL + PositionControl, + VelocityControl, + CurrentControl }; class MotionModule { protected: - bool enable; - std::string module_name; - CONTROL_MODE control_mode; + bool enable_; + std::string module_name_; + ControlMode control_mode_; public: - std::map result; - - virtual ~MotionModule() { } - - std::string GetModuleName() { return module_name; } - CONTROL_MODE GetControlMode() { return control_mode; } - - void SetModuleEnable(bool enable) - { - if(this->enable == enable) - return; - - this->enable = enable; - if(enable) - OnModuleEnable(); - else - OnModuleDisable(); - } - bool GetModuleEnable() { return enable; } - - virtual void OnModuleEnable() { } - virtual void OnModuleDisable() { } - - virtual void Initialize(const int control_cycle_msec, Robot *robot) = 0; - virtual void Process(std::map dxls, std::map sensors) = 0; - - virtual void Stop() = 0; - virtual bool IsRunning() = 0; + std::map result_; + + virtual ~MotionModule() { } + + std::string getModuleName() { return module_name_; } + ControlMode getControlMode() { return control_mode_; } + + void setModuleEnable(bool enable) + { + if(this->enable_ == enable) + return; + + this->enable_ = enable; + if(enable) + onModuleEnable(); + else + onModuleDisable(); + } + bool getModuleEnable() { return enable_; } + + virtual void onModuleEnable() { } + virtual void onModuleDisable() { } + + virtual void initialize(const int control_cycle_msec, Robot *robot) = 0; + virtual void process(std::map dxls, std::map sensors) = 0; + + virtual void stop() = 0; + virtual bool isRunning() = 0; }; } -#endif /* ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_MOTIONMODULE_H_ */ +#endif /* ROBOTIS_FRAMEWORK_COMMON_MOTION_MODULE_H_ */ diff --git a/robotis_framework_common/include/robotis_framework_common/sensor_module.h b/robotis_framework_common/include/robotis_framework_common/sensor_module.h index 1c604e9..81699e2 100644 --- a/robotis_framework_common/include/robotis_framework_common/sensor_module.h +++ b/robotis_framework_common/include/robotis_framework_common/sensor_module.h @@ -1,41 +1,71 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + /* - * SensorModule.h + * sensor_module.h * * Created on: 2016. 3. 30. * Author: zerom */ -#ifndef ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_SENSORMODULE_H_ -#define ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_SENSORMODULE_H_ +#ifndef ROBOTIS_FRAMEWORK_COMMON_SENSOR_MODULE_H_ +#define ROBOTIS_FRAMEWORK_COMMON_SENSOR_MODULE_H_ #include #include -#include "robotis_device/Robot.h" -#include "robotis_device/Dynamixel.h" -#include "robotis_framework_common/Singleton.h" +#include "singleton.h" +#include "robotis_device/robot.h" +#include "robotis_device/dynamixel.h" -namespace ROBOTIS +namespace robotis_framework { class SensorModule { protected: - std::string module_name; + std::string module_name_; public: - std::map result; + std::map result_; - virtual ~SensorModule() { } + virtual ~SensorModule() { } - std::string GetModuleName() { return module_name; } + std::string getModuleName() { return module_name_; } - virtual void Initialize(const int control_cycle_msec, Robot *robot) = 0; - virtual void Process(std::map dxls, std::map sensors) = 0; + virtual void initialize(const int control_cycle_msec, Robot *robot) = 0; + virtual void process(std::map dxls, std::map sensors) = 0; }; } -#endif /* ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_SENSORMODULE_H_ */ +#endif /* ROBOTIS_FRAMEWORK_COMMON_SENSOR_MODULE_H_ */ diff --git a/robotis_framework_common/include/robotis_framework_common/singleton.h b/robotis_framework_common/include/robotis_framework_common/singleton.h index 2b4c7d8..837b80b 100644 --- a/robotis_framework_common/include/robotis_framework_common/singleton.h +++ b/robotis_framework_common/include/robotis_framework_common/singleton.h @@ -1,44 +1,74 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + /* - * Singleton.h + * singleton.h * * Created on: 2016. 5. 17. * Author: zerom */ -#ifndef ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ -#define ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ +#ifndef ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ +#define ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ -namespace ROBOTIS +namespace robotis_framework { template class Singleton { private: - static T *unique_instance_; + static T *unique_instance_; protected: - Singleton() { } - Singleton(Singleton const&) { } - Singleton& operator=(Singleton const&) { return *this; } + Singleton() { } + Singleton(Singleton const&) { } + Singleton& operator=(Singleton const&) { return *this; } public: - static T* GetInstance() - { - if(unique_instance_ == NULL) - unique_instance_ = new T; - return unique_instance_; - } + static T* getInstance() + { + if(unique_instance_ == NULL) + unique_instance_ = new T; + return unique_instance_; + } - static void DestroyInstance() + static void destroyInstance() + { + if(unique_instance_) { - if(unique_instance_) - { - delete unique_instance_; - unique_instance_ = NULL; - } + delete unique_instance_; + unique_instance_ = NULL; } + } }; template T* Singleton::unique_instance_ = NULL; @@ -46,4 +76,4 @@ template T* Singleton::unique_instance_ = NULL; } -#endif /* ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ */ +#endif /* ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ */ diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml index feec7f8..820f4a2 100644 --- a/robotis_framework_common/package.xml +++ b/robotis_framework_common/package.xml @@ -1,52 +1,15 @@ robotis_framework_common - 0.0.0 + 0.1.0 The robotis_framework_common package + robotis - - - - robotis - - - - - BSD - - - - - - - - - - - - - - - - - - - - - - - - + ROBOTIS + catkin roscpp roscpp - - - - - - - \ No newline at end of file diff --git a/robotis_math/CMakeLists.txt b/robotis_math/CMakeLists.txt index 147809f..4d6421d 100644 --- a/robotis_math/CMakeLists.txt +++ b/robotis_math/CMakeLists.txt @@ -42,9 +42,9 @@ include_directories( ## Declare a C++ library add_library(robotis_math - src/RobotisMathBase.cpp - src/RobotisTrajectoryCalculator.cpp - src/RobotisLinearAlgebra.cpp + src/robotis_math_base.cpp + src/robotis_trajectory_calculator.cpp + src/robotis_linear_algebra.cpp ) ## Add cmake target dependencies of the library diff --git a/robotis_math/include/robotis_math/robotis_linear_algebra.h b/robotis_math/include/robotis_math/robotis_linear_algebra.h index 6188c50..1b5032a 100644 --- a/robotis_math/include/robotis_math/robotis_linear_algebra.h +++ b/robotis_math/include/robotis_math/robotis_linear_algebra.h @@ -15,7 +15,7 @@ #include -namespace ROBOTIS +namespace robotis_framework { Eigen::MatrixXd transitionXYZ ( double position_x, double position_y, double position_z ); diff --git a/robotis_math/include/robotis_math/robotis_math.h b/robotis_math/include/robotis_math/robotis_math.h index 752038d..f2afce1 100644 --- a/robotis_math/include/robotis_math/robotis_math.h +++ b/robotis_math/include/robotis_math/robotis_math.h @@ -8,6 +8,6 @@ #ifndef ROBOTIS_MATH_H_ #define ROBOTIS_MATH_H_ -#include "RobotisTrajectoryCalculator.h" +#include "robotis_trajectory_calculator.h" #endif /* ROBOTIS_MATH_H_ */ diff --git a/robotis_math/include/robotis_math/robotis_math_base.h b/robotis_math/include/robotis_math/robotis_math_base.h index 7b7c713..059c4ef 100644 --- a/robotis_math/include/robotis_math/robotis_math_base.h +++ b/robotis_math/include/robotis_math/robotis_math_base.h @@ -10,7 +10,7 @@ #include -namespace ROBOTIS +namespace robotis_framework { #define PRINT_VAR(X) std::cout << #X << " : " << X << std::endl diff --git a/robotis_math/include/robotis_math/robotis_trajectory_calculator.h b/robotis_math/include/robotis_math/robotis_trajectory_calculator.h index 3d1ff70..fcadbeb 100644 --- a/robotis_math/include/robotis_math/robotis_trajectory_calculator.h +++ b/robotis_math/include/robotis_math/robotis_trajectory_calculator.h @@ -9,12 +9,12 @@ #define ROBOTIS_TRAJECTORY_CALCULATOR_H_ -#include "RobotisMathBase.h" -#include "RobotisLinearAlgebra.h" +#include "robotis_linear_algebra.h" +#include "robotis_math_base.h" // minimum jerk trajectory -namespace ROBOTIS +namespace robotis_framework { Eigen::MatrixXd minimum_jerk_tra( double pos_start , double vel_start , double accel_start, diff --git a/robotis_math/src/robotis_linear_algebra.cpp b/robotis_math/src/robotis_linear_algebra.cpp index 30bb5ce..97f9f92 100644 --- a/robotis_math/src/robotis_linear_algebra.cpp +++ b/robotis_math/src/robotis_linear_algebra.cpp @@ -6,9 +6,9 @@ */ -#include "robotis_math/RobotisLinearAlgebra.h" +#include "../include/robotis_math/robotis_linear_algebra.h" -namespace ROBOTIS +namespace robotis_framework { Eigen::MatrixXd transitionXYZ ( double position_x, double position_y, double position_z ) diff --git a/robotis_math/src/robotis_math_base.cpp b/robotis_math/src/robotis_math_base.cpp index 0afb845..2b9610b 100644 --- a/robotis_math/src/robotis_math_base.cpp +++ b/robotis_math/src/robotis_math_base.cpp @@ -5,12 +5,12 @@ * Author: jay */ -#include "robotis_math/RobotisMathBase.h" +#include "../include/robotis_math/robotis_math_base.h" -namespace ROBOTIS +namespace robotis_framework { double sign( double x ) diff --git a/robotis_math/src/robotis_trajectory_calculator.cpp b/robotis_math/src/robotis_trajectory_calculator.cpp index 665015b..4d91a2c 100644 --- a/robotis_math/src/robotis_trajectory_calculator.cpp +++ b/robotis_math/src/robotis_trajectory_calculator.cpp @@ -7,7 +7,7 @@ -#include "robotis_math/RobotisTrajectoryCalculator.h" +#include "../include/robotis_math/robotis_trajectory_calculator.h" /* @@ -19,7 +19,7 @@ -namespace ROBOTIS +namespace robotis_framework { Eigen::MatrixXd minimum_jerk_tra( double pos_start , double vel_start , double accel_start, From 7403b1aaf9015788ad0964ca3e562a0373021008 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Fri, 3 Jun 2016 20:44:59 +0900 Subject: [PATCH 09/33] ROS C++ Coding Style is applied to robotis_math. --- .../robotis_math/robotis_linear_algebra.h | 32 ++++++++++++++++- .../include/robotis_math/robotis_math.h | 30 ++++++++++++++++ .../include/robotis_math/robotis_math_base.h | 32 ++++++++++++++++- .../robotis_trajectory_calculator.h | 32 ++++++++++++++++- robotis_math/src/robotis_linear_algebra.cpp | 34 ++++++++++++++++-- robotis_math/src/robotis_math_base.cpp | 36 ++++++++++++++++--- 6 files changed, 187 insertions(+), 9 deletions(-) diff --git a/robotis_math/include/robotis_math/robotis_linear_algebra.h b/robotis_math/include/robotis_math/robotis_linear_algebra.h index 1b5032a..9fecb82 100644 --- a/robotis_math/include/robotis_math/robotis_linear_algebra.h +++ b/robotis_math/include/robotis_math/robotis_linear_algebra.h @@ -1,5 +1,35 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + /* - * RobotisLinearAlgebra.h + * robotis_linear_algebra.h * * Created on: Mar 18, 2016 * Author: jay diff --git a/robotis_math/include/robotis_math/robotis_math.h b/robotis_math/include/robotis_math/robotis_math.h index f2afce1..61c33e5 100644 --- a/robotis_math/include/robotis_math/robotis_math.h +++ b/robotis_math/include/robotis_math/robotis_math.h @@ -1,3 +1,33 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + /* * robotis_math.h * diff --git a/robotis_math/include/robotis_math/robotis_math_base.h b/robotis_math/include/robotis_math/robotis_math_base.h index 059c4ef..436b0e4 100644 --- a/robotis_math/include/robotis_math/robotis_math_base.h +++ b/robotis_math/include/robotis_math/robotis_math_base.h @@ -1,5 +1,35 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + /* - * RobotisMathBase.h + * robotis_math_base.h * * Created on: 2016. 3. 28. * Author: JaySong diff --git a/robotis_math/include/robotis_math/robotis_trajectory_calculator.h b/robotis_math/include/robotis_math/robotis_trajectory_calculator.h index fcadbeb..a1c3807 100644 --- a/robotis_math/include/robotis_math/robotis_trajectory_calculator.h +++ b/robotis_math/include/robotis_math/robotis_trajectory_calculator.h @@ -1,5 +1,35 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + /* - * RobotisTrajectoryCalculator.h + * robotis_trajectory_calculator.h * * Created on: Mar 18, 2016 * Author: jay diff --git a/robotis_math/src/robotis_linear_algebra.cpp b/robotis_math/src/robotis_linear_algebra.cpp index 97f9f92..569a695 100644 --- a/robotis_math/src/robotis_linear_algebra.cpp +++ b/robotis_math/src/robotis_linear_algebra.cpp @@ -1,12 +1,42 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + /* - * RobotisLinearAlgebra.cpp + * robotis_linear_algebra.cpp * * Created on: Mar 18, 2016 * Author: jay */ -#include "../include/robotis_math/robotis_linear_algebra.h" +#include "robotis_math/robotis_linear_algebra.h" namespace robotis_framework { diff --git a/robotis_math/src/robotis_math_base.cpp b/robotis_math/src/robotis_math_base.cpp index 2b9610b..b5c43d3 100644 --- a/robotis_math/src/robotis_math_base.cpp +++ b/robotis_math/src/robotis_math_base.cpp @@ -1,13 +1,41 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + /* - * RobotisMathBase.cpp + * robotis_math_base.cpp * * Created on: Mar 18, 2016 * Author: jay */ -#include "../include/robotis_math/robotis_math_base.h" - - +#include "robotis_math/robotis_math_base.h" namespace robotis_framework From 3e8683bbf576a25d55e9ea21c6e996c9324f1fa9 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Fri, 3 Jun 2016 20:50:22 +0900 Subject: [PATCH 10/33] ROS C++ Coding Style is applied --- robotis_math/include/robotis_math/robotis_linear_algebra.h | 4 ++-- robotis_math/include/robotis_math/robotis_math_base.h | 4 ++-- robotis_math/src/robotis_linear_algebra.cpp | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/robotis_math/include/robotis_math/robotis_linear_algebra.h b/robotis_math/include/robotis_math/robotis_linear_algebra.h index 9fecb82..84b3e11 100644 --- a/robotis_math/include/robotis_math/robotis_linear_algebra.h +++ b/robotis_math/include/robotis_math/robotis_linear_algebra.h @@ -50,7 +50,7 @@ namespace robotis_framework Eigen::MatrixXd transitionXYZ ( double position_x, double position_y, double position_z ); Eigen::MatrixXd transformationXYZRPY ( double position_x, double position_y, double position_z , double roll , double pitch , double yaw ); -Eigen::MatrixXd InverseTransformation(Eigen::MatrixXd transform); +Eigen::MatrixXd inverseTransformation(Eigen::MatrixXd transform); Eigen::MatrixXd inertiaXYZ( double ixx, double ixy, double ixz , double iyy , double iyz, double izz ); @@ -70,7 +70,7 @@ Eigen::MatrixXd quaternion2rotation( Eigen::Quaterniond quaternion ); Eigen::MatrixXd rotation4d( double roll, double pitch, double yaw ); Eigen::MatrixXd hatto( Eigen::MatrixXd matrix3d ); -Eigen::MatrixXd Rodrigues( Eigen::MatrixXd hat_matrix , double angle ); +Eigen::MatrixXd rodrigues( Eigen::MatrixXd hat_matrix , double angle ); Eigen::MatrixXd rot2omega(Eigen::MatrixXd rotation ); Eigen::MatrixXd cross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b ); double dot(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b ); diff --git a/robotis_math/include/robotis_math/robotis_math_base.h b/robotis_math/include/robotis_math/robotis_math_base.h index 436b0e4..8088051 100644 --- a/robotis_math/include/robotis_math/robotis_math_base.h +++ b/robotis_math/include/robotis_math/robotis_math_base.h @@ -46,8 +46,8 @@ namespace robotis_framework #define PRINT_VAR(X) std::cout << #X << " : " << X << std::endl #define PRINT_MAT(X) std::cout << #X << ":\n" << X << std::endl << std::endl -#define deg2rad (M_PI / 180.0) -#define rad2deg (180.0 / M_PI) +#define DEGREE2RADIAN (M_PI / 180.0) +#define RADIAN2DEGREE (180.0 / M_PI) inline double powDI(double a, int b) { diff --git a/robotis_math/src/robotis_linear_algebra.cpp b/robotis_math/src/robotis_linear_algebra.cpp index 569a695..60cdf90 100644 --- a/robotis_math/src/robotis_linear_algebra.cpp +++ b/robotis_math/src/robotis_linear_algebra.cpp @@ -75,7 +75,7 @@ Eigen::MatrixXd transformationXYZRPY ( double position_x, double position_y, dou return _transformation; } -Eigen::MatrixXd InverseTransformation(Eigen::MatrixXd transform) +Eigen::MatrixXd inverseTransformation(Eigen::MatrixXd transform) { Eigen::Vector3d vecBOA; //If T is Transform Matrix A from B, the BOA is translation component coordi. B to coordi. A Eigen::Vector3d vec_x, vec_y, vec_z; @@ -261,7 +261,7 @@ Eigen::MatrixXd hatto( Eigen::MatrixXd matrix3d ) return _hatto; } -Eigen::MatrixXd Rodrigues( Eigen::MatrixXd hat_matrix , double angle ) +Eigen::MatrixXd rodrigues( Eigen::MatrixXd hat_matrix , double angle ) { Eigen::MatrixXd _E = Eigen::MatrixXd::Identity( 3 , 3 ); Eigen::MatrixXd _Rodrigues = _E + hat_matrix * sin( angle ) + hat_matrix * hat_matrix * ( 1 - cos( angle ) ); From 2e7b2c6c3552e5e394a4464e38b5a5913f7fba86 Mon Sep 17 00:00:00 2001 From: sch Date: Tue, 7 Jun 2016 14:18:34 +0900 Subject: [PATCH 11/33] ROS C++ coding style is applied to robotis_math --- robotis_math/CMakeLists.txt | 25 +- .../robotis_math/robotis_linear_algebra.h | 51 +- .../include/robotis_math/robotis_math.h | 4 +- .../include/robotis_math/robotis_math_base.h | 6 +- .../robotis_trajectory_calculator.h | 28 +- robotis_math/package.xml | 26 +- robotis_math/src/robotis_linear_algebra.cpp | 360 ++++++------- robotis_math/src/robotis_math_base.cpp | 19 +- .../src/robotis_trajectory_calculator.cpp | 491 +++++++++--------- 9 files changed, 470 insertions(+), 540 deletions(-) diff --git a/robotis_math/CMakeLists.txt b/robotis_math/CMakeLists.txt index 4d6421d..7ce8d9a 100644 --- a/robotis_math/CMakeLists.txt +++ b/robotis_math/CMakeLists.txt @@ -1,9 +1,6 @@ cmake_minimum_required(VERSION 2.8.3) project(robotis_math) -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS roscpp cmake_modules @@ -11,16 +8,10 @@ find_package(catkin REQUIRED COMPONENTS find_package(Eigen REQUIRED) - ################################### ## catkin specific configuration ## ################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need + catkin_package( INCLUDE_DIRS include LIBRARIES robotis_math @@ -32,34 +23,20 @@ catkin_package( ## Build ## ########### -## Specify additional locations of header files -## Your package locations should be listed before other locations include_directories( include ${catkin_INCLUDE_DIRS} ${Eigen_INCLUDE_DIRS} ) -## Declare a C++ library add_library(robotis_math src/robotis_math_base.cpp src/robotis_trajectory_calculator.cpp src/robotis_linear_algebra.cpp ) -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure add_dependencies(robotis_math ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -## Declare a C++ executable -# add_executable(robotis_math_node src/robotis_math_node.cpp) - -## Add cmake target dependencies of the executable -## same as for the library above -#add_dependencies(robotis_math_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against target_link_libraries(robotis_math ${catkin_LIBRARIES} ) diff --git a/robotis_math/include/robotis_math/robotis_linear_algebra.h b/robotis_math/include/robotis_math/robotis_linear_algebra.h index 84b3e11..6751b2b 100644 --- a/robotis_math/include/robotis_math/robotis_linear_algebra.h +++ b/robotis_math/include/robotis_math/robotis_linear_algebra.h @@ -31,8 +31,8 @@ /* * robotis_linear_algebra.h * - * Created on: Mar 18, 2016 - * Author: jay + * Created on: June 6, 2016 + * Author: sch */ #ifndef ROBOTIS_LINEAR_ALGEBRA_H_ @@ -48,32 +48,27 @@ namespace robotis_framework { -Eigen::MatrixXd transitionXYZ ( double position_x, double position_y, double position_z ); -Eigen::MatrixXd transformationXYZRPY ( double position_x, double position_y, double position_z , double roll , double pitch , double yaw ); -Eigen::MatrixXd inverseTransformation(Eigen::MatrixXd transform); - -Eigen::MatrixXd inertiaXYZ( double ixx, double ixy, double ixz , double iyy , double iyz, double izz ); - -Eigen::MatrixXd rotationX( double angle ); -Eigen::MatrixXd rotationY( double angle ); -Eigen::MatrixXd rotationZ( double angle ); - -Eigen::MatrixXd rotation2rpy( Eigen::MatrixXd rotation ); -Eigen::MatrixXd rpy2rotation( double roll, double pitch, double yaw ); - -Eigen::Quaterniond rpy2quaternion( double roll, double pitch, double yaw ); -Eigen::Quaterniond rotation2quaternion( Eigen::MatrixXd rotation ); - -Eigen::MatrixXd quaternion2rpy( Eigen::Quaterniond quaternion ); -Eigen::MatrixXd quaternion2rotation( Eigen::Quaterniond quaternion ); - -Eigen::MatrixXd rotation4d( double roll, double pitch, double yaw ); - -Eigen::MatrixXd hatto( Eigen::MatrixXd matrix3d ); -Eigen::MatrixXd rodrigues( Eigen::MatrixXd hat_matrix , double angle ); -Eigen::MatrixXd rot2omega(Eigen::MatrixXd rotation ); -Eigen::MatrixXd cross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b ); -double dot(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b ); +Eigen::MatrixXd getTransitionXYZ(double position_x, double position_y, double position_z); +Eigen::MatrixXd getTransformationXYZRPY(double position_x, double position_y, double position_z , double roll, double pitch, double yaw); +Eigen::MatrixXd getInverseTransformation(Eigen::MatrixXd transform); +Eigen::MatrixXd getInertiaXYZ(double ixx, double ixy, double ixz , double iyy , double iyz, double izz); +Eigen::MatrixXd getRotationX(double angle); +Eigen::MatrixXd getRotationY(double angle); +Eigen::MatrixXd getRotationZ(double angle); +Eigen::MatrixXd getRotation4d(double roll, double pitch, double yaw); + +Eigen::MatrixXd convertRotationToRPY(Eigen::MatrixXd rotation); +Eigen::MatrixXd convertRPYToRotation(double roll, double pitch, double yaw); +Eigen::Quaterniond convertRPYToQuaternion(double roll, double pitch, double yaw); +Eigen::Quaterniond convertRotationToQuaternion(Eigen::MatrixXd rotation); +Eigen::MatrixXd convertQuaternionToRPY(Eigen::Quaterniond quaternion); +Eigen::MatrixXd convertQuaternionToRotation(Eigen::Quaterniond quaternion); + +Eigen::MatrixXd hatto(Eigen::MatrixXd matrix3d); +Eigen::MatrixXd rodrigues(Eigen::MatrixXd hat_matrix, double angle); +Eigen::MatrixXd rot2omega(Eigen::MatrixXd rotation); +Eigen::MatrixXd cross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b); +double dot(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b); } diff --git a/robotis_math/include/robotis_math/robotis_math.h b/robotis_math/include/robotis_math/robotis_math.h index 61c33e5..caa6dc9 100644 --- a/robotis_math/include/robotis_math/robotis_math.h +++ b/robotis_math/include/robotis_math/robotis_math.h @@ -31,8 +31,8 @@ /* * robotis_math.h * - * Created on: Mar 18, 2016 - * Author: jay + * Created on: June 6, 2016 + * Author: sch */ #ifndef ROBOTIS_MATH_H_ diff --git a/robotis_math/include/robotis_math/robotis_math_base.h b/robotis_math/include/robotis_math/robotis_math_base.h index 8088051..14c4bc3 100644 --- a/robotis_math/include/robotis_math/robotis_math_base.h +++ b/robotis_math/include/robotis_math/robotis_math_base.h @@ -31,8 +31,8 @@ /* * robotis_math_base.h * - * Created on: 2016. 3. 28. - * Author: JaySong + * Created on: June 6, 2016 + * Author: sch */ #ifndef ROBOTIS_MATH_BASE_H_ @@ -54,7 +54,7 @@ inline double powDI(double a, int b) return (b == 0 ? 1 : (b > 0 ? a * powDI(a, b - 1) : 1 / powDI(a, -b))); } -double sign( double x ); +double sign(double x); } diff --git a/robotis_math/include/robotis_math/robotis_trajectory_calculator.h b/robotis_math/include/robotis_math/robotis_trajectory_calculator.h index a1c3807..79857df 100644 --- a/robotis_math/include/robotis_math/robotis_trajectory_calculator.h +++ b/robotis_math/include/robotis_math/robotis_trajectory_calculator.h @@ -31,14 +31,13 @@ /* * robotis_trajectory_calculator.h * - * Created on: Mar 18, 2016 - * Author: jay + * Created on: June 6, 2016 + * Author: sch */ #ifndef ROBOTIS_TRAJECTORY_CALCULATOR_H_ #define ROBOTIS_TRAJECTORY_CALCULATOR_H_ - #include "robotis_linear_algebra.h" #include "robotis_math_base.h" @@ -47,21 +46,20 @@ namespace robotis_framework { -Eigen::MatrixXd minimum_jerk_tra( double pos_start , double vel_start , double accel_start, - double pos_end , double vel_end , double accel_end, - double smp_time , double mov_time ); +Eigen::MatrixXd calcMinimumJerkTra(double pos_start, double vel_start, double accel_start, + double pos_end, double vel_end, double accel_end, + double smp_time, double mov_time); -Eigen::MatrixXd minimum_jerk_tra_via_n_qdqddq( int via_num, - double pos_start , double vel_start , double accel_start , - Eigen::MatrixXd pos_via, Eigen::MatrixXd vel_via, Eigen::MatrixXd accel_via, - double pos_end, double vel_end, double accel_end, - double smp_time, Eigen::MatrixXd via_time, double mov_time ); +Eigen::MatrixXd calcMinimumJerkTraWithViaPoints(int via_num, + double pos_start, double vel_start, double accel_start, + Eigen::MatrixXd pos_via, Eigen::MatrixXd vel_via, Eigen::MatrixXd accel_via, + double pos_end, double vel_end, double accel_end, + double smp_time, Eigen::MatrixXd via_time, double mov_time); -Eigen::MatrixXd arc3d_tra( double smp_time, double mov_time, - Eigen::MatrixXd center_point, Eigen::MatrixXd normal_vector, Eigen::MatrixXd start_point, - double rotation_angle, double cross_ratio ); +Eigen::MatrixXd calcArc3dTra(double smp_time, double mov_time, + Eigen::MatrixXd center_point, Eigen::MatrixXd normal_vector, Eigen::MatrixXd start_point, + double rotation_angle, double cross_ratio); } - #endif /* ROBOTIS_TRAJECTORY_CALCULATOR_H_ */ diff --git a/robotis_math/package.xml b/robotis_math/package.xml index 506f261..29d618b 100644 --- a/robotis_math/package.xml +++ b/robotis_math/package.xml @@ -4,28 +4,10 @@ 0.0.0 The robotis_math package - - - - jay - - - - - - BSD - - - - - - - - - - - - + sch + BSD + + sch catkin roscpp diff --git a/robotis_math/src/robotis_linear_algebra.cpp b/robotis_math/src/robotis_linear_algebra.cpp index 60cdf90..d85d744 100644 --- a/robotis_math/src/robotis_linear_algebra.cpp +++ b/robotis_math/src/robotis_linear_algebra.cpp @@ -31,291 +31,265 @@ /* * robotis_linear_algebra.cpp * - * Created on: Mar 18, 2016 - * Author: jay + * Created on: June 6, 2016 + * Author: sch */ - #include "robotis_math/robotis_linear_algebra.h" namespace robotis_framework { -Eigen::MatrixXd transitionXYZ ( double position_x, double position_y, double position_z ) +Eigen::MatrixXd getTransitionXYZ(double position_x, double position_y, double position_z) { - Eigen::MatrixXd _position(3,1); + Eigen::MatrixXd position(3,1); - _position << position_x, - position_y, - position_z; + position << + position_x, + position_y, + position_z; - return _position; + return position; } -Eigen::MatrixXd transformationXYZRPY ( double position_x, double position_y, double position_z , double roll , double pitch , double yaw ) +Eigen::MatrixXd getTransformationXYZRPY(double position_x, double position_y, double position_z , double roll , double pitch , double yaw) { -// Eigen::MatrixXd _position(3,1); -// -// _position << position_x, -// position_y, -// position_z; -// -// Eigen::MatrixXd _rotation = rpy2rotation( roll , pitch , yaw ); -// -// Eigen::MatrixXd _transformation(4,4); -// -// _transformation << _rotation , _position, -// 0 , 0 , 0 , 1; - - Eigen::MatrixXd _transformation = rotation4d(roll, pitch, yaw); - _transformation.coeffRef(0,3) = position_x; - _transformation.coeffRef(1,3) = position_y; - _transformation.coeffRef(2,3) = position_z; - - return _transformation; -} + Eigen::MatrixXd transformation = getRotation4d(roll, pitch, yaw); + transformation.coeffRef(0,3) = position_x; + transformation.coeffRef(1,3) = position_y; + transformation.coeffRef(2,3) = position_z; -Eigen::MatrixXd inverseTransformation(Eigen::MatrixXd transform) -{ - Eigen::Vector3d vecBOA; //If T is Transform Matrix A from B, the BOA is translation component coordi. B to coordi. A - Eigen::Vector3d vec_x, vec_y, vec_z; - Eigen::MatrixXd _invT(4,4); - - vecBOA.coeffRef(0) = -transform(0,3); vecBOA(1) = -transform(1,3); vecBOA(2) = -transform(2,3); - vec_x(0) = transform(0,0); vec_x(1) = transform(1,0); vec_x(2) = transform(2,0); - vec_y(0) = transform(0,1); vec_y(1) = transform(1,1); vec_y(2) = transform(2,1); - vec_z(0) = transform(0,2); vec_z(1) = transform(1,2); vec_z(2) = transform(2,2); -// -// -// // inv = [ x' | -AtoB??x ] -// // [ y' | -AtoB??y ] -// // [ z' | -AtoB??z ] -// // [ 0 0 0 | 1 ] -// - _invT << vec_x(0), vec_x(1), vec_x(2), vecBOA.dot(vec_x), - vec_y(0), vec_y(1), vec_y(2), vecBOA.dot(vec_y), - vec_z(0), vec_z(1), vec_z(2), vecBOA.dot(vec_z), - 0, 0, 0, 1; - - return _invT; + return transformation; } -Eigen::MatrixXd inertiaXYZ( double ixx, double ixy, double ixz , double iyy , double iyz, double izz ) +Eigen::MatrixXd getInverseTransformation(Eigen::MatrixXd transform) { - Eigen::MatrixXd _inertia(3,3); + // If T is Transform Matrix A from B, the BOA is translation component coordi. B to coordi. A + + Eigen::Vector3d vec_boa; + Eigen::Vector3d vec_x, vec_y, vec_z; + Eigen::MatrixXd inv_t(4,4); - _inertia << ixx , ixy , ixz, - ixy , iyy , iyz, - ixz , iyz , izz ; + vec_boa(0) = -transform(0,3); + vec_boa(1) = -transform(1,3); + vec_boa(2) = -transform(2,3); - return _inertia; + vec_x(0) = transform(0,0); vec_x(1) = transform(1,0); vec_x(2) = transform(2,0); + vec_y(0) = transform(0,1); vec_y(1) = transform(1,1); vec_y(2) = transform(2,1); + vec_z(0) = transform(0,2); vec_z(1) = transform(1,2); vec_z(2) = transform(2,2); + + inv_t << + vec_x(0), vec_x(1), vec_x(2), vec_boa.dot(vec_x), + vec_y(0), vec_y(1), vec_y(2), vec_boa.dot(vec_y), + vec_z(0), vec_z(1), vec_z(2), vec_boa.dot(vec_z), + 0, 0, 0, 1; + + return inv_t; } -Eigen::MatrixXd rotationX( double angle ) +Eigen::MatrixXd getInertiaXYZ(double ixx, double ixy, double ixz , double iyy , double iyz, double izz) { - Eigen::MatrixXd _rotation( 3 , 3 ); + Eigen::MatrixXd inertia(3,3); - _rotation << 1.0, 0.0, 0.0, - 0.0, cos( angle ), -sin( angle ), - 0.0, sin( angle ), cos( angle ); + inertia << + ixx, ixy, ixz, + ixy, iyy, iyz, + ixz, iyz, izz; - return _rotation; + return inertia; } -Eigen::MatrixXd rotationY( double angle ) +Eigen::MatrixXd getRotationX(double angle) { - Eigen::MatrixXd _rotation( 3 , 3 ); + Eigen::MatrixXd rotation(3,3); - _rotation << cos( angle ), 0.0, sin( angle ), - 0.0, 1.0, 0.0, - -sin( angle ), 0.0, cos( angle ); + rotation << + 1.0, 0.0, 0.0, + 0.0, cos(angle), -sin(angle), + 0.0, sin(angle), cos(angle); - return _rotation; + return rotation; } -Eigen::MatrixXd rotationZ( double angle ) +Eigen::MatrixXd getRotationY(double angle) { - Eigen::MatrixXd _rotation(3,3); + Eigen::MatrixXd rotation(3,3); - _rotation << cos( angle ), -sin( angle ), 0.0, - sin( angle ), cos( angle ), 0.0, - 0.0, 0.0, 1.0; + rotation << + cos(angle), 0.0, sin(angle), + 0.0, 1.0, 0.0, + -sin(angle), 0.0, cos(angle); - return _rotation; + return rotation; } -Eigen::MatrixXd rotation2rpy( Eigen::MatrixXd rotation ) +Eigen::MatrixXd getRotationZ(double angle) { - Eigen::MatrixXd _rpy = Eigen::MatrixXd::Zero( 3 , 1 ); + Eigen::MatrixXd rotation(3,3); - _rpy.coeffRef( 0 , 0 ) = atan2( rotation.coeff( 2 , 1 ), rotation.coeff( 2 , 2 ) ); - _rpy.coeffRef( 1 , 0 ) = atan2( -rotation.coeff( 2 , 0 ), sqrt( pow( rotation.coeff( 2 , 1 ) , 2 ) + pow( rotation.coeff( 2 , 2 ) , 2 ) ) ); - _rpy.coeffRef( 2 , 0 ) = atan2 ( rotation.coeff( 1 , 0 ) , rotation.coeff( 0 , 0 ) ); + rotation << + cos(angle), -sin(angle), 0.0, + sin(angle), cos(angle), 0.0, + 0.0, 0.0, 1.0; - return _rpy; + return rotation; } -Eigen::MatrixXd rpy2rotation( double roll, double pitch, double yaw ) +Eigen::MatrixXd getRotation4d(double roll, double pitch, double yaw ) { - Eigen::MatrixXd _rotation = rotationZ( yaw ) * rotationY( pitch ) * rotationX( roll ); - - return _rotation; + double sr = sin(roll), cr = cos(roll); + double sp = sin(pitch), cp = cos(pitch); + double sy = sin(yaw), cy = cos(yaw); + + Eigen::MatrixXd mat_roll(4,4); + Eigen::MatrixXd mat_pitch(4,4); + Eigen::MatrixXd mat_yaw(4,4); + + mat_roll << + 1, 0, 0, 0, + 0, cr, -sr, 0, + 0, sr, cr, 0, + 0, 0, 0, 1; + + mat_pitch << + cp, 0, sp, 0, + 0, 1, 0, 0, + -sp, 0, cp, 0, + 0, 0, 0, 1; + + mat_yaw << + cy, -sy, 0, 0, + sy, cy, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1; + + Eigen::MatrixXd mat_rpy = (mat_yaw*mat_pitch)*mat_roll; + + return mat_rpy; } -Eigen::Quaterniond rpy2quaternion( double roll, double pitch, double yaw ) +Eigen::MatrixXd convertRotationToRPY(Eigen::MatrixXd rotation) { - Eigen::MatrixXd _rotation = rpy2rotation( roll, pitch, yaw ); + Eigen::MatrixXd rpy = Eigen::MatrixXd::Zero(3,1); - Eigen::Matrix3d _rotation3d; - _rotation3d = _rotation.block<3,3>( 0 , 0); + rpy.coeffRef(0,0) = atan2(rotation.coeff(2,1), rotation.coeff(2,2)); + rpy.coeffRef(1,0) = atan2(-rotation.coeff(2,0), sqrt(pow(rotation.coeff(2,1), 2) + pow(rotation.coeff(2,2),2))); + rpy.coeffRef(2,0) = atan2 (rotation.coeff(1,0), rotation.coeff(0,0)); - Eigen::Quaterniond _quaternion; + return rpy; +} - _quaternion = _rotation3d; +Eigen::MatrixXd convertRPYToRotation(double roll, double pitch, double yaw) +{ + Eigen::MatrixXd rotation = getRotationZ(yaw)*getRotationY(pitch)*getRotationX(roll); - return _quaternion; + return rotation; } -Eigen::Quaterniond rotation2quaternion( Eigen::MatrixXd rotation ) +Eigen::Quaterniond convertRPYToQuaternion(double roll, double pitch, double yaw) { - Eigen::Matrix3d _rotation3d; + Eigen::MatrixXd rotation = convertRPYToRotation(roll,pitch,yaw); - _rotation3d = rotation.block<3,3>( 0 , 0); + Eigen::Matrix3d rotation3d; + rotation3d = rotation.block<3,3>(0,0); - Eigen::Quaterniond _quaternion; - _quaternion = _rotation3d; + Eigen::Quaterniond quaternion; + quaternion = rotation3d; - return _quaternion; + return quaternion; } -Eigen::MatrixXd quaternion2rpy( Eigen::Quaterniond quaternion ) +Eigen::Quaterniond convertRotationToQuaternion(Eigen::MatrixXd rotation) { - Eigen::MatrixXd _rpy = rotation2rpy( quaternion.toRotationMatrix() ); + Eigen::Matrix3d rotation3d; + rotation3d = rotation.block<3,3>(0,0); + + Eigen::Quaterniond quaternion; + quaternion = rotation3d; - return _rpy; + return quaternion; } -Eigen::MatrixXd quaternion2rotation( Eigen::Quaterniond quaternion ) +Eigen::MatrixXd convertQuaternionToRPY(Eigen::Quaterniond quaternion) { - Eigen::MatrixXd _rotation = quaternion.toRotationMatrix(); + Eigen::MatrixXd rpy = convertRotationToRPY(quaternion.toRotationMatrix()); - return _rotation; + return rpy; } -Eigen::MatrixXd rotation4d( double roll, double pitch, double yaw ) +Eigen::MatrixXd convertQuaternionToRotation(Eigen::Quaterniond quaternion) { -// Eigen::MatrixXd _rotation4d = Eigen::MatrixXd::Zero( 4 , 4 ); -// _rotation4d.coeffRef( 3 , 3 ) = 1.0; -// -// Eigen::MatrixXd _rotation = rotationZ( yaw ) * rotationY( pitch ) * rotationX( roll ); -// -//// _rotation4d.block<3,3>(0,0) = _rotation; -// _rotation4d.coeffRef(0,0) = _rotation.coeff(0,0); -// _rotation4d.coeffRef(0,1) = _rotation.coeff(0,1); -// _rotation4d.coeffRef(0,2) = _rotation.coeff(0,2); -// _rotation4d.coeffRef(1,0) = _rotation.coeff(1,0); -// _rotation4d.coeffRef(1,1) = _rotation.coeff(1,1); -// _rotation4d.coeffRef(1,2) = _rotation.coeff(1,2); -// _rotation4d.coeffRef(2,0) = _rotation.coeff(2,0); -// _rotation4d.coeffRef(2,1) = _rotation.coeff(2,1); -// _rotation4d.coeffRef(2,2) = _rotation.coeff(2,2); - -// return _rotation4d; - double sr = sin(roll), cr = cos(roll); - double sp = sin(pitch), cp = cos(pitch); - double sy = sin(yaw), cy = cos(yaw); - - Eigen::MatrixXd matRoll(4,4); - Eigen::MatrixXd matPitch(4,4); - Eigen::MatrixXd matYaw(4,4); - - matRoll << 1, 0, 0, 0, - 0, cr, -sr, 0, - 0, sr, cr, 0, - 0, 0, 0, 1; - - matPitch << cp, 0, sp, 0, - 0, 1, 0, 0, - -sp, 0, cp, 0, - 0, 0, 0, 1; - - matYaw << cy, -sy, 0, 0, - sy, cy, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1; - - - return (matYaw*matPitch)*matRoll; -} - + Eigen::MatrixXd rotation = quaternion.toRotationMatrix(); + return rotation; +} -Eigen::MatrixXd hatto( Eigen::MatrixXd matrix3d ) +Eigen::MatrixXd hatto(Eigen::MatrixXd matrix3d) { - Eigen::MatrixXd _hatto(3,3); + Eigen::MatrixXd hatto(3,3); - _hatto << 0.0, -matrix3d.coeff(2,0), matrix3d.coeff(1,0), - matrix3d.coeff(2,0), 0.0, -matrix3d.coeff(0,0), - -matrix3d.coeff(1,0), matrix3d.coeff(0,0), 0.0; + hatto << + 0.0, -matrix3d.coeff(2,0), matrix3d.coeff(1,0), + matrix3d.coeff(2,0), 0.0, -matrix3d.coeff(0,0), + -matrix3d.coeff(1,0), matrix3d.coeff(0,0), 0.0; - return _hatto; + return hatto; } -Eigen::MatrixXd rodrigues( Eigen::MatrixXd hat_matrix , double angle ) +Eigen::MatrixXd rodrigues(Eigen::MatrixXd hat_matrix, double angle) { - Eigen::MatrixXd _E = Eigen::MatrixXd::Identity( 3 , 3 ); - Eigen::MatrixXd _Rodrigues = _E + hat_matrix * sin( angle ) + hat_matrix * hat_matrix * ( 1 - cos( angle ) ); + Eigen::MatrixXd identity = Eigen::MatrixXd::Identity(3,3); + Eigen::MatrixXd rodrigues = identity+hat_matrix*sin(angle)+hat_matrix*hat_matrix*(1-cos(angle)); - return _Rodrigues; + return rodrigues; } -Eigen::MatrixXd rot2omega( Eigen::MatrixXd rotation ) +Eigen::MatrixXd rot2omega(Eigen::MatrixXd rotation) { - double _eps = 1e-12; - -// Eigen::MatrixXd _E = Eigen::MatrixXd::Identity( 3 , 3 ); - - double _alpha = ( rotation.coeff( 0 , 0 ) + rotation.coeff( 1 , 1 ) + rotation.coeff( 2 , 2 ) - 1.0 ) / 2.0 ; + double eps = 1e-12; - double _alpha_plus = fabs( _alpha - 1.0 ); + double alpha = (rotation.coeff(0,0)+rotation.coeff(1,1)+rotation.coeff(2,2)-1.0)/2.0; + double alpha_dash = fabs( alpha - 1.0 ); - Eigen::MatrixXd _rot2omega( 3 , 1 ); + Eigen::MatrixXd rot2omega(3,1); - if( _alpha_plus < _eps ) - { - _rot2omega << 0.0, - 0.0, - 0.0; - } - else - { - double _theta = acos( _alpha ); + if( alpha_dash < eps ) + { + rot2omega << + 0.0, + 0.0, + 0.0; + } + else + { + double theta = acos(alpha); - _rot2omega << rotation.coeff( 2 , 1 ) - rotation.coeff( 1 , 2 ), - rotation.coeff( 0 , 2 ) - rotation.coeff( 2 , 0 ), - rotation.coeff( 1 , 0 ) - rotation.coeff( 0 , 1 ); + rot2omega << + rotation.coeff(2,1)-rotation.coeff(1,2), + rotation.coeff(0,2)-rotation.coeff(2,0), + rotation.coeff(1,0)-rotation.coeff(0,1); - _rot2omega = 0.5 * ( _theta / sin( _theta ) ) * _rot2omega; - } + rot2omega = 0.5*(theta/sin(theta))*rot2omega; + } - return _rot2omega; + return rot2omega; } -Eigen::MatrixXd cross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b ) +Eigen::MatrixXd cross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b) { - Eigen::MatrixXd _cross( 3 , 1 ); + Eigen::MatrixXd cross(3,1); - _cross << vector3d_a.coeff( 1 , 0 ) * vector3d_b.coeff( 2 , 0 ) - vector3d_a.coeff( 2 , 0 ) * vector3d_b.coeff( 1 , 0 ), - vector3d_a.coeff( 2 , 0 ) * vector3d_b.coeff( 0 , 0 ) - vector3d_a.coeff( 0 , 0 ) * vector3d_b.coeff( 2 , 0 ), - vector3d_a.coeff( 0 , 0 ) * vector3d_b.coeff( 1 , 0 ) - vector3d_a.coeff( 1 , 0 ) * vector3d_b.coeff( 0 , 0 ); + cross << + vector3d_a.coeff(1,0)*vector3d_b.coeff(2,0)-vector3d_a.coeff(2,0)*vector3d_b.coeff(1,0), + vector3d_a.coeff(2,0)*vector3d_b.coeff(0,0)-vector3d_a.coeff(0,0)*vector3d_b.coeff(2,0), + vector3d_a.coeff(0,0)*vector3d_b.coeff(1,0)-vector3d_a.coeff(1,0)*vector3d_b.coeff(0,0); - return _cross; + return cross; } -double dot(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b ) +double dot(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b) { - return vector3d_a.dot(vector3d_b); - //return vector3d_a.coeff(0,0)*vector3d_b.coeff(0,0) + vector3d_a.coeff(1,0)*vector3d_b.coeff(1,0) + vector3d_a.coeff(2,0)*vector3d_b.coeff(2,0); + return vector3d_a.dot(vector3d_b); } } diff --git a/robotis_math/src/robotis_math_base.cpp b/robotis_math/src/robotis_math_base.cpp index b5c43d3..60eb1f4 100644 --- a/robotis_math/src/robotis_math_base.cpp +++ b/robotis_math/src/robotis_math_base.cpp @@ -31,24 +31,23 @@ /* * robotis_math_base.cpp * - * Created on: Mar 18, 2016 - * Author: jay + * Created on: June 6, 2016 + * Author: sch */ #include "robotis_math/robotis_math_base.h" - namespace robotis_framework { -double sign( double x ) +double sign(double x) { - if ( x < 0.0 ) - return -1.0; - else if ( x > 0.0) - return 1.0; - else - return 0.0; + if ( x < 0.0 ) + return -1.0; + else if ( x > 0.0) + return 1.0; + else + return 0.0; } } diff --git a/robotis_math/src/robotis_trajectory_calculator.cpp b/robotis_math/src/robotis_trajectory_calculator.cpp index 4d91a2c..4b776aa 100644 --- a/robotis_math/src/robotis_trajectory_calculator.cpp +++ b/robotis_math/src/robotis_trajectory_calculator.cpp @@ -1,30 +1,48 @@ -/* - * RobotisTrajectoryCalculator.cpp +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. * - * Created on: Mar 18, 2016 - * Author: jay - */ - - - -#include "../include/robotis_math/robotis_trajectory_calculator.h" - + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ /* - * trajectory.cpp + * robotis_trajectory_calculator.cpp * - * Created on: Jul 13, 2015 + * Created on: June 6, 2016 * Author: sch */ - +#include "../include/robotis_math/robotis_trajectory_calculator.h" namespace robotis_framework { -Eigen::MatrixXd minimum_jerk_tra( double pos_start , double vel_start , double accel_start, - double pos_end , double vel_end , double accel_end, - double smp_time , double mov_time ) +Eigen::MatrixXd calcMinimumJerkTra(double pos_start, double vel_start, double accel_start, + double pos_end, double vel_end, double accel_end, + double smp_time, double mov_time) /* simple minimum jerk trajectory @@ -37,288 +55,275 @@ Eigen::MatrixXd minimum_jerk_tra( double pos_start , double vel_start , double a accel_end : acceleration at final state smp_time : sampling time - mov_time : movement time -*/ + */ { - Eigen::MatrixXd ___C( 3 , 3 ); - Eigen::MatrixXd __C( 3 , 1 ); - - ___C << pow( mov_time , 3 ) , pow( mov_time , 4 ) , pow( mov_time , 5 ), - 3 * pow( mov_time , 2 ) , 4 * pow( mov_time , 3 ) , 5 * pow( mov_time , 4 ), - 6 * mov_time , 12 * pow( mov_time , 2 ) , 20 * pow( mov_time , 3 ); - - __C << pos_end - pos_start - vel_start * mov_time - accel_start * pow( mov_time , 2 ) / 2, - vel_end - vel_start - accel_start * mov_time, - accel_end - accel_start ; - - Eigen::Matrix _A = ___C.inverse() * __C; - - double _time_steps; - - _time_steps = mov_time / smp_time; - int __time_steps = round( _time_steps + 1 ); - - Eigen::MatrixXd _time = Eigen::MatrixXd::Zero( __time_steps , 1 ); - Eigen::MatrixXd _tra = Eigen::MatrixXd::Zero( __time_steps , 1 ); - - for ( int step = 0; step < __time_steps; step++ ) - _time.coeffRef( step , 0 ) = step * smp_time; - - for ( int step = 0; step < __time_steps; step++ ) - { - _tra.coeffRef( step , 0 ) = - pos_start + - vel_start * _time.coeff( step , 0 ) + - 0.5 * accel_start * pow( _time.coeff( step , 0 ) , 2 ) + - _A.coeff( 0 , 0 ) * pow( _time.coeff( step , 0 ) , 3 ) + - _A.coeff( 1 , 0 ) * pow( _time.coeff( step , 0 ) , 4 ) + - _A.coeff( 2 , 0 ) * pow( _time.coeff( step , 0 ) , 5 ); - } - - return _tra; + Eigen::MatrixXd poly_matrix(3,3); + Eigen::MatrixXd poly_vector(3,1); + + poly_matrix << + pow(mov_time,3), pow(mov_time,4), pow(mov_time,5), + 3*pow(mov_time,2), 4*pow(mov_time,3), 5*pow(mov_time,4), + 6*mov_time, 12*pow(mov_time,2), 20*pow(mov_time,3); + + poly_vector << + pos_end-pos_start-vel_start*mov_time-accel_start*pow(mov_time,2)/2, + vel_end-vel_start-accel_start*mov_time, + accel_end-accel_start ; + + Eigen::Matrix poly_coeff = poly_matrix.inverse() * poly_vector; + + double time_steps = mov_time/smp_time; + int all_time_steps = round(time_steps+1); + + Eigen::MatrixXd time = Eigen::MatrixXd::Zero(all_time_steps,1); + Eigen::MatrixXd minimum_jerk_tra = Eigen::MatrixXd::Zero(all_time_steps,1); + + for (int step=0; step j) + k = i ; + else + k = j ; - for (i=1; i<=via_num; i++){ - A1.coeffRef(3*i-3,0) = pow(via_time.coeff(i-1,0),3) ; - A1.coeffRef(3*i-3,1) = pow(via_time.coeff(i-1,0),4) ; - A1.coeffRef(3*i-3,2) = pow(via_time.coeff(i-1,0),5) ; + poly_matrix_part2.coeffRef(3*j-3,3*i-3) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),3)/6 ; + poly_matrix_part2.coeffRef(3*j-3,3*i-2) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),4)/24 ; + poly_matrix_part2.coeffRef(3*j-3,3*i-1) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),5)/120 ; - A1.coeffRef(3*i-2,0) = 3*pow(via_time.coeff(i-1,0),2) ; - A1.coeffRef(3*i-2,1) = 4*pow(via_time.coeff(i-1,0),3) ; - A1.coeffRef(3*i-2,2) = 5*pow(via_time.coeff(i-1,0),4) ; + poly_matrix_part2.coeffRef(3*j-2,3*i-3) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),2)/2 ; + poly_matrix_part2.coeffRef(3*j-2,3*i-2) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),3)/6 ; + poly_matrix_part2.coeffRef(3*j-2,3*i-1) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),4)/24 ; - A1.coeffRef(3*i-1,0) = 6*via_time.coeff(i-1,0) ; - A1.coeffRef(3*i-1,1) = 12*pow(via_time.coeff(i-1,0),2) ; - A1.coeffRef(3*i-1,2) = 20*pow(via_time.coeff(i-1,0),3) ; + poly_matrix_part2.coeffRef(3*j-1,3*i-3) = via_time.coeff(k-1,0)-via_time.coeff(i-1,0) ; + poly_matrix_part2.coeffRef(3*j-1,3*i-2) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),2)/2 ; + poly_matrix_part2.coeffRef(3*j-1,3*i-1) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),3)/6 ; } + } - Eigen::MatrixXd A2 = Eigen::MatrixXd::Zero(3*via_num,3*via_num); - - for (i=1; i<=via_num; i++){ - for (j=1; j<=via_num; j++){ - if (i > j){ - k = i ; - }else{ - k = j ; - } - A2.coeffRef(3*j-3,3*i-3) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),3)/6 ; - A2.coeffRef(3*j-3,3*i-2) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),4)/24 ; - A2.coeffRef(3*j-3,3*i-1) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),5)/120 ; - - A2.coeffRef(3*j-2,3*i-3) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),2)/2 ; - A2.coeffRef(3*j-2,3*i-2) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),3)/6 ; - A2.coeffRef(3*j-2,3*i-1) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),4)/24 ; - - A2.coeffRef(3*j-1,3*i-3) = via_time.coeff(k-1,0)-via_time.coeff(i-1,0) ; - A2.coeffRef(3*j-1,3*i-2) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),2)/2 ; - A2.coeffRef(3*j-1,3*i-1) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),3)/6 ; - } - } + Eigen::MatrixXd poly_matrix_part3 = Eigen::MatrixXd::Zero(3,3*via_num+3); + poly_matrix_part3.coeffRef(0,0) = pow(mov_time,3); + poly_matrix_part3.coeffRef(0,1) = pow(mov_time,4); + poly_matrix_part3.coeffRef(0,2) = pow(mov_time,5); - Eigen::MatrixXd A3 = Eigen::MatrixXd::Zero(3,3*via_num+3); + poly_matrix_part3.coeffRef(1,0) = 3*pow(mov_time,2); + poly_matrix_part3.coeffRef(1,1) = 4*pow(mov_time,3); + poly_matrix_part3.coeffRef(1,2) = 5*pow(mov_time,4); - A3.coeffRef(0,0) = pow(mov_time,3); - A3.coeffRef(0,1) = pow(mov_time,4); - A3.coeffRef(0,2) = pow(mov_time,5); + poly_matrix_part3.coeffRef(2,0) = 6*mov_time; + poly_matrix_part3.coeffRef(2,1) = 12*pow(mov_time,2); + poly_matrix_part3.coeffRef(2,2) = 20*pow(mov_time,3); - A3.coeffRef(1,0) = 3*pow(mov_time,2); - A3.coeffRef(1,1) = 4*pow(mov_time,3); - A3.coeffRef(1,2) = 5*pow(mov_time,4); + for (int i=1; i<=via_num; i++) + { + poly_matrix_part3.coeffRef(0,3*i) = pow(mov_time-via_time.coeff(i-1,0),3)/6 ; + poly_matrix_part3.coeffRef(1,3*i) = pow(mov_time-via_time.coeff(i-1,0),2)/2 ; + poly_matrix_part3.coeffRef(2,3*i) = mov_time-via_time.coeff(i-1,0) ; - A3.coeffRef(2,0) = 6*mov_time; - A3.coeffRef(2,1) = 12*pow(mov_time,2); - A3.coeffRef(2,2) = 20*pow(mov_time,3); + poly_matrix_part3.coeffRef(0,3*i+1) = pow(mov_time-via_time.coeff(i-1,0),4)/24 ; + poly_matrix_part3.coeffRef(1,3*i+1) = pow(mov_time-via_time.coeff(i-1,0),3)/6 ; + poly_matrix_part3.coeffRef(2,3*i+1) = pow(mov_time-via_time.coeff(i-1,0),2)/2 ; - for (i=1; i<=via_num; i++){ - A3.coeffRef(0,3*i) = pow(mov_time-via_time.coeff(i-1,0),3)/6 ; - A3.coeffRef(1,3*i) = pow(mov_time-via_time.coeff(i-1,0),2)/2 ; - A3.coeffRef(2,3*i) = mov_time-via_time.coeff(i-1,0) ; + poly_matrix_part3.coeffRef(0,3*i+2) = pow(mov_time-via_time.coeff(i-1,0),5)/120 ; + poly_matrix_part3.coeffRef(1,3*i+2) = pow(mov_time-via_time.coeff(i-1,0),4)/24 ; + poly_matrix_part3.coeffRef(2,3*i+2) = pow(mov_time-via_time.coeff(i-1,0),3)/6 ; + } - A3.coeffRef(0,3*i+1) = pow(mov_time-via_time.coeff(i-1,0),4)/24 ; - A3.coeffRef(1,3*i+1) = pow(mov_time-via_time.coeff(i-1,0),3)/6 ; - A3.coeffRef(2,3*i+1) = pow(mov_time-via_time.coeff(i-1,0),2)/2 ; + Eigen::MatrixXd poly_matrix = Eigen::MatrixXd::Zero(3*via_num+3,3*via_num+3); - A3.coeffRef(0,3*i+2) = pow(mov_time-via_time.coeff(i-1,0),5)/120 ; - A3.coeffRef(1,3*i+2) = pow(mov_time-via_time.coeff(i-1,0),4)/24 ; - A3.coeffRef(2,3*i+2) = pow(mov_time-via_time.coeff(i-1,0),3)/6 ; - } + poly_matrix.block(0,0,3*via_num,3) = poly_matrix_part1 ; + poly_matrix.block(0,3,3*via_num,3*via_num) = poly_matrix_part2 ; + poly_matrix.block(3*via_num,0,3,3*via_num+3) = poly_matrix_part3 ; - Eigen::MatrixXd A = Eigen::MatrixXd::Zero(3*via_num+3,3*via_num+3); + Eigen::MatrixXd poly_coeff(3*via_num+3,1); + //C = A.inverse()*B; + poly_coeff = poly_matrix.colPivHouseholderQr().solve(poly_vector); - A.block(0,0,3*via_num,3) = A1 ; - A.block(0,3,3*via_num,3*via_num) = A2 ; - A.block(3*via_num,0,3,3*via_num+3) = A3 ; + int all_time_steps; + all_time_steps = round(mov_time/smp_time) ; - /* Calculation Matrix C (coefficient of polynomial function) */ + Eigen::MatrixXd time_vector = Eigen::MatrixXd::Zero(all_time_steps+1,1); - Eigen::MatrixXd C(3*via_num+3,1); - //C = A.inverse()*B; - C = A.colPivHouseholderQr().solve(B); + for (int i=1; i<=all_time_steps+1; i++) + time_vector.coeffRef(i-1,0) = (i-1)*smp_time; - /* Time */ + Eigen::MatrixXd via_time_vector = Eigen::MatrixXd::Zero(via_num,1); - int NN; - double N; + for (int i=1; i<=via_num; i++) + via_time_vector.coeffRef(i-1,0) = round(via_time.coeff(i-1,0)/smp_time)+2; - N = mov_time/smp_time ; - NN = round(N) ; + Eigen::MatrixXd minimum_jerk_tra_with_via_points = Eigen::MatrixXd::Zero(all_time_steps+1,1); - Eigen::MatrixXd Time = Eigen::MatrixXd::Zero(NN+1,1); + for (int i=1; i<=all_time_steps+1; i++) + { + minimum_jerk_tra_with_via_points.coeffRef(i-1,0) = + pos_start + + vel_start*time_vector.coeff(i-1,0) + + 0.5*accel_start*pow(time_vector.coeff(i-1,0),2) + + poly_coeff.coeff(0,0)*pow(time_vector.coeff(i-1,0),3) + + poly_coeff.coeff(1,0)*pow(time_vector.coeff(i-1,0),4) + + poly_coeff.coeff(2,0)*pow(time_vector.coeff(i-1,0),5) ; + } - for (i=1; i<=NN+1; i++){ - Time.coeffRef(i-1,0) = (i-1)*smp_time; - } - - /* Time_via */ - - Eigen::MatrixXd Time_via = Eigen::MatrixXd::Zero(via_num,1); - - for (i=1; i<=via_num; i++){ - Time_via.coeffRef(i-1,0) = round(via_time.coeff(i-1,0)/smp_time)+2; - } - - /* Minimum Jerk Trajectory with Via-points */ - - Eigen::MatrixXd _tra_jerk_via = Eigen::MatrixXd::Zero(NN+1,1); - - for (i=1; i<=NN+1; i++){ - _tra_jerk_via.coeffRef(i-1,0) = - pos_start + - vel_start*Time.coeff(i-1,0) + - 0.5*accel_start*pow(Time.coeff(i-1,0),2) + - C.coeff(0,0)*pow(Time.coeff(i-1,0),3) + - C.coeff(1,0)*pow(Time.coeff(i-1,0),4) + - C.coeff(2,0)*pow(Time.coeff(i-1,0),5) ; - } - - for (i=1; i<=via_num; i++){ - for (j=Time_via.coeff(i-1,0); j<=NN+1; j++){ - _tra_jerk_via.coeffRef(j-1,0) = - _tra_jerk_via.coeff(j-1,0) + - C.coeff(3*i,0)*pow((Time.coeff(j-1,0)-via_time.coeff(i-1,0)),3)/6 + - C.coeff(3*i+1,0)*pow((Time.coeff(j-1,0)-via_time.coeff(i-1,0)),4)/24 + - C.coeff(3*i+2,0)*pow((Time.coeff(j-1,0)-via_time.coeff(i-1,0)),5)/120 ; + for (int i=1; i<=via_num; i++) + { + for (int j=via_time_vector.coeff(i-1,0); j<=all_time_steps+1; j++) + { + minimum_jerk_tra_with_via_points.coeffRef(j-1,0) = + minimum_jerk_tra_with_via_points.coeff(j-1,0) + + poly_coeff.coeff(3*i,0)*pow((time_vector.coeff(j-1,0)-via_time.coeff(i-1,0)),3)/6 + + poly_coeff.coeff(3*i+1,0)*pow((time_vector.coeff(j-1,0)-via_time.coeff(i-1,0)),4)/24 + + poly_coeff.coeff(3*i+2,0)*pow((time_vector.coeff(j-1,0)-via_time.coeff(i-1,0)),5)/120 ; - } } + } - return _tra_jerk_via; - + return minimum_jerk_tra_with_via_points; } -Eigen::MatrixXd arc3d_tra( double smp_time, double mov_time, - Eigen::MatrixXd center_point, Eigen::MatrixXd normal_vector, Eigen::MatrixXd start_point, - double rotation_angle, double cross_ratio ) +Eigen::MatrixXd calcArc3dTra(double smp_time, double mov_time, + Eigen::MatrixXd center_point, Eigen::MatrixXd normal_vector, Eigen::MatrixXd start_point, + double rotation_angle, double cross_ratio ) { - int _all_time_steps = int ( round( mov_time / smp_time ) ) + 1 ; - - Eigen::MatrixXd _angle_tra = minimum_jerk_tra( 0.0 , 0.0 , 0.0 , - rotation_angle , 0.0 , 0.0 , - smp_time , mov_time ); + int all_time_steps = int (round(mov_time/smp_time))+1; - Eigen::MatrixXd _pt = Eigen::MatrixXd::Zero( 3 , _all_time_steps ); + Eigen::MatrixXd angle_tra = calcMinimumJerkTra(0.0, 0.0, 0.0, + rotation_angle, 0.0, 0.0, + smp_time, mov_time); - for (int i = 0; i < _all_time_steps; i++ ) - { - double _t = ( ( double ) i ) * smp_time ; - - double _th = _angle_tra.coeff( i , 0 );//( _t / mov_time ) * rotation_angle; + Eigen::MatrixXd arc_tra = Eigen::MatrixXd::Zero(3,all_time_steps); - Eigen::MatrixXd _w_wedge( 3 , 3 ); + for (int step = 0; step < all_time_steps; step++ ) + { + double time = ((double)step)*smp_time; + double theta = angle_tra.coeff(step,0); - _w_wedge << 0.0 , -normal_vector.coeff(2,0), normal_vector.coeff(1,0), - normal_vector.coeff(2,0), 0.0 , -normal_vector.coeff(0,0), - -normal_vector.coeff(1,0), normal_vector.coeff(0,0), 0.0 ; + Eigen::MatrixXd weight_wedge(3,3); - Eigen::MatrixXd _E = Eigen::MatrixXd::Identity( 3 , 3 ); + weight_wedge << + 0.0, -normal_vector.coeff(2,0), normal_vector.coeff(1,0), + normal_vector.coeff(2,0), 0.0, -normal_vector.coeff(0,0), + -normal_vector.coeff(1,0), normal_vector.coeff(0,0), 0.0; - Eigen::MatrixXd _R = _E + _w_wedge * sin( _th ) + _w_wedge * _w_wedge * ( 1 - cos( _th ) ); + Eigen::MatrixXd identity = Eigen::MatrixXd::Identity(3,3); + Eigen::MatrixXd rotation = identity + weight_wedge*sin(theta) + weight_wedge*weight_wedge*(1-cos(theta)); + double cross = cross_ratio*(1.0-2.0*(abs(0.5-(time/mov_time)))); - double _cross = cross_ratio * ( 1.0 - 2.0 * ( abs ( 0.5 - _t/mov_time ) ) ); - - _pt.block( 0 , i , 3 , 1 ) = ( 1 + _cross ) * ( _R * ( start_point - center_point ) ) + center_point; - } + arc_tra.block(0,step,3,1) = (1+cross)*(rotation*(start_point-center_point))+center_point; + } - Eigen::MatrixXd _pt_trans = _pt.transpose(); + Eigen::MatrixXd act_tra_trans = arc_tra.transpose(); - return _pt_trans; + return act_tra_trans; } From 36f627f0350a5ddb0a53b7e687f50f5716538dc0 Mon Sep 17 00:00:00 2001 From: sch Date: Tue, 7 Jun 2016 16:54:01 +0900 Subject: [PATCH 12/33] ROS C++ coding style is applied to robotis_math -version2 --- .../robotis_math/robotis_linear_algebra.h | 18 +++++++-------- .../include/robotis_math/robotis_math.h | 8 +++---- .../include/robotis_math/robotis_math_base.h | 8 +++---- .../robotis_trajectory_calculator.h | 8 +++---- robotis_math/src/robotis_linear_algebra.cpp | 22 +++++++++---------- robotis_math/src/robotis_math_base.cpp | 2 +- .../src/robotis_trajectory_calculator.cpp | 2 +- 7 files changed, 34 insertions(+), 34 deletions(-) diff --git a/robotis_math/include/robotis_math/robotis_linear_algebra.h b/robotis_math/include/robotis_math/robotis_linear_algebra.h index 6751b2b..a460709 100644 --- a/robotis_math/include/robotis_math/robotis_linear_algebra.h +++ b/robotis_math/include/robotis_math/robotis_linear_algebra.h @@ -31,12 +31,12 @@ /* * robotis_linear_algebra.h * - * Created on: June 6, 2016 + * Created on: June 7, 2016 * Author: sch */ -#ifndef ROBOTIS_LINEAR_ALGEBRA_H_ -#define ROBOTIS_LINEAR_ALGEBRA_H_ +#ifndef ROBOTIS_MATH_ROBOTIS_LINEAR_ALGEBRA_H_ +#define ROBOTIS_MATH_ROBOTIS_LINEAR_ALGEBRA_H_ #include @@ -64,14 +64,14 @@ Eigen::Quaterniond convertRotationToQuaternion(Eigen::MatrixXd rotation); Eigen::MatrixXd convertQuaternionToRPY(Eigen::Quaterniond quaternion); Eigen::MatrixXd convertQuaternionToRotation(Eigen::Quaterniond quaternion); -Eigen::MatrixXd hatto(Eigen::MatrixXd matrix3d); -Eigen::MatrixXd rodrigues(Eigen::MatrixXd hat_matrix, double angle); -Eigen::MatrixXd rot2omega(Eigen::MatrixXd rotation); -Eigen::MatrixXd cross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b); -double dot(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b); +Eigen::MatrixXd calcHatto(Eigen::MatrixXd matrix3d); +Eigen::MatrixXd calcRodrigues(Eigen::MatrixXd hat_matrix, double angle); +Eigen::MatrixXd convertRotToOmega(Eigen::MatrixXd rotation); +Eigen::MatrixXd calcCross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b); +double calcInner(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b); } -#endif /* ROBOTIS_LINEAR_ALGEBRA_H_ */ +#endif /* ROBOTIS_MATH_ROBOTIS_LINEAR_ALGEBRA_H_ */ diff --git a/robotis_math/include/robotis_math/robotis_math.h b/robotis_math/include/robotis_math/robotis_math.h index caa6dc9..f78f4be 100644 --- a/robotis_math/include/robotis_math/robotis_math.h +++ b/robotis_math/include/robotis_math/robotis_math.h @@ -31,13 +31,13 @@ /* * robotis_math.h * - * Created on: June 6, 2016 + * Created on: June 7, 2016 * Author: sch */ -#ifndef ROBOTIS_MATH_H_ -#define ROBOTIS_MATH_H_ +#ifndef ROBOTIS_MATH_ROBOTIS_MATH_H_ +#define ROBOTIS_MATH_ROBOTIS_MATH_H_ #include "robotis_trajectory_calculator.h" -#endif /* ROBOTIS_MATH_H_ */ +#endif /* ROBOTIS_MATH_ROBOTIS_MATH_H_ */ diff --git a/robotis_math/include/robotis_math/robotis_math_base.h b/robotis_math/include/robotis_math/robotis_math_base.h index 14c4bc3..e54caca 100644 --- a/robotis_math/include/robotis_math/robotis_math_base.h +++ b/robotis_math/include/robotis_math/robotis_math_base.h @@ -31,12 +31,12 @@ /* * robotis_math_base.h * - * Created on: June 6, 2016 + * Created on: June 7, 2016 * Author: sch */ -#ifndef ROBOTIS_MATH_BASE_H_ -#define ROBOTIS_MATH_BASE_H_ +#ifndef ROBOTIS_MATH_ROBOTIS_MATH_BASE_H_ +#define ROBOTIS_MATH_ROBOTIS_MATH_BASE_H_ #include @@ -60,4 +60,4 @@ double sign(double x); -#endif /* ROBOTIS_MATH_BASE_H_ */ +#endif /* ROBOTIS_MATH_ROBOTIS_MATH_BASE_H_ */ diff --git a/robotis_math/include/robotis_math/robotis_trajectory_calculator.h b/robotis_math/include/robotis_math/robotis_trajectory_calculator.h index 79857df..56bf644 100644 --- a/robotis_math/include/robotis_math/robotis_trajectory_calculator.h +++ b/robotis_math/include/robotis_math/robotis_trajectory_calculator.h @@ -31,12 +31,12 @@ /* * robotis_trajectory_calculator.h * - * Created on: June 6, 2016 + * Created on: June 7, 2016 * Author: sch */ -#ifndef ROBOTIS_TRAJECTORY_CALCULATOR_H_ -#define ROBOTIS_TRAJECTORY_CALCULATOR_H_ +#ifndef ROBOTIS_MATH_ROBOTIS_TRAJECTORY_CALCULATOR_H_ +#define ROBOTIS_MATH_ROBOTIS_TRAJECTORY_CALCULATOR_H_ #include "robotis_linear_algebra.h" #include "robotis_math_base.h" @@ -62,4 +62,4 @@ Eigen::MatrixXd calcArc3dTra(double smp_time, double mov_time, } -#endif /* ROBOTIS_TRAJECTORY_CALCULATOR_H_ */ +#endif /* ROBOTIS_MATH_ROBOTIS_TRAJECTORY_CALCULATOR_H_ */ diff --git a/robotis_math/src/robotis_linear_algebra.cpp b/robotis_math/src/robotis_linear_algebra.cpp index d85d744..328089c 100644 --- a/robotis_math/src/robotis_linear_algebra.cpp +++ b/robotis_math/src/robotis_linear_algebra.cpp @@ -31,7 +31,7 @@ /* * robotis_linear_algebra.cpp * - * Created on: June 6, 2016 + * Created on: June 7, 2016 * Author: sch */ @@ -224,7 +224,7 @@ Eigen::MatrixXd convertQuaternionToRotation(Eigen::Quaterniond quaternion) return rotation; } -Eigen::MatrixXd hatto(Eigen::MatrixXd matrix3d) +Eigen::MatrixXd calcHatto(Eigen::MatrixXd matrix3d) { Eigen::MatrixXd hatto(3,3); @@ -236,7 +236,7 @@ Eigen::MatrixXd hatto(Eigen::MatrixXd matrix3d) return hatto; } -Eigen::MatrixXd rodrigues(Eigen::MatrixXd hat_matrix, double angle) +Eigen::MatrixXd calcRodrigues(Eigen::MatrixXd hat_matrix, double angle) { Eigen::MatrixXd identity = Eigen::MatrixXd::Identity(3,3); Eigen::MatrixXd rodrigues = identity+hat_matrix*sin(angle)+hat_matrix*hat_matrix*(1-cos(angle)); @@ -244,18 +244,18 @@ Eigen::MatrixXd rodrigues(Eigen::MatrixXd hat_matrix, double angle) return rodrigues; } -Eigen::MatrixXd rot2omega(Eigen::MatrixXd rotation) +Eigen::MatrixXd convertRotToOmega(Eigen::MatrixXd rotation) { double eps = 1e-12; double alpha = (rotation.coeff(0,0)+rotation.coeff(1,1)+rotation.coeff(2,2)-1.0)/2.0; double alpha_dash = fabs( alpha - 1.0 ); - Eigen::MatrixXd rot2omega(3,1); + Eigen::MatrixXd rot_to_omega(3,1); if( alpha_dash < eps ) { - rot2omega << + rot_to_omega << 0.0, 0.0, 0.0; @@ -264,18 +264,18 @@ Eigen::MatrixXd rot2omega(Eigen::MatrixXd rotation) { double theta = acos(alpha); - rot2omega << + rot_to_omega << rotation.coeff(2,1)-rotation.coeff(1,2), rotation.coeff(0,2)-rotation.coeff(2,0), rotation.coeff(1,0)-rotation.coeff(0,1); - rot2omega = 0.5*(theta/sin(theta))*rot2omega; + rot_to_omega = 0.5*(theta/sin(theta))*rot_to_omega; } - return rot2omega; + return rot_to_omega; } -Eigen::MatrixXd cross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b) +Eigen::MatrixXd calcCross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b) { Eigen::MatrixXd cross(3,1); @@ -287,7 +287,7 @@ Eigen::MatrixXd cross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b) return cross; } -double dot(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b) +double calcInner(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b) { return vector3d_a.dot(vector3d_b); } diff --git a/robotis_math/src/robotis_math_base.cpp b/robotis_math/src/robotis_math_base.cpp index 60eb1f4..bd37d51 100644 --- a/robotis_math/src/robotis_math_base.cpp +++ b/robotis_math/src/robotis_math_base.cpp @@ -31,7 +31,7 @@ /* * robotis_math_base.cpp * - * Created on: June 6, 2016 + * Created on: June 7, 2016 * Author: sch */ diff --git a/robotis_math/src/robotis_trajectory_calculator.cpp b/robotis_math/src/robotis_trajectory_calculator.cpp index 4b776aa..5f66ffc 100644 --- a/robotis_math/src/robotis_trajectory_calculator.cpp +++ b/robotis_math/src/robotis_trajectory_calculator.cpp @@ -31,7 +31,7 @@ /* * robotis_trajectory_calculator.cpp * - * Created on: June 6, 2016 + * Created on: June 7, 2016 * Author: sch */ From 65dec07b1a7f8df6c36baee71afa940c63412ee6 Mon Sep 17 00:00:00 2001 From: Jay-Song Date: Tue, 14 Jun 2016 11:05:28 +0900 Subject: [PATCH 13/33] Add getTranslation4D Func --- .../include/robotis_math/robotis_linear_algebra.h | 1 + robotis_math/src/robotis_linear_algebra.cpp | 13 +++++++++++++ 2 files changed, 14 insertions(+) diff --git a/robotis_math/include/robotis_math/robotis_linear_algebra.h b/robotis_math/include/robotis_math/robotis_linear_algebra.h index a460709..9c62268 100644 --- a/robotis_math/include/robotis_math/robotis_linear_algebra.h +++ b/robotis_math/include/robotis_math/robotis_linear_algebra.h @@ -56,6 +56,7 @@ Eigen::MatrixXd getRotationX(double angle); Eigen::MatrixXd getRotationY(double angle); Eigen::MatrixXd getRotationZ(double angle); Eigen::MatrixXd getRotation4d(double roll, double pitch, double yaw); +Eigen::MatrixXd getTranslation4D(double position_x, double position_y, double position_z); Eigen::MatrixXd convertRotationToRPY(Eigen::MatrixXd rotation); Eigen::MatrixXd convertRPYToRotation(double roll, double pitch, double yaw); diff --git a/robotis_math/src/robotis_linear_algebra.cpp b/robotis_math/src/robotis_linear_algebra.cpp index 328089c..7683810 100644 --- a/robotis_math/src/robotis_linear_algebra.cpp +++ b/robotis_math/src/robotis_linear_algebra.cpp @@ -168,6 +168,19 @@ Eigen::MatrixXd getRotation4d(double roll, double pitch, double yaw ) return mat_rpy; } +Eigen::MatrixXd getTranslation4D(double position_x, double position_y, double position_z) +{ + Eigen::MatrixXd mat_translation(4,4); + + mat_translation << + 1, 0, 0, position_x, + 0, 1, 0, position_y, + 0, 0, 1, position_z, + 0, 0, 0, 1; + + return mat_translation; +} + Eigen::MatrixXd convertRotationToRPY(Eigen::MatrixXd rotation) { Eigen::MatrixXd rpy = Eigen::MatrixXd::Zero(3,1); From b6ab5951b9dc1887160f8ee72bf84512236064cd Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Tue, 14 Jun 2016 11:10:54 +0900 Subject: [PATCH 14/33] - moved robotis_controller_msgs package to ROBOTIS-Framework-msgs repository --- robotis_controller_msgs/CMakeLists.txt | 48 ------------------- .../msg/JointCtrlModule.msg | 2 - robotis_controller_msgs/msg/StatusMsg.msg | 10 ---- robotis_controller_msgs/msg/SyncWriteItem.msg | 3 -- robotis_controller_msgs/package.xml | 24 ---------- .../srv/GetJointModule.srv | 4 -- 6 files changed, 91 deletions(-) delete mode 100644 robotis_controller_msgs/CMakeLists.txt delete mode 100644 robotis_controller_msgs/msg/JointCtrlModule.msg delete mode 100644 robotis_controller_msgs/msg/StatusMsg.msg delete mode 100644 robotis_controller_msgs/msg/SyncWriteItem.msg delete mode 100644 robotis_controller_msgs/package.xml delete mode 100644 robotis_controller_msgs/srv/GetJointModule.srv diff --git a/robotis_controller_msgs/CMakeLists.txt b/robotis_controller_msgs/CMakeLists.txt deleted file mode 100644 index d9f015c..0000000 --- a/robotis_controller_msgs/CMakeLists.txt +++ /dev/null @@ -1,48 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(robotis_controller_msgs) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - sensor_msgs - std_msgs - message_generation -) - -## Generate messages in the 'msg' folder -add_message_files( - FILES - SyncWriteItem.msg - JointCtrlModule.msg - StatusMsg.msg -) - -## Generate services in the 'srv' folder -add_service_files( - FILES - GetJointModule.srv -) - # Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -generate_messages( - DEPENDENCIES - std_msgs - sensor_msgs -) - -catkin_package( -# INCLUDE_DIRS include - CATKIN_DEPENDS sensor_msgs std_msgs -) - diff --git a/robotis_controller_msgs/msg/JointCtrlModule.msg b/robotis_controller_msgs/msg/JointCtrlModule.msg deleted file mode 100644 index b91eb4d..0000000 --- a/robotis_controller_msgs/msg/JointCtrlModule.msg +++ /dev/null @@ -1,2 +0,0 @@ -string[] joint_name -string[] module_name \ No newline at end of file diff --git a/robotis_controller_msgs/msg/StatusMsg.msg b/robotis_controller_msgs/msg/StatusMsg.msg deleted file mode 100644 index 47b706c..0000000 --- a/robotis_controller_msgs/msg/StatusMsg.msg +++ /dev/null @@ -1,10 +0,0 @@ -# Status Constants -uint8 STATUS_UNKNOWN = 0 -uint8 STATUS_INFO = 1 -uint8 STATUS_WARN = 2 -uint8 STATUS_ERROR = 3 - -std_msgs/Header header -uint8 type -string module_name -string status_msg \ No newline at end of file diff --git a/robotis_controller_msgs/msg/SyncWriteItem.msg b/robotis_controller_msgs/msg/SyncWriteItem.msg deleted file mode 100644 index 4d602b6..0000000 --- a/robotis_controller_msgs/msg/SyncWriteItem.msg +++ /dev/null @@ -1,3 +0,0 @@ -string item_name -string[] joint_name -uint32[] value \ No newline at end of file diff --git a/robotis_controller_msgs/package.xml b/robotis_controller_msgs/package.xml deleted file mode 100644 index 8d54012..0000000 --- a/robotis_controller_msgs/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - robotis_controller_msgs - 0.1.0 - The robotis_controller_msgs package - robotis - - BSD - - - - ROBOTIS - - catkin - - sensor_msgs - std_msgs - message_generation - - sensor_msgs - std_msgs - message_runtime - - \ No newline at end of file diff --git a/robotis_controller_msgs/srv/GetJointModule.srv b/robotis_controller_msgs/srv/GetJointModule.srv deleted file mode 100644 index bedde91..0000000 --- a/robotis_controller_msgs/srv/GetJointModule.srv +++ /dev/null @@ -1,4 +0,0 @@ -string[] joint_name ---- -string[] joint_name -string[] module_name \ No newline at end of file From 509c2cf483bb5517b3787ee888e405469511ed46 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Thu, 23 Jun 2016 14:31:44 +0900 Subject: [PATCH 15/33] - startTimer() : after bulkread txpacket(), need some sleep() --- .../src/robotis_controller/robotis_controller.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 8d8275d..340ee50 100644 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -631,6 +631,8 @@ void RobotisController::startTimer() it->second->txPacket(); } + usleep(8 * 1000); + int error; struct sched_param param; pthread_attr_t attr; From a3e386e14e17252c1e54405db074ee58883383b8 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Tue, 28 Jun 2016 15:51:49 +0900 Subject: [PATCH 16/33] - fixed typos - changed ROS_INFO -> fprintf (for processing speed) --- .../robotis_controller/robotis_controller.cpp | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 340ee50..30b565e 100644 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -433,7 +433,7 @@ void RobotisController::initializeDevice(const std::string init_file_path) if (dxl == NULL) { - ROS_WARN("Joint [%s] does not found.", joint_name.c_str()); + ROS_WARN("Joint [%s] was not found.", joint_name.c_str()); continue; } if (DEBUG_PRINT) @@ -451,7 +451,7 @@ void RobotisController::initializeDevice(const std::string init_file_path) ControlTableItem *item = dxl->ctrl_table_[item_name]; if (item == NULL) { - ROS_WARN("Control Item [%s] does not found.", item_name.c_str()); + ROS_WARN("Control Item [%s] was not found.", item_name.c_str()); continue; } @@ -596,9 +596,9 @@ void *RobotisController::timerThread(void *param) { if (controller->DEBUG_PRINT == true) { - ROS_WARN("[RobotisController::ThreadProc] NEXT TIME < CURR TIME.. (%f)[%ld.%09ld / %ld.%09ld]", - delta_nsec / 1000000.0, (long )next_time.tv_sec, (long )next_time.tv_nsec, - (long )curr_time.tv_sec, (long )curr_time.tv_nsec); + fprintf(stderr, "[RobotisController::ThreadProc] NEXT TIME < CURR TIME.. (%f)[%ld.%09ld / %ld.%09ld]", + delta_nsec / 1000000.0, (long )next_time.tv_sec, (long )next_time.tv_nsec, + (long )curr_time.tv_sec, (long )curr_time.tv_nsec); } // next_time = curr_time + 3 msec @@ -866,7 +866,7 @@ void RobotisController::process() if (DEBUG_PRINT) { time_duration = ros::Time::now() - start_time; - ROS_INFO("(%2.6f) BulkRead Rx & update state", time_duration.nsec * 0.000001); + fprintf(stderr, "(%2.6f) BulkRead Rx & update state", time_duration.nsec * 0.000001); } // Call SensorModule Process() @@ -885,7 +885,7 @@ void RobotisController::process() if (DEBUG_PRINT) { time_duration = ros::Time::now() - start_time; - ROS_INFO("(%2.6f) SensorModule Process() & save result", time_duration.nsec * 0.000001); + fprintf(stderr, "(%2.6f) SensorModule Process() & save result", time_duration.nsec * 0.000001); } if (controller_mode_ == MotionModuleMode) @@ -917,7 +917,7 @@ void RobotisController::process() if (result_state == NULL) { - ROS_INFO("[%s] %s", (*module_it)->getModuleName().c_str(), joint_name.c_str()); + fprintf(stderr, "[%s] %s", (*module_it)->getModuleName().c_str(), joint_name.c_str()); continue; } @@ -1005,7 +1005,7 @@ void RobotisController::process() if (DEBUG_PRINT) { time_duration = ros::Time::now() - start_time; - ROS_INFO("(%2.6f) MotionModule Process() & save result", time_duration.nsec * 0.000001); + fprintf(stderr, "(%2.6f) MotionModule Process() & save result", time_duration.nsec * 0.000001); } // SyncWrite @@ -1087,7 +1087,7 @@ void RobotisController::process() if (DEBUG_PRINT) { time_duration = ros::Time::now() - start_time; - ROS_INFO("(%2.6f) SyncWrite & BulkRead Tx", time_duration.nsec * 0.000001); + fprintf(stderr, "(%2.6f) SyncWrite & BulkRead Tx", time_duration.nsec * 0.000001); } // publish present & goal position @@ -1114,7 +1114,7 @@ void RobotisController::process() if (DEBUG_PRINT) { time_duration = ros::Time::now() - start_time; - ROS_WARN("(%2.6f) Process() DONE", time_duration.nsec * 0.000001); + fprintf(stderr, "(%2.6f) Process() DONE", time_duration.nsec * 0.000001); } is_process_running = false; From 6318166216570f952fe8f00619faae48248dc9e5 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Tue, 28 Jun 2016 16:38:53 +0900 Subject: [PATCH 17/33] adjusted position min/max value. (MX-28, XM-430) --- robotis_device/devices/dynamixel/MX-28.device | 2 +- robotis_device/devices/dynamixel/XM-430.device | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/robotis_device/devices/dynamixel/MX-28.device b/robotis_device/devices/dynamixel/MX-28.device index f1cfc21..ebcad6e 100644 --- a/robotis_device/devices/dynamixel/MX-28.device +++ b/robotis_device/devices/dynamixel/MX-28.device @@ -47,7 +47,7 @@ position_p_gain_item_name = position_p_gain 30 | goal_position | 2 | RW | RAM | -28672 | 28672 | Y 32 | goal_velocity | 2 | RW | RAM | 0 | 1023 | N 34 | goal_torque | 2 | RW | RAM | 0 | 1023 | N - 36 | present_position | 2 | R | RAM | 0 | 4095 | N + 36 | present_position | 2 | R | RAM | -32768 | 32767 | Y 38 | present_velocity | 2 | R | RAM | 0 | 2048 | N 40 | present_load | 2 | R | RAM | 0 | 2048 | N 42 | present_voltage | 1 | R | RAM | 50 | 250 | N diff --git a/robotis_device/devices/dynamixel/XM-430.device b/robotis_device/devices/dynamixel/XM-430.device index 9f17d37..7fcec12 100644 --- a/robotis_device/devices/dynamixel/XM-430.device +++ b/robotis_device/devices/dynamixel/XM-430.device @@ -59,14 +59,14 @@ position_p_gain_item_name = position_p_gain 104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N 108 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N 112 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N - 116 | goal_position | 4 | RW | RAM | 0 | 4095 | N + 116 | goal_position | 4 | RW | RAM | -1048575 | 1048575 | Y 120 | realtime_tick | 2 | R | RAM | 0 | 32767 | N 122 | moving | 1 | R | RAM | 0 | 1 | N 123 | moving_status | 1 | R | RAM | 0 | 255 | N 124 | present_pwm | 2 | R | RAM | 0 | 885 | N 126 | present_current | 2 | R | RAM | 0 | 1193 | N 128 | present_velocity | 4 | R | RAM | 0 | 1023 | N - 132 | present_position | 4 | R | RAM | 0 | 4095 | N + 132 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y 136 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N 140 | position_trajectory | 4 | R | RAM | 0 | 4095 | N 144 | present_input_voltage | 2 | R | RAM | 95 | 160 | N From e3381ca4240eff975758408e2704f0976d694d8e Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Tue, 28 Jun 2016 17:23:18 +0900 Subject: [PATCH 18/33] - added device file for MX-64 / MX-106 --- .../devices/dynamixel/MX-106.device | 63 +++++++++++++++++++ robotis_device/devices/dynamixel/MX-64.device | 63 +++++++++++++++++++ 2 files changed, 126 insertions(+) create mode 100644 robotis_device/devices/dynamixel/MX-106.device create mode 100644 robotis_device/devices/dynamixel/MX-64.device diff --git a/robotis_device/devices/dynamixel/MX-106.device b/robotis_device/devices/dynamixel/MX-106.device new file mode 100644 index 0000000..23b16f1 --- /dev/null +++ b/robotis_device/devices/dynamixel/MX-106.device @@ -0,0 +1,63 @@ +[device info] +model_name = MX-106 +device_type = dynamixel + +[type info] +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 2 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 3 | ID | 1 | RW | EEPROM | 0 | 252 | N + 4 | baudrate | 1 | RW | EEPROM | 0 | 252 | N + 5 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 6 | CW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N + 8 | CCW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | max_temperature_limit | 1 | RW | EEPROM | 0 | 99 | N + 12 | min_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N + 13 | max_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N + 14 | max_torque | 2 | RW | EEPROM | 0 | 1023 | N + 16 | status_return_level | 1 | RW | EEPROM | 0 | 2 | N + 17 | alarm_LED | 1 | RW | EEPROM | 0 | 127 | N + 18 | alarm_shutdown | 1 | RW | EEPROM | 0 | 127 | N + 20 | multi_turn_offset | 2 | RW | EEPROM | -26624 | 26624 | Y + 22 | resolution_dividor | 1 | RW | EEPROM | 1 | 255 | N + 24 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 25 | LED | 1 | RW | RAM | 0 | 1 | N + 26 | position_d_gain | 1 | RW | RAM | 0 | 254 | N + 27 | position_i_gain | 1 | RW | RAM | 0 | 254 | N + 28 | position_p_gain | 1 | RW | RAM | 0 | 254 | N + 30 | goal_position | 2 | RW | RAM | -28672 | 28672 | Y + 32 | goal_velocity | 2 | RW | RAM | 0 | 1023 | N + 34 | goal_torque | 2 | RW | RAM | 0 | 1023 | N + 36 | present_position | 2 | R | RAM | -32768 | 32767 | Y + 38 | present_velocity | 2 | R | RAM | 0 | 2048 | N + 40 | present_load | 2 | R | RAM | 0 | 2048 | N + 42 | present_voltage | 1 | R | RAM | 50 | 250 | N + 43 | present_temperature | 1 | R | RAM | 0 | 99 | N + 44 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 46 | is_moving | 1 | R | RAM | 0 | 1 | N + 47 | EEPROM_lock | 1 | RW | RAM | 0 | 1 | N + 48 | punch | 2 | RW | RAM | 0 | 1023 | N + 68 | current_consumption | 2 | RW | RAM | 0 | 4095 | N + 70 | torque_control_mode | 1 | RW | RAM | 0 | 1 | N + 71 | torque_control_goal | 2 | RW | RAM | 0 | 2047 | N + 73 | goal_acceleration | 1 | RW | RAM | 0 | 254 | N + diff --git a/robotis_device/devices/dynamixel/MX-64.device b/robotis_device/devices/dynamixel/MX-64.device new file mode 100644 index 0000000..2988c83 --- /dev/null +++ b/robotis_device/devices/dynamixel/MX-64.device @@ -0,0 +1,63 @@ +[device info] +model_name = MX-64 +device_type = dynamixel + +[type info] +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 2 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 3 | ID | 1 | RW | EEPROM | 0 | 252 | N + 4 | baudrate | 1 | RW | EEPROM | 0 | 252 | N + 5 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 6 | CW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N + 8 | CCW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | max_temperature_limit | 1 | RW | EEPROM | 0 | 99 | N + 12 | min_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N + 13 | max_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N + 14 | max_torque | 2 | RW | EEPROM | 0 | 1023 | N + 16 | status_return_level | 1 | RW | EEPROM | 0 | 2 | N + 17 | alarm_LED | 1 | RW | EEPROM | 0 | 127 | N + 18 | alarm_shutdown | 1 | RW | EEPROM | 0 | 127 | N + 20 | multi_turn_offset | 2 | RW | EEPROM | -26624 | 26624 | Y + 22 | resolution_dividor | 1 | RW | EEPROM | 1 | 255 | N + 24 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 25 | LED | 1 | RW | RAM | 0 | 1 | N + 26 | position_d_gain | 1 | RW | RAM | 0 | 254 | N + 27 | position_i_gain | 1 | RW | RAM | 0 | 254 | N + 28 | position_p_gain | 1 | RW | RAM | 0 | 254 | N + 30 | goal_position | 2 | RW | RAM | -28672 | 28672 | Y + 32 | goal_velocity | 2 | RW | RAM | 0 | 1023 | N + 34 | goal_torque | 2 | RW | RAM | 0 | 1023 | N + 36 | present_position | 2 | R | RAM | -32768 | 32767 | Y + 38 | present_velocity | 2 | R | RAM | 0 | 2048 | N + 40 | present_load | 2 | R | RAM | 0 | 2048 | N + 42 | present_voltage | 1 | R | RAM | 50 | 250 | N + 43 | present_temperature | 1 | R | RAM | 0 | 99 | N + 44 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 46 | is_moving | 1 | R | RAM | 0 | 1 | N + 47 | EEPROM_lock | 1 | RW | RAM | 0 | 1 | N + 48 | punch | 2 | RW | RAM | 0 | 1023 | N + 68 | current_consumption | 2 | RW | RAM | 0 | 4095 | N + 70 | torque_control_mode | 1 | RW | RAM | 0 | 1 | N + 71 | torque_control_goal | 2 | RW | RAM | 0 | 2047 | N + 73 | goal_acceleration | 1 | RW | RAM | 0 | 254 | N + From 68f2ac81f05f33b86bf34594a94eaaf513df5f3e Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Wed, 6 Jul 2016 16:26:40 +0900 Subject: [PATCH 19/33] - modified torque control code --- .../robotis_controller/robotis_controller.h | 4 +- .../robotis_controller/robotis_controller.cpp | 62 ++++++++++++++----- .../devices/dynamixel/H42-20-S300-R.device | 2 +- .../devices/dynamixel/H54-100-S500-R.device | 2 +- .../devices/dynamixel/H54-200-B500-R.device | 2 +- .../devices/dynamixel/H54-200-S500-R.device | 2 +- .../devices/dynamixel/L54-30-S400-R.device | 2 - .../devices/dynamixel/L54-30-S500-R.device | 2 - .../devices/dynamixel/L54-50-S290-R.device | 2 - .../devices/dynamixel/L54-50-S500-R.device | 2 - .../devices/dynamixel/M42-10-S260-R.device | 2 - .../devices/dynamixel/M54-40-S250-R.device | 2 - .../devices/dynamixel/M54-60-S250-R.device | 2 - .../devices/dynamixel/XM-430.device | 2 +- .../include/robotis_device/dynamixel.h | 8 +-- .../include/robotis_device/dynamixel_state.h | 4 +- .../src/robotis_device/dynamixel.cpp | 16 ++--- robotis_device/src/robotis_device/robot.cpp | 8 +-- 18 files changed, 72 insertions(+), 54 deletions(-) diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h index e2c8b64..e6befef 100644 --- a/robotis_controller/include/robotis_controller/robotis_controller.h +++ b/robotis_controller/include/robotis_controller/robotis_controller.h @@ -115,7 +115,9 @@ class RobotisController : public Singleton ros::Publisher present_joint_state_pub_; ros::Publisher current_module_pub_; - std::map gazebo_joint_pub_; + std::map gazebo_joint_position_pub_; + std::map gazebo_joint_velocity_pub_; + std::map gazebo_joint_effort_pub_; static void *timerThread(void *param); diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 30b565e..cd8fd0f 100644 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -159,8 +159,8 @@ void RobotisController::initializeSyncWrite() else if ((dxl->present_current_item_ != 0) && (dxl->bulk_read_items_[i]->item_name_ == dxl->present_current_item_->item_name_)) { - dxl->dxl_state_->present_current_ = dxl->convertValue2Current(read_data); - dxl->dxl_state_->goal_current_ = dxl->dxl_state_->present_current_; + dxl->dxl_state_->present_current_ = dxl->convertValue2Torque(read_data); + dxl->dxl_state_->goal_torque_ = dxl->dxl_state_->present_current_; } } } @@ -557,8 +557,12 @@ void RobotisController::msgQueueThread() { for (std::map::iterator it = robot_->dxls_.begin(); it != robot_->dxls_.end(); it++) { - gazebo_joint_pub_[it->first] = ros_node.advertise( - "/" + gazebo_robot_name_ + "/" + it->first + "_pos/command", 1); + gazebo_joint_position_pub_[it->first] = ros_node.advertise( + "/" + gazebo_robot_name_ + "/" + it->first + "_position/command", 1); + gazebo_joint_velocity_pub_[it->first] = ros_node.advertise( + "/" + gazebo_robot_name_ + "/" + it->first + "_velocity/command", 1); + gazebo_joint_effort_pub_[it->first] = ros_node.advertise( + "/" + gazebo_robot_name_ + "/" + it->first + "_effort/command", 1); } } @@ -814,13 +818,13 @@ void RobotisController::process() else if (dxl->present_velocity_item_ != 0 && item->item_name_ == dxl->present_velocity_item_->item_name_) dxl->dxl_state_->present_velocity_ = dxl->convertValue2Velocity(data); else if (dxl->present_current_item_ != 0 && item->item_name_ == dxl->present_current_item_->item_name_) - dxl->dxl_state_->present_current_ = dxl->convertValue2Current(data); + dxl->dxl_state_->present_current_ = dxl->convertValue2Torque(data); else if (dxl->goal_position_item_ != 0 && item->item_name_ == dxl->goal_position_item_->item_name_) dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset else if (dxl->goal_velocity_item_ != 0 && item->item_name_ == dxl->goal_velocity_item_->item_name_) dxl->dxl_state_->goal_velocity_ = dxl->convertValue2Velocity(data); else if (dxl->goal_current_item_ != 0 && item->item_name_ == dxl->goal_current_item_->item_name_) - dxl->dxl_state_->goal_current_ = dxl->convertValue2Current(data); + dxl->dxl_state_->goal_torque_ = dxl->convertValue2Torque(data); else dxl->dxl_state_->bulk_read_table_[item->item_name_] = data; } @@ -982,11 +986,11 @@ void RobotisController::process() } else if ((*module_it)->getControlMode() == CurrentControl) { - dxl_state->goal_current_ = result_state->goal_current_; + dxl_state->goal_torque_ = result_state->goal_torque_; if (gazebo_mode_ == false) { - uint32_t curr_data = dxl->convertCurrent2Value(dxl_state->goal_current_); + uint32_t curr_data = dxl->convertTorque2Value(dxl_state->goal_torque_); uint8_t sync_write_data[2] = { 0 }; sync_write_data[0] = DXL_LOBYTE(curr_data); sync_write_data[1] = DXL_HIBYTE(curr_data); @@ -1041,13 +1045,39 @@ void RobotisController::process() } else if (gazebo_mode_ == true) { - std_msgs::Float64 joint_msg; - - for (std::map::iterator dxl_it = robot_->dxls_.begin(); dxl_it != robot_->dxls_.end(); - dxl_it++) + for (std::list::iterator module_it = motion_modules_.begin(); module_it != motion_modules_.end(); module_it++) { - joint_msg.data = dxl_it->second->dxl_state_->goal_position_; - gazebo_joint_pub_[dxl_it->first].publish(joint_msg); + if ((*module_it)->getModuleEnable() == false) + continue; + + std_msgs::Float64 joint_msg; + + for (std::map::iterator dxl_it = robot_->dxls_.begin(); dxl_it != robot_->dxls_.end(); + dxl_it++) + { + std::string joint_name = dxl_it->first; + Dynamixel *dxl = dxl_it->second; + DynamixelState *dxl_state = dxl_it->second->dxl_state_; + + if (dxl->ctrl_module_name_ == (*module_it)->getModuleName()) + { + if ((*module_it)->getControlMode() == PositionControl) + { + joint_msg.data = dxl_state->goal_position_; + gazebo_joint_position_pub_[joint_name].publish(joint_msg); + } + else if ((*module_it)->getControlMode() == VelocityControl) + { + joint_msg.data = dxl_state->goal_velocity_; + gazebo_joint_velocity_pub_[joint_name].publish(joint_msg); + } + else if ((*module_it)->getControlMode() == CurrentControl) + { + joint_msg.data = dxl_state->goal_torque_; + gazebo_joint_effort_pub_[joint_name].publish(joint_msg); + } + } + } } } } @@ -1104,7 +1134,7 @@ void RobotisController::process() goal_state.name.push_back(joint_name); goal_state.position.push_back(dxl->dxl_state_->goal_position_); goal_state.velocity.push_back(dxl->dxl_state_->goal_velocity_); - goal_state.effort.push_back(dxl->dxl_state_->goal_current_); + goal_state.effort.push_back(dxl->dxl_state_->goal_torque_); } // -> publish present joint_states & goal joint states topic @@ -1510,7 +1540,7 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) } else if (mode == CurrentControl) { - uint32_t curr_data = dxl->convertCurrent2Value(dxl->dxl_state_->goal_current_); + uint32_t curr_data = dxl->convertTorque2Value(dxl->dxl_state_->goal_torque_); uint8_t sync_write_data[4] = { 0 }; sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(curr_data)); sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(curr_data)); diff --git a/robotis_device/devices/dynamixel/H42-20-S300-R.device b/robotis_device/devices/dynamixel/H42-20-S300-R.device index 1ed385f..e348f42 100644 --- a/robotis_device/devices/dynamixel/H42-20-S300-R.device +++ b/robotis_device/devices/dynamixel/H42-20-S300-R.device @@ -3,7 +3,7 @@ model_name = H42-20-S300-R device_type = dynamixel [type info] -current_ratio = 4.0283203125 +torque_to_current_value_ratio = 27.15146 value_of_0_radian_position = 0 value_of_min_radian_position = -151900 diff --git a/robotis_device/devices/dynamixel/H54-100-S500-R.device b/robotis_device/devices/dynamixel/H54-100-S500-R.device index 09ef828..cdf81d0 100644 --- a/robotis_device/devices/dynamixel/H54-100-S500-R.device +++ b/robotis_device/devices/dynamixel/H54-100-S500-R.device @@ -3,7 +3,7 @@ model_name = H54-100-S500-R device_type = dynamixel [type info] -current_ratio = 16.11328125 +torque_to_current_value_ratio = 9.66026 value_of_0_radian_position = 0 value_of_min_radian_position = -250950 diff --git a/robotis_device/devices/dynamixel/H54-200-B500-R.device b/robotis_device/devices/dynamixel/H54-200-B500-R.device index 8791d7c..d002f97 100644 --- a/robotis_device/devices/dynamixel/H54-200-B500-R.device +++ b/robotis_device/devices/dynamixel/H54-200-B500-R.device @@ -3,7 +3,7 @@ model_name = H54-200-B500-R device_type = dynamixel [type info] -current_ratio = 16.11328125 +torque_to_current_value_ratio = 9.09201 value_of_0_radian_position = 0 value_of_min_radian_position = -250950 diff --git a/robotis_device/devices/dynamixel/H54-200-S500-R.device b/robotis_device/devices/dynamixel/H54-200-S500-R.device index dba268c..7e18900 100644 --- a/robotis_device/devices/dynamixel/H54-200-S500-R.device +++ b/robotis_device/devices/dynamixel/H54-200-S500-R.device @@ -3,7 +3,7 @@ model_name = H54-200-S500-R device_type = dynamixel [type info] -current_ratio = 16.11328125 +torque_to_current_value_ratio = 9.09201 value_of_0_radian_position = 0 value_of_min_radian_position = -250950 diff --git a/robotis_device/devices/dynamixel/L54-30-S400-R.device b/robotis_device/devices/dynamixel/L54-30-S400-R.device index 8cdcff3..d684e67 100644 --- a/robotis_device/devices/dynamixel/L54-30-S400-R.device +++ b/robotis_device/devices/dynamixel/L54-30-S400-R.device @@ -3,8 +3,6 @@ model_name = L54-30-S400-R device_type = dynamixel [type info] -current_ratio = 16.11328125 - value_of_0_radian_position = 0 value_of_min_radian_position = -144198 value_of_max_radian_position = 144198 diff --git a/robotis_device/devices/dynamixel/L54-30-S500-R.device b/robotis_device/devices/dynamixel/L54-30-S500-R.device index db5a0b4..68848a2 100644 --- a/robotis_device/devices/dynamixel/L54-30-S500-R.device +++ b/robotis_device/devices/dynamixel/L54-30-S500-R.device @@ -3,8 +3,6 @@ model_name = L54-30-S500-R device_type = dynamixel [type info] -current_ratio = 16.11328125 - value_of_0_radian_position = 0 value_of_min_radian_position = -180684 value_of_max_radian_position = 180684 diff --git a/robotis_device/devices/dynamixel/L54-50-S290-R.device b/robotis_device/devices/dynamixel/L54-50-S290-R.device index cbd0453..3cbd843 100644 --- a/robotis_device/devices/dynamixel/L54-50-S290-R.device +++ b/robotis_device/devices/dynamixel/L54-50-S290-R.device @@ -3,8 +3,6 @@ model_name = L54-50-S290-R device_type = dynamixel [type info] -current_ratio = 16.11328125 - value_of_0_radian_position = 0 value_of_min_radian_position = -103860 value_of_max_radian_position = 103860 diff --git a/robotis_device/devices/dynamixel/L54-50-S500-R.device b/robotis_device/devices/dynamixel/L54-50-S500-R.device index d72f2ef..bfbf0c6 100644 --- a/robotis_device/devices/dynamixel/L54-50-S500-R.device +++ b/robotis_device/devices/dynamixel/L54-50-S500-R.device @@ -3,8 +3,6 @@ model_name = L54-50-S500-R device_type = dynamixel [type info] -current_ratio = 16.11328125 - value_of_0_radian_position = 0 value_of_min_radian_position = -180684 value_of_max_radian_position = 180684 diff --git a/robotis_device/devices/dynamixel/M42-10-S260-R.device b/robotis_device/devices/dynamixel/M42-10-S260-R.device index 8c96988..f21bb44 100644 --- a/robotis_device/devices/dynamixel/M42-10-S260-R.device +++ b/robotis_device/devices/dynamixel/M42-10-S260-R.device @@ -3,8 +3,6 @@ model_name = M42-10-S260-R device_type = dynamixel [type info] -current_ratio = 4.0283203125 - value_of_0_radian_position = 0 value_of_min_radian_position = -131584 value_of_max_radian_position = 131584 diff --git a/robotis_device/devices/dynamixel/M54-40-S250-R.device b/robotis_device/devices/dynamixel/M54-40-S250-R.device index 2bc7c1c..d9424ad 100644 --- a/robotis_device/devices/dynamixel/M54-40-S250-R.device +++ b/robotis_device/devices/dynamixel/M54-40-S250-R.device @@ -3,8 +3,6 @@ model_name = M54-40-S250-R device_type = dynamixel [type info] -current_ratio = 16.11328125 - value_of_0_radian_position = 0 value_of_min_radian_position = -125700 value_of_max_radian_position = 125700 diff --git a/robotis_device/devices/dynamixel/M54-60-S250-R.device b/robotis_device/devices/dynamixel/M54-60-S250-R.device index 72382de..284cac9 100644 --- a/robotis_device/devices/dynamixel/M54-60-S250-R.device +++ b/robotis_device/devices/dynamixel/M54-60-S250-R.device @@ -3,8 +3,6 @@ model_name = M54-60-S250-R device_type = dynamixel [type info] -current_ratio = 16.11328125 - value_of_0_radian_position = 0 value_of_min_radian_position = -125700 value_of_max_radian_position = 125700 diff --git a/robotis_device/devices/dynamixel/XM-430.device b/robotis_device/devices/dynamixel/XM-430.device index 7fcec12..3f33b73 100644 --- a/robotis_device/devices/dynamixel/XM-430.device +++ b/robotis_device/devices/dynamixel/XM-430.device @@ -3,7 +3,7 @@ model_name = XM-430 device_type = dynamixel [type info] -current_ratio = 2.69 +torque_to_current_value_ratio = 178.842161 value_of_0_radian_position = 2048 value_of_min_radian_position = 0 diff --git a/robotis_device/include/robotis_device/dynamixel.h b/robotis_device/include/robotis_device/dynamixel.h index 916f4c5..349020a 100644 --- a/robotis_device/include/robotis_device/dynamixel.h +++ b/robotis_device/include/robotis_device/dynamixel.h @@ -56,8 +56,8 @@ class Dynamixel : public Device std::string ctrl_module_name_; DynamixelState *dxl_state_; - double current_ratio_; - double velocity_ratio_; + double velocity_to_value_ratio_; + double torque_to_current_value_ratio_; int32_t value_of_0_radian_position_; int32_t value_of_min_radian_position_; @@ -82,8 +82,8 @@ class Dynamixel : public Device double convertValue2Velocity(int32_t value); int32_t convertVelocity2Value(double velocity); - double convertValue2Current(int16_t value); - int16_t convertCurrent2Value(double current); + double convertValue2Torque(int16_t value); + int16_t convertTorque2Value(double torque); }; } diff --git a/robotis_device/include/robotis_device/dynamixel_state.h b/robotis_device/include/robotis_device/dynamixel_state.h index b5f4d4d..833c0f3 100644 --- a/robotis_device/include/robotis_device/dynamixel_state.h +++ b/robotis_device/include/robotis_device/dynamixel_state.h @@ -58,7 +58,7 @@ class DynamixelState double present_current_; double goal_position_; double goal_velocity_; - double goal_current_; + double goal_torque_; double position_p_gain_; std::map bulk_read_table_; @@ -72,7 +72,7 @@ class DynamixelState present_current_(0.0), goal_position_(0.0), goal_velocity_(0.0), - goal_current_(0.0), + goal_torque_(0.0), position_p_gain_(0.0), position_offset_(0.0) { diff --git a/robotis_device/src/robotis_device/dynamixel.cpp b/robotis_device/src/robotis_device/dynamixel.cpp index c90e4eb..ba316b2 100644 --- a/robotis_device/src/robotis_device/dynamixel.cpp +++ b/robotis_device/src/robotis_device/dynamixel.cpp @@ -41,8 +41,8 @@ using namespace robotis_framework; Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version) : ctrl_module_name_("none"), - current_ratio_(1.0), - velocity_ratio_(1.0), + torque_to_current_value_ratio_(1.0), + velocity_to_value_ratio_(1.0), value_of_0_radian_position_(0), value_of_min_radian_position_(0), value_of_max_radian_position_(0), @@ -128,20 +128,20 @@ int32_t Dynamixel::convertRadian2Value(double radian) double Dynamixel::convertValue2Velocity(int32_t value) { - return (double) value * velocity_ratio_; + return (double) value / velocity_to_value_ratio_; } int32_t Dynamixel::convertVelocity2Value(double velocity) { - return (int32_t) (velocity / velocity_ratio_);; + return (int32_t) (velocity * velocity_to_value_ratio_);; } -double Dynamixel::convertValue2Current(int16_t value) +double Dynamixel::convertValue2Torque(int16_t value) { - return (double) value * current_ratio_; + return (double) value / torque_to_current_value_ratio_; } -int16_t Dynamixel::convertCurrent2Value(double current) +int16_t Dynamixel::convertTorque2Value(double torque) { - return (int16_t) (current / current_ratio_); + return (int16_t) (torque * torque_to_current_value_ratio_); } diff --git a/robotis_device/src/robotis_device/robot.cpp b/robotis_device/src/robotis_device/robot.cpp index 1ae3fb9..900a9b9 100644 --- a/robotis_device/src/robotis_device/robot.cpp +++ b/robotis_device/src/robotis_device/robot.cpp @@ -366,10 +366,10 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float if (tokens.size() != 2) continue; - if (tokens[0] == "current_ratio") - dxl->current_ratio_ = std::atof(tokens[1].c_str()); - else if (tokens[0] == "velocity_ratio") - dxl->velocity_ratio_ = std::atof(tokens[1].c_str()); + if (tokens[0] == "torque_to_current_value_ratio") + dxl->torque_to_current_value_ratio_ = std::atof(tokens[1].c_str()); + else if (tokens[0] == "velocity_to_value_ratio") + dxl->velocity_to_value_ratio_ = std::atof(tokens[1].c_str()); else if (tokens[0] == "value_of_0_radian_position") dxl->value_of_0_radian_position_ = std::atoi(tokens[1].c_str()); From 55e2f6e0f8df4648d7c9508920e5052ec93261a1 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Wed, 6 Jul 2016 16:35:54 +0900 Subject: [PATCH 20/33] - rename (present_current_ -> present_torque_) --- .../src/robotis_controller/robotis_controller.cpp | 10 +++++----- .../include/robotis_device/dynamixel_state.h | 4 ++-- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index cd8fd0f..479f38b 100644 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -159,8 +159,8 @@ void RobotisController::initializeSyncWrite() else if ((dxl->present_current_item_ != 0) && (dxl->bulk_read_items_[i]->item_name_ == dxl->present_current_item_->item_name_)) { - dxl->dxl_state_->present_current_ = dxl->convertValue2Torque(read_data); - dxl->dxl_state_->goal_torque_ = dxl->dxl_state_->present_current_; + dxl->dxl_state_->present_torque_ = dxl->convertValue2Torque(read_data); + dxl->dxl_state_->goal_torque_ = dxl->dxl_state_->present_torque_; } } } @@ -818,7 +818,7 @@ void RobotisController::process() else if (dxl->present_velocity_item_ != 0 && item->item_name_ == dxl->present_velocity_item_->item_name_) dxl->dxl_state_->present_velocity_ = dxl->convertValue2Velocity(data); else if (dxl->present_current_item_ != 0 && item->item_name_ == dxl->present_current_item_->item_name_) - dxl->dxl_state_->present_current_ = dxl->convertValue2Torque(data); + dxl->dxl_state_->present_torque_ = dxl->convertValue2Torque(data); else if (dxl->goal_position_item_ != 0 && item->item_name_ == dxl->goal_position_item_->item_name_) dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset else if (dxl->goal_velocity_item_ != 0 && item->item_name_ == dxl->goal_velocity_item_->item_name_) @@ -1129,7 +1129,7 @@ void RobotisController::process() present_state.name.push_back(joint_name); present_state.position.push_back(dxl->dxl_state_->present_position_); present_state.velocity.push_back(dxl->dxl_state_->present_velocity_); - present_state.effort.push_back(dxl->dxl_state_->present_current_); + present_state.effort.push_back(dxl->dxl_state_->present_torque_); goal_state.name.push_back(joint_name); goal_state.position.push_back(dxl->dxl_state_->goal_position_); @@ -1607,7 +1607,7 @@ void RobotisController::gazeboJointStatesCallback(const sensor_msgs::JointState: { d_it->second->dxl_state_->present_position_ = msg->position[i]; d_it->second->dxl_state_->present_velocity_ = msg->velocity[i]; - d_it->second->dxl_state_->present_current_ = msg->effort[i]; + d_it->second->dxl_state_->present_torque_ = msg->effort[i]; } } diff --git a/robotis_device/include/robotis_device/dynamixel_state.h b/robotis_device/include/robotis_device/dynamixel_state.h index 833c0f3..a318a1f 100644 --- a/robotis_device/include/robotis_device/dynamixel_state.h +++ b/robotis_device/include/robotis_device/dynamixel_state.h @@ -55,7 +55,7 @@ class DynamixelState double present_position_; double present_velocity_; - double present_current_; + double present_torque_; double goal_position_; double goal_velocity_; double goal_torque_; @@ -69,7 +69,7 @@ class DynamixelState : update_time_stamp_(0, 0), present_position_(0.0), present_velocity_(0.0), - present_current_(0.0), + present_torque_(0.0), goal_position_(0.0), goal_velocity_(0.0), goal_torque_(0.0), From 27915069263fe93fc69b2bb26098990aea677a97 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Wed, 6 Jul 2016 18:24:33 +0900 Subject: [PATCH 21/33] - rename ControlMode(CurrentControl -> TorqueControl) - rename (port_to_sync_write_torque_ -> port_to_sync_write_current_) --- .../robotis_controller/robotis_controller.h | 2 +- .../robotis_controller/robotis_controller.cpp | 40 +++++++++---------- .../robotis_framework_common/motion_module.h | 2 +- 3 files changed, 22 insertions(+), 22 deletions(-) diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h index e6befef..ae91370 100644 --- a/robotis_controller/include/robotis_controller/robotis_controller.h +++ b/robotis_controller/include/robotis_controller/robotis_controller.h @@ -107,7 +107,7 @@ class RobotisController : public Singleton /* sync write */ std::map port_to_sync_write_position_; std::map port_to_sync_write_velocity_; - std::map port_to_sync_write_torque_; + std::map port_to_sync_write_current_; std::map port_to_sync_write_position_p_gain_; /* publisher */ diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 479f38b..dfa8ffe 100644 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -106,8 +106,8 @@ void RobotisController::initializeSyncWrite() if (it->second != NULL) it->second->clearParam(); } - for (std::map::iterator it = port_to_sync_write_torque_.begin(); - it != port_to_sync_write_torque_.end(); it++) + for (std::map::iterator it = port_to_sync_write_current_.begin(); + it != port_to_sync_write_current_.end(); it++) { if (it->second != NULL) it->second->clearParam(); @@ -231,7 +231,7 @@ bool RobotisController::initialize(const std::string robot_file_path, const std: if (default_device->goal_current_item_ != 0) { - port_to_sync_write_torque_[port_name] + port_to_sync_write_current_[port_name] = new dynamixel::GroupSyncWrite(port, default_pkt_handler, default_device->goal_current_item_->address_, @@ -707,8 +707,8 @@ void RobotisController::stopTimer() if (it->second != NULL) it->second->clearParam(); } - for (std::map::iterator it = port_to_sync_write_torque_.begin(); - it != port_to_sync_write_torque_.end(); it++) + for (std::map::iterator it = port_to_sync_write_current_.begin(); + it != port_to_sync_write_current_.end(); it++) { if (it->second != NULL) it->second->clearParam(); @@ -984,7 +984,7 @@ void RobotisController::process() port_to_sync_write_velocity_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data); } } - else if ((*module_it)->getControlMode() == CurrentControl) + else if ((*module_it)->getControlMode() == TorqueControl) { dxl_state->goal_torque_ = result_state->goal_torque_; @@ -995,8 +995,8 @@ void RobotisController::process() sync_write_data[0] = DXL_LOBYTE(curr_data); sync_write_data[1] = DXL_HIBYTE(curr_data); - if (port_to_sync_write_torque_[dxl->port_name_] != NULL) - port_to_sync_write_torque_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data); + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data); } } } @@ -1036,8 +1036,8 @@ void RobotisController::process() if (it->second != NULL) it->second->txPacket(); } - for (std::map::iterator it = port_to_sync_write_torque_.begin(); - it != port_to_sync_write_torque_.end(); it++) + for (std::map::iterator it = port_to_sync_write_current_.begin(); + it != port_to_sync_write_current_.end(); it++) { if (it->second != NULL) it->second->txPacket(); @@ -1071,7 +1071,7 @@ void RobotisController::process() joint_msg.data = dxl_state->goal_velocity_; gazebo_joint_velocity_pub_[joint_name].publish(joint_msg); } - else if ((*module_it)->getControlMode() == CurrentControl) + else if ((*module_it)->getControlMode() == TorqueControl) { joint_msg.data = dxl_state->goal_torque_; gazebo_joint_effort_pub_[joint_name].publish(joint_msg); @@ -1477,8 +1477,8 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) if (port_to_sync_write_position_[dxl->port_name_] != NULL) port_to_sync_write_position_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); - if (port_to_sync_write_torque_[dxl->port_name_] != NULL) - port_to_sync_write_torque_[dxl->port_name_]->removeParam(dxl->id_); + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->removeParam(dxl->id_); if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) port_to_sync_write_velocity_[dxl->port_name_]->removeParam(dxl->id_); } @@ -1516,8 +1516,8 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) if (port_to_sync_write_position_[dxl->port_name_] != NULL) port_to_sync_write_position_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); - if (port_to_sync_write_torque_[dxl->port_name_] != NULL) - port_to_sync_write_torque_[dxl->port_name_]->removeParam(dxl->id_); + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->removeParam(dxl->id_); if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) port_to_sync_write_velocity_[dxl->port_name_]->removeParam(dxl->id_); } @@ -1533,12 +1533,12 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) port_to_sync_write_velocity_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); - if (port_to_sync_write_torque_[dxl->port_name_] != NULL) - port_to_sync_write_torque_[dxl->port_name_]->removeParam(dxl->id_); + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->removeParam(dxl->id_); if (port_to_sync_write_position_[dxl->port_name_] != NULL) port_to_sync_write_position_[dxl->port_name_]->removeParam(dxl->id_); } - else if (mode == CurrentControl) + else if (mode == TorqueControl) { uint32_t curr_data = dxl->convertTorque2Value(dxl->dxl_state_->goal_torque_); uint8_t sync_write_data[4] = { 0 }; @@ -1547,8 +1547,8 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(curr_data)); sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(curr_data)); - if (port_to_sync_write_torque_[dxl->port_name_] != NULL) - port_to_sync_write_torque_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) port_to_sync_write_velocity_[dxl->port_name_]->removeParam(dxl->id_); diff --git a/robotis_framework_common/include/robotis_framework_common/motion_module.h b/robotis_framework_common/include/robotis_framework_common/motion_module.h index c602f34..e85d938 100644 --- a/robotis_framework_common/include/robotis_framework_common/motion_module.h +++ b/robotis_framework_common/include/robotis_framework_common/motion_module.h @@ -53,7 +53,7 @@ enum ControlMode { PositionControl, VelocityControl, - CurrentControl + TorqueControl }; class MotionModule From e8792df69730ff53c02fa4afb638c37de20c9912 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Mon, 25 Jul 2016 11:35:12 +0900 Subject: [PATCH 22/33] added XM-430-W210 / XM-430-W350 device file. --- .../robotis_controller/robotis_controller.cpp | 10 +-- .../devices/dynamixel/XM-430-W210.device | 78 +++++++++++++++++++ .../devices/dynamixel/XM-430-W350.device | 78 +++++++++++++++++++ .../devices/dynamixel/XM-430.device | 2 +- 4 files changed, 162 insertions(+), 6 deletions(-) create mode 100644 robotis_device/devices/dynamixel/XM-430-W210.device create mode 100644 robotis_device/devices/dynamixel/XM-430-W350.device diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index dfa8ffe..292b2ff 100644 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -870,7 +870,7 @@ void RobotisController::process() if (DEBUG_PRINT) { time_duration = ros::Time::now() - start_time; - fprintf(stderr, "(%2.6f) BulkRead Rx & update state", time_duration.nsec * 0.000001); + fprintf(stderr, "(%2.6f) BulkRead Rx & update state \n", time_duration.nsec * 0.000001); } // Call SensorModule Process() @@ -889,7 +889,7 @@ void RobotisController::process() if (DEBUG_PRINT) { time_duration = ros::Time::now() - start_time; - fprintf(stderr, "(%2.6f) SensorModule Process() & save result", time_duration.nsec * 0.000001); + fprintf(stderr, "(%2.6f) SensorModule Process() & save result \n", time_duration.nsec * 0.000001); } if (controller_mode_ == MotionModuleMode) @@ -1009,7 +1009,7 @@ void RobotisController::process() if (DEBUG_PRINT) { time_duration = ros::Time::now() - start_time; - fprintf(stderr, "(%2.6f) MotionModule Process() & save result", time_duration.nsec * 0.000001); + fprintf(stderr, "(%2.6f) MotionModule Process() & save result \n", time_duration.nsec * 0.000001); } // SyncWrite @@ -1117,7 +1117,7 @@ void RobotisController::process() if (DEBUG_PRINT) { time_duration = ros::Time::now() - start_time; - fprintf(stderr, "(%2.6f) SyncWrite & BulkRead Tx", time_duration.nsec * 0.000001); + fprintf(stderr, "(%2.6f) SyncWrite & BulkRead Tx \n", time_duration.nsec * 0.000001); } // publish present & goal position @@ -1144,7 +1144,7 @@ void RobotisController::process() if (DEBUG_PRINT) { time_duration = ros::Time::now() - start_time; - fprintf(stderr, "(%2.6f) Process() DONE", time_duration.nsec * 0.000001); + fprintf(stderr, "(%2.6f) Process() DONE \n", time_duration.nsec * 0.000001); } is_process_running = false; diff --git a/robotis_device/devices/dynamixel/XM-430-W210.device b/robotis_device/devices/dynamixel/XM-430-W210.device new file mode 100644 index 0000000..178fc4f --- /dev/null +++ b/robotis_device/devices/dynamixel/XM-430-W210.device @@ -0,0 +1,78 @@ +[device info] +model_name = XM-430-W210 +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 235.53647082 +velocity_to_value_ratio = 41.707853 + +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 7 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 16 | N + 20 | homing_offset | 4 | RW | EEPROM | -1044479 | 1044479 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 1023 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 1193 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 32767 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 1023 | N + 48 | max_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 52 | min_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 64 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 65 | LED | 1 | RW | RAM | 0 | 1 | N + 68 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 69 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 70 | hardware_error_status | 1 | R | RAM | 0 | 255 | N + 76 | velocity_i_gain | 2 | RW | RAM | 0 | 32767 | N + 78 | velocity_p_gain | 2 | RW | RAM | 0 | 32767 | N + 80 | position_d_gain | 2 | RW | RAM | 0 | 32767 | N + 82 | position_i_gain | 2 | RW | RAM | 0 | 32767 | N + 84 | position_p_gain | 2 | RW | RAM | 0 | 32767 | N + 88 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 90 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 100 | goal_pwm | 2 | RW | RAM | 0 | 885 | N + 102 | goal_current | 2 | RW | RAM | 0 | 1193 | N + 104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N + 108 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 112 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 116 | goal_position | 4 | RW | RAM | -1048575 | 1048575 | Y + 120 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 122 | moving | 1 | R | RAM | 0 | 1 | N + 123 | moving_status | 1 | R | RAM | 0 | 255 | N + 124 | present_pwm | 2 | R | RAM | 0 | 885 | N + 126 | present_current | 2 | R | RAM | 0 | 1193 | N + 128 | present_velocity | 4 | R | RAM | 0 | 1023 | N + 132 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 136 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 140 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 144 | present_input_voltage | 2 | R | RAM | 95 | 160 | N + 146 | present_temperature | 1 | R | RAM | 0 | 100 | N + 168 | indirect_address_1 | 2 | RW | RAM | 0 | 65535 | N + 224 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 578 | indirect_address_29 | 2 | RW | RAM | 0 | 65535 | N + 634 | indirect_data_29 | 1 | RW | RAM | 0 | 255 | N diff --git a/robotis_device/devices/dynamixel/XM-430-W350.device b/robotis_device/devices/dynamixel/XM-430-W350.device new file mode 100644 index 0000000..ed6c17f --- /dev/null +++ b/robotis_device/devices/dynamixel/XM-430-W350.device @@ -0,0 +1,78 @@ +[device info] +model_name = XM-430-W350 +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 149.795386991 +velocity_to_value_ratio = 41.707853 + +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 7 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 16 | N + 20 | homing_offset | 4 | RW | EEPROM | -1044479 | 1044479 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 1023 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 1193 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 32767 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 1023 | N + 48 | max_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 52 | min_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 64 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 65 | LED | 1 | RW | RAM | 0 | 1 | N + 68 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 69 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 70 | hardware_error_status | 1 | R | RAM | 0 | 255 | N + 76 | velocity_i_gain | 2 | RW | RAM | 0 | 32767 | N + 78 | velocity_p_gain | 2 | RW | RAM | 0 | 32767 | N + 80 | position_d_gain | 2 | RW | RAM | 0 | 32767 | N + 82 | position_i_gain | 2 | RW | RAM | 0 | 32767 | N + 84 | position_p_gain | 2 | RW | RAM | 0 | 32767 | N + 88 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 90 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 100 | goal_pwm | 2 | RW | RAM | 0 | 885 | N + 102 | goal_current | 2 | RW | RAM | 0 | 1193 | N + 104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N + 108 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 112 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 116 | goal_position | 4 | RW | RAM | -1048575 | 1048575 | Y + 120 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 122 | moving | 1 | R | RAM | 0 | 1 | N + 123 | moving_status | 1 | R | RAM | 0 | 255 | N + 124 | present_pwm | 2 | R | RAM | 0 | 885 | N + 126 | present_current | 2 | R | RAM | 0 | 1193 | N + 128 | present_velocity | 4 | R | RAM | 0 | 1023 | N + 132 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 136 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 140 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 144 | present_input_voltage | 2 | R | RAM | 95 | 160 | N + 146 | present_temperature | 1 | R | RAM | 0 | 100 | N + 168 | indirect_address_1 | 2 | RW | RAM | 0 | 65535 | N + 224 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 578 | indirect_address_29 | 2 | RW | RAM | 0 | 65535 | N + 634 | indirect_data_29 | 1 | RW | RAM | 0 | 255 | N diff --git a/robotis_device/devices/dynamixel/XM-430.device b/robotis_device/devices/dynamixel/XM-430.device index 3f33b73..67d523f 100644 --- a/robotis_device/devices/dynamixel/XM-430.device +++ b/robotis_device/devices/dynamixel/XM-430.device @@ -3,7 +3,7 @@ model_name = XM-430 device_type = dynamixel [type info] -torque_to_current_value_ratio = 178.842161 +torque_to_current_value_ratio = 149.795386991 value_of_0_radian_position = 2048 value_of_min_radian_position = 0 From 2420333894face622e5ed828b6d8b42ff4fc993a Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Thu, 28 Jul 2016 17:04:50 +0900 Subject: [PATCH 23/33] - fixed robotis_device build_depend. --- robotis_device/CMakeLists.txt | 1 - robotis_device/package.xml | 1 - 2 files changed, 2 deletions(-) diff --git a/robotis_device/CMakeLists.txt b/robotis_device/CMakeLists.txt index 6c53ff0..e492450 100644 --- a/robotis_device/CMakeLists.txt +++ b/robotis_device/CMakeLists.txt @@ -5,7 +5,6 @@ find_package(catkin REQUIRED COMPONENTS roscpp rospy dynamixel_sdk - robotis_framework_common ) catkin_package( diff --git a/robotis_device/package.xml b/robotis_device/package.xml index 8e494db..6f7294a 100644 --- a/robotis_device/package.xml +++ b/robotis_device/package.xml @@ -17,7 +17,6 @@ roscpp rospy dynamixel_sdk - robotis_framework_common roscpp rospy From 6df646ae090c36ca0364fa1e8ed12c110c271139 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Thu, 28 Jul 2016 17:07:38 +0900 Subject: [PATCH 24/33] - modify the code simple (using auto / range based for loop) --- robotis_controller/CMakeLists.txt | 2 + .../robotis_controller/robotis_controller.cpp | 304 ++++++++---------- 2 files changed, 140 insertions(+), 166 deletions(-) diff --git a/robotis_controller/CMakeLists.txt b/robotis_controller/CMakeLists.txt index 42486cc..97c4a6d 100644 --- a/robotis_controller/CMakeLists.txt +++ b/robotis_controller/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 2.8.3) project(robotis_controller) +set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") + find_package(catkin REQUIRED COMPONENTS roscpp roslib diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 292b2ff..c568d82 100644 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -62,14 +62,9 @@ void RobotisController::initializeSyncWrite() return; ROS_INFO("FIRST BULKREAD"); - // bulkread twice - for (std::map::iterator it = port_to_bulk_read_.begin(); - it != port_to_bulk_read_.end(); it++) - { - it->second->txRxPacket(); - } - for (std::map::iterator it = port_to_bulk_read_.begin(); - it != port_to_bulk_read_.end(); it++) + for (auto& it : port_to_bulk_read_) + it.second->txRxPacket(); + for(auto& it : port_to_bulk_read_) { int error_count = 0; int result = COMM_SUCCESS; @@ -81,43 +76,39 @@ void RobotisController::initializeSyncWrite() exit(-1); } usleep(10 * 1000); - result = it->second->txRxPacket(); + result = it.second->txRxPacket(); } while (result != COMM_SUCCESS); } init_pose_loaded_ = true; ROS_INFO("FIRST BULKREAD END"); // clear syncwrite param setting - for (std::map::iterator it = port_to_sync_write_position_.begin(); - it != port_to_sync_write_position_.end(); it++) + for (auto& it : port_to_sync_write_position_) { - if (it->second != NULL) - it->second->clearParam(); + if (it.second != NULL) + it.second->clearParam(); } - for (std::map::iterator it = port_to_sync_write_position_p_gain_.begin(); - it != port_to_sync_write_position_p_gain_.end(); it++) + for (auto& it : port_to_sync_write_position_p_gain_) { - if (it->second != NULL) - it->second->clearParam(); + if (it.second != NULL) + it.second->clearParam(); } - for (std::map::iterator it = port_to_sync_write_velocity_.begin(); - it != port_to_sync_write_velocity_.end(); it++) + for (auto& it : port_to_sync_write_velocity_) { - if (it->second != NULL) - it->second->clearParam(); + if (it.second != NULL) + it.second->clearParam(); } - for (std::map::iterator it = port_to_sync_write_current_.begin(); - it != port_to_sync_write_current_.end(); it++) + for (auto& it : port_to_sync_write_current_) { - if (it->second != NULL) - it->second->clearParam(); + if (it.second != NULL) + it.second->clearParam(); } // set init syncwrite param(from data of bulkread) - for (std::map::iterator it = robot_->dxls_.begin(); it != robot_->dxls_.end(); it++) + for (auto& it : robot_->dxls_) { - std::string joint_name = it->first; - Dynamixel *dxl = it->second; + std::string joint_name = it.first; + Dynamixel *dxl = it.second; for (int i = 0; i < dxl->bulk_read_items_.size(); i++) { @@ -180,11 +171,10 @@ bool RobotisController::initialize(const std::string robot_file_path, const std: return true; } - for (std::map::iterator it = robot_->ports_.begin(); - it != robot_->ports_.end(); it++) + for (auto& it : robot_->ports_) { - std::string port_name = it->first; - dynamixel::PortHandler *port = it->second; + std::string port_name = it.first; + dynamixel::PortHandler *port = it.second; dynamixel::PacketHandler *default_pkt_handler = dynamixel::PacketHandler::getPacketHandler(2.0); if (port->setBaudRate(port->getBaudRate()) == false) @@ -195,8 +185,8 @@ bool RobotisController::initialize(const std::string robot_file_path, const std: // get the default device info of the port std::string default_device_name = robot_->port_default_device_[port_name]; - std::map::iterator dxl_it = robot_->dxls_.find(default_device_name); - std::map::iterator sensor_it = robot_->sensors_.find(default_device_name); + auto dxl_it = robot_->dxls_.find(default_device_name); + auto sensor_it = robot_->sensors_.find(default_device_name); if (dxl_it != robot_->dxls_.end()) { Dynamixel *default_device = dxl_it->second; @@ -248,10 +238,10 @@ bool RobotisController::initialize(const std::string robot_file_path, const std: } // (for loop) check all dxls are connected. - for (std::map::iterator it = robot_->dxls_.begin(); it != robot_->dxls_.end(); it++) + for (auto& it : robot_->dxls_) { - std::string joint_name = it->first; - Dynamixel *dxl = it->second; + std::string joint_name = it.first; + Dynamixel *dxl = it.second; if (ping(joint_name) != 0) { @@ -264,10 +254,10 @@ bool RobotisController::initialize(const std::string robot_file_path, const std: initializeDevice(init_file_path); // [ BulkRead ] StartAddress : Present Position , Length : 10 ( Position/Velocity/Current ) - for (std::map::iterator it = robot_->dxls_.begin(); it != robot_->dxls_.end(); it++) + for (auto& it : robot_->dxls_) { - std::string joint_name = it->first; - Dynamixel *dxl = it->second; + std::string joint_name = it.first; + Dynamixel *dxl = it.second; if (dxl == NULL) continue; @@ -283,7 +273,7 @@ bool RobotisController::initialize(const std::string robot_file_path, const std: // } // calculate bulk read start address & data length - std::map::iterator indirect_addr_it = dxl->ctrl_table_.find(INDIRECT_ADDRESS_1); + auto indirect_addr_it = dxl->ctrl_table_.find(INDIRECT_ADDRESS_1); if (indirect_addr_it != dxl->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist { if (dxl->bulk_read_items_.size() != 0) @@ -338,10 +328,10 @@ bool RobotisController::initialize(const std::string robot_file_path, const std: writeCtrlItem(joint_name, dxl->torque_enable_item_->item_name_, 1); } - for (std::map::iterator it = robot_->sensors_.begin(); it != robot_->sensors_.end(); it++) + for (auto& it : robot_->sensors_) { - std::string sensor_name = it->first; - Sensor *sensor = it->second; + std::string sensor_name = it.first; + Sensor *sensor = it.second; if (sensor == NULL) continue; @@ -350,7 +340,7 @@ bool RobotisController::initialize(const std::string robot_file_path, const std: int bulkread_data_length = 0; // calculate bulk read start address & data length - std::map::iterator indirect_addr_it = sensor->ctrl_table_.find(INDIRECT_ADDRESS_1); + auto indirect_addr_it = sensor->ctrl_table_.find(INDIRECT_ADDRESS_1); if (indirect_addr_it != sensor->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist { if (sensor->bulk_read_items_.size() != 0) @@ -427,7 +417,7 @@ void RobotisController::initializeDevice(const std::string init_file_path) continue; Dynamixel *dxl = NULL; - std::map::iterator dxl_it = robot_->dxls_.find(joint_name); + auto dxl_it = robot_->dxls_.find(joint_name); if (dxl_it != robot_->dxls_.end()) dxl = dxl_it->second; @@ -555,14 +545,14 @@ void RobotisController::msgQueueThread() if (gazebo_mode_ == true) { - for (std::map::iterator it = robot_->dxls_.begin(); it != robot_->dxls_.end(); it++) + for (auto& it : robot_->dxls_) { - gazebo_joint_position_pub_[it->first] = ros_node.advertise( - "/" + gazebo_robot_name_ + "/" + it->first + "_position/command", 1); - gazebo_joint_velocity_pub_[it->first] = ros_node.advertise( - "/" + gazebo_robot_name_ + "/" + it->first + "_velocity/command", 1); - gazebo_joint_effort_pub_[it->first] = ros_node.advertise( - "/" + gazebo_robot_name_ + "/" + it->first + "_effort/command", 1); + gazebo_joint_position_pub_[it.first] = ros_node.advertise( + "/" + gazebo_robot_name_ + "/" + it.first + "_position/command", 1); + gazebo_joint_velocity_pub_[it.first] = ros_node.advertise( + "/" + gazebo_robot_name_ + "/" + it.first + "_velocity/command", 1); + gazebo_joint_effort_pub_[it.first] = ros_node.advertise( + "/" + gazebo_robot_name_ + "/" + it.first + "_effort/command", 1); } } @@ -629,10 +619,9 @@ void RobotisController::startTimer() { initializeSyncWrite(); - for (std::map::iterator it = port_to_bulk_read_.begin(); - it != port_to_bulk_read_.end(); it++) + for (auto& it : port_to_bulk_read_) { - it->second->txPacket(); + it.second->txPacket(); } usleep(8 * 1000); @@ -682,36 +671,31 @@ void RobotisController::stopTimer() if ((error = pthread_join(this->timer_thread_, NULL)) != 0) exit(-1); - for (std::map::iterator it = port_to_bulk_read_.begin(); - it != port_to_bulk_read_.end(); it++) + for (auto& it : port_to_bulk_read_) { - if (it->second != NULL) - it->second->rxPacket(); + if (it.second != NULL) + it.second->rxPacket(); } - for (std::map::iterator it = port_to_sync_write_position_.begin(); - it != port_to_sync_write_position_.end(); it++) + for (auto& it : port_to_sync_write_position_) { - if (it->second != NULL) - it->second->clearParam(); + if (it.second != NULL) + it.second->clearParam(); } - for (std::map::iterator it = port_to_sync_write_position_p_gain_.begin(); - it != port_to_sync_write_position_p_gain_.end(); it++) + for (auto& it : port_to_sync_write_position_p_gain_) { - if (it->second != NULL) - it->second->clearParam(); + if (it.second != NULL) + it.second->clearParam(); } - for (std::map::iterator it = port_to_sync_write_velocity_.begin(); - it != port_to_sync_write_velocity_.end(); it++) + for (auto& it : port_to_sync_write_velocity_) { - if (it->second != NULL) - it->second->clearParam(); + if (it.second != NULL) + it.second->clearParam(); } - for (std::map::iterator it = port_to_sync_write_current_.begin(); - it != port_to_sync_write_current_.end(); it++) + for (auto& it : port_to_sync_write_current_) { - if (it->second != NULL) - it->second->clearParam(); + if (it.second != NULL) + it.second->clearParam(); } } else @@ -752,7 +736,7 @@ void RobotisController::loadOffset(const std::string path) std::string joint_name = it->first.as(); double offset = it->second.as(); - std::map::iterator dxl_it = robot_->dxls_.find(joint_name); + auto dxl_it = robot_->dxls_.find(joint_name); if (dxl_it != robot_->dxls_.end()) dxl_it->second->dxl_state_->position_offset_ = offset; } @@ -786,21 +770,19 @@ void RobotisController::process() if (gazebo_mode_ == false) { // BulkRead Rx - for (std::map::iterator it = port_to_bulk_read_.begin(); - it != port_to_bulk_read_.end(); it++) + for (auto& it : port_to_bulk_read_) { - robot_->ports_[it->first]->setPacketTimeout(0.0); - it->second->rxPacket(); + robot_->ports_[it.first]->setPacketTimeout(0.0); + it.second->rxPacket(); } if (robot_->dxls_.size() > 0) { - for (std::map::iterator dxl_it = robot_->dxls_.begin(); - dxl_it != robot_->dxls_.end(); dxl_it++) + for (auto& dxl_it : robot_->dxls_) { - Dynamixel *dxl = dxl_it->second; - std::string port_name = dxl_it->second->port_name_; - std::string joint_name = dxl_it->first; + Dynamixel *dxl = dxl_it.second; + std::string port_name = dxl_it.second->port_name_; + std::string joint_name = dxl_it.first; if (dxl->bulk_read_items_.size() > 0) { @@ -838,12 +820,11 @@ void RobotisController::process() if (robot_->sensors_.size() > 0) { - for (std::map::iterator sensor_it = robot_->sensors_.begin(); - sensor_it != robot_->sensors_.end(); sensor_it++) + for (auto& sensor_it : robot_->sensors_) { - Sensor *sensor = sensor_it->second; - std::string port_name = sensor_it->second->port_name_; - std::string sensor_name = sensor_it->first; + Sensor *sensor = sensor_it.second; + std::string port_name = sensor_it.second->port_name_; + std::string sensor_name = sensor_it.first; if (sensor->bulk_read_items_.size() > 0) { @@ -877,12 +858,12 @@ void RobotisController::process() // -> for loop : call SensorModule list -> Process() if (sensor_modules_.size() > 0) { - for (std::list::iterator module_it = sensor_modules_.begin(); module_it != sensor_modules_.end(); module_it++) + for (auto module_it = sensor_modules_.begin(); module_it != sensor_modules_.end(); module_it++) { (*module_it)->process(robot_->dxls_, robot_->sensors_); - for (std::map::iterator it = (*module_it)->result_.begin(); it != (*module_it)->result_.end(); it++) - sensor_result_[it->first] = it->second; + for (auto& it : (*module_it)->result_) + sensor_result_[it.first] = it.second; } } @@ -900,7 +881,7 @@ void RobotisController::process() { queue_mutex_.lock(); - for (std::list::iterator module_it = motion_modules_.begin(); module_it != motion_modules_.end(); module_it++) + for (auto module_it = motion_modules_.begin(); module_it != motion_modules_.end(); module_it++) { if ((*module_it)->getModuleEnable() == false) continue; @@ -908,11 +889,11 @@ void RobotisController::process() (*module_it)->process(robot_->dxls_, sensor_result_); // for loop : joint list - for (std::map::iterator dxl_it = robot_->dxls_.begin(); dxl_it != robot_->dxls_.end(); dxl_it++) + for (auto& dxl_it : robot_->dxls_) { - std::string joint_name = dxl_it->first; - Dynamixel *dxl = dxl_it->second; - DynamixelState *dxl_state = dxl_it->second->dxl_state_; + std::string joint_name = dxl_it.first; + Dynamixel *dxl = dxl_it.second; + DynamixelState *dxl_state = dxl_it.second->dxl_state_; if (dxl->ctrl_module_name_ == (*module_it)->getModuleName()) { @@ -1017,47 +998,42 @@ void RobotisController::process() { if (port_to_sync_write_position_p_gain_.size() > 0) { - for (std::map::iterator it = port_to_sync_write_position_p_gain_.begin(); - it != port_to_sync_write_position_p_gain_.end(); it++) + for (auto& it : port_to_sync_write_position_p_gain_) { - it->second->txPacket(); - it->second->clearParam(); + it.second->txPacket(); + it.second->clearParam(); } } - for (std::map::iterator it = port_to_sync_write_position_.begin(); - it != port_to_sync_write_position_.end(); it++) + for (auto& it : port_to_sync_write_position_) { - if (it->second != NULL) - it->second->txPacket(); + if (it.second != NULL) + it.second->txPacket(); } - for (std::map::iterator it = port_to_sync_write_velocity_.begin(); - it != port_to_sync_write_velocity_.end(); it++) + for (auto& it : port_to_sync_write_velocity_) { - if (it->second != NULL) - it->second->txPacket(); + if (it.second != NULL) + it.second->txPacket(); } - for (std::map::iterator it = port_to_sync_write_current_.begin(); - it != port_to_sync_write_current_.end(); it++) + for (auto& it : port_to_sync_write_current_) { - if (it->second != NULL) - it->second->txPacket(); + if (it.second != NULL) + it.second->txPacket(); } } else if (gazebo_mode_ == true) { - for (std::list::iterator module_it = motion_modules_.begin(); module_it != motion_modules_.end(); module_it++) + for (auto module_it = motion_modules_.begin(); module_it != motion_modules_.end(); module_it++) { if ((*module_it)->getModuleEnable() == false) continue; std_msgs::Float64 joint_msg; - for (std::map::iterator dxl_it = robot_->dxls_.begin(); dxl_it != robot_->dxls_.end(); - dxl_it++) + for (auto& dxl_it : robot_->dxls_) { - std::string joint_name = dxl_it->first; - Dynamixel *dxl = dxl_it->second; - DynamixelState *dxl_state = dxl_it->second->dxl_state_; + std::string joint_name = dxl_it.first; + Dynamixel *dxl = dxl_it.second; + DynamixelState *dxl_state = dxl_it.second->dxl_state_; if (dxl->ctrl_module_name_ == (*module_it)->getModuleName()) { @@ -1085,11 +1061,10 @@ void RobotisController::process() { queue_mutex_.lock(); - for (std::map::iterator it = port_to_sync_write_position_.begin(); - it != port_to_sync_write_position_.end(); it++) + for (auto& it : port_to_sync_write_position_) { - it->second->txPacket(); - it->second->clearParam(); + it.second->txPacket(); + it.second->clearParam(); } if (direct_sync_write_.size() > 0) @@ -1110,8 +1085,8 @@ void RobotisController::process() // BulkRead Tx if (gazebo_mode_ == false) { - for (std::map::iterator it = port_to_bulk_read_.begin(); it != port_to_bulk_read_.end(); it++) - it->second->txPacket(); + for (auto& it : port_to_bulk_read_) + it.second->txPacket(); } if (DEBUG_PRINT) @@ -1121,10 +1096,10 @@ void RobotisController::process() } // publish present & goal position - for (std::map::iterator dxl_it = robot_->dxls_.begin(); dxl_it != robot_->dxls_.end(); dxl_it++) + for (auto& dxl_it : robot_->dxls_) { - std::string joint_name = dxl_it->first; - Dynamixel *dxl = dxl_it->second; + std::string joint_name = dxl_it.first; + Dynamixel *dxl = dxl_it.second; present_state.name.push_back(joint_name); present_state.position.push_back(dxl->dxl_state_->present_position_); @@ -1153,7 +1128,7 @@ void RobotisController::process() void RobotisController::addMotionModule(MotionModule *module) { // check whether the module name already exists - for (std::list::iterator m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) { if ((*m_it)->getModuleName() == module->getModuleName()) { @@ -1175,7 +1150,7 @@ void RobotisController::removeMotionModule(MotionModule *module) void RobotisController::addSensorModule(SensorModule *module) { // check whether the module name already exists - for (std::list::iterator m_it = sensor_modules_.begin(); m_it != sensor_modules_.end(); m_it++) + for (auto m_it = sensor_modules_.begin(); m_it != sensor_modules_.end(); m_it++) { if ((*m_it)->getModuleName() == module->getModuleName()) { @@ -1301,7 +1276,7 @@ void RobotisController::setJointCtrlModuleCallback(const robotis_controller_msgs for (unsigned int idx = 0; idx < msg->joint_name.size(); idx++) { Dynamixel *dxl = NULL; - std::map::iterator dxl_it = robot_->dxls_.find((std::string) (msg->joint_name[idx])); + auto dxl_it = robot_->dxls_.find((std::string) (msg->joint_name[idx])); if (dxl_it != robot_->dxls_.end()) dxl = dxl_it->second; else @@ -1315,7 +1290,7 @@ void RobotisController::setJointCtrlModuleCallback(const robotis_controller_msgs } // check whether the module is using this joint - for (std::list::iterator m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) { if ((*m_it)->getModuleName() == msg->module_name[idx]) { @@ -1328,15 +1303,15 @@ void RobotisController::setJointCtrlModuleCallback(const robotis_controller_msgs } } - for (std::list::iterator m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) { // set all modules -> disable (*m_it)->setModuleEnable(false); // set all used modules -> enable - for (std::map::iterator d_it = robot_->dxls_.begin(); d_it != robot_->dxls_.end(); d_it++) + for (auto& d_it : robot_->dxls_) { - if (d_it->second->ctrl_module_name_ == (*m_it)->getModuleName()) + if (d_it.second->ctrl_module_name_ == (*m_it)->getModuleName()) { (*m_it)->setModuleEnable(true); break; @@ -1350,10 +1325,10 @@ void RobotisController::setJointCtrlModuleCallback(const robotis_controller_msgs queue_mutex_.unlock(); robotis_controller_msgs::JointCtrlModule current_module_msg; - for (std::map::iterator dxl_iter = robot_->dxls_.begin(); dxl_iter != robot_->dxls_.end(); ++dxl_iter) + for (auto& dxl_iter : robot_->dxls_) { - current_module_msg.joint_name.push_back(dxl_iter->first); - current_module_msg.module_name.push_back(dxl_iter->second->ctrl_module_name_); + current_module_msg.joint_name.push_back(dxl_iter.first); + current_module_msg.module_name.push_back(dxl_iter.second->ctrl_module_name_); } if (current_module_msg.joint_name.size() == current_module_msg.module_name.size()) @@ -1365,7 +1340,7 @@ bool RobotisController::getCtrlModuleCallback(robotis_controller_msgs::GetJointM { for (unsigned int idx = 0; idx < req.joint_name.size(); idx++) { - std::map::iterator d_it = robot_->dxls_.find((std::string) (req.joint_name[idx])); + auto d_it = robot_->dxls_.find((std::string) (req.joint_name[idx])); if (d_it != robot_->dxls_.end()) { res.joint_name.push_back(req.joint_name[idx]); @@ -1387,7 +1362,7 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) if (ctrl_module == "" || ctrl_module == "none") { // enqueue all modules in order to stop - for (std::list::iterator m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) { if ((*m_it)->getModuleEnable() == true) stop_modules.push_back(*m_it); @@ -1395,24 +1370,22 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) } else { - for (std::list::iterator m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) { // if it exist if ((*m_it)->getModuleName() == ctrl_module) { // enqueue the module which lost control of joint in order to stop - for (std::map::iterator result_it = (*m_it)->result_.begin(); - result_it != (*m_it)->result_.end(); result_it++) + for (auto& result_it : (*m_it)->result_) { - std::map::iterator d_it = robot_->dxls_.find(result_it->first); + auto d_it = robot_->dxls_.find(result_it.first); if (d_it != robot_->dxls_.end()) { // enqueue if (d_it->second->ctrl_module_name_ != ctrl_module) { - for (std::list::iterator stop_m_it = motion_modules_.begin(); - stop_m_it != motion_modules_.end(); stop_m_it++) + for (auto stop_m_it = motion_modules_.begin(); stop_m_it != motion_modules_.end(); stop_m_it++) { if (((*stop_m_it)->getModuleName() == d_it->second->ctrl_module_name_) && ((*stop_m_it)->getModuleEnable() == true)) @@ -1431,13 +1404,13 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) // stop the module stop_modules.unique(); - for (std::list::iterator stop_m_it = stop_modules.begin(); stop_m_it != stop_modules.end(); stop_m_it++) + for (auto stop_m_it = stop_modules.begin(); stop_m_it != stop_modules.end(); stop_m_it++) { (*stop_m_it)->stop(); } // wait to stop - for (std::list::iterator stop_m_it = stop_modules.begin(); stop_m_it != stop_modules.end(); stop_m_it++) + for (auto stop_m_it = stop_modules.begin(); stop_m_it != stop_modules.end(); stop_m_it++) { while ((*stop_m_it)->isRunning()) usleep(CONTROL_CYCLE_MSEC * 1000); @@ -1453,15 +1426,15 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) if ((ctrl_module == "") || (ctrl_module == "none")) { // set all modules -> disable - for (std::list::iterator m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) { (*m_it)->setModuleEnable(false); } // set dxl's control module to "none" - for (std::map::iterator d_it = robot_->dxls_.begin(); d_it != robot_->dxls_.end(); d_it++) + for (auto& d_it : robot_->dxls_) { - Dynamixel *dxl = d_it->second; + Dynamixel *dxl = d_it.second; dxl->ctrl_module_name_ = "none"; if (gazebo_mode_ == true) @@ -1486,16 +1459,15 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) else { // check whether the module exist - for (std::list::iterator m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) { // if it exist if ((*m_it)->getModuleName() == ctrl_module) { ControlMode mode = (*m_it)->getControlMode(); - for (std::map::iterator result_it = (*m_it)->result_.begin(); - result_it != (*m_it)->result_.end(); result_it++) + for (auto& result_it : (*m_it)->result_) { - std::map::iterator d_it = robot_->dxls_.find(result_it->first); + auto d_it = robot_->dxls_.find(result_it.first); if (d_it != robot_->dxls_.end()) { Dynamixel *dxl = d_it->second; @@ -1563,15 +1535,15 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) } } - for (std::list::iterator m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) { // set all modules -> disable (*m_it)->setModuleEnable(false); // set all used modules -> enable - for (std::map::iterator d_it = robot_->dxls_.begin(); d_it != robot_->dxls_.end(); d_it++) + for (auto& d_it : robot_->dxls_) { - if (d_it->second->ctrl_module_name_ == (*m_it)->getModuleName()) + if (d_it.second->ctrl_module_name_ == (*m_it)->getModuleName()) { (*m_it)->setModuleEnable(true); break; @@ -1586,10 +1558,10 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) // publish current module robotis_controller_msgs::JointCtrlModule current_module_msg; - for (std::map::iterator dxl_iter = robot_->dxls_.begin(); dxl_iter != robot_->dxls_.end(); ++dxl_iter) + for (auto& dxl_iter : robot_->dxls_) { - current_module_msg.joint_name.push_back(dxl_iter->first); - current_module_msg.module_name.push_back(dxl_iter->second->ctrl_module_name_); + current_module_msg.joint_name.push_back(dxl_iter.first); + current_module_msg.module_name.push_back(dxl_iter.second->ctrl_module_name_); } if (current_module_msg.joint_name.size() == current_module_msg.module_name.size()) @@ -1602,7 +1574,7 @@ void RobotisController::gazeboJointStatesCallback(const sensor_msgs::JointState: for (unsigned int i = 0; i < msg->name.size(); i++) { - std::map::iterator d_it = robot_->dxls_.find((std::string) msg->name[i]); + auto d_it = robot_->dxls_.find((std::string) msg->name[i]); if (d_it != robot_->dxls_.end()) { d_it->second->dxl_state_->present_position_ = msg->position[i]; @@ -1613,8 +1585,8 @@ void RobotisController::gazeboJointStatesCallback(const sensor_msgs::JointState: if (init_pose_loaded_ == false) { - for (std::map::iterator it = robot_->dxls_.begin(); it != robot_->dxls_.end(); it++) - it->second->dxl_state_->goal_position_ = it->second->dxl_state_->present_position_; + for (auto& it : robot_->dxls_) + it.second->dxl_state_->goal_position_ = it.second->dxl_state_->present_position_; init_pose_loaded_ = true; } From 54d93f872c7afe1a9f53ea5584615ee0b9b29cc5 Mon Sep 17 00:00:00 2001 From: Jay-Song Date: Wed, 10 Aug 2016 10:48:42 +0900 Subject: [PATCH 25/33] Add new functions to robotis_math step data define fifth polynomial trajectory --- .../robotis_math/robotis_linear_algebra.h | 3 + .../robotis_trajectory_calculator.h | 44 +++ .../include/robotis_math/step_data_define.h | 83 +++++ robotis_math/src/robotis_linear_algebra.cpp | 14 + .../src/robotis_trajectory_calculator.cpp | 284 ++++++++++++++++++ 5 files changed, 428 insertions(+) create mode 100644 robotis_math/include/robotis_math/step_data_define.h diff --git a/robotis_math/include/robotis_math/robotis_linear_algebra.h b/robotis_math/include/robotis_math/robotis_linear_algebra.h index 9c62268..f8cd69e 100644 --- a/robotis_math/include/robotis_math/robotis_linear_algebra.h +++ b/robotis_math/include/robotis_math/robotis_linear_algebra.h @@ -44,6 +44,7 @@ #define EIGEN_NO_STATIC_ASSERT #include +#include "step_data_define.h" namespace robotis_framework { @@ -71,6 +72,8 @@ Eigen::MatrixXd convertRotToOmega(Eigen::MatrixXd rotation); Eigen::MatrixXd calcCross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b); double calcInner(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b); +Pose3D getPose3DfromTransformMatrix(Eigen::MatrixXd transform); + } diff --git a/robotis_math/include/robotis_math/robotis_trajectory_calculator.h b/robotis_math/include/robotis_math/robotis_trajectory_calculator.h index 56bf644..09eaa36 100644 --- a/robotis_math/include/robotis_math/robotis_trajectory_calculator.h +++ b/robotis_math/include/robotis_math/robotis_trajectory_calculator.h @@ -60,6 +60,50 @@ Eigen::MatrixXd calcArc3dTra(double smp_time, double mov_time, Eigen::MatrixXd center_point, Eigen::MatrixXd normal_vector, Eigen::MatrixXd start_point, double rotation_angle, double cross_ratio); + +class FifthOrderPolynomialTrajectory +{ +public: + FifthOrderPolynomialTrajectory(double initial_time, double initial_pos, double initial_vel, double initial_acc, + double final_time, double final_pos, double final_vel, double final_acc); + FifthOrderPolynomialTrajectory(); + ~FifthOrderPolynomialTrajectory(); + + bool changeTrajectory(double final_pos, double final_vel, double final_acc); + bool changeTrajectory(double final_time, double final_pos, double final_vel, double final_acc); + bool changeTrajectory(double initial_time, double initial_pos, double initial_vel, double initial_acc, + double final_time, double final_pos, double final_vel, double final_acc); + + double getPosition(double time); + double getVelocity(double time); + double getAcceleration(double time); + + void setTime(double time); + double getPosition(); + double getVelocity(); + double getAcceleration(); + + double initial_time_; + double initial_pos_; + double initial_vel_; + double initial_acc_; + + double current_time_; + double current_pos_; + double current_vel_; + double current_acc_; + + double final_time_; + double final_pos_; + double final_vel_; + double final_acc_; + + Eigen::MatrixXd position_coeff_; + Eigen::MatrixXd velocity_coeff_; + Eigen::MatrixXd acceleration_coeff_; + Eigen::MatrixXd time_variables_; +}; + } #endif /* ROBOTIS_MATH_ROBOTIS_TRAJECTORY_CALCULATOR_H_ */ diff --git a/robotis_math/include/robotis_math/step_data_define.h b/robotis_math/include/robotis_math/step_data_define.h new file mode 100644 index 0000000..135d445 --- /dev/null +++ b/robotis_math/include/robotis_math/step_data_define.h @@ -0,0 +1,83 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + +/* + * step_data_define.h + * + * Created on: 2016. 8. 10. + * Author: Jay Song + */ + +#ifndef ROBOTIS_MATH_STEP_DATA_DEFINE_H_ +#define ROBOTIS_MATH_STEP_DATA_DEFINE_H_ + +namespace robotis_framework +{ + +typedef struct +{ + double x, y, z; +} Position3D; + +typedef struct +{ + double x, y, z, roll, pitch, yaw; +} Pose3D; + +typedef struct +{ + int moving_foot; + double foot_z_swap, body_z_swap; + double shoulder_swing_gain, elbow_swing_gain; + double waist_roll_angle, waist_pitch_angle, waist_yaw_angle; + Pose3D left_foot_pose; + Pose3D right_foot_pose; + Pose3D body_pose; +} StepPositionData; + +typedef struct +{ + int walking_state; + double abs_step_time, dsp_ratio; + double start_time_delay_ratio_x, start_time_delay_ratio_y, start_time_delay_ratio_z; + double start_time_delay_ratio_roll, start_time_delay_ratio_pitch, start_time_delay_ratio_yaw; + double finish_time_advance_ratio_x, finish_time_advance_ratio_y, finish_time_advance_ratio_z; + double finish_time_advance_ratio_roll, finish_time_advance_ratio_pitch, finish_time_advance_ratio_yaw; +} StepTimeData; + +typedef struct +{ + StepPositionData position_data; + StepTimeData time_data; +} StepData; + +} + +#endif /* ROBOTIS_MATH_STEP_DATA_DEFINE_H_ */ diff --git a/robotis_math/src/robotis_linear_algebra.cpp b/robotis_math/src/robotis_linear_algebra.cpp index 7683810..293fef3 100644 --- a/robotis_math/src/robotis_linear_algebra.cpp +++ b/robotis_math/src/robotis_linear_algebra.cpp @@ -305,4 +305,18 @@ double calcInner(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b) return vector3d_a.dot(vector3d_b); } +Pose3D getPose3DfromTransformMatrix(Eigen::MatrixXd transform) +{ + Pose3D pose_3d; + + pose_3d.x = transform.coeff(0, 3); + pose_3d.y = transform.coeff(1, 3); + pose_3d.z = transform.coeff(2, 3); + pose_3d.roll = atan2( transform.coeff(2,1), transform.coeff(2,2)); + pose_3d.pitch = atan2(-transform.coeff(2,0), sqrt(transform.coeff(2,1)*transform.coeff(2,1) + transform.coeff(2,2)*transform.coeff(2,2)) ); + pose_3d.yaw = atan2( transform.coeff(1,0), transform.coeff(0,0)); + + return pose_3d; +} + } diff --git a/robotis_math/src/robotis_trajectory_calculator.cpp b/robotis_math/src/robotis_trajectory_calculator.cpp index 5f66ffc..9c8554d 100644 --- a/robotis_math/src/robotis_trajectory_calculator.cpp +++ b/robotis_math/src/robotis_trajectory_calculator.cpp @@ -327,4 +327,288 @@ Eigen::MatrixXd calcArc3dTra(double smp_time, double mov_time, } + +FifthOrderPolynomialTrajectory::FifthOrderPolynomialTrajectory(double initial_time, double initial_pos, double initial_vel, double initial_acc, + double final_time, double final_pos, double final_vel, double final_acc) +{ + position_coeff_.resize(6, 1); + velocity_coeff_.resize(6, 1); + acceleration_coeff_.resize(6, 1); + time_variables_.resize(1, 6); + + position_coeff_.fill(0); + velocity_coeff_.fill(0); + acceleration_coeff_.fill(0); + time_variables_.fill(0); + + if(final_time > initial_time) + { + initial_time_ = initial_time; + initial_pos_ = initial_pos; + initial_vel_ = initial_vel; + initial_acc_ = initial_acc; + + current_time_ = initial_time; + current_pos_ = initial_pos; + current_vel_ = initial_vel; + current_acc_ = initial_acc; + + final_time_ = final_time; + final_pos_ = final_pos; + final_vel_ = final_vel; + final_acc_ = final_acc; + + Eigen::MatrixXd time_mat; + Eigen::MatrixXd conditions_mat; + + time_mat.resize(6,6); + time_mat << powDI(initial_time_, 5), powDI(initial_time_, 4), powDI(initial_time_, 3), powDI(initial_time_, 2), initial_time_, 1.0, + 5.0*powDI(initial_time_, 4), 4.0*powDI(initial_time_, 3), 3.0*powDI(initial_time_, 2), 2.0*initial_time_, 1.0, 0.0, + 20.0*powDI(initial_time_, 3), 12.0*powDI(initial_time_, 2), 6.0*initial_time_, 2.0, 0.0, 0.0; + powDI(final_time_, 5), powDI(final_time_, 4), powDI(final_time_, 3), powDI(final_time_, 2), final_time_, 1.0, + 5.0*powDI(final_time_, 4), 4.0*powDI(final_time_, 3), 3.0*powDI(final_time_, 2), 2.0*final_time_, 1.0, 0.0, + 20.0*powDI(final_time_, 3), 12.0*powDI(final_time_, 2), 6.0*final_time_, 2.0, 0.0, 0.0; + + conditions_mat.resize(6,1); + conditions_mat << initial_pos_, initial_vel_, initial_acc_, final_pos_, final_vel_, final_acc_; + + position_coeff_ = time_mat.inverse() * conditions_mat; + velocity_coeff_ << 0.0, + 5.0*position_coeff_.coeff(0,0), + 4.0*position_coeff_.coeff(0,1), + 3.0*position_coeff_.coeff(0,2), + 2.0*position_coeff_.coeff(0,3), + 1.0*position_coeff_.coeff(0,4); + acceleration_coeff_ << 0.0, + 0.0, + 20.0*position_coeff_.coeff(0,0), + 12.0*position_coeff_.coeff(0,1), + 6.0*position_coeff_.coeff(0,2), + 2.0*position_coeff_.coeff(0,3); + } + +} + +FifthOrderPolynomialTrajectory::FifthOrderPolynomialTrajectory() +{ + initial_time_ = 0; + initial_pos_ = 0; + initial_vel_ = 0; + initial_acc_ = 0; + + current_time_ = 0; + current_pos_ = 0; + current_vel_ = 0; + current_acc_ = 0; + + final_time_ = 0; + final_pos_ = 0; + final_vel_ = 0; + final_acc_ = 0; + + position_coeff_.resize(6, 1); + velocity_coeff_.resize(6, 1); + acceleration_coeff_.resize(6, 1); + time_variables_.resize(1, 6); + + position_coeff_.fill(0); + velocity_coeff_.fill(0); + acceleration_coeff_.fill(0); + time_variables_.fill(0); +} + +FifthOrderPolynomialTrajectory::~FifthOrderPolynomialTrajectory() +{ + +} + +bool FifthOrderPolynomialTrajectory::changeTrajectory(double final_pos, double final_vel, double final_acc) +{ + final_pos_ = final_pos; + final_vel_ = final_vel; + final_acc_ = final_acc; + + Eigen::MatrixXd time_mat; + Eigen::MatrixXd conditions_mat; + + time_mat.resize(6,6); + time_mat << powDI(initial_time_, 5), powDI(initial_time_, 4), powDI(initial_time_, 3), powDI(initial_time_, 2), initial_time_, 1.0, + 5.0*powDI(initial_time_, 4), 4.0*powDI(initial_time_, 3), 3.0*powDI(initial_time_, 2), 2.0*initial_time_, 1.0, 0.0, + 20.0*powDI(initial_time_, 3), 12.0*powDI(initial_time_, 2), 6.0*initial_time_, 2.0, 0.0, 0.0; + powDI(final_time_, 5), powDI(final_time_, 4), powDI(final_time_, 3), powDI(final_time_, 2), final_time_, 1.0, + 5.0*powDI(final_time_, 4), 4.0*powDI(final_time_, 3), 3.0*powDI(final_time_, 2), 2.0*final_time_, 1.0, 0.0, + 20.0*powDI(final_time_, 3), 12.0*powDI(final_time_, 2), 6.0*final_time_, 2.0, 0.0, 0.0; + + conditions_mat.resize(6,1); + conditions_mat << initial_pos_, initial_vel_, initial_acc_, final_pos_, final_vel_, final_acc_; + + position_coeff_ = time_mat.inverse() * conditions_mat; + velocity_coeff_ << 0.0, + 5.0*position_coeff_.coeff(0,0), + 4.0*position_coeff_.coeff(0,1), + 3.0*position_coeff_.coeff(0,2), + 2.0*position_coeff_.coeff(0,3), + 1.0*position_coeff_.coeff(0,4); + acceleration_coeff_ << 0.0, + 0.0, + 20.0*position_coeff_.coeff(0,0), + 12.0*position_coeff_.coeff(0,1), + 6.0*position_coeff_.coeff(0,2), + 2.0*position_coeff_.coeff(0,3); + return true; +} + +bool FifthOrderPolynomialTrajectory::changeTrajectory(double final_time, double final_pos, double final_vel, double final_acc) +{ + if(final_time < initial_time_) + return false; + + final_time_ = final_time; + return changeTrajectory(final_pos, final_vel, final_acc); +} + +bool FifthOrderPolynomialTrajectory::changeTrajectory(double initial_time, double initial_pos, double initial_vel, double initial_acc, + double final_time, double final_pos, double final_vel, double final_acc) +{ + if(final_time < initial_time) + return false; + + initial_time_ = initial_time; + initial_pos_ = initial_pos; + initial_vel_ = initial_vel; + initial_acc_ = initial_acc; + + final_time_ = final_time; + + return changeTrajectory(final_pos, final_vel, final_acc); +} + +double FifthOrderPolynomialTrajectory::getPosition(double time) +{ + if(time >= final_time_) + { + current_time_ = final_time_; + current_pos_ = final_pos_; + current_vel_ = final_vel_; + current_acc_ = final_acc_; + return final_pos_; + } + else if(time <= initial_time_ ) + { + current_time_ = initial_time_; + current_pos_ = initial_pos_; + current_vel_ = initial_vel_; + current_acc_ = initial_acc_; + return initial_pos_; + } + else + { + current_time_ = time; + time_variables_ << powDI(time, 5), powDI(time, 4), powDI(time, 3), powDI(time, 2), time, 1.0; + current_pos_ = (time_variables_ * position_coeff_).coeff(0,0); + current_vel_ = (time_variables_ * velocity_coeff_).coeff(0,0); + current_acc_ = (time_variables_ * acceleration_coeff_).coeff(0,0); + return current_pos_; + } +} + +double FifthOrderPolynomialTrajectory::getVelocity(double time) +{ + if(time >= final_time_) + { + current_time_ = final_time_; + current_pos_ = final_pos_; + current_vel_ = final_vel_; + current_acc_ = final_acc_; + return final_vel_; + } + else if(time <= initial_time_ ) + { + current_time_ = initial_time_; + current_pos_ = initial_pos_; + current_vel_ = initial_vel_; + current_acc_ = initial_acc_; + return initial_vel_; + } + else + { + current_time_ = time; + time_variables_ << powDI(time, 5), powDI(time, 4), powDI(time, 3), powDI(time, 2), time, 1.0; + current_pos_ = (time_variables_ * position_coeff_).coeff(0,0); + current_vel_ = (time_variables_ * velocity_coeff_).coeff(0,0); + current_acc_ = (time_variables_ * acceleration_coeff_).coeff(0,0); + return current_vel_; + } +} + +double FifthOrderPolynomialTrajectory::getAcceleration(double time) +{ + if(time >= final_time_) + { + current_time_ = final_time_; + current_pos_ = final_pos_; + current_vel_ = final_vel_; + current_acc_ = final_acc_; + return final_acc_; + } + else if(time <= initial_time_ ) + { + current_time_ = initial_time_; + current_pos_ = initial_pos_; + current_vel_ = initial_vel_; + current_acc_ = initial_acc_; + return initial_acc_; + } + else + { + current_time_ = time; + time_variables_ << powDI(time, 5), powDI(time, 4), powDI(time, 3), powDI(time, 2), time, 1.0; + current_pos_ = (time_variables_ * position_coeff_).coeff(0,0); + current_vel_ = (time_variables_ * velocity_coeff_).coeff(0,0); + current_acc_ = (time_variables_ * acceleration_coeff_).coeff(0,0); + return current_acc_; + } +} + +void FifthOrderPolynomialTrajectory::setTime(double time) +{ + if(time >= final_time_) + { + current_time_ = final_time_; + current_pos_ = final_pos_; + current_vel_ = final_vel_; + current_acc_ = final_acc_; + } + else if(time <= initial_time_ ) + { + current_time_ = initial_time_; + current_pos_ = initial_pos_; + current_vel_ = initial_vel_; + current_acc_ = initial_acc_; + } + else + { + current_time_ = time; + time_variables_ << powDI(time, 5), powDI(time, 4), powDI(time, 3), powDI(time, 2), time, 1.0; + current_pos_ = (time_variables_ * position_coeff_).coeff(0,0); + current_vel_ = (time_variables_ * velocity_coeff_).coeff(0,0); + current_acc_ = (time_variables_ * acceleration_coeff_).coeff(0,0); + } +} + +double FifthOrderPolynomialTrajectory::getPosition() +{ + return current_pos_; +} + +double FifthOrderPolynomialTrajectory::getVelocity() +{ + return current_vel_; +} + +double FifthOrderPolynomialTrajectory::getAcceleration() +{ + return current_acc_; +} + } From 525ab6cf756ea4340333be00169bc35e790bfeb4 Mon Sep 17 00:00:00 2001 From: s-changhyun Date: Wed, 10 Aug 2016 14:40:32 +0900 Subject: [PATCH 26/33] add function --- .../robotis_controller/robotis_controller.h | 2 +- .../robotis_trajectory_calculator.h | 4 + .../src/robotis_trajectory_calculator.cpp | 80 ++++++++++++++++++- 3 files changed, 82 insertions(+), 4 deletions(-) diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h index ae91370..a24f870 100644 --- a/robotis_controller/include/robotis_controller/robotis_controller.h +++ b/robotis_controller/include/robotis_controller/robotis_controller.h @@ -93,7 +93,7 @@ class RobotisController : public Singleton void initializeSyncWrite(); public: - static const int CONTROL_CYCLE_MSEC = 8; // 8 msec + static const int CONTROL_CYCLE_MSEC = 4; // 8 msec bool DEBUG_PRINT; Robot *robot_; diff --git a/robotis_math/include/robotis_math/robotis_trajectory_calculator.h b/robotis_math/include/robotis_math/robotis_trajectory_calculator.h index 56bf644..1d1f532 100644 --- a/robotis_math/include/robotis_math/robotis_trajectory_calculator.h +++ b/robotis_math/include/robotis_math/robotis_trajectory_calculator.h @@ -50,6 +50,10 @@ Eigen::MatrixXd calcMinimumJerkTra(double pos_start, double vel_start, double ac double pos_end, double vel_end, double accel_end, double smp_time, double mov_time); +Eigen::MatrixXd calcMinimumJerkTraPlus(double pos_start, double vel_start, double accel_start, + double pos_end, double vel_end, double accel_end, + double smp_time, double mov_time); + Eigen::MatrixXd calcMinimumJerkTraWithViaPoints(int via_num, double pos_start, double vel_start, double accel_start, Eigen::MatrixXd pos_via, Eigen::MatrixXd vel_via, Eigen::MatrixXd accel_via, diff --git a/robotis_math/src/robotis_trajectory_calculator.cpp b/robotis_math/src/robotis_trajectory_calculator.cpp index 5f66ffc..bddb6c1 100644 --- a/robotis_math/src/robotis_trajectory_calculator.cpp +++ b/robotis_math/src/robotis_trajectory_calculator.cpp @@ -40,9 +40,11 @@ namespace robotis_framework { -Eigen::MatrixXd calcMinimumJerkTra(double pos_start, double vel_start, double accel_start, - double pos_end, double vel_end, double accel_end, - double smp_time, double mov_time) +Eigen::MatrixXd calcMinimumJerkTra( + double pos_start, double vel_start, double accel_start, + double pos_end, double vel_end, double accel_end, + double smp_time, double mov_time + ) /* simple minimum jerk trajectory @@ -97,6 +99,78 @@ Eigen::MatrixXd calcMinimumJerkTra(double pos_start, double vel_start, double ac return minimum_jerk_tra; } +Eigen::MatrixXd calcMinimumJerkTraPlus( + double pos_start, double vel_start, double accel_start, + double pos_end, double vel_end, double accel_end, + double smp_time, double mov_time + ) +/* + simple minimum jerk trajectory + + pos_start : position at initial state + vel_start : velocity at initial state + accel_start : acceleration at initial state + + pos_end : position at final state + vel_end : velocity at final state + accel_end : acceleration at final state + + smp_time : sampling time + mov_time : movement time + */ + +{ + Eigen::MatrixXd poly_matrix(3,3); + Eigen::MatrixXd poly_vector(3,1); + + poly_matrix << + pow(mov_time,3), pow(mov_time,4), pow(mov_time,5), + 3*pow(mov_time,2), 4*pow(mov_time,3), 5*pow(mov_time,4), + 6*mov_time, 12*pow(mov_time,2), 20*pow(mov_time,3); + + poly_vector << + pos_end-pos_start-vel_start*mov_time-accel_start*pow(mov_time,2)/2, + vel_end-vel_start-accel_start*mov_time, + accel_end-accel_start ; + + Eigen::Matrix poly_coeff = poly_matrix.inverse() * poly_vector; + + double time_steps = mov_time/smp_time; + int all_time_steps = round(time_steps+1); + + Eigen::MatrixXd time = Eigen::MatrixXd::Zero(all_time_steps,1); + Eigen::MatrixXd minimum_jerk_tra = Eigen::MatrixXd::Zero(all_time_steps,3); + + for (int step=0; step Date: Wed, 10 Aug 2016 20:50:56 +0900 Subject: [PATCH 27/33] fix bug --- .../include/robotis_controller/robotis_controller.h | 2 +- robotis_math/src/robotis_trajectory_calculator.cpp | 5 +++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h index a24f870..ae91370 100644 --- a/robotis_controller/include/robotis_controller/robotis_controller.h +++ b/robotis_controller/include/robotis_controller/robotis_controller.h @@ -93,7 +93,7 @@ class RobotisController : public Singleton void initializeSyncWrite(); public: - static const int CONTROL_CYCLE_MSEC = 4; // 8 msec + static const int CONTROL_CYCLE_MSEC = 8; // 8 msec bool DEBUG_PRINT; Robot *robot_; diff --git a/robotis_math/src/robotis_trajectory_calculator.cpp b/robotis_math/src/robotis_trajectory_calculator.cpp index bc3b359..c436310 100644 --- a/robotis_math/src/robotis_trajectory_calculator.cpp +++ b/robotis_math/src/robotis_trajectory_calculator.cpp @@ -35,7 +35,7 @@ * Author: sch */ -#include "../include/robotis_math/robotis_trajectory_calculator.h" +#include "robotis_math/robotis_trajectory_calculator.h" namespace robotis_framework { @@ -508,7 +508,7 @@ bool FifthOrderPolynomialTrajectory::changeTrajectory(double final_pos, double time_mat.resize(6,6); time_mat << powDI(initial_time_, 5), powDI(initial_time_, 4), powDI(initial_time_, 3), powDI(initial_time_, 2), initial_time_, 1.0, 5.0*powDI(initial_time_, 4), 4.0*powDI(initial_time_, 3), 3.0*powDI(initial_time_, 2), 2.0*initial_time_, 1.0, 0.0, - 20.0*powDI(initial_time_, 3), 12.0*powDI(initial_time_, 2), 6.0*initial_time_, 2.0, 0.0, 0.0; + 20.0*powDI(initial_time_, 3), 12.0*powDI(initial_time_, 2), 6.0*initial_time_, 2.0, 0.0, 0.0, powDI(final_time_, 5), powDI(final_time_, 4), powDI(final_time_, 3), powDI(final_time_, 2), final_time_, 1.0, 5.0*powDI(final_time_, 4), 4.0*powDI(final_time_, 3), 3.0*powDI(final_time_, 2), 2.0*final_time_, 1.0, 0.0, 20.0*powDI(final_time_, 3), 12.0*powDI(final_time_, 2), 6.0*final_time_, 2.0, 0.0, 0.0; @@ -529,6 +529,7 @@ bool FifthOrderPolynomialTrajectory::changeTrajectory(double final_pos, double 12.0*position_coeff_.coeff(0,1), 6.0*position_coeff_.coeff(0,2), 2.0*position_coeff_.coeff(0,3); + return true; } From ed7f92e0dfd33f63f4b582f2cfad5af9461a7a42 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Thu, 11 Aug 2016 20:02:06 +0900 Subject: [PATCH 28/33] - SyncWriteItem bug fixed. --- .../robotis_controller/robotis_controller.cpp | 30 +++++++++++++++---- 1 file changed, 25 insertions(+), 5 deletions(-) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index c568d82..0029e4f 100644 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -1173,11 +1173,31 @@ void RobotisController::syncWriteItemCallback(const robotis_controller_msgs::Syn { for (int i = 0; i < msg->joint_name.size(); i++) { - Dynamixel *dxl = robot_->dxls_[msg->joint_name[i]]; - ControlTableItem *item = dxl->ctrl_table_[msg->item_name]; + Device *device; - dynamixel::PortHandler *port = robot_->ports_[dxl->port_name_]; - dynamixel::PacketHandler *packet_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + auto d_it1 = robot_->dxls_.find(msg->joint_name[i]); + if (d_it1 != robot_->dxls_.end()) + { + device = d_it1->second; + } + else + { + auto d_it2 = robot_->sensors_.find(msg->joint_name[i]); + if (d_it2 != robot_->sensors_.end()) + { + device = d_it2->second; + } + else + { + // could not find the device + continue; + } + } + + ControlTableItem *item = device->ctrl_table_[msg->item_name]; + + dynamixel::PortHandler *port = robot_->ports_[device->port_name_]; + dynamixel::PacketHandler *packet_handler = dynamixel::PacketHandler::getPacketHandler(device->protocol_version_); if (item->access_type_ == Read) continue; @@ -1215,7 +1235,7 @@ void RobotisController::syncWriteItemCallback(const robotis_controller_msgs::Syn data[2] = DXL_LOBYTE(DXL_HIWORD((uint32_t)msg->value[i])); data[3] = DXL_HIBYTE(DXL_HIWORD((uint32_t)msg->value[i])); } - direct_sync_write_[idx]->addParam(dxl->id_, data); + direct_sync_write_[idx]->addParam(device->id_, data); delete[] data; } } From f1f9c888867f58ef0f48f3671730e1e77e662a79 Mon Sep 17 00:00:00 2001 From: Jay-Song Date: Thu, 11 Aug 2016 20:07:58 +0900 Subject: [PATCH 29/33] Remove robotis_math robotis_math is moved to ROBOTIS_Math repository --- robotis_math/CMakeLists.txt | 42 -- .../robotis_math/robotis_linear_algebra.h | 81 -- .../include/robotis_math/robotis_math.h | 43 -- .../include/robotis_math/robotis_math_base.h | 63 -- .../robotis_trajectory_calculator.h | 113 --- .../include/robotis_math/step_data_define.h | 83 --- robotis_math/package.xml | 19 - robotis_math/src/robotis_linear_algebra.cpp | 322 -------- robotis_math/src/robotis_math_base.cpp | 53 -- .../src/robotis_trajectory_calculator.cpp | 689 ------------------ 10 files changed, 1508 deletions(-) delete mode 100644 robotis_math/CMakeLists.txt delete mode 100644 robotis_math/include/robotis_math/robotis_linear_algebra.h delete mode 100644 robotis_math/include/robotis_math/robotis_math.h delete mode 100644 robotis_math/include/robotis_math/robotis_math_base.h delete mode 100644 robotis_math/include/robotis_math/robotis_trajectory_calculator.h delete mode 100644 robotis_math/include/robotis_math/step_data_define.h delete mode 100644 robotis_math/package.xml delete mode 100644 robotis_math/src/robotis_linear_algebra.cpp delete mode 100644 robotis_math/src/robotis_math_base.cpp delete mode 100644 robotis_math/src/robotis_trajectory_calculator.cpp diff --git a/robotis_math/CMakeLists.txt b/robotis_math/CMakeLists.txt deleted file mode 100644 index 7ce8d9a..0000000 --- a/robotis_math/CMakeLists.txt +++ /dev/null @@ -1,42 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(robotis_math) - -find_package(catkin REQUIRED COMPONENTS - roscpp - cmake_modules -) - -find_package(Eigen REQUIRED) - -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS include - LIBRARIES robotis_math - CATKIN_DEPENDS roscpp -# DEPENDS system_lib -) - -########### -## Build ## -########### - -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${Eigen_INCLUDE_DIRS} -) - -add_library(robotis_math - src/robotis_math_base.cpp - src/robotis_trajectory_calculator.cpp - src/robotis_linear_algebra.cpp -) - -add_dependencies(robotis_math ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -target_link_libraries(robotis_math - ${catkin_LIBRARIES} -) diff --git a/robotis_math/include/robotis_math/robotis_linear_algebra.h b/robotis_math/include/robotis_math/robotis_linear_algebra.h deleted file mode 100644 index f8cd69e..0000000 --- a/robotis_math/include/robotis_math/robotis_linear_algebra.h +++ /dev/null @@ -1,81 +0,0 @@ -/******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of ROBOTIS nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ - -/* - * robotis_linear_algebra.h - * - * Created on: June 7, 2016 - * Author: sch - */ - -#ifndef ROBOTIS_MATH_ROBOTIS_LINEAR_ALGEBRA_H_ -#define ROBOTIS_MATH_ROBOTIS_LINEAR_ALGEBRA_H_ - -#include - -#define EIGEN_NO_DEBUG -#define EIGEN_NO_STATIC_ASSERT - -#include -#include "step_data_define.h" - -namespace robotis_framework -{ - -Eigen::MatrixXd getTransitionXYZ(double position_x, double position_y, double position_z); -Eigen::MatrixXd getTransformationXYZRPY(double position_x, double position_y, double position_z , double roll, double pitch, double yaw); -Eigen::MatrixXd getInverseTransformation(Eigen::MatrixXd transform); -Eigen::MatrixXd getInertiaXYZ(double ixx, double ixy, double ixz , double iyy , double iyz, double izz); -Eigen::MatrixXd getRotationX(double angle); -Eigen::MatrixXd getRotationY(double angle); -Eigen::MatrixXd getRotationZ(double angle); -Eigen::MatrixXd getRotation4d(double roll, double pitch, double yaw); -Eigen::MatrixXd getTranslation4D(double position_x, double position_y, double position_z); - -Eigen::MatrixXd convertRotationToRPY(Eigen::MatrixXd rotation); -Eigen::MatrixXd convertRPYToRotation(double roll, double pitch, double yaw); -Eigen::Quaterniond convertRPYToQuaternion(double roll, double pitch, double yaw); -Eigen::Quaterniond convertRotationToQuaternion(Eigen::MatrixXd rotation); -Eigen::MatrixXd convertQuaternionToRPY(Eigen::Quaterniond quaternion); -Eigen::MatrixXd convertQuaternionToRotation(Eigen::Quaterniond quaternion); - -Eigen::MatrixXd calcHatto(Eigen::MatrixXd matrix3d); -Eigen::MatrixXd calcRodrigues(Eigen::MatrixXd hat_matrix, double angle); -Eigen::MatrixXd convertRotToOmega(Eigen::MatrixXd rotation); -Eigen::MatrixXd calcCross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b); -double calcInner(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b); - -Pose3D getPose3DfromTransformMatrix(Eigen::MatrixXd transform); - -} - - - -#endif /* ROBOTIS_MATH_ROBOTIS_LINEAR_ALGEBRA_H_ */ diff --git a/robotis_math/include/robotis_math/robotis_math.h b/robotis_math/include/robotis_math/robotis_math.h deleted file mode 100644 index f78f4be..0000000 --- a/robotis_math/include/robotis_math/robotis_math.h +++ /dev/null @@ -1,43 +0,0 @@ -/******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of ROBOTIS nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ - -/* - * robotis_math.h - * - * Created on: June 7, 2016 - * Author: sch - */ - -#ifndef ROBOTIS_MATH_ROBOTIS_MATH_H_ -#define ROBOTIS_MATH_ROBOTIS_MATH_H_ - -#include "robotis_trajectory_calculator.h" - -#endif /* ROBOTIS_MATH_ROBOTIS_MATH_H_ */ diff --git a/robotis_math/include/robotis_math/robotis_math_base.h b/robotis_math/include/robotis_math/robotis_math_base.h deleted file mode 100644 index e54caca..0000000 --- a/robotis_math/include/robotis_math/robotis_math_base.h +++ /dev/null @@ -1,63 +0,0 @@ -/******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of ROBOTIS nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ - -/* - * robotis_math_base.h - * - * Created on: June 7, 2016 - * Author: sch - */ - -#ifndef ROBOTIS_MATH_ROBOTIS_MATH_BASE_H_ -#define ROBOTIS_MATH_ROBOTIS_MATH_BASE_H_ - -#include - -namespace robotis_framework -{ - -#define PRINT_VAR(X) std::cout << #X << " : " << X << std::endl -#define PRINT_MAT(X) std::cout << #X << ":\n" << X << std::endl << std::endl - -#define DEGREE2RADIAN (M_PI / 180.0) -#define RADIAN2DEGREE (180.0 / M_PI) - -inline double powDI(double a, int b) -{ - return (b == 0 ? 1 : (b > 0 ? a * powDI(a, b - 1) : 1 / powDI(a, -b))); -} - -double sign(double x); - -} - - - -#endif /* ROBOTIS_MATH_ROBOTIS_MATH_BASE_H_ */ diff --git a/robotis_math/include/robotis_math/robotis_trajectory_calculator.h b/robotis_math/include/robotis_math/robotis_trajectory_calculator.h deleted file mode 100644 index 670264f..0000000 --- a/robotis_math/include/robotis_math/robotis_trajectory_calculator.h +++ /dev/null @@ -1,113 +0,0 @@ -/******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of ROBOTIS nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ - -/* - * robotis_trajectory_calculator.h - * - * Created on: June 7, 2016 - * Author: sch - */ - -#ifndef ROBOTIS_MATH_ROBOTIS_TRAJECTORY_CALCULATOR_H_ -#define ROBOTIS_MATH_ROBOTIS_TRAJECTORY_CALCULATOR_H_ - -#include "robotis_linear_algebra.h" -#include "robotis_math_base.h" - -// minimum jerk trajectory - -namespace robotis_framework -{ - -Eigen::MatrixXd calcMinimumJerkTra(double pos_start, double vel_start, double accel_start, - double pos_end, double vel_end, double accel_end, - double smp_time, double mov_time); - -Eigen::MatrixXd calcMinimumJerkTraPlus(double pos_start, double vel_start, double accel_start, - double pos_end, double vel_end, double accel_end, - double smp_time, double mov_time); - -Eigen::MatrixXd calcMinimumJerkTraWithViaPoints(int via_num, - double pos_start, double vel_start, double accel_start, - Eigen::MatrixXd pos_via, Eigen::MatrixXd vel_via, Eigen::MatrixXd accel_via, - double pos_end, double vel_end, double accel_end, - double smp_time, Eigen::MatrixXd via_time, double mov_time); - -Eigen::MatrixXd calcArc3dTra(double smp_time, double mov_time, - Eigen::MatrixXd center_point, Eigen::MatrixXd normal_vector, Eigen::MatrixXd start_point, - double rotation_angle, double cross_ratio); - - -class FifthOrderPolynomialTrajectory -{ -public: - FifthOrderPolynomialTrajectory(double initial_time, double initial_pos, double initial_vel, double initial_acc, - double final_time, double final_pos, double final_vel, double final_acc); - FifthOrderPolynomialTrajectory(); - ~FifthOrderPolynomialTrajectory(); - - bool changeTrajectory(double final_pos, double final_vel, double final_acc); - bool changeTrajectory(double final_time, double final_pos, double final_vel, double final_acc); - bool changeTrajectory(double initial_time, double initial_pos, double initial_vel, double initial_acc, - double final_time, double final_pos, double final_vel, double final_acc); - - double getPosition(double time); - double getVelocity(double time); - double getAcceleration(double time); - - void setTime(double time); - double getPosition(); - double getVelocity(); - double getAcceleration(); - - double initial_time_; - double initial_pos_; - double initial_vel_; - double initial_acc_; - - double current_time_; - double current_pos_; - double current_vel_; - double current_acc_; - - double final_time_; - double final_pos_; - double final_vel_; - double final_acc_; - - Eigen::MatrixXd position_coeff_; - Eigen::MatrixXd velocity_coeff_; - Eigen::MatrixXd acceleration_coeff_; - Eigen::MatrixXd time_variables_; -}; - -} - -#endif /* ROBOTIS_MATH_ROBOTIS_TRAJECTORY_CALCULATOR_H_ */ diff --git a/robotis_math/include/robotis_math/step_data_define.h b/robotis_math/include/robotis_math/step_data_define.h deleted file mode 100644 index 135d445..0000000 --- a/robotis_math/include/robotis_math/step_data_define.h +++ /dev/null @@ -1,83 +0,0 @@ -/******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of ROBOTIS nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ - -/* - * step_data_define.h - * - * Created on: 2016. 8. 10. - * Author: Jay Song - */ - -#ifndef ROBOTIS_MATH_STEP_DATA_DEFINE_H_ -#define ROBOTIS_MATH_STEP_DATA_DEFINE_H_ - -namespace robotis_framework -{ - -typedef struct -{ - double x, y, z; -} Position3D; - -typedef struct -{ - double x, y, z, roll, pitch, yaw; -} Pose3D; - -typedef struct -{ - int moving_foot; - double foot_z_swap, body_z_swap; - double shoulder_swing_gain, elbow_swing_gain; - double waist_roll_angle, waist_pitch_angle, waist_yaw_angle; - Pose3D left_foot_pose; - Pose3D right_foot_pose; - Pose3D body_pose; -} StepPositionData; - -typedef struct -{ - int walking_state; - double abs_step_time, dsp_ratio; - double start_time_delay_ratio_x, start_time_delay_ratio_y, start_time_delay_ratio_z; - double start_time_delay_ratio_roll, start_time_delay_ratio_pitch, start_time_delay_ratio_yaw; - double finish_time_advance_ratio_x, finish_time_advance_ratio_y, finish_time_advance_ratio_z; - double finish_time_advance_ratio_roll, finish_time_advance_ratio_pitch, finish_time_advance_ratio_yaw; -} StepTimeData; - -typedef struct -{ - StepPositionData position_data; - StepTimeData time_data; -} StepData; - -} - -#endif /* ROBOTIS_MATH_STEP_DATA_DEFINE_H_ */ diff --git a/robotis_math/package.xml b/robotis_math/package.xml deleted file mode 100644 index 29d618b..0000000 --- a/robotis_math/package.xml +++ /dev/null @@ -1,19 +0,0 @@ - - - robotis_math - 0.0.0 - The robotis_math package - - sch - BSD - - sch - - catkin - roscpp - cmake_modules - - roscpp - cmake_modules - - \ No newline at end of file diff --git a/robotis_math/src/robotis_linear_algebra.cpp b/robotis_math/src/robotis_linear_algebra.cpp deleted file mode 100644 index 293fef3..0000000 --- a/robotis_math/src/robotis_linear_algebra.cpp +++ /dev/null @@ -1,322 +0,0 @@ -/******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of ROBOTIS nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ - -/* - * robotis_linear_algebra.cpp - * - * Created on: June 7, 2016 - * Author: sch - */ - -#include "robotis_math/robotis_linear_algebra.h" - -namespace robotis_framework -{ - -Eigen::MatrixXd getTransitionXYZ(double position_x, double position_y, double position_z) -{ - Eigen::MatrixXd position(3,1); - - position << - position_x, - position_y, - position_z; - - return position; -} - -Eigen::MatrixXd getTransformationXYZRPY(double position_x, double position_y, double position_z , double roll , double pitch , double yaw) -{ - Eigen::MatrixXd transformation = getRotation4d(roll, pitch, yaw); - transformation.coeffRef(0,3) = position_x; - transformation.coeffRef(1,3) = position_y; - transformation.coeffRef(2,3) = position_z; - - return transformation; -} - -Eigen::MatrixXd getInverseTransformation(Eigen::MatrixXd transform) -{ - // If T is Transform Matrix A from B, the BOA is translation component coordi. B to coordi. A - - Eigen::Vector3d vec_boa; - Eigen::Vector3d vec_x, vec_y, vec_z; - Eigen::MatrixXd inv_t(4,4); - - vec_boa(0) = -transform(0,3); - vec_boa(1) = -transform(1,3); - vec_boa(2) = -transform(2,3); - - vec_x(0) = transform(0,0); vec_x(1) = transform(1,0); vec_x(2) = transform(2,0); - vec_y(0) = transform(0,1); vec_y(1) = transform(1,1); vec_y(2) = transform(2,1); - vec_z(0) = transform(0,2); vec_z(1) = transform(1,2); vec_z(2) = transform(2,2); - - inv_t << - vec_x(0), vec_x(1), vec_x(2), vec_boa.dot(vec_x), - vec_y(0), vec_y(1), vec_y(2), vec_boa.dot(vec_y), - vec_z(0), vec_z(1), vec_z(2), vec_boa.dot(vec_z), - 0, 0, 0, 1; - - return inv_t; -} - -Eigen::MatrixXd getInertiaXYZ(double ixx, double ixy, double ixz , double iyy , double iyz, double izz) -{ - Eigen::MatrixXd inertia(3,3); - - inertia << - ixx, ixy, ixz, - ixy, iyy, iyz, - ixz, iyz, izz; - - return inertia; -} - -Eigen::MatrixXd getRotationX(double angle) -{ - Eigen::MatrixXd rotation(3,3); - - rotation << - 1.0, 0.0, 0.0, - 0.0, cos(angle), -sin(angle), - 0.0, sin(angle), cos(angle); - - return rotation; -} - -Eigen::MatrixXd getRotationY(double angle) -{ - Eigen::MatrixXd rotation(3,3); - - rotation << - cos(angle), 0.0, sin(angle), - 0.0, 1.0, 0.0, - -sin(angle), 0.0, cos(angle); - - return rotation; -} - -Eigen::MatrixXd getRotationZ(double angle) -{ - Eigen::MatrixXd rotation(3,3); - - rotation << - cos(angle), -sin(angle), 0.0, - sin(angle), cos(angle), 0.0, - 0.0, 0.0, 1.0; - - return rotation; -} - -Eigen::MatrixXd getRotation4d(double roll, double pitch, double yaw ) -{ - double sr = sin(roll), cr = cos(roll); - double sp = sin(pitch), cp = cos(pitch); - double sy = sin(yaw), cy = cos(yaw); - - Eigen::MatrixXd mat_roll(4,4); - Eigen::MatrixXd mat_pitch(4,4); - Eigen::MatrixXd mat_yaw(4,4); - - mat_roll << - 1, 0, 0, 0, - 0, cr, -sr, 0, - 0, sr, cr, 0, - 0, 0, 0, 1; - - mat_pitch << - cp, 0, sp, 0, - 0, 1, 0, 0, - -sp, 0, cp, 0, - 0, 0, 0, 1; - - mat_yaw << - cy, -sy, 0, 0, - sy, cy, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1; - - Eigen::MatrixXd mat_rpy = (mat_yaw*mat_pitch)*mat_roll; - - return mat_rpy; -} - -Eigen::MatrixXd getTranslation4D(double position_x, double position_y, double position_z) -{ - Eigen::MatrixXd mat_translation(4,4); - - mat_translation << - 1, 0, 0, position_x, - 0, 1, 0, position_y, - 0, 0, 1, position_z, - 0, 0, 0, 1; - - return mat_translation; -} - -Eigen::MatrixXd convertRotationToRPY(Eigen::MatrixXd rotation) -{ - Eigen::MatrixXd rpy = Eigen::MatrixXd::Zero(3,1); - - rpy.coeffRef(0,0) = atan2(rotation.coeff(2,1), rotation.coeff(2,2)); - rpy.coeffRef(1,0) = atan2(-rotation.coeff(2,0), sqrt(pow(rotation.coeff(2,1), 2) + pow(rotation.coeff(2,2),2))); - rpy.coeffRef(2,0) = atan2 (rotation.coeff(1,0), rotation.coeff(0,0)); - - return rpy; -} - -Eigen::MatrixXd convertRPYToRotation(double roll, double pitch, double yaw) -{ - Eigen::MatrixXd rotation = getRotationZ(yaw)*getRotationY(pitch)*getRotationX(roll); - - return rotation; -} - -Eigen::Quaterniond convertRPYToQuaternion(double roll, double pitch, double yaw) -{ - Eigen::MatrixXd rotation = convertRPYToRotation(roll,pitch,yaw); - - Eigen::Matrix3d rotation3d; - rotation3d = rotation.block<3,3>(0,0); - - Eigen::Quaterniond quaternion; - quaternion = rotation3d; - - return quaternion; -} - -Eigen::Quaterniond convertRotationToQuaternion(Eigen::MatrixXd rotation) -{ - Eigen::Matrix3d rotation3d; - rotation3d = rotation.block<3,3>(0,0); - - Eigen::Quaterniond quaternion; - quaternion = rotation3d; - - return quaternion; -} - -Eigen::MatrixXd convertQuaternionToRPY(Eigen::Quaterniond quaternion) -{ - Eigen::MatrixXd rpy = convertRotationToRPY(quaternion.toRotationMatrix()); - - return rpy; -} - -Eigen::MatrixXd convertQuaternionToRotation(Eigen::Quaterniond quaternion) -{ - Eigen::MatrixXd rotation = quaternion.toRotationMatrix(); - - return rotation; -} - -Eigen::MatrixXd calcHatto(Eigen::MatrixXd matrix3d) -{ - Eigen::MatrixXd hatto(3,3); - - hatto << - 0.0, -matrix3d.coeff(2,0), matrix3d.coeff(1,0), - matrix3d.coeff(2,0), 0.0, -matrix3d.coeff(0,0), - -matrix3d.coeff(1,0), matrix3d.coeff(0,0), 0.0; - - return hatto; -} - -Eigen::MatrixXd calcRodrigues(Eigen::MatrixXd hat_matrix, double angle) -{ - Eigen::MatrixXd identity = Eigen::MatrixXd::Identity(3,3); - Eigen::MatrixXd rodrigues = identity+hat_matrix*sin(angle)+hat_matrix*hat_matrix*(1-cos(angle)); - - return rodrigues; -} - -Eigen::MatrixXd convertRotToOmega(Eigen::MatrixXd rotation) -{ - double eps = 1e-12; - - double alpha = (rotation.coeff(0,0)+rotation.coeff(1,1)+rotation.coeff(2,2)-1.0)/2.0; - double alpha_dash = fabs( alpha - 1.0 ); - - Eigen::MatrixXd rot_to_omega(3,1); - - if( alpha_dash < eps ) - { - rot_to_omega << - 0.0, - 0.0, - 0.0; - } - else - { - double theta = acos(alpha); - - rot_to_omega << - rotation.coeff(2,1)-rotation.coeff(1,2), - rotation.coeff(0,2)-rotation.coeff(2,0), - rotation.coeff(1,0)-rotation.coeff(0,1); - - rot_to_omega = 0.5*(theta/sin(theta))*rot_to_omega; - } - - return rot_to_omega; -} - -Eigen::MatrixXd calcCross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b) -{ - Eigen::MatrixXd cross(3,1); - - cross << - vector3d_a.coeff(1,0)*vector3d_b.coeff(2,0)-vector3d_a.coeff(2,0)*vector3d_b.coeff(1,0), - vector3d_a.coeff(2,0)*vector3d_b.coeff(0,0)-vector3d_a.coeff(0,0)*vector3d_b.coeff(2,0), - vector3d_a.coeff(0,0)*vector3d_b.coeff(1,0)-vector3d_a.coeff(1,0)*vector3d_b.coeff(0,0); - - return cross; -} - -double calcInner(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b) -{ - return vector3d_a.dot(vector3d_b); -} - -Pose3D getPose3DfromTransformMatrix(Eigen::MatrixXd transform) -{ - Pose3D pose_3d; - - pose_3d.x = transform.coeff(0, 3); - pose_3d.y = transform.coeff(1, 3); - pose_3d.z = transform.coeff(2, 3); - pose_3d.roll = atan2( transform.coeff(2,1), transform.coeff(2,2)); - pose_3d.pitch = atan2(-transform.coeff(2,0), sqrt(transform.coeff(2,1)*transform.coeff(2,1) + transform.coeff(2,2)*transform.coeff(2,2)) ); - pose_3d.yaw = atan2( transform.coeff(1,0), transform.coeff(0,0)); - - return pose_3d; -} - -} diff --git a/robotis_math/src/robotis_math_base.cpp b/robotis_math/src/robotis_math_base.cpp deleted file mode 100644 index bd37d51..0000000 --- a/robotis_math/src/robotis_math_base.cpp +++ /dev/null @@ -1,53 +0,0 @@ -/******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of ROBOTIS nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ - -/* - * robotis_math_base.cpp - * - * Created on: June 7, 2016 - * Author: sch - */ - -#include "robotis_math/robotis_math_base.h" - -namespace robotis_framework -{ - -double sign(double x) -{ - if ( x < 0.0 ) - return -1.0; - else if ( x > 0.0) - return 1.0; - else - return 0.0; -} - -} diff --git a/robotis_math/src/robotis_trajectory_calculator.cpp b/robotis_math/src/robotis_trajectory_calculator.cpp deleted file mode 100644 index c436310..0000000 --- a/robotis_math/src/robotis_trajectory_calculator.cpp +++ /dev/null @@ -1,689 +0,0 @@ -/******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of ROBOTIS nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ - -/* - * robotis_trajectory_calculator.cpp - * - * Created on: June 7, 2016 - * Author: sch - */ - -#include "robotis_math/robotis_trajectory_calculator.h" - -namespace robotis_framework -{ - -Eigen::MatrixXd calcMinimumJerkTra( - double pos_start, double vel_start, double accel_start, - double pos_end, double vel_end, double accel_end, - double smp_time, double mov_time - ) -/* - simple minimum jerk trajectory - - pos_start : position at initial state - vel_start : velocity at initial state - accel_start : acceleration at initial state - - pos_end : position at final state - vel_end : velocity at final state - accel_end : acceleration at final state - - smp_time : sampling time - mov_time : movement time - */ - -{ - Eigen::MatrixXd poly_matrix(3,3); - Eigen::MatrixXd poly_vector(3,1); - - poly_matrix << - pow(mov_time,3), pow(mov_time,4), pow(mov_time,5), - 3*pow(mov_time,2), 4*pow(mov_time,3), 5*pow(mov_time,4), - 6*mov_time, 12*pow(mov_time,2), 20*pow(mov_time,3); - - poly_vector << - pos_end-pos_start-vel_start*mov_time-accel_start*pow(mov_time,2)/2, - vel_end-vel_start-accel_start*mov_time, - accel_end-accel_start ; - - Eigen::Matrix poly_coeff = poly_matrix.inverse() * poly_vector; - - double time_steps = mov_time/smp_time; - int all_time_steps = round(time_steps+1); - - Eigen::MatrixXd time = Eigen::MatrixXd::Zero(all_time_steps,1); - Eigen::MatrixXd minimum_jerk_tra = Eigen::MatrixXd::Zero(all_time_steps,1); - - for (int step=0; step poly_coeff = poly_matrix.inverse() * poly_vector; - - double time_steps = mov_time/smp_time; - int all_time_steps = round(time_steps+1); - - Eigen::MatrixXd time = Eigen::MatrixXd::Zero(all_time_steps,1); - Eigen::MatrixXd minimum_jerk_tra = Eigen::MatrixXd::Zero(all_time_steps,3); - - for (int step=0; step j) - k = i ; - else - k = j ; - - poly_matrix_part2.coeffRef(3*j-3,3*i-3) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),3)/6 ; - poly_matrix_part2.coeffRef(3*j-3,3*i-2) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),4)/24 ; - poly_matrix_part2.coeffRef(3*j-3,3*i-1) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),5)/120 ; - - poly_matrix_part2.coeffRef(3*j-2,3*i-3) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),2)/2 ; - poly_matrix_part2.coeffRef(3*j-2,3*i-2) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),3)/6 ; - poly_matrix_part2.coeffRef(3*j-2,3*i-1) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),4)/24 ; - - poly_matrix_part2.coeffRef(3*j-1,3*i-3) = via_time.coeff(k-1,0)-via_time.coeff(i-1,0) ; - poly_matrix_part2.coeffRef(3*j-1,3*i-2) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),2)/2 ; - poly_matrix_part2.coeffRef(3*j-1,3*i-1) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),3)/6 ; - } - } - - - Eigen::MatrixXd poly_matrix_part3 = Eigen::MatrixXd::Zero(3,3*via_num+3); - - poly_matrix_part3.coeffRef(0,0) = pow(mov_time,3); - poly_matrix_part3.coeffRef(0,1) = pow(mov_time,4); - poly_matrix_part3.coeffRef(0,2) = pow(mov_time,5); - - poly_matrix_part3.coeffRef(1,0) = 3*pow(mov_time,2); - poly_matrix_part3.coeffRef(1,1) = 4*pow(mov_time,3); - poly_matrix_part3.coeffRef(1,2) = 5*pow(mov_time,4); - - poly_matrix_part3.coeffRef(2,0) = 6*mov_time; - poly_matrix_part3.coeffRef(2,1) = 12*pow(mov_time,2); - poly_matrix_part3.coeffRef(2,2) = 20*pow(mov_time,3); - - for (int i=1; i<=via_num; i++) - { - poly_matrix_part3.coeffRef(0,3*i) = pow(mov_time-via_time.coeff(i-1,0),3)/6 ; - poly_matrix_part3.coeffRef(1,3*i) = pow(mov_time-via_time.coeff(i-1,0),2)/2 ; - poly_matrix_part3.coeffRef(2,3*i) = mov_time-via_time.coeff(i-1,0) ; - - poly_matrix_part3.coeffRef(0,3*i+1) = pow(mov_time-via_time.coeff(i-1,0),4)/24 ; - poly_matrix_part3.coeffRef(1,3*i+1) = pow(mov_time-via_time.coeff(i-1,0),3)/6 ; - poly_matrix_part3.coeffRef(2,3*i+1) = pow(mov_time-via_time.coeff(i-1,0),2)/2 ; - - poly_matrix_part3.coeffRef(0,3*i+2) = pow(mov_time-via_time.coeff(i-1,0),5)/120 ; - poly_matrix_part3.coeffRef(1,3*i+2) = pow(mov_time-via_time.coeff(i-1,0),4)/24 ; - poly_matrix_part3.coeffRef(2,3*i+2) = pow(mov_time-via_time.coeff(i-1,0),3)/6 ; - } - - Eigen::MatrixXd poly_matrix = Eigen::MatrixXd::Zero(3*via_num+3,3*via_num+3); - - poly_matrix.block(0,0,3*via_num,3) = poly_matrix_part1 ; - poly_matrix.block(0,3,3*via_num,3*via_num) = poly_matrix_part2 ; - poly_matrix.block(3*via_num,0,3,3*via_num+3) = poly_matrix_part3 ; - - Eigen::MatrixXd poly_coeff(3*via_num+3,1); - //C = A.inverse()*B; - poly_coeff = poly_matrix.colPivHouseholderQr().solve(poly_vector); - - int all_time_steps; - all_time_steps = round(mov_time/smp_time) ; - - Eigen::MatrixXd time_vector = Eigen::MatrixXd::Zero(all_time_steps+1,1); - - for (int i=1; i<=all_time_steps+1; i++) - time_vector.coeffRef(i-1,0) = (i-1)*smp_time; - - Eigen::MatrixXd via_time_vector = Eigen::MatrixXd::Zero(via_num,1); - - for (int i=1; i<=via_num; i++) - via_time_vector.coeffRef(i-1,0) = round(via_time.coeff(i-1,0)/smp_time)+2; - - Eigen::MatrixXd minimum_jerk_tra_with_via_points = Eigen::MatrixXd::Zero(all_time_steps+1,1); - - for (int i=1; i<=all_time_steps+1; i++) - { - minimum_jerk_tra_with_via_points.coeffRef(i-1,0) = - pos_start + - vel_start*time_vector.coeff(i-1,0) + - 0.5*accel_start*pow(time_vector.coeff(i-1,0),2) + - poly_coeff.coeff(0,0)*pow(time_vector.coeff(i-1,0),3) + - poly_coeff.coeff(1,0)*pow(time_vector.coeff(i-1,0),4) + - poly_coeff.coeff(2,0)*pow(time_vector.coeff(i-1,0),5) ; - } - - for (int i=1; i<=via_num; i++) - { - for (int j=via_time_vector.coeff(i-1,0); j<=all_time_steps+1; j++) - { - minimum_jerk_tra_with_via_points.coeffRef(j-1,0) = - minimum_jerk_tra_with_via_points.coeff(j-1,0) + - poly_coeff.coeff(3*i,0)*pow((time_vector.coeff(j-1,0)-via_time.coeff(i-1,0)),3)/6 + - poly_coeff.coeff(3*i+1,0)*pow((time_vector.coeff(j-1,0)-via_time.coeff(i-1,0)),4)/24 + - poly_coeff.coeff(3*i+2,0)*pow((time_vector.coeff(j-1,0)-via_time.coeff(i-1,0)),5)/120 ; - - } - } - - return minimum_jerk_tra_with_via_points; -} - -Eigen::MatrixXd calcArc3dTra(double smp_time, double mov_time, - Eigen::MatrixXd center_point, Eigen::MatrixXd normal_vector, Eigen::MatrixXd start_point, - double rotation_angle, double cross_ratio ) -{ - int all_time_steps = int (round(mov_time/smp_time))+1; - - Eigen::MatrixXd angle_tra = calcMinimumJerkTra(0.0, 0.0, 0.0, - rotation_angle, 0.0, 0.0, - smp_time, mov_time); - - Eigen::MatrixXd arc_tra = Eigen::MatrixXd::Zero(3,all_time_steps); - - for (int step = 0; step < all_time_steps; step++ ) - { - double time = ((double)step)*smp_time; - double theta = angle_tra.coeff(step,0); - - Eigen::MatrixXd weight_wedge(3,3); - - weight_wedge << - 0.0, -normal_vector.coeff(2,0), normal_vector.coeff(1,0), - normal_vector.coeff(2,0), 0.0, -normal_vector.coeff(0,0), - -normal_vector.coeff(1,0), normal_vector.coeff(0,0), 0.0; - - Eigen::MatrixXd identity = Eigen::MatrixXd::Identity(3,3); - Eigen::MatrixXd rotation = identity + weight_wedge*sin(theta) + weight_wedge*weight_wedge*(1-cos(theta)); - double cross = cross_ratio*(1.0-2.0*(abs(0.5-(time/mov_time)))); - - arc_tra.block(0,step,3,1) = (1+cross)*(rotation*(start_point-center_point))+center_point; - } - - Eigen::MatrixXd act_tra_trans = arc_tra.transpose(); - - return act_tra_trans; -} - - - -FifthOrderPolynomialTrajectory::FifthOrderPolynomialTrajectory(double initial_time, double initial_pos, double initial_vel, double initial_acc, - double final_time, double final_pos, double final_vel, double final_acc) -{ - position_coeff_.resize(6, 1); - velocity_coeff_.resize(6, 1); - acceleration_coeff_.resize(6, 1); - time_variables_.resize(1, 6); - - position_coeff_.fill(0); - velocity_coeff_.fill(0); - acceleration_coeff_.fill(0); - time_variables_.fill(0); - - if(final_time > initial_time) - { - initial_time_ = initial_time; - initial_pos_ = initial_pos; - initial_vel_ = initial_vel; - initial_acc_ = initial_acc; - - current_time_ = initial_time; - current_pos_ = initial_pos; - current_vel_ = initial_vel; - current_acc_ = initial_acc; - - final_time_ = final_time; - final_pos_ = final_pos; - final_vel_ = final_vel; - final_acc_ = final_acc; - - Eigen::MatrixXd time_mat; - Eigen::MatrixXd conditions_mat; - - time_mat.resize(6,6); - time_mat << powDI(initial_time_, 5), powDI(initial_time_, 4), powDI(initial_time_, 3), powDI(initial_time_, 2), initial_time_, 1.0, - 5.0*powDI(initial_time_, 4), 4.0*powDI(initial_time_, 3), 3.0*powDI(initial_time_, 2), 2.0*initial_time_, 1.0, 0.0, - 20.0*powDI(initial_time_, 3), 12.0*powDI(initial_time_, 2), 6.0*initial_time_, 2.0, 0.0, 0.0; - powDI(final_time_, 5), powDI(final_time_, 4), powDI(final_time_, 3), powDI(final_time_, 2), final_time_, 1.0, - 5.0*powDI(final_time_, 4), 4.0*powDI(final_time_, 3), 3.0*powDI(final_time_, 2), 2.0*final_time_, 1.0, 0.0, - 20.0*powDI(final_time_, 3), 12.0*powDI(final_time_, 2), 6.0*final_time_, 2.0, 0.0, 0.0; - - conditions_mat.resize(6,1); - conditions_mat << initial_pos_, initial_vel_, initial_acc_, final_pos_, final_vel_, final_acc_; - - position_coeff_ = time_mat.inverse() * conditions_mat; - velocity_coeff_ << 0.0, - 5.0*position_coeff_.coeff(0,0), - 4.0*position_coeff_.coeff(0,1), - 3.0*position_coeff_.coeff(0,2), - 2.0*position_coeff_.coeff(0,3), - 1.0*position_coeff_.coeff(0,4); - acceleration_coeff_ << 0.0, - 0.0, - 20.0*position_coeff_.coeff(0,0), - 12.0*position_coeff_.coeff(0,1), - 6.0*position_coeff_.coeff(0,2), - 2.0*position_coeff_.coeff(0,3); - } - -} - -FifthOrderPolynomialTrajectory::FifthOrderPolynomialTrajectory() -{ - initial_time_ = 0; - initial_pos_ = 0; - initial_vel_ = 0; - initial_acc_ = 0; - - current_time_ = 0; - current_pos_ = 0; - current_vel_ = 0; - current_acc_ = 0; - - final_time_ = 0; - final_pos_ = 0; - final_vel_ = 0; - final_acc_ = 0; - - position_coeff_.resize(6, 1); - velocity_coeff_.resize(6, 1); - acceleration_coeff_.resize(6, 1); - time_variables_.resize(1, 6); - - position_coeff_.fill(0); - velocity_coeff_.fill(0); - acceleration_coeff_.fill(0); - time_variables_.fill(0); -} - -FifthOrderPolynomialTrajectory::~FifthOrderPolynomialTrajectory() -{ - -} - -bool FifthOrderPolynomialTrajectory::changeTrajectory(double final_pos, double final_vel, double final_acc) -{ - final_pos_ = final_pos; - final_vel_ = final_vel; - final_acc_ = final_acc; - - Eigen::MatrixXd time_mat; - Eigen::MatrixXd conditions_mat; - - time_mat.resize(6,6); - time_mat << powDI(initial_time_, 5), powDI(initial_time_, 4), powDI(initial_time_, 3), powDI(initial_time_, 2), initial_time_, 1.0, - 5.0*powDI(initial_time_, 4), 4.0*powDI(initial_time_, 3), 3.0*powDI(initial_time_, 2), 2.0*initial_time_, 1.0, 0.0, - 20.0*powDI(initial_time_, 3), 12.0*powDI(initial_time_, 2), 6.0*initial_time_, 2.0, 0.0, 0.0, - powDI(final_time_, 5), powDI(final_time_, 4), powDI(final_time_, 3), powDI(final_time_, 2), final_time_, 1.0, - 5.0*powDI(final_time_, 4), 4.0*powDI(final_time_, 3), 3.0*powDI(final_time_, 2), 2.0*final_time_, 1.0, 0.0, - 20.0*powDI(final_time_, 3), 12.0*powDI(final_time_, 2), 6.0*final_time_, 2.0, 0.0, 0.0; - - conditions_mat.resize(6,1); - conditions_mat << initial_pos_, initial_vel_, initial_acc_, final_pos_, final_vel_, final_acc_; - - position_coeff_ = time_mat.inverse() * conditions_mat; - velocity_coeff_ << 0.0, - 5.0*position_coeff_.coeff(0,0), - 4.0*position_coeff_.coeff(0,1), - 3.0*position_coeff_.coeff(0,2), - 2.0*position_coeff_.coeff(0,3), - 1.0*position_coeff_.coeff(0,4); - acceleration_coeff_ << 0.0, - 0.0, - 20.0*position_coeff_.coeff(0,0), - 12.0*position_coeff_.coeff(0,1), - 6.0*position_coeff_.coeff(0,2), - 2.0*position_coeff_.coeff(0,3); - - return true; -} - -bool FifthOrderPolynomialTrajectory::changeTrajectory(double final_time, double final_pos, double final_vel, double final_acc) -{ - if(final_time < initial_time_) - return false; - - final_time_ = final_time; - return changeTrajectory(final_pos, final_vel, final_acc); -} - -bool FifthOrderPolynomialTrajectory::changeTrajectory(double initial_time, double initial_pos, double initial_vel, double initial_acc, - double final_time, double final_pos, double final_vel, double final_acc) -{ - if(final_time < initial_time) - return false; - - initial_time_ = initial_time; - initial_pos_ = initial_pos; - initial_vel_ = initial_vel; - initial_acc_ = initial_acc; - - final_time_ = final_time; - - return changeTrajectory(final_pos, final_vel, final_acc); -} - -double FifthOrderPolynomialTrajectory::getPosition(double time) -{ - if(time >= final_time_) - { - current_time_ = final_time_; - current_pos_ = final_pos_; - current_vel_ = final_vel_; - current_acc_ = final_acc_; - return final_pos_; - } - else if(time <= initial_time_ ) - { - current_time_ = initial_time_; - current_pos_ = initial_pos_; - current_vel_ = initial_vel_; - current_acc_ = initial_acc_; - return initial_pos_; - } - else - { - current_time_ = time; - time_variables_ << powDI(time, 5), powDI(time, 4), powDI(time, 3), powDI(time, 2), time, 1.0; - current_pos_ = (time_variables_ * position_coeff_).coeff(0,0); - current_vel_ = (time_variables_ * velocity_coeff_).coeff(0,0); - current_acc_ = (time_variables_ * acceleration_coeff_).coeff(0,0); - return current_pos_; - } -} - -double FifthOrderPolynomialTrajectory::getVelocity(double time) -{ - if(time >= final_time_) - { - current_time_ = final_time_; - current_pos_ = final_pos_; - current_vel_ = final_vel_; - current_acc_ = final_acc_; - return final_vel_; - } - else if(time <= initial_time_ ) - { - current_time_ = initial_time_; - current_pos_ = initial_pos_; - current_vel_ = initial_vel_; - current_acc_ = initial_acc_; - return initial_vel_; - } - else - { - current_time_ = time; - time_variables_ << powDI(time, 5), powDI(time, 4), powDI(time, 3), powDI(time, 2), time, 1.0; - current_pos_ = (time_variables_ * position_coeff_).coeff(0,0); - current_vel_ = (time_variables_ * velocity_coeff_).coeff(0,0); - current_acc_ = (time_variables_ * acceleration_coeff_).coeff(0,0); - return current_vel_; - } -} - -double FifthOrderPolynomialTrajectory::getAcceleration(double time) -{ - if(time >= final_time_) - { - current_time_ = final_time_; - current_pos_ = final_pos_; - current_vel_ = final_vel_; - current_acc_ = final_acc_; - return final_acc_; - } - else if(time <= initial_time_ ) - { - current_time_ = initial_time_; - current_pos_ = initial_pos_; - current_vel_ = initial_vel_; - current_acc_ = initial_acc_; - return initial_acc_; - } - else - { - current_time_ = time; - time_variables_ << powDI(time, 5), powDI(time, 4), powDI(time, 3), powDI(time, 2), time, 1.0; - current_pos_ = (time_variables_ * position_coeff_).coeff(0,0); - current_vel_ = (time_variables_ * velocity_coeff_).coeff(0,0); - current_acc_ = (time_variables_ * acceleration_coeff_).coeff(0,0); - return current_acc_; - } -} - -void FifthOrderPolynomialTrajectory::setTime(double time) -{ - if(time >= final_time_) - { - current_time_ = final_time_; - current_pos_ = final_pos_; - current_vel_ = final_vel_; - current_acc_ = final_acc_; - } - else if(time <= initial_time_ ) - { - current_time_ = initial_time_; - current_pos_ = initial_pos_; - current_vel_ = initial_vel_; - current_acc_ = initial_acc_; - } - else - { - current_time_ = time; - time_variables_ << powDI(time, 5), powDI(time, 4), powDI(time, 3), powDI(time, 2), time, 1.0; - current_pos_ = (time_variables_ * position_coeff_).coeff(0,0); - current_vel_ = (time_variables_ * velocity_coeff_).coeff(0,0); - current_acc_ = (time_variables_ * acceleration_coeff_).coeff(0,0); - } -} - -double FifthOrderPolynomialTrajectory::getPosition() -{ - return current_pos_; -} - -double FifthOrderPolynomialTrajectory::getVelocity() -{ - return current_vel_; -} - -double FifthOrderPolynomialTrajectory::getAcceleration() -{ - return current_acc_; -} - -} From a415fff32f067decd588766053234a542cd6a43f Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Fri, 12 Aug 2016 10:47:06 +0900 Subject: [PATCH 30/33] - delete dynamixel_sdk (Use ROBOTIS-GIT/DynamixelSDK repository) --- dynamixel_sdk/CMakeLists.txt | 38 - dynamixel_sdk/include/dynamixel_sdk.h | 59 - .../include/dynamixel_sdk/group_bulk_read.h | 92 -- .../include/dynamixel_sdk/group_bulk_write.h | 87 -- .../include/dynamixel_sdk/group_sync_read.h | 91 -- .../include/dynamixel_sdk/group_sync_write.h | 86 -- .../include/dynamixel_sdk/packet_handler.h | 164 --- .../include/dynamixel_sdk/port_handler.h | 94 -- .../dynamixel_sdk/protocol1_packet_handler.h | 129 -- .../dynamixel_sdk/protocol2_packet_handler.h | 134 --- .../dynamixel_sdk_linux/port_handler_linux.h | 93 -- .../port_handler_windows.h | 93 -- dynamixel_sdk/package.xml | 15 - .../src/dynamixel_sdk/group_bulk_read.cpp | 237 ---- .../src/dynamixel_sdk/group_bulk_write.cpp | 168 --- .../src/dynamixel_sdk/group_sync_read.cpp | 203 ---- .../src/dynamixel_sdk/group_sync_write.cpp | 147 --- .../src/dynamixel_sdk/packet_handler.cpp | 60 - .../src/dynamixel_sdk/port_handler.cpp | 63 - .../protocol1_packet_handler.cpp | 727 ------------ .../protocol2_packet_handler.cpp | 1036 ----------------- .../port_handler_linux.cpp | 276 ----- .../port_handler_windows.cpp | 245 ---- 23 files changed, 4337 deletions(-) delete mode 100644 dynamixel_sdk/CMakeLists.txt delete mode 100644 dynamixel_sdk/include/dynamixel_sdk.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk/group_bulk_read.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk/group_bulk_write.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk/group_sync_read.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk/group_sync_write.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk/packet_handler.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk/port_handler.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk/protocol1_packet_handler.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk/protocol2_packet_handler.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk_linux/port_handler_linux.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk_windows/port_handler_windows.h delete mode 100644 dynamixel_sdk/package.xml delete mode 100644 dynamixel_sdk/src/dynamixel_sdk/group_bulk_read.cpp delete mode 100644 dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp delete mode 100644 dynamixel_sdk/src/dynamixel_sdk/group_sync_read.cpp delete mode 100644 dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp delete mode 100644 dynamixel_sdk/src/dynamixel_sdk/packet_handler.cpp delete mode 100644 dynamixel_sdk/src/dynamixel_sdk/port_handler.cpp delete mode 100644 dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp delete mode 100644 dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp delete mode 100644 dynamixel_sdk/src/dynamixel_sdk_linux/port_handler_linux.cpp delete mode 100644 dynamixel_sdk/src/dynamixel_sdk_windows/port_handler_windows.cpp diff --git a/dynamixel_sdk/CMakeLists.txt b/dynamixel_sdk/CMakeLists.txt deleted file mode 100644 index 658dfd4..0000000 --- a/dynamixel_sdk/CMakeLists.txt +++ /dev/null @@ -1,38 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(dynamixel_sdk) - -find_package(catkin REQUIRED COMPONENTS - roscpp -) - -catkin_package( - INCLUDE_DIRS include - LIBRARIES dynamixel_sdk -# CATKIN_DEPENDS roscpp -# DEPENDS system_lib -) - -# include_directories(include) -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -add_library(dynamixel_sdk - src/${PROJECT_NAME}/packet_handler.cpp - src/${PROJECT_NAME}/protocol1_packet_handler.cpp - src/${PROJECT_NAME}/protocol2_packet_handler.cpp - src/${PROJECT_NAME}/group_sync_read.cpp - src/${PROJECT_NAME}/group_sync_write.cpp - src/${PROJECT_NAME}/group_bulk_read.cpp - src/${PROJECT_NAME}/group_bulk_write.cpp - src/${PROJECT_NAME}/port_handler.cpp - src/${PROJECT_NAME}_linux/port_handler_linux.cpp -) - -## Specify libraries to link a library or executable target against -target_link_libraries(dynamixel_sdk - ${catkin_LIBRARIES} -) - diff --git a/dynamixel_sdk/include/dynamixel_sdk.h b/dynamixel_sdk/include/dynamixel_sdk.h deleted file mode 100644 index a0fdf2a..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk.h +++ /dev/null @@ -1,59 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * dynamixel_sdk.h - * - * Created on: 2016. 3. 8. - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_DYNAMIXELSDK_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_DYNAMIXELSDK_H_ - - -#include "dynamixel_sdk/group_bulk_read.h" -#include "dynamixel_sdk/group_bulk_write.h" -#include "dynamixel_sdk/group_sync_read.h" -#include "dynamixel_sdk/group_sync_write.h" -#include "dynamixel_sdk/protocol1_packet_handler.h" -#include "dynamixel_sdk/protocol2_packet_handler.h" - -#ifdef __linux__ - #include "dynamixel_sdk_linux/port_handler_linux.h" -#endif - -#if defined(_WIN32) || defined(_WIN64) - #include "dynamixel_sdk_windows/port_handler_windows.h" -#endif - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_DYNAMIXELSDK_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/group_bulk_read.h b/dynamixel_sdk/include/dynamixel_sdk/group_bulk_read.h deleted file mode 100644 index 29b0dba..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/group_bulk_read.h +++ /dev/null @@ -1,92 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * group_bulk_read.h - * - * Created on: 2016. 1. 28. - * Author: zerom, leon - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_ - - -#include -#include -#include "port_handler.h" -#include "packet_handler.h" - -namespace dynamixel -{ - -class WINDECLSPEC GroupBulkRead -{ - private: - PortHandler *port_; - PacketHandler *ph_; - - std::vector id_list_; - std::map address_list_; // - std::map length_list_; // - std::map data_list_; // - - bool last_result_; - bool is_param_changed_; - - uint8_t *param_; - - void makeParam(); - - public: - GroupBulkRead(PortHandler *port, PacketHandler *ph); - ~GroupBulkRead() { clearParam(); } - - PortHandler *getPortHandler() { return port_; } - PacketHandler *getPacketHandler() { return ph_; } - - bool addParam (uint8_t id, uint16_t start_address, uint16_t data_length); - void removeParam (uint8_t id); - void clearParam (); - - int txPacket(); - int rxPacket(); - int txRxPacket(); - - bool isAvailable (uint8_t id, uint16_t address, uint16_t data_length); - uint32_t getData (uint8_t id, uint16_t address, uint16_t data_length); -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/group_bulk_write.h b/dynamixel_sdk/include/dynamixel_sdk/group_bulk_write.h deleted file mode 100644 index 71bd9ec..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/group_bulk_write.h +++ /dev/null @@ -1,87 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * group_bulk_write.h - * - * Created on: 2016. 2. 2. - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_ - - -#include -#include -#include "port_handler.h" -#include "packet_handler.h" - -namespace dynamixel -{ - -class WINDECLSPEC GroupBulkWrite -{ - private: - PortHandler *port_; - PacketHandler *ph_; - - std::vector id_list_; - std::map address_list_; // - std::map length_list_; // - std::map data_list_; // - - bool is_param_changed_; - - uint8_t *param_; - uint16_t param_length_; - - void makeParam(); - - public: - GroupBulkWrite(PortHandler *port, PacketHandler *ph); - ~GroupBulkWrite() { clearParam(); } - - PortHandler *getPortHandler() { return port_; } - PacketHandler *getPacketHandler() { return ph_; } - - bool addParam (uint8_t id, uint16_t start_address, uint16_t data_length, uint8_t *data); - void removeParam (uint8_t id); - bool changeParam (uint8_t id, uint16_t start_address, uint16_t data_length, uint8_t *data); - void clearParam (); - - int txPacket(); -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/group_sync_read.h b/dynamixel_sdk/include/dynamixel_sdk/group_sync_read.h deleted file mode 100644 index ec2a208..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/group_sync_read.h +++ /dev/null @@ -1,91 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * group_sync_read.h - * - * Created on: 2016. 2. 2. - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_ - - -#include -#include -#include "port_handler.h" -#include "packet_handler.h" - -namespace dynamixel -{ - -class WINDECLSPEC GroupSyncRead -{ - private: - PortHandler *port_; - PacketHandler *ph_; - - std::vector id_list_; - std::map data_list_; // - - bool last_result_; - bool is_param_changed_; - - uint8_t *param_; - uint16_t start_address_; - uint16_t data_length_; - - void makeParam(); - - public: - GroupSyncRead(PortHandler *port, PacketHandler *ph, uint16_t start_address, uint16_t data_length); - ~GroupSyncRead() { clearParam(); } - - PortHandler *getPortHandler() { return port_; } - PacketHandler *getPacketHandler() { return ph_; } - - bool addParam (uint8_t id); - void removeParam (uint8_t id); - void clearParam (); - - int txPacket(); - int rxPacket(); - int txRxPacket(); - - bool isAvailable (uint8_t id, uint16_t address, uint16_t data_length); - uint32_t getData (uint8_t id, uint16_t address, uint16_t data_length); -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/group_sync_write.h b/dynamixel_sdk/include/dynamixel_sdk/group_sync_write.h deleted file mode 100644 index 1c85666..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/group_sync_write.h +++ /dev/null @@ -1,86 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * group_sync_write.h - * - * Created on: 2016. 1. 28. - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_ - - -#include -#include -#include "port_handler.h" -#include "packet_handler.h" - -namespace dynamixel -{ - -class WINDECLSPEC GroupSyncWrite -{ - private: - PortHandler *port_; - PacketHandler *ph_; - - std::vector id_list_; - std::map data_list_; // - - bool is_param_changed_; - - uint8_t *param_; - uint16_t start_address_; - uint16_t data_length_; - - void makeParam(); - - public: - GroupSyncWrite(PortHandler *port, PacketHandler *ph, uint16_t start_address, uint16_t data_length); - ~GroupSyncWrite() { clearParam(); } - - PortHandler *getPortHandler() { return port_; } - PacketHandler *getPacketHandler() { return ph_; } - - bool addParam (uint8_t id, uint8_t *data); - void removeParam (uint8_t id); - bool changeParam (uint8_t id, uint8_t *data); - void clearParam (); - - int txPacket(); -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/packet_handler.h b/dynamixel_sdk/include/dynamixel_sdk/packet_handler.h deleted file mode 100644 index 3816cbb..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/packet_handler.h +++ /dev/null @@ -1,164 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * packet_handler.h - * - * Created on: 2016. 1. 26. - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ - - -#include -#include -#include "port_handler.h" - -#define BROADCAST_ID 0xFE // 254 -#define MAX_ID 0xFC // 252 - -/* Macro for Control Table Value */ -#define DXL_MAKEWORD(a, b) ((unsigned short)(((unsigned char)(((unsigned long)(a)) & 0xff)) | ((unsigned short)((unsigned char)(((unsigned long)(b)) & 0xff))) << 8)) -#define DXL_MAKEDWORD(a, b) ((unsigned int)(((unsigned short)(((unsigned long)(a)) & 0xffff)) | ((unsigned int)((unsigned short)(((unsigned long)(b)) & 0xffff))) << 16)) -#define DXL_LOWORD(l) ((unsigned short)(((unsigned long)(l)) & 0xffff)) -#define DXL_HIWORD(l) ((unsigned short)((((unsigned long)(l)) >> 16) & 0xffff)) -#define DXL_LOBYTE(w) ((unsigned char)(((unsigned long)(w)) & 0xff)) -#define DXL_HIBYTE(w) ((unsigned char)((((unsigned long)(w)) >> 8) & 0xff)) - -/* Instruction for DXL Protocol */ -#define INST_PING 1 -#define INST_READ 2 -#define INST_WRITE 3 -#define INST_REG_WRITE 4 -#define INST_ACTION 5 -#define INST_FACTORY_RESET 6 -#define INST_SYNC_WRITE 131 // 0x83 -#define INST_BULK_READ 146 // 0x92 -// --- Only for 2.0 --- // -#define INST_REBOOT 8 -#define INST_STATUS 85 // 0x55 -#define INST_SYNC_READ 130 // 0x82 -#define INST_BULK_WRITE 147 // 0x93 - -// Communication Result -#define COMM_SUCCESS 0 // tx or rx packet communication success -#define COMM_PORT_BUSY -1000 // Port is busy (in use) -#define COMM_TX_FAIL -1001 // Failed transmit instruction packet -#define COMM_RX_FAIL -1002 // Failed get status packet -#define COMM_TX_ERROR -2000 // Incorrect instruction packet -#define COMM_RX_WAITING -3000 // Now recieving status packet -#define COMM_RX_TIMEOUT -3001 // There is no status packet -#define COMM_RX_CORRUPT -3002 // Incorrect status packet -#define COMM_NOT_AVAILABLE -9000 // - -namespace dynamixel -{ - -class WINDECLSPEC PacketHandler -{ - protected: - PacketHandler() { } - - public: - static PacketHandler *getPacketHandler(float protocol_version = 2.0); - - virtual ~PacketHandler() { } - - 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; - - virtual int ping (PortHandler *port, uint8_t id, uint8_t *error = 0) = 0; - virtual int ping (PortHandler *port, uint8_t id, uint16_t *model_number, uint8_t *error = 0) = 0; - - // broadcastPing - virtual int broadcastPing (PortHandler *port, std::vector &id_list) = 0; - - virtual int action (PortHandler *port, uint8_t id) = 0; - virtual int reboot (PortHandler *port, uint8_t id, uint8_t *error = 0) = 0; - virtual int factoryReset (PortHandler *port, uint8_t id, uint8_t option = 0, uint8_t *error = 0) = 0; - - - virtual int readTx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length) = 0; - virtual int readRx (PortHandler *port, uint16_t length, uint8_t *data, uint8_t *error = 0) = 0; - virtual int readTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0) = 0; - - virtual int read1ByteTx (PortHandler *port, uint8_t id, uint16_t address) = 0; - virtual int read1ByteRx (PortHandler *port, uint8_t *data, uint8_t *error = 0) = 0; - virtual int read1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error = 0) = 0; - - virtual int read2ByteTx (PortHandler *port, uint8_t id, uint16_t address) = 0; - virtual int read2ByteRx (PortHandler *port, uint16_t *data, uint8_t *error = 0) = 0; - virtual int read2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error = 0) = 0; - - virtual int read4ByteTx (PortHandler *port, uint8_t id, uint16_t address) = 0; - virtual int read4ByteRx (PortHandler *port, uint32_t *data, uint8_t *error = 0) = 0; - virtual int read4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error = 0) = 0; - - virtual int writeTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) = 0; - virtual int writeTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0) = 0; - - virtual int write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data) = 0; - virtual int write1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error = 0) = 0; - - virtual int write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data) = 0; - virtual int write2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error = 0) = 0; - - virtual int write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data) = 0; - virtual int write4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error = 0) = 0; - - virtual int regWriteTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) = 0; - virtual int regWriteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0) = 0; - - virtual int syncReadTx (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) = 0; - // SyncReadRx -> GroupSyncRead class - // SyncReadTxRx -> GroupSyncRead class - - virtual int syncWriteTxOnly (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) = 0; - - virtual int bulkReadTx (PortHandler *port, uint8_t *param, uint16_t param_length) = 0; - // BulkReadRx -> GroupBulkRead class - // BulkReadTxRx -> GroupBulkRead class - - virtual int bulkWriteTxOnly (PortHandler *port, uint8_t *param, uint16_t param_length) = 0; -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/port_handler.h b/dynamixel_sdk/include/dynamixel_sdk/port_handler.h deleted file mode 100644 index dfc83fd..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/port_handler.h +++ /dev/null @@ -1,94 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * port_handler.h - * - * Created on: 2016. 1. 26. - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_ - -#undef __linux__ -#define __linux__ - -#ifdef __linux__ -#define WINDECLSPEC -#elif defined(_WIN32) || defined(_WIN64) -#ifdef WINDLLEXPORT -#define WINDECLSPEC __declspec(dllexport) -#else -#define WINDECLSPEC __declspec(dllimport) -#endif -#endif - -#include - -namespace dynamixel -{ - -class WINDECLSPEC PortHandler -{ - public: - static const int DEFAULT_BAUDRATE_ = 1000000; - - static PortHandler *getPortHandler(const char *port_name); - - bool is_using_; - - virtual ~PortHandler() { } - - virtual bool openPort() = 0; - virtual void closePort() = 0; - virtual void clearPort() = 0; - - virtual void setPortName(const char* port_name) = 0; - virtual char *getPortName() = 0; - - virtual bool setBaudRate(const int baudrate) = 0; - virtual int getBaudRate() = 0; - - virtual int getBytesAvailable() = 0; - - virtual int readPort(uint8_t *packet, int length) = 0; - virtual int writePort(uint8_t *packet, int length) = 0; - - virtual void setPacketTimeout(uint16_t packet_length) = 0; - virtual void setPacketTimeout(double msec) = 0; - virtual bool isPacketTimeout() = 0; -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/protocol1_packet_handler.h b/dynamixel_sdk/include/dynamixel_sdk/protocol1_packet_handler.h deleted file mode 100644 index 758556f..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/protocol1_packet_handler.h +++ /dev/null @@ -1,129 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * protocol1_packet_handler.h - * - * Created on: 2016. 1. 26. - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ - - -#include "packet_handler.h" - -namespace dynamixel -{ - -class WINDECLSPEC Protocol1PacketHandler : public PacketHandler -{ - private: - static Protocol1PacketHandler *unique_instance_; - - Protocol1PacketHandler(); - - public: - static Protocol1PacketHandler *getInstance() { return unique_instance_; } - - virtual ~Protocol1PacketHandler() { } - - 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); - - int ping (PortHandler *port, uint8_t id, uint8_t *error = 0); - int ping (PortHandler *port, uint8_t id, uint16_t *model_number, uint8_t *error = 0); - - // broadcastPing - int broadcastPing (PortHandler *port, std::vector &id_list); - - int action (PortHandler *port, uint8_t id); - int reboot (PortHandler *port, uint8_t id, uint8_t *error = 0); - int factoryReset (PortHandler *port, uint8_t id, uint8_t option, uint8_t *error = 0); - - - int readTx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length); - int readRx (PortHandler *port, uint16_t length, uint8_t *data, uint8_t *error = 0); - int readTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - - int read1ByteTx (PortHandler *port, uint8_t id, uint16_t address); - int read1ByteRx (PortHandler *port, uint8_t *data, uint8_t *error = 0); - int read1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error = 0); - - int read2ByteTx (PortHandler *port, uint8_t id, uint16_t address); - int read2ByteRx (PortHandler *port, uint16_t *data, uint8_t *error = 0); - int read2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error = 0); - - int read4ByteTx (PortHandler *port, uint8_t id, uint16_t address); - int read4ByteRx (PortHandler *port, uint32_t *data, uint8_t *error = 0); - int read4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error = 0); - - int writeTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data); - int writeTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - - int write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data); - int write1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error = 0); - - int write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data); - int write2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error = 0); - - int write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data); - int write4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error = 0); - - int regWriteTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data); - int regWriteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - - int syncReadTx (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length); - // SyncReadRx -> GroupSyncRead class - // SyncReadTxRx -> GroupSyncRead class - - // param : ID1 DATA0 DATA1 ... DATAn ID2 DATA0 DATA1 ... DATAn ID3 DATA0 DATA1 ... DATAn - int syncWriteTxOnly (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length); - - // param : LEN1 ID1 ADDR1 LEN2 ID2 ADDR2 ... - int bulkReadTx (PortHandler *port, uint8_t *param, uint16_t param_length); - // BulkReadRx -> GroupBulkRead class - // BulkReadTxRx -> GroupBulkRead class - - int bulkWriteTxOnly (PortHandler *port, uint8_t *param, uint16_t param_length); -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/protocol2_packet_handler.h b/dynamixel_sdk/include/dynamixel_sdk/protocol2_packet_handler.h deleted file mode 100644 index ff7e2d1..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/protocol2_packet_handler.h +++ /dev/null @@ -1,134 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * protocol2_packet_handler.h - * - * Created on: 2016. 1. 26. - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ - - -#include "packet_handler.h" - -namespace dynamixel -{ - -class WINDECLSPEC Protocol2PacketHandler : public PacketHandler -{ - private: - static Protocol2PacketHandler *unique_instance_; - - Protocol2PacketHandler(); - - uint16_t updateCRC(uint16_t crc_accum, uint8_t *data_blk_ptr, uint16_t data_blk_size); - void addStuffing(uint8_t *packet); - void removeStuffing(uint8_t *packet); - - public: - static Protocol2PacketHandler *getInstance() { return unique_instance_; } - - virtual ~Protocol2PacketHandler() { } - - 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); - - int ping (PortHandler *port, uint8_t id, uint8_t *error = 0); - int ping (PortHandler *port, uint8_t id, uint16_t *model_number, uint8_t *error = 0); - - // broadcastPing - int broadcastPing (PortHandler *port, std::vector &id_list); - - int action (PortHandler *port, uint8_t id); - int reboot (PortHandler *port, uint8_t id, uint8_t *error = 0); - int factoryReset (PortHandler *port, uint8_t id, uint8_t option, uint8_t *error = 0); - - - int readTx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length); - int readRx (PortHandler *port, uint16_t length, uint8_t *data, uint8_t *error = 0); - int readTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - - int read1ByteTx (PortHandler *port, uint8_t id, uint16_t address); - int read1ByteRx (PortHandler *port, uint8_t *data, uint8_t *error = 0); - int read1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error = 0); - - int read2ByteTx (PortHandler *port, uint8_t id, uint16_t address); - int read2ByteRx (PortHandler *port, uint16_t *data, uint8_t *error = 0); - int read2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error = 0); - - int read4ByteTx (PortHandler *port, uint8_t id, uint16_t address); - int read4ByteRx (PortHandler *port, uint32_t *data, uint8_t *error = 0); - int read4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error = 0); - - int writeTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data); - int writeTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - - int write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data); - int write1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error = 0); - - int write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data); - int write2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error = 0); - - int write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data); - int write4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error = 0); - - int regWriteTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data); - int regWriteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - - int syncReadTx (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length); - // SyncReadRx -> GroupSyncRead class - // SyncReadTxRx -> GroupSyncRead class - - // param : ID1 DATA0 DATA1 ... DATAn ID2 DATA0 DATA1 ... DATAn ID3 DATA0 DATA1 ... DATAn - int syncWriteTxOnly (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length); - - // param : ID1 ADDR_L1 ADDR_H1 LEN_L1 LEN_H1 ID2 ADDR_L2 ADDR_H2 LEN_L2 LEN_H2 ... - int bulkReadTx (PortHandler *port, uint8_t *param, uint16_t param_length); - // BulkReadRx -> GroupBulkRead class - // BulkReadTxRx -> GroupBulkRead class - - // param : ID1 START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H DATA0 DATA1 ... DATAn ID2 START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H DATA0 DATA1 ... DATAn - int bulkWriteTxOnly (PortHandler *port, uint8_t *param, uint16_t param_length); -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk_linux/port_handler_linux.h b/dynamixel_sdk/include/dynamixel_sdk_linux/port_handler_linux.h deleted file mode 100644 index 3a9ed24..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk_linux/port_handler_linux.h +++ /dev/null @@ -1,93 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * port_handler_linux.h - * - * Created on: 2016. 1. 26. - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ - - -#include "dynamixel_sdk/port_handler.h" - -namespace dynamixel -{ - -class PortHandlerLinux : public PortHandler -{ - private: - int socket_fd_; - int baudrate_; - char port_name_[30]; - - double packet_start_time_; - double packet_timeout_; - double tx_time_per_byte; - - bool setupPort(const int cflag_baud); - bool setCustomBaudrate(int speed); - int getCFlagBaud(const int baudrate); - - double getCurrentTime(); - double getTimeSinceStart(); - - public: - PortHandlerLinux(const char *port_name); - virtual ~PortHandlerLinux() { closePort(); } - - bool openPort(); - void closePort(); - void clearPort(); - - void setPortName(const char *port_name); - char *getPortName(); - - bool setBaudRate(const int baudrate); - int getBaudRate(); - - int getBytesAvailable(); - - int readPort(uint8_t *packet, int length); - int writePort(uint8_t *packet, int length); - - void setPacketTimeout(uint16_t packet_length); - void setPacketTimeout(double msec); - bool isPacketTimeout(); -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk_windows/port_handler_windows.h b/dynamixel_sdk/include/dynamixel_sdk_windows/port_handler_windows.h deleted file mode 100644 index 09583cb..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk_windows/port_handler_windows.h +++ /dev/null @@ -1,93 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: Leon Ryu Woon Jung */ - -/* -* port_handler_windows.h -* -* Created on: 2016. 4. 26. -*/ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_WINDOWS_PORTHANDLERWINDOWS_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_WINDOWS_PORTHANDLERWINDOWS_H_ - -#include - -#include "dynamixel_sdk/port_handler.h" - -namespace dynamixel -{ -class WINDECLSPEC PortHandlerWindows : public PortHandler -{ - private: - HANDLE serial_handle_; - LARGE_INTEGER freq_, counter_; - - int baudrate_; - char port_name_[30]; - - double packet_start_time_; - double packet_timeout_; - double tx_time_per_byte_; - - bool setupPort(const int baudrate); - - double getCurrentTime(); - double getTimeSinceStart(); - - public: - PortHandlerWindows(const char *port_name); - virtual ~PortHandlerWindows() { closePort(); } - - bool openPort(); - void closePort(); - void clearPort(); - - void setPortName(const char *port_name); - char *getPortName(); - - bool setBaudRate(const int baudrate); - int getBaudRate(); - - int getBytesAvailable(); - - int readPort(uint8_t *packet, int length); - int writePort(uint8_t *packet, int length); - - void setPacketTimeout(uint16_t packet_length); - void setPacketTimeout(double msec); - bool isPacketTimeout(); -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERWINDOWS_H_ */ diff --git a/dynamixel_sdk/package.xml b/dynamixel_sdk/package.xml deleted file mode 100644 index ae8e683..0000000 --- a/dynamixel_sdk/package.xml +++ /dev/null @@ -1,15 +0,0 @@ - - - dynamixel_sdk - 0.1.0 - The dynamixel_sdk package - - robotis - BSD - - ROBOTIS - - catkin - roscpp - roscpp - \ No newline at end of file diff --git a/dynamixel_sdk/src/dynamixel_sdk/group_bulk_read.cpp b/dynamixel_sdk/src/dynamixel_sdk/group_bulk_read.cpp deleted file mode 100644 index 7ae113b..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/group_bulk_read.cpp +++ /dev/null @@ -1,237 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * group_bulk_read.cpp - * - * Created on: 2016. 1. 28. - */ -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include -#include -#include "dynamixel_sdk/group_bulk_read.h" - -using namespace dynamixel; - -GroupBulkRead::GroupBulkRead(PortHandler *port, PacketHandler *ph) - : port_(port), - ph_(ph), - last_result_(false), - is_param_changed_(false), - param_(0) -{ - clearParam(); -} - -void GroupBulkRead::makeParam() -{ - if (id_list_.size() == 0) - return; - - if (param_ != 0) - delete[] param_; - param_ = 0; - - if (ph_->getProtocolVersion() == 1.0) - { - param_ = new uint8_t[id_list_.size() * 3]; // ID(1) + ADDR(1) + LENGTH(1) - } - else // 2.0 - { - param_ = new uint8_t[id_list_.size() * 5]; // ID(1) + ADDR(2) + LENGTH(2) - } - - int idx = 0; - for (unsigned int i = 0; i < id_list_.size(); i++) - { - uint8_t id = id_list_[i]; - if (ph_->getProtocolVersion() == 1.0) - { - param_[idx++] = (uint8_t)length_list_[id]; // LEN - param_[idx++] = id; // ID - param_[idx++] = (uint8_t)address_list_[id]; // ADDR - } - else // 2.0 - { - param_[idx++] = id; // ID - param_[idx++] = DXL_LOBYTE(address_list_[id]); // ADDR_L - param_[idx++] = DXL_HIBYTE(address_list_[id]); // ADDR_H - param_[idx++] = DXL_LOBYTE(length_list_[id]); // LEN_L - param_[idx++] = DXL_HIBYTE(length_list_[id]); // LEN_H - } - } -} - -bool GroupBulkRead::addParam(uint8_t id, uint16_t start_address, uint16_t data_length) -{ - if (std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist - return false; - - id_list_.push_back(id); - length_list_[id] = data_length; - address_list_[id] = start_address; - data_list_[id] = new uint8_t[data_length]; - - is_param_changed_ = true; - return true; -} - -void GroupBulkRead::removeParam(uint8_t id) -{ - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if (it == id_list_.end()) // NOT exist - return; - - id_list_.erase(it); - address_list_.erase(id); - length_list_.erase(id); - delete[] data_list_[id]; - data_list_.erase(id); - - is_param_changed_ = true; -} - -void GroupBulkRead::clearParam() -{ - if (id_list_.size() == 0) - return; - - for (unsigned int i = 0; i < id_list_.size(); i++) - delete[] data_list_[id_list_[i]]; - - id_list_.clear(); - address_list_.clear(); - length_list_.clear(); - data_list_.clear(); - if (param_ != 0) - delete[] param_; - param_ = 0; -} - -int GroupBulkRead::txPacket() -{ - if (id_list_.size() == 0) - return COMM_NOT_AVAILABLE; - - if (is_param_changed_ == true) - makeParam(); - - if (ph_->getProtocolVersion() == 1.0) - { - return ph_->bulkReadTx(port_, param_, id_list_.size() * 3); - } - else // 2.0 - { - return ph_->bulkReadTx(port_, param_, id_list_.size() * 5); - } -} - -int GroupBulkRead::rxPacket() -{ - int cnt = id_list_.size(); - int result = COMM_RX_FAIL; - - last_result_ = false; - - if (cnt == 0) - return COMM_NOT_AVAILABLE; - - for (int i = 0; i < cnt; i++) - { - uint8_t id = id_list_[i]; - - result = ph_->readRx(port_, length_list_[id], data_list_[id]); - if (result != COMM_SUCCESS) - { - fprintf(stderr, "[GroupBulkRead::rxPacket] ID %d result : %d !!!!!!!!!!\n", id, result); - return result; - } - } - - if (result == COMM_SUCCESS) - last_result_ = true; - - return result; -} - -int GroupBulkRead::txRxPacket() -{ - int result = COMM_TX_FAIL; - - result = txPacket(); - if (result != COMM_SUCCESS) - return result; - - return rxPacket(); -} - -bool GroupBulkRead::isAvailable(uint8_t id, uint16_t address, uint16_t data_length) -{ - uint16_t start_addr; - - if (last_result_ == false || data_list_.find(id) == data_list_.end()) - return false; - - start_addr = address_list_[id]; - - if (address < start_addr || start_addr + length_list_[id] - data_length < address) - return false; - - return true; -} - -uint32_t GroupBulkRead::getData(uint8_t id, uint16_t address, uint16_t data_length) -{ - if (isAvailable(id, address, data_length) == false) - return 0; - - uint16_t start_addr = address_list_[id]; - - switch(data_length) - { - case 1: - return data_list_[id][address - start_addr]; - - case 2: - return DXL_MAKEWORD(data_list_[id][address - start_addr], data_list_[id][address - start_addr + 1]); - - case 4: - return DXL_MAKEDWORD(DXL_MAKEWORD(data_list_[id][address - start_addr + 0], data_list_[id][address - start_addr + 1]), - DXL_MAKEWORD(data_list_[id][address - start_addr + 2], data_list_[id][address - start_addr + 3])); - - default: - return 0; - } -} diff --git a/dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp b/dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp deleted file mode 100644 index 7b84151..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp +++ /dev/null @@ -1,168 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * group_bulk_write.cpp - * - * Created on: 2016. 2. 2. - */ -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include -#include "dynamixel_sdk/group_bulk_write.h" - -using namespace dynamixel; - -GroupBulkWrite::GroupBulkWrite(PortHandler *port, PacketHandler *ph) - : port_(port), - ph_(ph), - is_param_changed_(false), - param_(0), - param_length_(0) -{ - clearParam(); -} - -void GroupBulkWrite::makeParam() -{ - if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) - return; - - if (param_ != 0) - delete[] param_; - param_ = 0; - - param_length_ = 0; - for (unsigned int i = 0; i < id_list_.size(); i++) - param_length_ += 1 + 2 + 2 + length_list_[id_list_[i]]; - - param_ = new uint8_t[param_length_]; - - int idx = 0; - for (unsigned int i = 0; i < id_list_.size(); i++) - { - uint8_t id = id_list_[i]; - if (data_list_[id] == 0) - return; - - param_[idx++] = id; - param_[idx++] = DXL_LOBYTE(address_list_[id]); - param_[idx++] = DXL_HIBYTE(address_list_[id]); - param_[idx++] = DXL_LOBYTE(length_list_[id]); - param_[idx++] = DXL_HIBYTE(length_list_[id]); - for (int c = 0; c < length_list_[id]; c++) - param_[idx++] = (data_list_[id])[c]; - } -} - -bool GroupBulkWrite::addParam(uint8_t id, uint16_t start_address, uint16_t data_length, uint8_t *data) -{ - if (ph_->getProtocolVersion() == 1.0) - return false; - - if (std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist - return false; - - id_list_.push_back(id); - address_list_[id] = start_address; - length_list_[id] = data_length; - data_list_[id] = new uint8_t[data_length]; - for (int c = 0; c < data_length; c++) - data_list_[id][c] = data[c]; - - is_param_changed_ = true; - return true; -} -void GroupBulkWrite::removeParam(uint8_t id) -{ - if (ph_->getProtocolVersion() == 1.0) - return; - - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if (it == id_list_.end()) // NOT exist - return; - - id_list_.erase(it); - address_list_.erase(id); - length_list_.erase(id); - delete[] data_list_[id]; - data_list_.erase(id); - - is_param_changed_ = true; -} -bool GroupBulkWrite::changeParam(uint8_t id, uint16_t start_address, uint16_t data_length, uint8_t *data) -{ - if (ph_->getProtocolVersion() == 1.0) - return false; - - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if (it == id_list_.end()) // NOT exist - return false; - - address_list_[id] = start_address; - length_list_[id] = data_length; - delete[] data_list_[id]; - data_list_[id] = new uint8_t[data_length]; - for (int c = 0; c < data_length; c++) - data_list_[id][c] = data[c]; - - is_param_changed_ = true; - return true; -} -void GroupBulkWrite::clearParam() -{ - if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) - return; - - for (unsigned int i = 0; i < id_list_.size(); i++) - delete[] data_list_[id_list_[i]]; - - id_list_.clear(); - address_list_.clear(); - length_list_.clear(); - data_list_.clear(); - if (param_ != 0) - delete[] param_; - param_ = 0; -} -int GroupBulkWrite::txPacket() -{ - if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) - return COMM_NOT_AVAILABLE; - - if (is_param_changed_ == true) - makeParam(); - - return ph_->bulkWriteTxOnly(port_, param_, param_length_); -} diff --git a/dynamixel_sdk/src/dynamixel_sdk/group_sync_read.cpp b/dynamixel_sdk/src/dynamixel_sdk/group_sync_read.cpp deleted file mode 100644 index 13818cc..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/group_sync_read.cpp +++ /dev/null @@ -1,203 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * group_sync_read.cpp - * - * Created on: 2016. 2. 2. - */ -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include -#include "dynamixel_sdk/group_sync_read.h" - -using namespace dynamixel; - -GroupSyncRead::GroupSyncRead(PortHandler *port, PacketHandler *ph, uint16_t start_address, uint16_t data_length) - : port_(port), - ph_(ph), - last_result_(false), - is_param_changed_(false), - param_(0), - start_address_(start_address), - data_length_(data_length) -{ - clearParam(); -} - -void GroupSyncRead::makeParam() -{ - if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) - return; - - if (param_ != 0) - delete[] param_; - param_ = 0; - - param_ = new uint8_t[id_list_.size() * 1]; // ID(1) - - int idx = 0; - for (unsigned int i = 0; i < id_list_.size(); i++) - param_[idx++] = id_list_[i]; -} - -bool GroupSyncRead::addParam(uint8_t id) -{ - if (ph_->getProtocolVersion() == 1.0) - return false; - - if (std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist - return false; - - id_list_.push_back(id); - data_list_[id] = new uint8_t[data_length_]; - - is_param_changed_ = true; - return true; -} -void GroupSyncRead::removeParam(uint8_t id) -{ - if (ph_->getProtocolVersion() == 1.0) - return; - - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if (it == id_list_.end()) // NOT exist - return; - - id_list_.erase(it); - delete[] data_list_[id]; - data_list_.erase(id); - - is_param_changed_ = true; -} -void GroupSyncRead::clearParam() -{ - if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) - return; - - for (unsigned int i = 0; i < id_list_.size(); i++) - delete[] data_list_[id_list_[i]]; - - id_list_.clear(); - data_list_.clear(); - if (param_ != 0) - delete[] param_; - param_ = 0; -} - -int GroupSyncRead::txPacket() -{ - if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) - return COMM_NOT_AVAILABLE; - - if (is_param_changed_ == true) - makeParam(); - - return ph_->syncReadTx(port_, start_address_, data_length_, param_, (uint16_t)id_list_.size() * 1); -} - -int GroupSyncRead::rxPacket() -{ - last_result_ = false; - - if (ph_->getProtocolVersion() == 1.0) - return COMM_NOT_AVAILABLE; - - int cnt = id_list_.size(); - int result = COMM_RX_FAIL; - - if (cnt == 0) - return COMM_NOT_AVAILABLE; - - for (int i = 0; i < cnt; i++) - { - uint8_t id = id_list_[i]; - - result = ph_->readRx(port_, data_length_, data_list_[id]); - if (result != COMM_SUCCESS) - return result; - } - - if (result == COMM_SUCCESS) - last_result_ = true; - - return result; -} - -int GroupSyncRead::txRxPacket() -{ - if (ph_->getProtocolVersion() == 1.0) - return COMM_NOT_AVAILABLE; - - int result = COMM_TX_FAIL; - - result = txPacket(); - if (result != COMM_SUCCESS) - return result; - - return rxPacket(); -} - -bool GroupSyncRead::isAvailable(uint8_t id, uint16_t address, uint16_t data_length) -{ - if (ph_->getProtocolVersion() == 1.0 || last_result_ == false || data_list_.find(id) == data_list_.end()) - return false; - - if (address < start_address_ || start_address_ + data_length_ - data_length < address) - return false; - - return true; -} - -uint32_t GroupSyncRead::getData(uint8_t id, uint16_t address, uint16_t data_length) -{ - if (isAvailable(id, address, data_length) == false) - return 0; - - switch(data_length) - { - case 1: - return data_list_[id][address - start_address_]; - - case 2: - return DXL_MAKEWORD(data_list_[id][address - start_address_], data_list_[id][address - start_address_ + 1]); - - case 4: - return DXL_MAKEDWORD(DXL_MAKEWORD(data_list_[id][address - start_address_ + 0], data_list_[id][address - start_address_ + 1]), - DXL_MAKEWORD(data_list_[id][address - start_address_ + 2], data_list_[id][address - start_address_ + 3])); - - default: - return 0; - } -} diff --git a/dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp b/dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp deleted file mode 100644 index 94dea6f..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp +++ /dev/null @@ -1,147 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * group_sync_write.cpp - * - * Created on: 2016. 1. 28. - */ -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include -#include "dynamixel_sdk/group_sync_write.h" - -using namespace dynamixel; - -GroupSyncWrite::GroupSyncWrite(PortHandler *port, PacketHandler *ph, uint16_t start_address, uint16_t data_length) - : port_(port), - ph_(ph), - is_param_changed_(false), - param_(0), - start_address_(start_address), - data_length_(data_length) -{ - clearParam(); -} - -void GroupSyncWrite::makeParam() -{ - if (id_list_.size() == 0) return; - - if (param_ != 0) - delete[] param_; - param_ = 0; - - param_ = new uint8_t[id_list_.size() * (1 + data_length_)]; // ID(1) + DATA(data_length) - - int idx = 0; - for (unsigned int i = 0; i < id_list_.size(); i++) - { - uint8_t id = id_list_[i]; - if (data_list_[id] == 0) - return; - - param_[idx++] = id; - for (int c = 0; c < data_length_; c++) - param_[idx++] = (data_list_[id])[c]; - } -} - -bool GroupSyncWrite::addParam(uint8_t id, uint8_t *data) -{ - if (std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist - return false; - - id_list_.push_back(id); - data_list_[id] = new uint8_t[data_length_]; - for (int c = 0; c < data_length_; c++) - data_list_[id][c] = data[c]; - - is_param_changed_ = true; - return true; -} - -void GroupSyncWrite::removeParam(uint8_t id) -{ - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if (it == id_list_.end()) // NOT exist - return; - - id_list_.erase(it); - delete[] data_list_[id]; - data_list_.erase(id); - - is_param_changed_ = true; -} - -bool GroupSyncWrite::changeParam(uint8_t id, uint8_t *data) -{ - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if (it == id_list_.end()) // NOT exist - return false; - - delete[] data_list_[id]; - data_list_[id] = new uint8_t[data_length_]; - for (int c = 0; c < data_length_; c++) - data_list_[id][c] = data[c]; - - is_param_changed_ = true; - return true; -} - -void GroupSyncWrite::clearParam() -{ - if (id_list_.size() == 0) - return; - - for (unsigned int i = 0; i < id_list_.size(); i++) - delete[] data_list_[id_list_[i]]; - - id_list_.clear(); - data_list_.clear(); - if (param_ != 0) - delete[] param_; - param_ = 0; -} - -int GroupSyncWrite::txPacket() -{ - if (id_list_.size() == 0) - return COMM_NOT_AVAILABLE; - - if (is_param_changed_ == true) - makeParam(); - - return ph_->syncWriteTxOnly(port_, start_address_, data_length_, param_, id_list_.size() * (1 + data_length_)); -} diff --git a/dynamixel_sdk/src/dynamixel_sdk/packet_handler.cpp b/dynamixel_sdk/src/dynamixel_sdk/packet_handler.cpp deleted file mode 100644 index 04fdc20..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/packet_handler.cpp +++ /dev/null @@ -1,60 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * packet_handler.cpp - * - * Created on: 2016. 1. 26. - */ -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include "dynamixel_sdk/packet_handler.h" -#include "dynamixel_sdk/protocol1_packet_handler.h" -#include "dynamixel_sdk/protocol2_packet_handler.h" - -using namespace dynamixel; - -PacketHandler *PacketHandler::getPacketHandler(float protocol_version) -{ - if (protocol_version == 1.0) - { - return (PacketHandler *)(Protocol1PacketHandler::getInstance()); - } - else if (protocol_version == 2.0) - { - return (PacketHandler *)(Protocol2PacketHandler::getInstance()); - } - - return (PacketHandler *)(Protocol2PacketHandler::getInstance()); -} diff --git a/dynamixel_sdk/src/dynamixel_sdk/port_handler.cpp b/dynamixel_sdk/src/dynamixel_sdk/port_handler.cpp deleted file mode 100644 index 14c0c28..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/port_handler.cpp +++ /dev/null @@ -1,63 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * port_handler.cpp - * - * Created on: 2016. 2. 5. - */ -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include "dynamixel_sdk/port_handler.h" - -#ifdef __linux__ - #include "dynamixel_sdk_linux/port_handler_linux.h" -#endif - -#if defined(_WIN32) || defined(_WIN64) - #include "dynamixel_sdk_windows/port_handler_windows.h" -#endif - -using namespace dynamixel; - -PortHandler *PortHandler::getPortHandler(const char *port_name) -{ -#ifdef __linux__ - return (PortHandler *)(new PortHandlerLinux(port_name)); -#endif - -#if defined(_WIN32) || defined(_WIN64) - return (PortHandler *)(new PortHandlerWindows(port_name)); -#endif -} diff --git a/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp b/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp deleted file mode 100644 index 5129164..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp +++ /dev/null @@ -1,727 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * protocol1_packet_handler.cpp - * - * Created on: 2016. 1. 26. - */ -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include -#include -#include "dynamixel_sdk/protocol1_packet_handler.h" - -#define TXPACKET_MAX_LEN (250) -#define RXPACKET_MAX_LEN (250) - -///////////////// for Protocol 1.0 Packet ///////////////// -#define PKT_HEADER0 0 -#define PKT_HEADER1 1 -#define PKT_ID 2 -#define PKT_LENGTH 3 -#define PKT_INSTRUCTION 4 -#define PKT_ERROR 4 -#define PKT_PARAMETER0 5 - -///////////////// Protocol 1.0 Error bit ///////////////// -#define ERRBIT_VOLTAGE 1 // Supplied voltage is out of the range (operating volatage set in the control table) -#define ERRBIT_ANGLE 2 // Goal position is written out of the range (from CW angle limit to CCW angle limit) -#define ERRBIT_OVERHEAT 4 // Temperature is out of the range (operating temperature set in the control table) -#define ERRBIT_RANGE 8 // Command(setting value) is out of the range for use. -#define ERRBIT_CHECKSUM 16 // Instruction packet checksum is incorrect. -#define ERRBIT_OVERLOAD 32 // The current load cannot be controlled by the set torque. -#define ERRBIT_INSTRUCTION 64 // Undefined instruction or delivering the action command without the reg_write command. - -using namespace dynamixel; - -Protocol1PacketHandler *Protocol1PacketHandler::unique_instance_ = new Protocol1PacketHandler(); - -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; - uint8_t total_packet_length = txpacket[PKT_LENGTH] + 4; // 4: HEADER0 HEADER1 ID LENGTH - uint8_t written_packet_length = 0; - - if (port->is_using_) - return COMM_PORT_BUSY; - port->is_using_ = true; - - // check max packet length - if (total_packet_length > TXPACKET_MAX_LEN) - { - port->is_using_ = false; - return COMM_TX_ERROR; - } - - // make packet header - txpacket[PKT_HEADER0] = 0xFF; - txpacket[PKT_HEADER1] = 0xFF; - - // add a checksum to the packet - for (int idx = 2; idx < total_packet_length - 1; idx++) // except header, checksum - checksum += txpacket[idx]; - txpacket[total_packet_length - 1] = ~checksum; - - // tx packet - port->clearPort(); - written_packet_length = port->writePort(txpacket, total_packet_length); - if (total_packet_length != written_packet_length) - { - port->is_using_ = false; - return COMM_TX_FAIL; - } - - return COMM_SUCCESS; -} - -int Protocol1PacketHandler::rxPacket(PortHandler *port, uint8_t *rxpacket) -{ - int result = COMM_TX_FAIL; - - uint8_t checksum = 0; - uint8_t rx_length = 0; - uint8_t wait_length = 6; // minimum length (HEADER0 HEADER1 ID LENGTH ERROR CHKSUM) - - while(true) - { - rx_length += port->readPort(&rxpacket[rx_length], wait_length - rx_length); - if (rx_length >= wait_length) - { - uint8_t idx = 0; - - // find packet header - for (idx = 0; idx < (rx_length - 1); idx++) - { - if (rxpacket[idx] == 0xFF && rxpacket[idx+1] == 0xFF) - break; - } - - if (idx == 0) // found at the beginning of the packet - { - if (rxpacket[PKT_ID] > 0xFD || // unavailable ID - rxpacket[PKT_LENGTH] > RXPACKET_MAX_LEN || // unavailable Length - rxpacket[PKT_ERROR] >= 0x64) // unavailable Error - { - // remove the first byte in the packet - for (uint8_t s = 0; s < rx_length - 1; s++) - rxpacket[s] = rxpacket[1 + s]; - //memcpy(&rxpacket[0], &rxpacket[idx], rx_length - idx); - rx_length -= 1; - continue; - } - - // re-calculate the exact length of the rx packet - if (wait_length != rxpacket[PKT_LENGTH] + PKT_LENGTH + 1) - { - wait_length = rxpacket[PKT_LENGTH] + PKT_LENGTH + 1; - continue; - } - - if (rx_length < wait_length) - { - // check timeout - if (port->isPacketTimeout() == true) - { - if (rx_length == 0) - { - result = COMM_RX_TIMEOUT; - } - else - { - result = COMM_RX_CORRUPT; - } - break; - } - else - { - continue; - } - } - - // calculate checksum - for (int i = 2; i < wait_length - 1; i++) // except header, checksum - checksum += rxpacket[i]; - checksum = ~checksum; - - // verify checksum - if (rxpacket[wait_length - 1] == checksum) - { - result = COMM_SUCCESS; - } - else - { - result = COMM_RX_CORRUPT; - } - break; - } - else - { - // remove unnecessary packets - for (uint8_t s = 0; s < rx_length - idx; s++) - rxpacket[s] = rxpacket[idx + s]; - //memcpy(&rxpacket[0], &rxpacket[idx], rx_length - idx); - rx_length -= idx; - } - } - else - { - // check timeout - if (port->isPacketTimeout() == true) - { - if (rx_length == 0) - { - result = COMM_RX_TIMEOUT; - } - else - { - result = COMM_RX_CORRUPT; - } - break; - } - } - } - port->is_using_ = false; - - return result; -} - -// NOT for BulkRead instruction -int Protocol1PacketHandler::txRxPacket(PortHandler *port, uint8_t *txpacket, uint8_t *rxpacket, uint8_t *error) -{ - int result = COMM_TX_FAIL; - - // tx packet - result = txPacket(port, txpacket); - if (result != COMM_SUCCESS) - return result; - - // (ID == Broadcast ID && NOT BulkRead) == no need to wait for status packet - // (Instruction == action) == no need to wait for status packet - if ((txpacket[PKT_ID] == BROADCAST_ID && txpacket[PKT_INSTRUCTION] != INST_BULK_READ) || - (txpacket[PKT_INSTRUCTION] == INST_ACTION)) - { - port->is_using_ = false; - return result; - } - - // set packet timeout - if (txpacket[PKT_INSTRUCTION] == INST_READ) - { - port->setPacketTimeout((uint16_t)(txpacket[PKT_PARAMETER0+1] + 6)); - } - else - { - port->setPacketTimeout((uint16_t)6); - } - - // rx packet - result = rxPacket(port, rxpacket); - // check txpacket ID == rxpacket ID - if (txpacket[PKT_ID] != rxpacket[PKT_ID]) - result = rxPacket(port, rxpacket); - - if (result == COMM_SUCCESS && txpacket[PKT_ID] != BROADCAST_ID) - { - if (error != 0) - *error = (uint8_t)rxpacket[PKT_ERROR]; - } - return result; -} - -int Protocol1PacketHandler::ping(PortHandler *port, uint8_t id, uint8_t *error) -{ - return ping(port, id, 0, error); -} - -int Protocol1PacketHandler::ping(PortHandler *port, uint8_t id, uint16_t *model_number, uint8_t *error) -{ - int result = COMM_TX_FAIL; - - uint8_t txpacket[6] = {0}; - uint8_t rxpacket[6] = {0}; - - if (id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = 2; - txpacket[PKT_INSTRUCTION] = INST_PING; - - result = txRxPacket(port, txpacket, rxpacket, error); - if (result == COMM_SUCCESS && model_number != 0) - { - uint8_t data_read[2] = {0}; - result = readTxRx(port, id, 0, 2, data_read); // Address 0 : Model Number - if (result == COMM_SUCCESS) *model_number = DXL_MAKEWORD(data_read[0], data_read[1]); - } - - return result; -} - -int Protocol1PacketHandler::broadcastPing(PortHandler *port, std::vector &id_list) -{ - return COMM_NOT_AVAILABLE; -} - -int Protocol1PacketHandler::action(PortHandler *port, uint8_t id) -{ - uint8_t txpacket[6] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = 2; - txpacket[PKT_INSTRUCTION] = INST_ACTION; - - return txRxPacket(port, txpacket, 0); -} - -int Protocol1PacketHandler::reboot(PortHandler *port, uint8_t id, uint8_t *error) -{ - return COMM_NOT_AVAILABLE; -} - -int Protocol1PacketHandler::factoryReset(PortHandler *port, uint8_t id, uint8_t option, uint8_t *error) -{ - uint8_t txpacket[6] = {0}; - uint8_t rxpacket[6] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = 2; - txpacket[PKT_INSTRUCTION] = INST_FACTORY_RESET; - - return txRxPacket(port, txpacket, rxpacket, error); -} - -int Protocol1PacketHandler::readTx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length) -{ - int result = COMM_TX_FAIL; - - uint8_t txpacket[8] = {0}; - - if (id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = 4; - txpacket[PKT_INSTRUCTION] = INST_READ; - txpacket[PKT_PARAMETER0+0] = (uint8_t)address; - txpacket[PKT_PARAMETER0+1] = (uint8_t)length; - - result = txPacket(port, txpacket); - - // set packet timeout - if (result == COMM_SUCCESS) - port->setPacketTimeout((uint16_t)(length+6)); - - return result; -} - -int Protocol1PacketHandler::readRx(PortHandler *port, uint16_t length, uint8_t *data, uint8_t *error) -{ - int result = COMM_TX_FAIL; - uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN);//(length+6); - //uint8_t *rxpacket = new uint8_t[length+6]; - - result = rxPacket(port, rxpacket); - if (result == COMM_SUCCESS) - { - if (error != 0) - { - *error = (uint8_t)rxpacket[PKT_ERROR]; - } - for (uint8_t s = 0; s < length; s++) - { - data[s] = rxpacket[PKT_PARAMETER0 + s]; - } - //memcpy(data, &rxpacket[PKT_PARAMETER0], length); - } - - free(rxpacket); - //delete[] rxpacket; - return result; -} - -int Protocol1PacketHandler::readTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) -{ - int result = COMM_TX_FAIL; - - uint8_t txpacket[8] = {0}; - uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN);//(length+6); - - if (id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = 4; - txpacket[PKT_INSTRUCTION] = INST_READ; - txpacket[PKT_PARAMETER0+0] = (uint8_t)address; - txpacket[PKT_PARAMETER0+1] = (uint8_t)length; - - result = txRxPacket(port, txpacket, rxpacket, error); - if (result == COMM_SUCCESS) - { - if (error != 0) - { - *error = (uint8_t)rxpacket[PKT_ERROR]; - } - for (uint8_t s = 0; s < length; s++) - { - data[s] = rxpacket[PKT_PARAMETER0 + s]; - } - //memcpy(data, &rxpacket[PKT_PARAMETER0], length); - } - - free(rxpacket); - //delete[] rxpacket; - return result; -} - -int Protocol1PacketHandler::read1ByteTx(PortHandler *port, uint8_t id, uint16_t address) -{ - return readTx(port, id, address, 1); -} -int Protocol1PacketHandler::read1ByteRx(PortHandler *port, uint8_t *data, uint8_t *error) -{ - uint8_t data_read[1] = {0}; - int result = readRx(port, 1, data_read, error); - if (result == COMM_SUCCESS) - *data = data_read[0]; - return result; -} -int Protocol1PacketHandler::read1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error) -{ - uint8_t data_read[1] = {0}; - int result = readTxRx(port, id, address, 1, data_read, error); - if (result == COMM_SUCCESS) - *data = data_read[0]; - return result; -} - -int Protocol1PacketHandler::read2ByteTx(PortHandler *port, uint8_t id, uint16_t address) -{ - return readTx(port, id, address, 2); -} -int Protocol1PacketHandler::read2ByteRx(PortHandler *port, uint16_t *data, uint8_t *error) -{ - uint8_t data_read[2] = {0}; - int result = readRx(port, 2, data_read, error); - if (result == COMM_SUCCESS) - *data = DXL_MAKEWORD(data_read[0], data_read[1]); - return result; -} -int Protocol1PacketHandler::read2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error) -{ - uint8_t data_read[2] = {0}; - int result = readTxRx(port, id, address, 2, data_read, error); - if (result == COMM_SUCCESS) - *data = DXL_MAKEWORD(data_read[0], data_read[1]); - return result; -} - -int Protocol1PacketHandler::read4ByteTx(PortHandler *port, uint8_t id, uint16_t address) -{ - return COMM_NOT_AVAILABLE; -} -int Protocol1PacketHandler::read4ByteRx(PortHandler *port, uint32_t *data, uint8_t *error) -{ - return COMM_NOT_AVAILABLE; -} -int Protocol1PacketHandler::read4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error) -{ - return COMM_NOT_AVAILABLE; -} - -int Protocol1PacketHandler::writeTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) -{ - int result = COMM_TX_FAIL; - - uint8_t *txpacket = (uint8_t *)malloc(length+7); - //uint8_t *txpacket = new uint8_t[length+7]; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = length+3; - txpacket[PKT_INSTRUCTION] = INST_WRITE; - txpacket[PKT_PARAMETER0] = (uint8_t)address; - - for (uint8_t s = 0; s < length; s++) - txpacket[PKT_PARAMETER0+1+s] = data[s]; - //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); - - result = txPacket(port, txpacket); - port->is_using_ = false; - - free(txpacket); - //delete[] txpacket; - return result; -} - -int Protocol1PacketHandler::writeTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) -{ - int result = COMM_TX_FAIL; - - uint8_t *txpacket = (uint8_t *)malloc(length+7); //#6->7 - //uint8_t *txpacket = new uint8_t[length+7]; - uint8_t rxpacket[6] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = length+3; - txpacket[PKT_INSTRUCTION] = INST_WRITE; - txpacket[PKT_PARAMETER0] = (uint8_t)address; - - for (uint8_t s = 0; s < length; s++) - txpacket[PKT_PARAMETER0+1+s] = data[s]; - //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); - - result = txRxPacket(port, txpacket, rxpacket, error); - - free(txpacket); - //delete[] txpacket; - return result; -} - -int Protocol1PacketHandler::write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data) -{ - uint8_t data_write[1] = { data }; - return writeTxOnly(port, id, address, 1, data_write); -} -int Protocol1PacketHandler::write1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error) -{ - uint8_t data_write[1] = { data }; - return writeTxRx(port, id, address, 1, data_write, error); -} - -int Protocol1PacketHandler::write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data) -{ - uint8_t data_write[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; - return writeTxOnly(port, id, address, 2, data_write); -} -int Protocol1PacketHandler::write2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error) -{ - uint8_t data_write[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; - return writeTxRx(port, id, address, 2, data_write, error); -} - -int Protocol1PacketHandler::write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data) -{ - return COMM_NOT_AVAILABLE; -} -int Protocol1PacketHandler::write4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error) -{ - return COMM_NOT_AVAILABLE; -} - -int Protocol1PacketHandler::regWriteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) -{ - int result = COMM_TX_FAIL; - - uint8_t *txpacket = (uint8_t *)malloc(length+6); - //uint8_t *txpacket = new uint8_t[length+6]; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = length+3; - txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; - txpacket[PKT_PARAMETER0] = (uint8_t)address; - - for (uint8_t s = 0; s < length; s++) - txpacket[PKT_PARAMETER0+1+s] = data[s]; - //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); - - result = txPacket(port, txpacket); - port->is_using_ = false; - - free(txpacket); - //delete[] txpacket; - return result; -} - -int Protocol1PacketHandler::regWriteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) -{ - int result = COMM_TX_FAIL; - - uint8_t *txpacket = (uint8_t *)malloc(length+6); - //uint8_t *txpacket = new uint8_t[length+6]; - uint8_t rxpacket[6] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = length+3; - txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; - txpacket[PKT_PARAMETER0] = (uint8_t)address; - - for (uint8_t s = 0; s < length; s++) - txpacket[PKT_PARAMETER0+1+s] = data[s]; - //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); - - result = txRxPacket(port, txpacket, rxpacket, error); - - free(txpacket); - //delete[] txpacket; - return result; -} - -int Protocol1PacketHandler::syncReadTx(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) -{ - return COMM_NOT_AVAILABLE; -} - -int Protocol1PacketHandler::syncWriteTxOnly(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) -{ - int result = COMM_TX_FAIL; - - uint8_t *txpacket = (uint8_t *)malloc(param_length+8); - // 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN ... CHKSUM - //uint8_t *txpacket = new uint8_t[param_length + 8]; - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH] = param_length + 4; // 4: INST START_ADDR DATA_LEN ... CHKSUM - txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE; - txpacket[PKT_PARAMETER0+0] = start_address; - txpacket[PKT_PARAMETER0+1] = data_length; - - for (uint8_t s = 0; s < param_length; s++) - txpacket[PKT_PARAMETER0+2+s] = param[s]; - //memcpy(&txpacket[PKT_PARAMETER0+2], param, param_length); - - result = txRxPacket(port, txpacket, 0, 0); - - free(txpacket); - //delete[] txpacket; - return result; -} - -int Protocol1PacketHandler::bulkReadTx(PortHandler *port, uint8_t *param, uint16_t param_length) -{ - int result = COMM_TX_FAIL; - - uint8_t *txpacket = (uint8_t *)malloc(param_length+7); - // 7: HEADER0 HEADER1 ID LEN INST 0x00 ... CHKSUM - //uint8_t *txpacket = new uint8_t[param_length + 7]; - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH] = param_length + 3; // 3: INST 0x00 ... CHKSUM - txpacket[PKT_INSTRUCTION] = INST_BULK_READ; - txpacket[PKT_PARAMETER0+0] = 0x00; - - for (uint8_t s = 0; s < param_length; s++) - txpacket[PKT_PARAMETER0+1+s] = param[s]; - //memcpy(&txpacket[PKT_PARAMETER0+1], param, param_length); - - result = txPacket(port, txpacket); - if (result == COMM_SUCCESS) - { - int wait_length = 0; - for (int i = 0; i < param_length; i += 3) - wait_length += param[i] + 7; - port->setPacketTimeout((uint16_t)wait_length); - } - - free(txpacket); - //delete[] txpacket; - return result; -} - -int Protocol1PacketHandler::bulkWriteTxOnly(PortHandler *port, uint8_t *param, uint16_t param_length) -{ - return COMM_NOT_AVAILABLE; -} diff --git a/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp b/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp deleted file mode 100644 index f814229..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp +++ /dev/null @@ -1,1036 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * protocol2_packet_handler.cpp - * - * Created on: 2016. 1. 26. - */ - -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include -#include -#include -#include "dynamixel_sdk/protocol2_packet_handler.h" - -#define TXPACKET_MAX_LEN (4*1024) -#define RXPACKET_MAX_LEN (4*1024) - -///////////////// for Protocol 2.0 Packet ///////////////// -#define PKT_HEADER0 0 -#define PKT_HEADER1 1 -#define PKT_HEADER2 2 -#define PKT_RESERVED 3 -#define PKT_ID 4 -#define PKT_LENGTH_L 5 -#define PKT_LENGTH_H 6 -#define PKT_INSTRUCTION 7 -#define PKT_ERROR 8 -#define PKT_PARAMETER0 8 - -///////////////// Protocol 2.0 Error bit ///////////////// -#define ERRNUM_RESULT_FAIL 1 // Failed to process the instruction packet. -#define ERRNUM_INSTRUCTION 2 // Instruction error -#define ERRNUM_CRC 3 // CRC check error -#define ERRNUM_DATA_RANGE 4 // Data range error -#define ERRNUM_DATA_LENGTH 5 // Data length error -#define ERRNUM_DATA_LIMIT 6 // Data limit error -#define ERRNUM_ACCESS 7 // Access error - -#define ERRBIT_ALERT 128 //When the device has a problem, this bit is set to 1. Check "Device Status Check" value. - -using namespace dynamixel; - -Protocol2PacketHandler *Protocol2PacketHandler::unique_instance_ = new Protocol2PacketHandler(); - -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 not_alert_error = error & ~ERRBIT_ALERT; - - switch(not_alert_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; - uint16_t crc_table[256] = {0x0000, - 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011, - 0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, - 0x0022, 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, - 0x8077, 0x0072, 0x0050, 0x8055, 0x805F, 0x005A, 0x804B, - 0x004E, 0x0044, 0x8041, 0x80C3, 0x00C6, 0x00CC, 0x80C9, - 0x00D8, 0x80DD, 0x80D7, 0x00D2, 0x00F0, 0x80F5, 0x80FF, - 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1, 0x00A0, 0x80A5, - 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1, 0x8093, - 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082, - 0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, - 0x0192, 0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, - 0x01A4, 0x81A1, 0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, - 0x01FE, 0x01F4, 0x81F1, 0x81D3, 0x01D6, 0x01DC, 0x81D9, - 0x01C8, 0x81CD, 0x81C7, 0x01C2, 0x0140, 0x8145, 0x814F, - 0x014A, 0x815B, 0x015E, 0x0154, 0x8151, 0x8173, 0x0176, - 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162, 0x8123, - 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132, - 0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, - 0x8101, 0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, - 0x8317, 0x0312, 0x0330, 0x8335, 0x833F, 0x033A, 0x832B, - 0x032E, 0x0324, 0x8321, 0x0360, 0x8365, 0x836F, 0x036A, - 0x837B, 0x037E, 0x0374, 0x8371, 0x8353, 0x0356, 0x035C, - 0x8359, 0x0348, 0x834D, 0x8347, 0x0342, 0x03C0, 0x83C5, - 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1, 0x83F3, - 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2, - 0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, - 0x03B2, 0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, - 0x0384, 0x8381, 0x0280, 0x8285, 0x828F, 0x028A, 0x829B, - 0x029E, 0x0294, 0x8291, 0x82B3, 0x02B6, 0x02BC, 0x82B9, - 0x02A8, 0x82AD, 0x82A7, 0x02A2, 0x82E3, 0x02E6, 0x02EC, - 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2, 0x02D0, 0x82D5, - 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1, 0x8243, - 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252, - 0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, - 0x8261, 0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, - 0x0234, 0x8231, 0x8213, 0x0216, 0x021C, 0x8219, 0x0208, - 0x820D, 0x8207, 0x0202 }; - - for (uint16_t j = 0; j < data_blk_size; j++) - { - i = ((uint16_t)(crc_accum >> 8) ^ *data_blk_ptr++) & 0xFF; - crc_accum = (crc_accum << 8) ^ crc_table[i]; - } - - return crc_accum; -} - -void Protocol2PacketHandler::addStuffing(uint8_t *packet) -{ - int i = 0, index = 0; - int packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]); - int packet_length_out = packet_length_in; - uint8_t temp[TXPACKET_MAX_LEN] = {0}; - - for (uint8_t s = PKT_HEADER0; s <= PKT_LENGTH_H; s++) - temp[s] = packet[s]; // FF FF FD XX ID LEN_L LEN_H - //memcpy(temp, packet, PKT_LENGTH_H+1); - index = PKT_INSTRUCTION; - for (i = 0; i < packet_length_in - 2; i++) // except CRC - { - temp[index++] = packet[i+PKT_INSTRUCTION]; - if (packet[i+PKT_INSTRUCTION] == 0xFD && packet[i+PKT_INSTRUCTION-1] == 0xFF && packet[i+PKT_INSTRUCTION-2] == 0xFF) - { // FF FF FD - temp[index++] = 0xFD; - packet_length_out++; - } - } - temp[index++] = packet[PKT_INSTRUCTION+packet_length_in-2]; - temp[index++] = packet[PKT_INSTRUCTION+packet_length_in-1]; - - - ////////////////////////// - if (packet_length_in != packet_length_out) - packet = (uint8_t *)realloc(packet, index * sizeof(uint8_t)); - - /////////////////////////// - - for (uint8_t s = 0; s < index; s++) - packet[s] = temp[s]; - //memcpy(packet, temp, index); - packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out); - packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out); -} - -void Protocol2PacketHandler::removeStuffing(uint8_t *packet) -{ - int i = 0, index = 0; - int packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]); - int packet_length_out = packet_length_in; - - index = PKT_INSTRUCTION; - for (i = 0; i < packet_length_in - 2; i++) // except CRC - { - if (packet[i+PKT_INSTRUCTION] == 0xFD && packet[i+PKT_INSTRUCTION+1] == 0xFD && packet[i+PKT_INSTRUCTION-1] == 0xFF && packet[i+PKT_INSTRUCTION-2] == 0xFF) - { // FF FF FD FD - packet_length_out--; - i++; - } - packet[index++] = packet[i+PKT_INSTRUCTION]; - } - packet[index++] = packet[PKT_INSTRUCTION+packet_length_in-2]; - packet[index++] = packet[PKT_INSTRUCTION+packet_length_in-1]; - - packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out); - packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out); -} - -int Protocol2PacketHandler::txPacket(PortHandler *port, uint8_t *txpacket) -{ - uint16_t total_packet_length = 0; - uint16_t written_packet_length = 0; - - if (port->is_using_) - return COMM_PORT_BUSY; - port->is_using_ = true; - - // byte stuffing for header - addStuffing(txpacket); - - // check max packet length - total_packet_length = DXL_MAKEWORD(txpacket[PKT_LENGTH_L], txpacket[PKT_LENGTH_H]) + 7; - // 7: HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H - if (total_packet_length > TXPACKET_MAX_LEN) - { - port->is_using_ = false; - return COMM_TX_ERROR; - } - - // make packet header - txpacket[PKT_HEADER0] = 0xFF; - txpacket[PKT_HEADER1] = 0xFF; - txpacket[PKT_HEADER2] = 0xFD; - txpacket[PKT_RESERVED] = 0x00; - - // add CRC16 - uint16_t crc = updateCRC(0, txpacket, total_packet_length - 2); // 2: CRC16 - txpacket[total_packet_length - 2] = DXL_LOBYTE(crc); - txpacket[total_packet_length - 1] = DXL_HIBYTE(crc); - - // tx packet - port->clearPort(); - written_packet_length = port->writePort(txpacket, total_packet_length); - if (total_packet_length != written_packet_length) - { - port->is_using_ = false; - return COMM_TX_FAIL; - } - - return COMM_SUCCESS; -} - -int Protocol2PacketHandler::rxPacket(PortHandler *port, uint8_t *rxpacket) -{ - int result = COMM_TX_FAIL; - - uint16_t rx_length = 0; - uint16_t wait_length = 11; // minimum length (HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H INST ERROR CRC16_L CRC16_H) - - while(true) - { - rx_length += port->readPort(&rxpacket[rx_length], wait_length - rx_length); - if (rx_length >= wait_length) - { - uint16_t idx = 0; - - // find packet header - for (idx = 0; idx < (rx_length - 3); idx++) - { - if ((rxpacket[idx] == 0xFF) && (rxpacket[idx+1] == 0xFF) && (rxpacket[idx+2] == 0xFD) && (rxpacket[idx+3] != 0xFD)) - break; - } - - if (idx == 0) // found at the beginning of the packet - { - if (rxpacket[PKT_RESERVED] != 0x00 || - rxpacket[PKT_ID] > 0xFC || - DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) > RXPACKET_MAX_LEN || - rxpacket[PKT_INSTRUCTION] != 0x55) - { - // remove the first byte in the packet - for (uint8_t s = 0; s < rx_length - 1; s++) - rxpacket[s] = rxpacket[1 + s]; - //memcpy(&rxpacket[0], &rxpacket[idx], rx_length - idx); - rx_length -= 1; - continue; - } - - // re-calculate the exact length of the rx packet - if (wait_length != DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1) - { - wait_length = DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1; - continue; - } - - if (rx_length < wait_length) - { - // check timeout - if (port->isPacketTimeout() == true) - { - if (rx_length == 0) - { - result = COMM_RX_TIMEOUT; - } - else - { - result = COMM_RX_CORRUPT; - } - break; - } - else - { - continue; - } - } - - // verify CRC16 - uint16_t crc = DXL_MAKEWORD(rxpacket[wait_length-2], rxpacket[wait_length-1]); - if (updateCRC(0, rxpacket, wait_length - 2) == crc) - { - result = COMM_SUCCESS; - } - else - { - result = COMM_RX_CORRUPT; - } - break; - } - else - { - // remove unnecessary packets - for (uint8_t s = 0; s < rx_length - idx; s++) - rxpacket[s] = rxpacket[idx + s]; - //memcpy(&rxpacket[0], &rxpacket[idx], rx_length - idx); - rx_length -= idx; - } - } - else - { - // check timeout - if (port->isPacketTimeout() == true) - { - if (rx_length == 0) - { - result = COMM_RX_TIMEOUT; - } - else - { - result = COMM_RX_CORRUPT; - } - break; - } - } - } - port->is_using_ = false; - - if (result == COMM_SUCCESS) - removeStuffing(rxpacket); - - return result; -} - -// NOT for BulkRead / SyncRead instruction -int Protocol2PacketHandler::txRxPacket(PortHandler *port, uint8_t *txpacket, uint8_t *rxpacket, uint8_t *error) -{ - int result = COMM_TX_FAIL; - - // tx packet - result = txPacket(port, txpacket); - if (result != COMM_SUCCESS) - return result; - - // (ID == Broadcast ID && NOT BulkRead) == no need to wait for status packet - // (Instruction == action) == no need to wait for status packet - if ((txpacket[PKT_ID] == BROADCAST_ID && txpacket[PKT_INSTRUCTION] != INST_BULK_READ) || - (txpacket[PKT_ID] == BROADCAST_ID && txpacket[PKT_INSTRUCTION] != INST_SYNC_READ) || - (txpacket[PKT_INSTRUCTION] == INST_ACTION)) - { - port->is_using_ = false; - return result; - } - - // set packet timeout - if (txpacket[PKT_INSTRUCTION] == INST_READ) - { - port->setPacketTimeout((uint16_t)(DXL_MAKEWORD(txpacket[PKT_PARAMETER0+2], txpacket[PKT_PARAMETER0+3]) + 11)); - } - else - { - port->setPacketTimeout((uint16_t)11); - // HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H INST ERROR CRC16_L CRC16_H - } - - // rx packet - result = rxPacket(port, rxpacket); - // check txpacket ID == rxpacket ID - if (txpacket[PKT_ID] != rxpacket[PKT_ID]) - result = rxPacket(port, rxpacket); - - if (result == COMM_SUCCESS && txpacket[PKT_ID] != BROADCAST_ID) - { - if (error != 0) - *error = (uint8_t)rxpacket[PKT_ERROR]; - } - - return result; -} - -int Protocol2PacketHandler::ping(PortHandler *port, uint8_t id, uint8_t *error) -{ - return ping(port, id, 0, error); -} - -int Protocol2PacketHandler::ping(PortHandler *port, uint8_t id, uint16_t *model_number, uint8_t *error) -{ - int result = COMM_TX_FAIL; - - uint8_t txpacket[10] = {0}; - uint8_t rxpacket[14] = {0}; - - if (id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 3; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_PING; - - result = txRxPacket(port, txpacket, rxpacket, error); - if (result == COMM_SUCCESS && model_number != 0) - *model_number = DXL_MAKEWORD(rxpacket[PKT_PARAMETER0+1], rxpacket[PKT_PARAMETER0+2]); - - return result; -} - -int Protocol2PacketHandler::broadcastPing(PortHandler *port, std::vector &id_list) -{ - const int STATUS_LENGTH = 14; - int result = COMM_TX_FAIL; - - id_list.clear(); - - uint16_t rx_length = 0; - uint16_t wait_length = STATUS_LENGTH * MAX_ID; - - uint8_t txpacket[10] = {0}; - uint8_t rxpacket[STATUS_LENGTH * MAX_ID] = {0}; - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH_L] = 3; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_PING; - - result = txPacket(port, txpacket); - if (result != COMM_SUCCESS) - { - port->is_using_ = false; - return result; - } - - // set rx timeout - port->setPacketTimeout((uint16_t)(wait_length * 30)); - - while(1) - { - rx_length += port->readPort(&rxpacket[rx_length], wait_length - rx_length); - if (port->isPacketTimeout() == true)// || rx_length >= wait_length) - break; - } - - port->is_using_ = false; - - if (rx_length == 0) - return COMM_RX_TIMEOUT; - - while(1) - { - if (rx_length < STATUS_LENGTH) - return COMM_RX_CORRUPT; - - uint16_t idx = 0; - - // find packet header - for (idx = 0; idx < (rx_length - 2); idx++) - { - if (rxpacket[idx] == 0xFF && rxpacket[idx+1] == 0xFF && rxpacket[idx+2] == 0xFD) - break; - } - - if (idx == 0) // found at the beginning of the packet - { - // verify CRC16 - uint16_t crc = DXL_MAKEWORD(rxpacket[STATUS_LENGTH-2], rxpacket[STATUS_LENGTH-1]); - - if (updateCRC(0, rxpacket, STATUS_LENGTH - 2) == crc) - { - result = COMM_SUCCESS; - - id_list.push_back(rxpacket[PKT_ID]); - - for (uint8_t s = 0; s < rx_length - STATUS_LENGTH; s++) - rxpacket[s] = rxpacket[STATUS_LENGTH + s]; - rx_length -= STATUS_LENGTH; - - if (rx_length == 0) - return result; - } - else - { - result = COMM_RX_CORRUPT; - - // remove header (0xFF 0xFF 0xFD) - for (uint8_t s = 0; s < rx_length - 3; s++) - rxpacket[s] = rxpacket[3 + s]; - rx_length -= 3; - } - } - else - { - // remove unnecessary packets - for (uint8_t s = 0; s < rx_length - idx; s++) - rxpacket[s] = rxpacket[idx + s]; - rx_length -= idx; - } - } - - return result; -} - -int Protocol2PacketHandler::action(PortHandler *port, uint8_t id) -{ - uint8_t txpacket[10] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 3; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_ACTION; - - return txRxPacket(port, txpacket, 0); -} - -int Protocol2PacketHandler::reboot(PortHandler *port, uint8_t id, uint8_t *error) -{ - uint8_t txpacket[10] = {0}; - uint8_t rxpacket[11] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 3; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_REBOOT; - - return txRxPacket(port, txpacket, rxpacket, error); -} - -int Protocol2PacketHandler::factoryReset(PortHandler *port, uint8_t id, uint8_t option, uint8_t *error) -{ - uint8_t txpacket[11] = {0}; - uint8_t rxpacket[11] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 4; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_FACTORY_RESET; - txpacket[PKT_PARAMETER0] = option; - - return txRxPacket(port, txpacket, rxpacket, error); -} - -int Protocol2PacketHandler::readTx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length) -{ - int result = COMM_TX_FAIL; - - uint8_t txpacket[14] = {0}; - - if (id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 7; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_READ; - txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); - txpacket[PKT_PARAMETER0+2] = (uint8_t)DXL_LOBYTE(length); - txpacket[PKT_PARAMETER0+3] = (uint8_t)DXL_HIBYTE(length); - - result = txPacket(port, txpacket); - - // set packet timeout - if (result == COMM_SUCCESS) - port->setPacketTimeout((uint16_t)(length + 11)); - - return result; -} - -int Protocol2PacketHandler::readRx(PortHandler *port, uint16_t length, uint8_t *data, uint8_t *error) -{ - int result = COMM_TX_FAIL; - uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN); - //(length + 11 + (length/3)); // (length/3): consider stuffing - //uint8_t *rxpacket = new uint8_t[length + 11 + (length/3)]; // (length/3): consider stuffing - - result = rxPacket(port, rxpacket); - if (result == COMM_SUCCESS) - { - if (error != 0) - *error = (uint8_t)rxpacket[PKT_ERROR]; - for (uint8_t s = 0; s < length; s++) - data[s] = rxpacket[PKT_PARAMETER0 + 1 + s]; - //memcpy(data, &rxpacket[PKT_PARAMETER0+1], length); - } - - free(rxpacket); - //delete[] rxpacket; - return result; -} - -int Protocol2PacketHandler::readTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) -{ - int result = COMM_TX_FAIL; - - uint8_t txpacket[14] = {0}; - uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN); - //(length + 11 + (length/3)); // (length/3): consider stuffing - - if (id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 7; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_READ; - txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); - txpacket[PKT_PARAMETER0+2] = (uint8_t)DXL_LOBYTE(length); - txpacket[PKT_PARAMETER0+3] = (uint8_t)DXL_HIBYTE(length); - - result = txRxPacket(port, txpacket, rxpacket, error); - if (result == COMM_SUCCESS) - { - if (error != 0) - *error = (uint8_t)rxpacket[PKT_ERROR]; - for (uint8_t s = 0; s < length; s++) - data[s] = rxpacket[PKT_PARAMETER0 + 1 + s]; - //memcpy(data, &rxpacket[PKT_PARAMETER0+1], length); - } - - free(rxpacket); - //delete[] rxpacket; - return result; -} - -int Protocol2PacketHandler::read1ByteTx(PortHandler *port, uint8_t id, uint16_t address) -{ - return readTx(port, id, address, 1); -} -int Protocol2PacketHandler::read1ByteRx(PortHandler *port, uint8_t *data, uint8_t *error) -{ - uint8_t data_read[1] = {0}; - int result = readRx(port, 1, data_read, error); - if (result == COMM_SUCCESS) - *data = data_read[0]; - return result; -} -int Protocol2PacketHandler::read1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error) -{ - uint8_t data_read[1] = {0}; - int result = readTxRx(port, id, address, 1, data_read, error); - if (result == COMM_SUCCESS) - *data = data_read[0]; - return result; -} - -int Protocol2PacketHandler::read2ByteTx(PortHandler *port, uint8_t id, uint16_t address) -{ - return readTx(port, id, address, 2); -} -int Protocol2PacketHandler::read2ByteRx(PortHandler *port, uint16_t *data, uint8_t *error) -{ - uint8_t data_read[2] = {0}; - int result = readRx(port, 2, data_read, error); - if (result == COMM_SUCCESS) - *data = DXL_MAKEWORD(data_read[0], data_read[1]); - return result; -} -int Protocol2PacketHandler::read2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error) -{ - uint8_t data_read[2] = {0}; - int result = readTxRx(port, id, address, 2, data_read, error); - if (result == COMM_SUCCESS) - *data = DXL_MAKEWORD(data_read[0], data_read[1]); - return result; -} - -int Protocol2PacketHandler::read4ByteTx(PortHandler *port, uint8_t id, uint16_t address) -{ - return readTx(port, id, address, 4); -} -int Protocol2PacketHandler::read4ByteRx(PortHandler *port, uint32_t *data, uint8_t *error) -{ - uint8_t data_read[4] = {0}; - int result = readRx(port, 4, data_read, error); - if (result == COMM_SUCCESS) - *data = DXL_MAKEDWORD(DXL_MAKEWORD(data_read[0], data_read[1]), DXL_MAKEWORD(data_read[2], data_read[3])); - return result; -} -int Protocol2PacketHandler::read4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error) -{ - uint8_t data_read[4] = {0}; - int result = readTxRx(port, id, address, 4, data_read, error); - if (result == COMM_SUCCESS) - *data = DXL_MAKEDWORD(DXL_MAKEWORD(data_read[0], data_read[1]), DXL_MAKEWORD(data_read[2], data_read[3])); - return result; -} - - -int Protocol2PacketHandler::writeTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) -{ - int result = COMM_TX_FAIL; - - uint8_t *txpacket = (uint8_t *)malloc(length+12); - //uint8_t *txpacket = new uint8_t[length+12]; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); - txpacket[PKT_INSTRUCTION] = INST_WRITE; - txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); - - for (uint8_t s = 0; s < length; s++) - txpacket[PKT_PARAMETER0+2+s] = data[s]; - //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); - - result = txPacket(port, txpacket); - port->is_using_ = false; - - free(txpacket); - //delete[] txpacket; - return result; -} - -int Protocol2PacketHandler::writeTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) -{ - int result = COMM_TX_FAIL; - - uint8_t *txpacket = (uint8_t *)malloc(length + 12); - //uint8_t *txpacket = new uint8_t[length+12]; - uint8_t rxpacket[11] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); - txpacket[PKT_INSTRUCTION] = INST_WRITE; - txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); - - for (uint8_t s = 0; s < length; s++) - txpacket[PKT_PARAMETER0+2+s] = data[s]; - //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); - - result = txRxPacket(port, txpacket, rxpacket, error); - - free(txpacket); - //delete[] txpacket; - return result; -} - -int Protocol2PacketHandler::write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data) -{ - uint8_t data_write[1] = { data }; - return writeTxOnly(port, id, address, 1, data_write); -} -int Protocol2PacketHandler::write1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error) -{ - uint8_t data_write[1] = { data }; - return writeTxRx(port, id, address, 1, data_write, error); -} - -int Protocol2PacketHandler::write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data) -{ - uint8_t data_write[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; - return writeTxOnly(port, id, address, 2, data_write); -} -int Protocol2PacketHandler::write2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error) -{ - uint8_t data_write[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; - return writeTxRx(port, id, address, 2, data_write, error); -} - -int Protocol2PacketHandler::write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data) -{ - uint8_t data_write[4] = { DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data)) }; - return writeTxOnly(port, id, address, 4, data_write); -} -int Protocol2PacketHandler::write4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error) -{ - uint8_t data_write[4] = { DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data)) }; - return writeTxRx(port, id, address, 4, data_write, error); -} - -int Protocol2PacketHandler::regWriteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) -{ - int result = COMM_TX_FAIL; - - uint8_t *txpacket = (uint8_t *)malloc(length + 12); - //uint8_t *txpacket = new uint8_t[length+12]; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); - txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; - txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); - - for (uint8_t s = 0; s < length; s++) - txpacket[PKT_PARAMETER0+2+s] = data[s]; - //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); - - result = txPacket(port, txpacket); - port->is_using_ = false; - - free(txpacket); - //delete[] txpacket; - return result; -} - -int Protocol2PacketHandler::regWriteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) -{ - int result = COMM_TX_FAIL; - - uint8_t *txpacket = (uint8_t *)malloc(length + 12); - //uint8_t *txpacket = new uint8_t[length+12]; - uint8_t rxpacket[11] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); - txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; - txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); - - for (uint8_t s = 0; s < length; s++) - txpacket[PKT_PARAMETER0+2+s] = data[s]; - //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); - - result = txRxPacket(port, txpacket, rxpacket, error); - - free(txpacket); - //delete[] txpacket; - return result; -} - -int Protocol2PacketHandler::syncReadTx(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) -{ - int result = COMM_TX_FAIL; - - uint8_t *txpacket = (uint8_t *)malloc(param_length + 14); - // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - txpacket[PKT_INSTRUCTION] = INST_SYNC_READ; - txpacket[PKT_PARAMETER0+0] = DXL_LOBYTE(start_address); - txpacket[PKT_PARAMETER0+1] = DXL_HIBYTE(start_address); - txpacket[PKT_PARAMETER0+2] = DXL_LOBYTE(data_length); - txpacket[PKT_PARAMETER0+3] = DXL_HIBYTE(data_length); - - for (uint8_t s = 0; s < param_length; s++) - txpacket[PKT_PARAMETER0+4+s] = param[s]; - //memcpy(&txpacket[PKT_PARAMETER0+4], param, param_length); - - result = txPacket(port, txpacket); - if (result == COMM_SUCCESS) - port->setPacketTimeout((uint16_t)((11 + data_length) * param_length)); - - free(txpacket); - return result; -} - -int Protocol2PacketHandler::syncWriteTxOnly(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) -{ - int result = COMM_TX_FAIL; - - uint8_t *txpacket = (uint8_t *)malloc(param_length + 14); - //uint8_t *txpacket = new uint8_t[param_length + 14]; - // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE; - txpacket[PKT_PARAMETER0+0] = DXL_LOBYTE(start_address); - txpacket[PKT_PARAMETER0+1] = DXL_HIBYTE(start_address); - txpacket[PKT_PARAMETER0+2] = DXL_LOBYTE(data_length); - txpacket[PKT_PARAMETER0+3] = DXL_HIBYTE(data_length); - - for (uint8_t s = 0; s < param_length; s++) - txpacket[PKT_PARAMETER0+4+s] = param[s]; - //memcpy(&txpacket[PKT_PARAMETER0+4], param, param_length); - - result = txRxPacket(port, txpacket, 0, 0); - - free(txpacket); - //delete[] txpacket; - return result; -} - -int Protocol2PacketHandler::bulkReadTx(PortHandler *port, uint8_t *param, uint16_t param_length) -{ - int result = COMM_TX_FAIL; - - uint8_t *txpacket = (uint8_t *)malloc(param_length + 10); - //uint8_t *txpacket = new uint8_t[param_length + 10]; - // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H - txpacket[PKT_INSTRUCTION] = INST_BULK_READ; - - for (uint8_t s = 0; s < param_length; s++) - txpacket[PKT_PARAMETER0+s] = param[s]; - //memcpy(&txpacket[PKT_PARAMETER0], param, param_length); - - result = txPacket(port, txpacket); - if (result == COMM_SUCCESS) - { - int wait_length = 0; - for (int i = 0; i < param_length; i += 5) - wait_length += DXL_MAKEWORD(param[i+3], param[i+4]) + 10; - port->setPacketTimeout((uint16_t)wait_length); - } - - free(txpacket); - //delete[] txpacket; - return result; -} - -int Protocol2PacketHandler::bulkWriteTxOnly(PortHandler *port, uint8_t *param, uint16_t param_length) -{ - int result = COMM_TX_FAIL; - - uint8_t *txpacket = (uint8_t *)malloc(param_length + 10); - //uint8_t *txpacket = new uint8_t[param_length + 10]; - // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H - txpacket[PKT_INSTRUCTION] = INST_BULK_WRITE; - - for (uint8_t s = 0; s < param_length; s++) - txpacket[PKT_PARAMETER0+s] = param[s]; - //memcpy(&txpacket[PKT_PARAMETER0], param, param_length); - - result = txRxPacket(port, txpacket, 0, 0); - - free(txpacket); - //delete[] txpacket; - return result; -} diff --git a/dynamixel_sdk/src/dynamixel_sdk_linux/port_handler_linux.cpp b/dynamixel_sdk/src/dynamixel_sdk_linux/port_handler_linux.cpp deleted file mode 100644 index 0cc11e0..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk_linux/port_handler_linux.cpp +++ /dev/null @@ -1,276 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * port_handler_linux.cpp - * - * Created on: 2016. 1. 26. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "dynamixel_sdk_linux/port_handler_linux.h" - -#define LATENCY_TIMER 4 // msec (USB latency timer) - -using namespace dynamixel; - -PortHandlerLinux::PortHandlerLinux(const char *port_name) - : socket_fd_(-1), - baudrate_(DEFAULT_BAUDRATE_), - packet_start_time_(0.0), - packet_timeout_(0.0), - tx_time_per_byte(0.0) -{ - is_using_ = false; - setPortName(port_name); -} - -bool PortHandlerLinux::openPort() -{ - return setBaudRate(baudrate_); -} - -void PortHandlerLinux::closePort() -{ - if(socket_fd_ != -1) - close(socket_fd_); - socket_fd_ = -1; -} - -void PortHandlerLinux::clearPort() -{ - tcflush(socket_fd_, TCIOFLUSH); -} - -void PortHandlerLinux::setPortName(const char *port_name) -{ - strcpy(port_name_, port_name); -} - -char *PortHandlerLinux::getPortName() -{ - return port_name_; -} - -// TODO: baud number ?? -bool PortHandlerLinux::setBaudRate(const int baudrate) -{ - int baud = getCFlagBaud(baudrate); - - closePort(); - - if(baud <= 0) // custom baudrate - { - setupPort(B38400); - baudrate_ = baudrate; - return setCustomBaudrate(baudrate); - } - else - { - baudrate_ = baudrate; - return setupPort(baud); - } -} - -int PortHandlerLinux::getBaudRate() -{ - return baudrate_; -} - -int PortHandlerLinux::getBytesAvailable() -{ - int bytes_available; - ioctl(socket_fd_, FIONREAD, &bytes_available); - return bytes_available; -} - -int PortHandlerLinux::readPort(uint8_t *packet, int length) -{ - return read(socket_fd_, packet, length); -} - -int PortHandlerLinux::writePort(uint8_t *packet, int length) -{ - return write(socket_fd_, packet, length); -} - -void PortHandlerLinux::setPacketTimeout(uint16_t packet_length) -{ - packet_start_time_ = getCurrentTime(); - packet_timeout_ = (tx_time_per_byte * (double)packet_length) + (LATENCY_TIMER * 2.0) + 2.0; -} - -void PortHandlerLinux::setPacketTimeout(double msec) -{ - packet_start_time_ = getCurrentTime(); - packet_timeout_ = msec; -} - -bool PortHandlerLinux::isPacketTimeout() -{ - if(getTimeSinceStart() > packet_timeout_) - { - packet_timeout_ = 0; - return true; - } - return false; -} - -double PortHandlerLinux::getCurrentTime() -{ - struct timespec tv; - clock_gettime( CLOCK_REALTIME, &tv); - return ((double)tv.tv_sec*1000.0 + (double)tv.tv_nsec*0.001*0.001); -} - -double PortHandlerLinux::getTimeSinceStart() -{ - double time; - - time = getCurrentTime() - packet_start_time_; - if(time < 0.0) - packet_start_time_ = getCurrentTime(); - - return time; -} - -bool PortHandlerLinux::setupPort(int cflag_baud) -{ - struct termios newtio; - - socket_fd_ = open(port_name_, O_RDWR|O_NOCTTY|O_NONBLOCK); - if(socket_fd_ < 0) - { - printf("[PortHandlerLinux::SetupPort] Error opening serial port!\n"); - return false; - } - - bzero(&newtio, sizeof(newtio)); // clear struct for new port settings - - newtio.c_cflag = cflag_baud | CS8 | CLOCAL | CREAD; - newtio.c_iflag = IGNPAR; - newtio.c_oflag = 0; - newtio.c_lflag = 0; - newtio.c_cc[VTIME] = 0; - newtio.c_cc[VMIN] = 0; - - // clean the buffer and activate the settings for the port - tcflush(socket_fd_, TCIFLUSH); - tcsetattr(socket_fd_, TCSANOW, &newtio); - - tx_time_per_byte = (1000.0 / (double)baudrate_) * 10.0; - return true; -} - -bool PortHandlerLinux::setCustomBaudrate(int speed) -{ - // try to set a custom divisor - struct serial_struct ss; - if(ioctl(socket_fd_, TIOCGSERIAL, &ss) != 0) - { - printf("[PortHandlerLinux::SetCustomBaudrate] TIOCGSERIAL failed!\n"); - return false; - } - - ss.flags = (ss.flags & ~ASYNC_SPD_MASK) | ASYNC_SPD_CUST; - ss.custom_divisor = (ss.baud_base + (speed / 2)) / speed; - int closest_speed = ss.baud_base / ss.custom_divisor; - - if(closest_speed < speed * 98 / 100 || closest_speed > speed * 102 / 100) - { - printf("[PortHandlerLinux::SetCustomBaudrate] Cannot set speed to %d, closest is %d \n", speed, closest_speed); - return false; - } - - if(ioctl(socket_fd_, TIOCSSERIAL, &ss) < 0) - { - printf("[PortHandlerLinux::SetCustomBaudrate] TIOCSSERIAL failed!\n"); - return false; - } - - tx_time_per_byte = (1000.0 / (double)speed) * 10.0; - return true; -} - -int PortHandlerLinux::getCFlagBaud(int baudrate) -{ - switch(baudrate) - { - case 9600: - return B9600; - case 19200: - return B19200; - case 38400: - return B38400; - case 57600: - return B57600; - case 115200: - return B115200; - case 230400: - return B230400; - case 460800: - return B460800; - case 500000: - return B500000; - case 576000: - return B576000; - case 921600: - return B921600; - case 1000000: - return B1000000; - case 1152000: - return B1152000; - case 1500000: - return B1500000; - case 2000000: - return B2000000; - case 2500000: - return B2500000; - case 3000000: - return B3000000; - case 3500000: - return B3500000; - case 4000000: - return B4000000; - default: - return -1; - } -} diff --git a/dynamixel_sdk/src/dynamixel_sdk_windows/port_handler_windows.cpp b/dynamixel_sdk/src/dynamixel_sdk_windows/port_handler_windows.cpp deleted file mode 100644 index c47e76f..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk_windows/port_handler_windows.cpp +++ /dev/null @@ -1,245 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: Leon Ryu Woon Jung */ - -/* -* PortHandlerWindows.cpp -* -* Created on: 2016. 4. 06. -*/ -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include "dynamixel_sdk_windows/port_handler_windows.h" - -#include -#include -#include - -#define LATENCY_TIMER 16 // msec (USB latency timer) - -using namespace dynamixel; - -PortHandlerWindows::PortHandlerWindows(const char *port_name) - : serial_handle_(INVALID_HANDLE_VALUE), - baudrate_(DEFAULT_BAUDRATE_), - packet_start_time_(0.0), - packet_timeout_(0.0), - tx_time_per_byte_(0.0) -{ - is_using_ = false; - - char buffer[15]; - sprintf_s(buffer, sizeof(buffer), "\\\\.\\", port_name); - setPortName(port_name); -} - -bool PortHandlerWindows::openPort() -{ - return setBaudRate(baudrate_); -} - -void PortHandlerWindows::closePort() -{ - if (serial_handle_ != INVALID_HANDLE_VALUE) - { - CloseHandle(serial_handle_); - serial_handle_ = INVALID_HANDLE_VALUE; - } -} - -void PortHandlerWindows::clearPort() -{ - PurgeComm(serial_handle_, PURGE_RXABORT | PURGE_RXCLEAR); -} - -void PortHandlerWindows::setPortName(const char *port_name) -{ - strcpy_s(port_name_, sizeof(port_name_), port_name); -} - -char *PortHandlerWindows::getPortName() -{ - return port_name_; -} - -bool PortHandlerWindows::setBaudRate(const int baudrate) -{ - closePort(); - - baudrate_ = baudrate; - return setupPort(baudrate); -} - -int PortHandlerWindows::getBaudRate() -{ - return baudrate_; -} - -int PortHandlerWindows::getBytesAvailable() -{ - DWORD retbyte = 2; - BOOL res = DeviceIoControl(serial_handle_, GENERIC_READ | GENERIC_WRITE, NULL, 0, 0, 0, &retbyte, (LPOVERLAPPED)NULL); - - printf("%d", (int)res); - return (int)retbyte; -} - -int PortHandlerWindows::readPort(uint8_t *packet, int length) -{ - DWORD dwRead = 0; - - if (ReadFile(serial_handle_, packet, (DWORD)length, &dwRead, NULL) == FALSE) - return -1; - - return (int)dwRead; -} - -int PortHandlerWindows::writePort(uint8_t *packet, int length) -{ - DWORD dwWrite = 0; - - if (WriteFile(serial_handle_, packet, (DWORD)length, &dwWrite, NULL) == FALSE) - return -1; - - return (int)dwWrite; -} - -void PortHandlerWindows::setPacketTimeout(uint16_t packet_length) -{ - packet_start_time_ = getCurrentTime(); - packet_timeout_ = (tx_time_per_byte_ * (double)packet_length) + (LATENCY_TIMER * 2.0) + 2.0; -} - -void PortHandlerWindows::setPacketTimeout(double msec) -{ - packet_start_time_ = getCurrentTime(); - packet_timeout_ = msec; -} - -bool PortHandlerWindows::isPacketTimeout() -{ - if (getTimeSinceStart() > packet_timeout_) - { - packet_timeout_ = 0; - return true; - } - return false; -} - -double PortHandlerWindows::getCurrentTime() -{ - QueryPerformanceCounter(&counter_); - QueryPerformanceFrequency(&freq_); - return (double)counter_.QuadPart / (double)freq_.QuadPart * 1000.0; -} - -double PortHandlerWindows::getTimeSinceStart() -{ - double time; - - time = getCurrentTime() - packet_start_time_; - if (time < 0.0) packet_start_time_ = getCurrentTime(); - - return time; -} - -bool PortHandlerWindows::setupPort(int baudrate) -{ - DCB dcb; - COMMTIMEOUTS timeouts; - DWORD dwError; - - closePort(); - - serial_handle_ = CreateFileA(port_name_, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, NULL); - if (serial_handle_ == INVALID_HANDLE_VALUE) - { - printf("[PortHandlerWindows::SetupPort] Error opening serial port!\n"); - return false; - } - - dcb.DCBlength = sizeof(DCB); - if (GetCommState(serial_handle_, &dcb) == FALSE) - goto DXL_HAL_OPEN_ERROR; - - // Set baudrate - dcb.BaudRate = (DWORD)baudrate; - dcb.ByteSize = 8; // Data bit = 8bit - dcb.Parity = NOPARITY; // No parity - dcb.StopBits = ONESTOPBIT; // Stop bit = 1 - dcb.fParity = NOPARITY; // No Parity check - dcb.fBinary = 1; // Binary mode - dcb.fNull = 0; // Get Null byte - dcb.fAbortOnError = 0; - dcb.fErrorChar = 0; - // Not using XOn/XOff - dcb.fOutX = 0; - dcb.fInX = 0; - // Not using H/W flow control - dcb.fDtrControl = DTR_CONTROL_DISABLE; - dcb.fRtsControl = RTS_CONTROL_DISABLE; - dcb.fDsrSensitivity = 0; - dcb.fOutxDsrFlow = 0; - dcb.fOutxCtsFlow = 0; - - if (SetCommState(serial_handle_, &dcb) == FALSE) - goto DXL_HAL_OPEN_ERROR; - - if (SetCommMask(serial_handle_, 0) == FALSE) // Not using Comm event - goto DXL_HAL_OPEN_ERROR; - if (SetupComm(serial_handle_, 4096, 4096) == FALSE) // Buffer size (Rx,Tx) - goto DXL_HAL_OPEN_ERROR; - if (PurgeComm(serial_handle_, PURGE_TXABORT | PURGE_TXCLEAR | PURGE_RXABORT | PURGE_RXCLEAR) == FALSE) // Clear buffer - goto DXL_HAL_OPEN_ERROR; - if (ClearCommError(serial_handle_, &dwError, NULL) == FALSE) - goto DXL_HAL_OPEN_ERROR; - - if (GetCommTimeouts(serial_handle_, &timeouts) == FALSE) - goto DXL_HAL_OPEN_ERROR; - // Timeout (Not using timeout) - // Immediatly return - timeouts.ReadIntervalTimeout = 0; - timeouts.ReadTotalTimeoutMultiplier = 0; - timeouts.ReadTotalTimeoutConstant = 1; // must not be zero. - timeouts.WriteTotalTimeoutMultiplier = 0; - timeouts.WriteTotalTimeoutConstant = 0; - if (SetCommTimeouts(serial_handle_, &timeouts) == FALSE) - goto DXL_HAL_OPEN_ERROR; - - tx_time_per_byte_ = (1000.0 / (double)baudrate_) * 10.0; - return true; - -DXL_HAL_OPEN_ERROR: - closePort(); - return false; -} From 0b212111274038fe65a06aed86cfcfde24edf67c Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Fri, 12 Aug 2016 16:47:19 +0900 Subject: [PATCH 31/33] - added velocity p/i/d gain and position i/d gain sync_write code. --- .../robotis_controller/robotis_controller.h | 5 + .../robotis_controller/robotis_controller.cpp | 242 +++++++++++++++++- .../devices/dynamixel/GRIPPER.device | 3 + .../devices/dynamixel/H42-20-S300-R.device | 3 + .../devices/dynamixel/H54-100-S500-R.device | 3 + .../devices/dynamixel/H54-200-B500-R.device | 3 + .../devices/dynamixel/H54-200-S500-R.device | 3 + .../devices/dynamixel/L42-10-S300-R.device | 3 + .../devices/dynamixel/L54-30-S400-R.device | 3 + .../devices/dynamixel/L54-30-S500-R.device | 3 + .../devices/dynamixel/L54-50-S290-R.device | 3 + .../devices/dynamixel/L54-50-S500-R.device | 3 + .../devices/dynamixel/M42-10-S260-R.device | 3 + .../devices/dynamixel/M54-40-S250-R.device | 3 + .../devices/dynamixel/M54-60-S250-R.device | 3 + .../devices/dynamixel/MX-106.device | 3 + robotis_device/devices/dynamixel/MX-28.device | 3 + robotis_device/devices/dynamixel/MX-64.device | 3 + .../devices/dynamixel/XM-430-W210.device | 3 + .../devices/dynamixel/XM-430-W350.device | 3 + .../devices/dynamixel/XM-430.device | 3 + .../include/robotis_device/dynamixel.h | 5 + .../include/robotis_device/dynamixel_state.h | 10 + .../src/robotis_device/dynamixel.cpp | 7 +- 24 files changed, 319 insertions(+), 7 deletions(-) diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h index ae91370..65d7821 100644 --- a/robotis_controller/include/robotis_controller/robotis_controller.h +++ b/robotis_controller/include/robotis_controller/robotis_controller.h @@ -109,6 +109,11 @@ class RobotisController : public Singleton std::map port_to_sync_write_velocity_; std::map port_to_sync_write_current_; std::map port_to_sync_write_position_p_gain_; + std::map port_to_sync_write_position_i_gain_; + std::map port_to_sync_write_position_d_gain_; + std::map port_to_sync_write_velocity_p_gain_; + std::map port_to_sync_write_velocity_i_gain_; + std::map port_to_sync_write_velocity_d_gain_; /* publisher */ ros::Publisher goal_joint_state_pub_; diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 0029e4f..35c81b1 100644 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -93,11 +93,36 @@ void RobotisController::initializeSyncWrite() if (it.second != NULL) it.second->clearParam(); } + for (auto& it : port_to_sync_write_position_i_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_position_d_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } for (auto& it : port_to_sync_write_velocity_) { if (it.second != NULL) it.second->clearParam(); } + for (auto& it : port_to_sync_write_velocity_p_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_velocity_i_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_velocity_d_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } for (auto& it : port_to_sync_write_current_) { if (it.second != NULL) @@ -141,12 +166,37 @@ void RobotisController::initializeSyncWrite() { dxl->dxl_state_->position_p_gain_ = read_data; } + else if ((dxl->position_i_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->position_i_gain_item_->item_name_)) + { + dxl->dxl_state_->position_i_gain_ = read_data; + } + else if ((dxl->position_d_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->position_d_gain_item_->item_name_)) + { + dxl->dxl_state_->position_d_gain_ = read_data; + } else if ((dxl->present_velocity_item_ != 0) && (dxl->bulk_read_items_[i]->item_name_ == dxl->present_velocity_item_->item_name_)) { dxl->dxl_state_->present_velocity_ = dxl->convertValue2Velocity(read_data); dxl->dxl_state_->goal_velocity_ = dxl->dxl_state_->present_velocity_; } + else if ((dxl->velocity_p_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->velocity_p_gain_item_->item_name_)) + { + dxl->dxl_state_->velocity_p_gain_ = read_data; + } + else if ((dxl->velocity_i_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->velocity_i_gain_item_->item_name_)) + { + dxl->dxl_state_->velocity_i_gain_ = read_data; + } + else if ((dxl->velocity_d_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->velocity_d_gain_item_->item_name_)) + { + dxl->dxl_state_->velocity_d_gain_ = read_data; + } else if ((dxl->present_current_item_ != 0) && (dxl->bulk_read_items_[i]->item_name_ == dxl->present_current_item_->item_name_)) { @@ -210,6 +260,24 @@ bool RobotisController::initialize(const std::string robot_file_path, const std: default_device->position_p_gain_item_->data_length_); } + if (default_device->position_i_gain_item_ != 0) + { + port_to_sync_write_position_i_gain_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->position_i_gain_item_->address_, + default_device->position_i_gain_item_->data_length_); + } + + if (default_device->position_d_gain_item_ != 0) + { + port_to_sync_write_position_d_gain_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->position_d_gain_item_->address_, + default_device->position_d_gain_item_->data_length_); + } + if (default_device->goal_velocity_item_ != 0) { port_to_sync_write_velocity_[port_name] @@ -219,6 +287,33 @@ bool RobotisController::initialize(const std::string robot_file_path, const std: default_device->goal_velocity_item_->data_length_); } + if (default_device->velocity_p_gain_item_ != 0) + { + port_to_sync_write_velocity_p_gain_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->velocity_p_gain_item_->address_, + default_device->velocity_p_gain_item_->data_length_); + } + + if (default_device->velocity_i_gain_item_ != 0) + { + port_to_sync_write_velocity_i_gain_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->velocity_i_gain_item_->address_, + default_device->velocity_i_gain_item_->data_length_); + } + + if (default_device->velocity_d_gain_item_ != 0) + { + port_to_sync_write_velocity_d_gain_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->velocity_d_gain_item_->address_, + default_device->velocity_d_gain_item_->data_length_); + } + if (default_device->goal_current_item_ != 0) { port_to_sync_write_current_[port_name] @@ -687,11 +782,36 @@ void RobotisController::stopTimer() if (it.second != NULL) it.second->clearParam(); } + for (auto& it : port_to_sync_write_position_i_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_position_d_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } for (auto& it : port_to_sync_write_velocity_) { if (it.second != NULL) it.second->clearParam(); } + for (auto& it : port_to_sync_write_velocity_p_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_velocity_i_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_velocity_d_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } for (auto& it : port_to_sync_write_current_) { if (it.second != NULL) @@ -937,14 +1057,42 @@ void RobotisController::process() if (dxl_state->position_p_gain_ != result_state->position_p_gain_) { dxl_state->position_p_gain_ = result_state->position_p_gain_; - uint8_t sync_write_p_gain[4] = { 0 }; - sync_write_p_gain[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->position_p_gain_)); - sync_write_p_gain[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->position_p_gain_)); - sync_write_p_gain[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->position_p_gain_)); - sync_write_p_gain[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->position_p_gain_)); + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->position_p_gain_)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->position_p_gain_)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->position_p_gain_)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->position_p_gain_)); if (port_to_sync_write_position_p_gain_[dxl->port_name_] != NULL) - port_to_sync_write_position_p_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_p_gain); + port_to_sync_write_position_p_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + } + + // if position i gain value is changed -> sync write + if (dxl_state->position_i_gain_ != result_state->position_i_gain_) + { + dxl_state->position_i_gain_ = result_state->position_i_gain_; + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->position_i_gain_)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->position_i_gain_)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->position_i_gain_)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->position_i_gain_)); + + if (port_to_sync_write_position_i_gain_[dxl->port_name_] != NULL) + port_to_sync_write_position_i_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + } + + // if position d gain value is changed -> sync write + if (dxl_state->position_d_gain_ != result_state->position_d_gain_) + { + dxl_state->position_d_gain_ = result_state->position_d_gain_; + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->position_d_gain_)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->position_d_gain_)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->position_d_gain_)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->position_d_gain_)); + + if (port_to_sync_write_position_d_gain_[dxl->port_name_] != NULL) + port_to_sync_write_position_d_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); } } } @@ -963,6 +1111,48 @@ void RobotisController::process() if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) port_to_sync_write_velocity_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data); + + // if velocity p gain gain value is changed -> sync write + if (dxl_state->velocity_p_gain_ != result_state->velocity_p_gain_) + { + dxl_state->velocity_p_gain_ = result_state->velocity_p_gain_; + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_p_gain_)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_p_gain_)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_p_gain_)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_p_gain_)); + + if (port_to_sync_write_velocity_p_gain_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_p_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + } + + // if velocity i gain value is changed -> sync write + if (dxl_state->velocity_i_gain_ != result_state->velocity_i_gain_) + { + dxl_state->velocity_i_gain_ = result_state->velocity_i_gain_; + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_i_gain_)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_i_gain_)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_i_gain_)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_i_gain_)); + + if (port_to_sync_write_velocity_i_gain_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_i_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + } + + // if velocity d gain value is changed -> sync write + if (dxl_state->velocity_d_gain_ != result_state->velocity_d_gain_) + { + dxl_state->velocity_d_gain_ = result_state->velocity_d_gain_; + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_d_gain_)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_d_gain_)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_d_gain_)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_d_gain_)); + + if (port_to_sync_write_velocity_d_gain_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_d_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + } } } else if ((*module_it)->getControlMode() == TorqueControl) @@ -1004,6 +1194,46 @@ void RobotisController::process() it.second->clearParam(); } } + if (port_to_sync_write_position_i_gain_.size() > 0) + { + for (auto& it : port_to_sync_write_position_i_gain_) + { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_position_d_gain_.size() > 0) + { + for (auto& it : port_to_sync_write_position_d_gain_) + { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_velocity_p_gain_.size() > 0) + { + for (auto& it : port_to_sync_write_velocity_p_gain_) + { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_velocity_i_gain_.size() > 0) + { + for (auto& it : port_to_sync_write_velocity_i_gain_) + { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_velocity_d_gain_.size() > 0) + { + for (auto& it : port_to_sync_write_velocity_d_gain_) + { + it.second->txPacket(); + it.second->clearParam(); + } + } for (auto& it : port_to_sync_write_position_) { if (it.second != NULL) diff --git a/robotis_device/devices/dynamixel/GRIPPER.device b/robotis_device/devices/dynamixel/GRIPPER.device index e95ff17..4267c99 100644 --- a/robotis_device/devices/dynamixel/GRIPPER.device +++ b/robotis_device/devices/dynamixel/GRIPPER.device @@ -19,6 +19,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/H42-20-S300-R.device b/robotis_device/devices/dynamixel/H42-20-S300-R.device index e348f42..95686ff 100644 --- a/robotis_device/devices/dynamixel/H42-20-S300-R.device +++ b/robotis_device/devices/dynamixel/H42-20-S300-R.device @@ -21,6 +21,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/H54-100-S500-R.device b/robotis_device/devices/dynamixel/H54-100-S500-R.device index cdf81d0..a251ddd 100644 --- a/robotis_device/devices/dynamixel/H54-100-S500-R.device +++ b/robotis_device/devices/dynamixel/H54-100-S500-R.device @@ -21,6 +21,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/H54-200-B500-R.device b/robotis_device/devices/dynamixel/H54-200-B500-R.device index d002f97..20a280a 100644 --- a/robotis_device/devices/dynamixel/H54-200-B500-R.device +++ b/robotis_device/devices/dynamixel/H54-200-B500-R.device @@ -21,6 +21,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/H54-200-S500-R.device b/robotis_device/devices/dynamixel/H54-200-S500-R.device index 7e18900..5b7dc76 100644 --- a/robotis_device/devices/dynamixel/H54-200-S500-R.device +++ b/robotis_device/devices/dynamixel/H54-200-S500-R.device @@ -21,6 +21,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/L42-10-S300-R.device b/robotis_device/devices/dynamixel/L42-10-S300-R.device index 8a4ca5a..84e556d 100644 --- a/robotis_device/devices/dynamixel/L42-10-S300-R.device +++ b/robotis_device/devices/dynamixel/L42-10-S300-R.device @@ -19,6 +19,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/L54-30-S400-R.device b/robotis_device/devices/dynamixel/L54-30-S400-R.device index d684e67..9dc5d36 100644 --- a/robotis_device/devices/dynamixel/L54-30-S400-R.device +++ b/robotis_device/devices/dynamixel/L54-30-S400-R.device @@ -19,6 +19,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/L54-30-S500-R.device b/robotis_device/devices/dynamixel/L54-30-S500-R.device index 68848a2..fa15941 100644 --- a/robotis_device/devices/dynamixel/L54-30-S500-R.device +++ b/robotis_device/devices/dynamixel/L54-30-S500-R.device @@ -19,6 +19,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/L54-50-S290-R.device b/robotis_device/devices/dynamixel/L54-50-S290-R.device index 3cbd843..8c2f1a2 100644 --- a/robotis_device/devices/dynamixel/L54-50-S290-R.device +++ b/robotis_device/devices/dynamixel/L54-50-S290-R.device @@ -19,6 +19,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/L54-50-S500-R.device b/robotis_device/devices/dynamixel/L54-50-S500-R.device index bfbf0c6..991e159 100644 --- a/robotis_device/devices/dynamixel/L54-50-S500-R.device +++ b/robotis_device/devices/dynamixel/L54-50-S500-R.device @@ -19,6 +19,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/M42-10-S260-R.device b/robotis_device/devices/dynamixel/M42-10-S260-R.device index f21bb44..4c25631 100644 --- a/robotis_device/devices/dynamixel/M42-10-S260-R.device +++ b/robotis_device/devices/dynamixel/M42-10-S260-R.device @@ -19,6 +19,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/M54-40-S250-R.device b/robotis_device/devices/dynamixel/M54-40-S250-R.device index d9424ad..76796b1 100644 --- a/robotis_device/devices/dynamixel/M54-40-S250-R.device +++ b/robotis_device/devices/dynamixel/M54-40-S250-R.device @@ -19,6 +19,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/M54-60-S250-R.device b/robotis_device/devices/dynamixel/M54-60-S250-R.device index 284cac9..a6cfa2a 100644 --- a/robotis_device/devices/dynamixel/M54-60-S250-R.device +++ b/robotis_device/devices/dynamixel/M54-60-S250-R.device @@ -19,6 +19,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/MX-106.device b/robotis_device/devices/dynamixel/MX-106.device index 23b16f1..4b8c63d 100644 --- a/robotis_device/devices/dynamixel/MX-106.device +++ b/robotis_device/devices/dynamixel/MX-106.device @@ -19,6 +19,9 @@ goal_current_item_name = position_d_gain_item_name = position_d_gain position_i_gain_item_name = position_i_gain position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = +velocity_p_gain_item_name = [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/MX-28.device b/robotis_device/devices/dynamixel/MX-28.device index ebcad6e..ff67f2a 100644 --- a/robotis_device/devices/dynamixel/MX-28.device +++ b/robotis_device/devices/dynamixel/MX-28.device @@ -19,6 +19,9 @@ goal_current_item_name = position_d_gain_item_name = position_d_gain position_i_gain_item_name = position_i_gain position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = +velocity_p_gain_item_name = [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/MX-64.device b/robotis_device/devices/dynamixel/MX-64.device index 2988c83..06c0a4a 100644 --- a/robotis_device/devices/dynamixel/MX-64.device +++ b/robotis_device/devices/dynamixel/MX-64.device @@ -19,6 +19,9 @@ goal_current_item_name = position_d_gain_item_name = position_d_gain position_i_gain_item_name = position_i_gain position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = +velocity_p_gain_item_name = [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/XM-430-W210.device b/robotis_device/devices/dynamixel/XM-430-W210.device index 178fc4f..f42ae57 100644 --- a/robotis_device/devices/dynamixel/XM-430-W210.device +++ b/robotis_device/devices/dynamixel/XM-430-W210.device @@ -22,6 +22,9 @@ goal_current_item_name = goal_current position_d_gain_item_name = position_d_gain position_i_gain_item_name = position_i_gain position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/XM-430-W350.device b/robotis_device/devices/dynamixel/XM-430-W350.device index ed6c17f..ee14157 100644 --- a/robotis_device/devices/dynamixel/XM-430-W350.device +++ b/robotis_device/devices/dynamixel/XM-430-W350.device @@ -22,6 +22,9 @@ goal_current_item_name = goal_current position_d_gain_item_name = position_d_gain position_i_gain_item_name = position_i_gain position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/XM-430.device b/robotis_device/devices/dynamixel/XM-430.device index 67d523f..c3a7bad 100644 --- a/robotis_device/devices/dynamixel/XM-430.device +++ b/robotis_device/devices/dynamixel/XM-430.device @@ -21,6 +21,9 @@ goal_current_item_name = goal_current position_d_gain_item_name = position_d_gain position_i_gain_item_name = position_i_gain position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/include/robotis_device/dynamixel.h b/robotis_device/include/robotis_device/dynamixel.h index 349020a..adff01b 100644 --- a/robotis_device/include/robotis_device/dynamixel.h +++ b/robotis_device/include/robotis_device/dynamixel.h @@ -73,6 +73,11 @@ class Dynamixel : public Device ControlTableItem *goal_velocity_item_; ControlTableItem *goal_current_item_; ControlTableItem *position_p_gain_item_; + ControlTableItem *position_i_gain_item_; + ControlTableItem *position_d_gain_item_; + ControlTableItem *velocity_p_gain_item_; + ControlTableItem *velocity_i_gain_item_; + ControlTableItem *velocity_d_gain_item_; Dynamixel(int id, std::string model_name, float protocol_version); diff --git a/robotis_device/include/robotis_device/dynamixel_state.h b/robotis_device/include/robotis_device/dynamixel_state.h index a318a1f..5f4ae36 100644 --- a/robotis_device/include/robotis_device/dynamixel_state.h +++ b/robotis_device/include/robotis_device/dynamixel_state.h @@ -60,6 +60,11 @@ class DynamixelState double goal_velocity_; double goal_torque_; double position_p_gain_; + double position_i_gain_; + double position_d_gain_; + double velocity_p_gain_; + double velocity_i_gain_; + double velocity_d_gain_; std::map bulk_read_table_; @@ -74,6 +79,11 @@ class DynamixelState goal_velocity_(0.0), goal_torque_(0.0), position_p_gain_(0.0), + position_i_gain_(0.0), + position_d_gain_(0.0), + velocity_p_gain_(0.0), + velocity_i_gain_(0.0), + velocity_d_gain_(0.0), position_offset_(0.0) { bulk_read_table_.clear(); diff --git a/robotis_device/src/robotis_device/dynamixel.cpp b/robotis_device/src/robotis_device/dynamixel.cpp index ba316b2..cdd5470 100644 --- a/robotis_device/src/robotis_device/dynamixel.cpp +++ b/robotis_device/src/robotis_device/dynamixel.cpp @@ -55,7 +55,12 @@ Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version) goal_position_item_(0), goal_velocity_item_(0), goal_current_item_(0), - position_p_gain_item_(0) + position_p_gain_item_(0), + position_i_gain_item_(0), + position_d_gain_item_(0), + velocity_p_gain_item_(0), + velocity_i_gain_item_(0), + velocity_d_gain_item_(0) { this->id_ = id; this->model_name_ = model_name; From fbddaf2df8ad82251529971acdb9d2b8373f5095 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Mon, 22 Aug 2016 10:35:06 +0900 Subject: [PATCH 32/33] changed some debug messages. --- .../src/robotis_controller/robotis_controller.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 1bea26e..2017293 100644 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -673,7 +673,7 @@ void *RobotisController::timerThread(void *param) static struct timespec next_time; static struct timespec curr_time; - ROS_INFO("controller::thread_proc"); + ROS_DEBUG("controller::thread_proc started"); clock_gettime(CLOCK_MONOTONIC, &next_time); @@ -748,7 +748,7 @@ void RobotisController::startTimer() // create and start the thread if ((error = pthread_create(&this->timer_thread_, &attr, this->timerThread, this)) != 0) { - ROS_ERROR("timer thread create fail!!"); + ROS_ERROR("Creating timer thread failed!!"); exit(-1); } } @@ -1035,8 +1035,6 @@ void RobotisController::process() if ((*module_it)->getControlMode() == PositionControl) { -// if(result_state->goal_position == 0 && dxl->id == 3) -// ROS_INFO("[MODULE:%s][ID:%2d] goal position = %f", (*module_it)->GetModuleName().c_str(), dxl->id, dxl_state->goal_position); dxl_state->goal_position_ = result_state->goal_position_; if (gazebo_mode_ == false) @@ -1434,7 +1432,7 @@ void RobotisController::syncWriteItemCallback(const robotis_controller_msgs::Syn } else { - // could not find the device + ROS_WARN("[SyncWriteItem] Unknown device : %s", msg->joint_name[i].c_str()); continue; } } From adaaaea42b5cfb885d9b5412f7e0c5bc155701d8 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Mon, 22 Aug 2016 15:49:38 +0900 Subject: [PATCH 33/33] - bug fixed (position pid gain & velocity pid gain sync write). - added velocity_to_value_ratio to DXL Pro-H series. --- .../robotis_controller/robotis_controller.cpp | 10 +++---- .../devices/dynamixel/H42-20-S300-R.device | 2 +- .../devices/dynamixel/H54-100-S500-R.device | 2 +- .../devices/dynamixel/H54-200-B500-R.device | 2 +- .../devices/dynamixel/H54-200-S500-R.device | 2 +- .../include/robotis_device/dynamixel_state.h | 26 ++++++++-------- robotis_device/src/robotis_device/robot.cpp | 30 +++++++++++++++++++ 7 files changed, 52 insertions(+), 22 deletions(-) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 2017293..acbe656 100644 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -1047,11 +1047,11 @@ void RobotisController::process() sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos_data)); sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos_data)); - if (abs(pos_data) > 151800) - { - printf("goal_pos : %f | position_offset : %f | pos_data : %d\n", - dxl_state->goal_position_, dxl_state->position_offset_, pos_data); - } +// if (abs(pos_data) > 151800) +// { +// printf("goal_pos : %f | position_offset : %f | pos_data : %d\n", +// dxl_state->goal_position_, dxl_state->position_offset_, pos_data); +// } if (port_to_sync_write_position_[dxl->port_name_] != NULL) port_to_sync_write_position_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data); diff --git a/robotis_device/devices/dynamixel/H42-20-S300-R.device b/robotis_device/devices/dynamixel/H42-20-S300-R.device index 95686ff..f669d8d 100644 --- a/robotis_device/devices/dynamixel/H42-20-S300-R.device +++ b/robotis_device/devices/dynamixel/H42-20-S300-R.device @@ -4,7 +4,7 @@ device_type = dynamixel [type info] torque_to_current_value_ratio = 27.15146 - +velocity_to_value_ratio = 2900.59884 value_of_0_radian_position = 0 value_of_min_radian_position = -151900 value_of_max_radian_position = 151900 diff --git a/robotis_device/devices/dynamixel/H54-100-S500-R.device b/robotis_device/devices/dynamixel/H54-100-S500-R.device index a251ddd..96fffa3 100644 --- a/robotis_device/devices/dynamixel/H54-100-S500-R.device +++ b/robotis_device/devices/dynamixel/H54-100-S500-R.device @@ -4,7 +4,7 @@ device_type = dynamixel [type info] torque_to_current_value_ratio = 9.66026 - +velocity_to_value_ratio = 4793.01226 value_of_0_radian_position = 0 value_of_min_radian_position = -250950 value_of_max_radian_position = 250950 diff --git a/robotis_device/devices/dynamixel/H54-200-B500-R.device b/robotis_device/devices/dynamixel/H54-200-B500-R.device index 20a280a..aa79b4b 100644 --- a/robotis_device/devices/dynamixel/H54-200-B500-R.device +++ b/robotis_device/devices/dynamixel/H54-200-B500-R.device @@ -4,7 +4,7 @@ device_type = dynamixel [type info] torque_to_current_value_ratio = 9.09201 - +velocity_to_value_ratio = 4793.01226 value_of_0_radian_position = 0 value_of_min_radian_position = -250950 value_of_max_radian_position = 250950 diff --git a/robotis_device/devices/dynamixel/H54-200-S500-R.device b/robotis_device/devices/dynamixel/H54-200-S500-R.device index 5b7dc76..76f6300 100644 --- a/robotis_device/devices/dynamixel/H54-200-S500-R.device +++ b/robotis_device/devices/dynamixel/H54-200-S500-R.device @@ -4,7 +4,7 @@ device_type = dynamixel [type info] torque_to_current_value_ratio = 9.09201 - +velocity_to_value_ratio = 4793.01226 value_of_0_radian_position = 0 value_of_min_radian_position = -250950 value_of_max_radian_position = 250950 diff --git a/robotis_device/include/robotis_device/dynamixel_state.h b/robotis_device/include/robotis_device/dynamixel_state.h index 5f4ae36..57de9f6 100644 --- a/robotis_device/include/robotis_device/dynamixel_state.h +++ b/robotis_device/include/robotis_device/dynamixel_state.h @@ -59,12 +59,12 @@ class DynamixelState double goal_position_; double goal_velocity_; double goal_torque_; - double position_p_gain_; - double position_i_gain_; - double position_d_gain_; - double velocity_p_gain_; - double velocity_i_gain_; - double velocity_d_gain_; + int position_p_gain_; + int position_i_gain_; + int position_d_gain_; + int velocity_p_gain_; + int velocity_i_gain_; + int velocity_d_gain_; std::map bulk_read_table_; @@ -78,13 +78,13 @@ class DynamixelState goal_position_(0.0), goal_velocity_(0.0), goal_torque_(0.0), - position_p_gain_(0.0), - position_i_gain_(0.0), - position_d_gain_(0.0), - velocity_p_gain_(0.0), - velocity_i_gain_(0.0), - velocity_d_gain_(0.0), - position_offset_(0.0) + position_p_gain_(0), + position_i_gain_(0), + position_d_gain_(0), + velocity_p_gain_(0), + velocity_i_gain_(0), + velocity_d_gain_(0), + position_offset_(0) { bulk_read_table_.clear(); } diff --git a/robotis_device/src/robotis_device/robot.cpp b/robotis_device/src/robotis_device/robot.cpp index 900a9b9..3fc9b8b 100644 --- a/robotis_device/src/robotis_device/robot.cpp +++ b/robotis_device/src/robotis_device/robot.cpp @@ -327,6 +327,12 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float std::string goal_position_item_name = ""; std::string goal_velocity_item_name = ""; std::string goal_current_item_name = ""; + std::string position_d_gain_item_name = ""; + std::string position_i_gain_item_name = ""; + std::string position_p_gain_item_name = ""; + std::string velocity_d_gain_item_name = ""; + std::string velocity_i_gain_item_name = ""; + std::string velocity_p_gain_item_name = ""; while (!file.eof()) { @@ -396,6 +402,18 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float goal_velocity_item_name = tokens[1]; else if (tokens[0] == "goal_current_item_name") goal_current_item_name = tokens[1]; + else if (tokens[0] == "position_d_gain_item_name") + position_d_gain_item_name = tokens[1]; + else if (tokens[0] == "position_i_gain_item_name") + position_i_gain_item_name = tokens[1]; + else if (tokens[0] == "position_p_gain_item_name") + position_p_gain_item_name = tokens[1]; + else if (tokens[0] == "velocity_d_gain_item_name") + velocity_d_gain_item_name = tokens[1]; + else if (tokens[0] == "velocity_i_gain_item_name") + velocity_i_gain_item_name = tokens[1]; + else if (tokens[0] == "velocity_p_gain_item_name") + velocity_p_gain_item_name = tokens[1]; } else if (session == SESSION_CONTROL_TABLE) { @@ -444,6 +462,18 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float dxl->goal_velocity_item_ = dxl->ctrl_table_[goal_velocity_item_name]; if (dxl->ctrl_table_[goal_current_item_name] != NULL) dxl->goal_current_item_ = dxl->ctrl_table_[goal_current_item_name]; + if (dxl->ctrl_table_[position_d_gain_item_name] != NULL) + dxl->position_d_gain_item_ = dxl->ctrl_table_[position_d_gain_item_name]; + if (dxl->ctrl_table_[position_i_gain_item_name] != NULL) + dxl->position_i_gain_item_ = dxl->ctrl_table_[position_i_gain_item_name]; + if (dxl->ctrl_table_[position_p_gain_item_name] != NULL) + dxl->position_p_gain_item_ = dxl->ctrl_table_[position_p_gain_item_name]; + if (dxl->ctrl_table_[velocity_d_gain_item_name] != NULL) + dxl->velocity_d_gain_item_ = dxl->ctrl_table_[velocity_d_gain_item_name]; + if (dxl->ctrl_table_[velocity_i_gain_item_name] != NULL) + dxl->velocity_i_gain_item_ = dxl->ctrl_table_[velocity_i_gain_item_name]; + if (dxl->ctrl_table_[velocity_p_gain_item_name] != NULL) + dxl->velocity_p_gain_item_ = dxl->ctrl_table_[velocity_p_gain_item_name]; fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), dxl->id_, dxl->model_name_.c_str()); //std::cout << "[ID:" << (int)(_dxl->id) << "] " << _dxl->model_name << " added. (" << port << ")" << std::endl;