From 9e1300eedafd278fd7ba86c1d70d4dbcea335d46 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Mon, 16 May 2016 16:32:26 +0900 Subject: [PATCH] - XM-430 / CM-740 device file added. - Sensor device added. --- .../robotis_controller/RobotisController.h | 1 + .../robotis_controller/RobotisController.cpp | 301 ++++++++++-------- robotis_device/CMakeLists.txt | 1 + .../devices/dynamixel/XM-430.device | 77 +++++ robotis_device/devices/sensor/CM-740.device | 25 ++ .../include/robotis_device/Device.h | 37 +++ .../include/robotis_device/Dynamixel.h | 19 +- robotis_device/include/robotis_device/Robot.h | 5 +- .../include/robotis_device/Sensor.h | 16 +- .../src/robotis_device/Dynamixel.cpp | 14 +- robotis_device/src/robotis_device/Robot.cpp | 184 +++++++++-- robotis_device/src/robotis_device/Sensor.cpp | 21 ++ 12 files changed, 513 insertions(+), 188 deletions(-) create mode 100644 robotis_device/devices/dynamixel/XM-430.device create mode 100644 robotis_device/devices/sensor/CM-740.device create mode 100644 robotis_device/include/robotis_device/Device.h create mode 100644 robotis_device/src/robotis_device/Sensor.cpp diff --git a/robotis_controller/include/robotis_controller/RobotisController.h b/robotis_controller/include/robotis_controller/RobotisController.h index 6db923b..c28256c 100644 --- a/robotis_controller/include/robotis_controller/RobotisController.h +++ b/robotis_controller/include/robotis_controller/RobotisController.h @@ -94,6 +94,7 @@ public: static RobotisController *GetInstance() { return unique_instance_; } bool Initialize(const std::string robot_file_path, const std::string init_file_path); + void DeviceInit(const std::string init_file_path); void Process(); void AddMotionModule(MotionModule *module); diff --git a/robotis_controller/src/robotis_controller/RobotisController.cpp b/robotis_controller/src/robotis_controller/RobotisController.cpp index 20855f6..60cdf91 100644 --- a/robotis_controller/src/robotis_controller/RobotisController.cpp +++ b/robotis_controller/src/robotis_controller/RobotisController.cpp @@ -68,17 +68,6 @@ void RobotisController::InitSyncWrite() std::string _joint_name = _it->first; Dynamixel *_dxl = _it->second; - CONTROL_MODE _ctrl_mode = POSITION_CONTROL; - - for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) - { - if((*_m_it)->module_name == _dxl->ctrl_module_name) - { - _ctrl_mode = (*_m_it)->control_mode; - break; - } - } - for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++) { UINT32_T _read_data = 0; @@ -102,24 +91,17 @@ void RobotisController::InitSyncWrite() _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; - if(_ctrl_mode == POSITION_CONTROL) - port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); + port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_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; - - if(_ctrl_mode == VELOCITY_CONTROL) - port_to_sync_write_velocity[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); } 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; - - if(_ctrl_mode == CURRENT_CONTROL) - port_to_sync_write_torque[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); } } } @@ -139,8 +121,6 @@ bool RobotisController::Initialize(const std::string robot_file_path, const std: return true; } - // TODO: TEMPORARY CODE !! - /* temporary code start */ for(std::map::iterator _it = robot->ports.begin(); _it != robot->ports.end(); _it++) { std::string _port_name = _it->first; @@ -154,31 +134,40 @@ bool RobotisController::Initialize(const std::string robot_file_path, const std: exit(-1); } - Dynamixel *_port_default_dxl = robot->dxls[robot->port_default_joint[_port_name]]; - if(_port_default_dxl != NULL) - _default_pkt_handler = PacketHandler::GetPacketHandler(_port_default_dxl->protocol_version); + // 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); + + 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); + + 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); + } port_to_bulk_read[_port_name] = new GroupBulkRead(_port, _default_pkt_handler); - - port_to_sync_write_position[_port_name] = new GroupSyncWrite(_port, - _default_pkt_handler, - _port_default_dxl->goal_position_item->address, - _port_default_dxl->goal_position_item->data_length); - - port_to_sync_write_velocity[_port_name] = new GroupSyncWrite(_port, - _default_pkt_handler, - _port_default_dxl->goal_velocity_item->address, - _port_default_dxl->goal_velocity_item->data_length); - - port_to_sync_write_torque[_port_name] = new GroupSyncWrite(_port, - _default_pkt_handler, - _port_default_dxl->goal_current_item->address, - _port_default_dxl->goal_current_item->data_length); - } - // TODO: - // for loop ping --> no response : return false + // (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; @@ -188,11 +177,93 @@ bool RobotisController::Initialize(const std::string robot_file_path, const std: { usleep(10*1000); if(Ping(_joint_name) != 0) - ROS_ERROR("JOINT[%s] does NOT response!!", _joint_name.c_str()); + ROS_ERROR("JOINT[%s] does NOT respond!!", _joint_name.c_str()); } } - // joint(dynamixel) initialize + DeviceInit(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++) + { + std::string _joint_name = _it->first; + Dynamixel *_dxl = _it->second; + + if(_dxl == NULL) + continue; + + 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; + } + + // TODO: modifing + 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 + { + 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; + } + } + } + } + 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; + } + } + +// 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); + + // 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); + } + + queue_thread_ = boost::thread(boost::bind(&RobotisController::QueueThread, this)); + return true; +} + +void RobotisController::DeviceInit(const std::string init_file_path) +{ + // device initialize if(DEBUG_PRINT) ROS_WARN("INIT FILE LOAD"); YAML::Node _doc; @@ -287,86 +358,6 @@ bool RobotisController::Initialize(const std::string robot_file_path, const std: } catch(const std::exception& e) { ROS_INFO("Dynamixel Init file not found."); } - - - // [ 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(_dxl == NULL) - continue; - - 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; - } - - // TODO: modifing - 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 - { - 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; - } - } - } - } - 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; - } - } - -// 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); - - // 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); - } - - /* temporary code end */ - - queue_thread_ = boost::thread(boost::bind(&RobotisController::QueueThread, this)); - return true; } void RobotisController::GazeboThread() @@ -1120,7 +1111,20 @@ void RobotisController::SetCtrlModuleThread(std::string ctrl_module) // set dxl's control module to "none" for(std::map::iterator _d_it = robot->dxls.begin(); _d_it != robot->dxls.end(); _d_it++) { - _d_it->second->ctrl_module_name = "none"; + Dynamixel *_dxl = _d_it->second; + _dxl->ctrl_module_name = "none"; + + 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)); + + 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); } } else @@ -1131,12 +1135,57 @@ void RobotisController::SetCtrlModuleThread(std::string ctrl_module) // if it exist if((*_m_it)->module_name == ctrl_module) { + CONTROL_MODE _mode = (*_m_it)->control_mode; 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()) { - _d_it->second->ctrl_module_name = ctrl_module; + Dynamixel *_dxl = _d_it->second; + _dxl->ctrl_module_name = ctrl_module; + + 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)); + + 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); + } + 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)); + + 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); + } + 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)); + + 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); + } } } diff --git a/robotis_device/CMakeLists.txt b/robotis_device/CMakeLists.txt index efe9e20..8bd7be5 100644 --- a/robotis_device/CMakeLists.txt +++ b/robotis_device/CMakeLists.txt @@ -39,6 +39,7 @@ include_directories( ## Declare a C++ library add_library(robotis_device src/robotis_device/Robot.cpp + src/robotis_device/Sensor.cpp src/robotis_device/Dynamixel.cpp ) diff --git a/robotis_device/devices/dynamixel/XM-430.device b/robotis_device/devices/dynamixel/XM-430.device new file mode 100644 index 0000000..9f17d37 --- /dev/null +++ b/robotis_device/devices/dynamixel/XM-430.device @@ -0,0 +1,77 @@ +[device info] +model_name = XM-430 +device_type = dynamixel + +[type info] +current_ratio = 2.69 + +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 | 0 | 4095 | N + 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 + 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/sensor/CM-740.device b/robotis_device/devices/sensor/CM-740.device new file mode 100644 index 0000000..700d43a --- /dev/null +++ b/robotis_device/devices/sensor/CM-740.device @@ -0,0 +1,25 @@ +[device info] +model_name = CM-740 +device_type = sensor + +[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 + 16 | status_return_level | 1 | RW | EEPROM | 0 | 2 | N + 24 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 25 | LED | 1 | RW | RAM | 0 | 7 | N + 26 | LED_5 | 2 | RW | RAM | 0 | 32767 | N + 28 | LED_6 | 2 | RW | RAM | 0 | 32767 | N + 30 | button | 1 | RW | RAM | 0 | 3 | N + 38 | gyro_z | 2 | R | RAM | 0 | 1023 | N + 40 | gyro_y | 2 | R | RAM | 0 | 1023 | N + 42 | gyro_x | 2 | R | RAM | 0 | 1023 | N + 44 | acc_x | 2 | R | RAM | 0 | 1023 | N + 46 | acc_y | 2 | R | RAM | 0 | 1023 | N + 48 | acc_z | 2 | R | RAM | 0 | 1023 | N + 50 | present_voltage | 1 | R | RAM | 50 | 250 | N + \ No newline at end of file diff --git a/robotis_device/include/robotis_device/Device.h b/robotis_device/include/robotis_device/Device.h new file mode 100644 index 0000000..d9fd0bf --- /dev/null +++ b/robotis_device/include/robotis_device/Device.h @@ -0,0 +1,37 @@ +/* + * 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_ + + +#include +#include +#include +#include "ControlTableItem.h" + +namespace ROBOTIS +{ + +class Device +{ +public: + UINT8_T id; + float protocol_version; + std::string model_name; + std::string port_name; + + std::map ctrl_table; + std::vector bulk_read_items; + + virtual ~Device() { } +}; + +} + + +#endif /* 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 index 1a65f72..64d4a4a 100644 --- a/robotis_device/include/robotis_device/Dynamixel.h +++ b/robotis_device/include/robotis_device/Dynamixel.h @@ -5,29 +5,23 @@ * Author: zerom */ -#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_DEVICE_DYNAMIXEL_H_ -#define ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_DEVICE_DYNAMIXEL_H_ +#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXEL_H_ +#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXEL_H_ #include #include #include +#include "Device.h" #include "DynamixelState.h" #include "ControlTableItem.h" namespace ROBOTIS { -class Dynamixel +class Dynamixel : public Device { public: - UINT8_T id; - std::string model_name; - float protocol_version; - - std::map ctrl_table; // string: item name - - std::string port_name; std::string ctrl_module_name; DynamixelState *dxl_state; @@ -48,9 +42,6 @@ public: ControlTableItem *goal_velocity_item; ControlTableItem *goal_current_item; - std::vector bulk_read_items; - std::map indirect_address_table; - Dynamixel(int id, std::string model_name, float protocol_version); double ConvertValue2Radian(INT32_T value); @@ -66,4 +57,4 @@ public: } -#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_DEVICE_DYNAMIXEL_H_ */ +#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXEL_H_ */ diff --git a/robotis_device/include/robotis_device/Robot.h b/robotis_device/include/robotis_device/Robot.h index d169d35..2473e7c 100644 --- a/robotis_device/include/robotis_device/Robot.h +++ b/robotis_device/include/robotis_device/Robot.h @@ -21,14 +21,15 @@ class Robot { public: std::map ports; // string: port name - std::map port_default_joint; // port name, default joint name + std::map port_default_device; // port name, default device 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); - 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); }; } diff --git a/robotis_device/include/robotis_device/Sensor.h b/robotis_device/include/robotis_device/Sensor.h index ca3e28a..78de619 100644 --- a/robotis_device/include/robotis_device/Sensor.h +++ b/robotis_device/include/robotis_device/Sensor.h @@ -5,30 +5,26 @@ * Author: zerom */ -#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_SENSOR_H_ -#define ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_SENSOR_H_ +#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSOR_H_ +#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSOR_H_ #include #include #include +#include "Device.h" #include "ControlTableItem.h" namespace ROBOTIS { -class Sensor +class Sensor : public Device { + std::map sensed_value; public: - UINT8_T id; - std::string model_name; - float protocol_version; - - std::map ctrl_table; - Sensor(int id, std::string model_name, float protocol_version); }; } -#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_SENSOR_H_ */ +#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSOR_H_ */ diff --git a/robotis_device/src/robotis_device/Dynamixel.cpp b/robotis_device/src/robotis_device/Dynamixel.cpp index 2ccb3a0..daf454d 100644 --- a/robotis_device/src/robotis_device/Dynamixel.cpp +++ b/robotis_device/src/robotis_device/Dynamixel.cpp @@ -5,16 +5,12 @@ * Author: zerom */ -#include "../include/robotis_device/Dynamixel.h" +#include "robotis_device/Dynamixel.h" using namespace ROBOTIS; Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version) - : id(id), - model_name(model_name), - port_name(""), - ctrl_module_name("none"), - protocol_version(protocol_version), + : ctrl_module_name("none"), current_ratio(1.0), velocity_ratio(1.0), value_of_0_radian_position(0), @@ -30,11 +26,15 @@ Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version) goal_velocity_item(0), goal_current_item(0) { + this->id = id; + this->model_name = model_name; + this->port_name = ""; + this->protocol_version = protocol_version; + ctrl_table.clear(); dxl_state = new DynamixelState(); bulk_read_items.clear(); - indirect_address_table.clear(); } double Dynamixel::ConvertValue2Radian(INT32_T value) diff --git a/robotis_device/src/robotis_device/Robot.cpp b/robotis_device/src/robotis_device/Robot.cpp index f397c3c..dc1b50e 100644 --- a/robotis_device/src/robotis_device/Robot.cpp +++ b/robotis_device/src/robotis_device/Robot.cpp @@ -78,7 +78,7 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) ports[tokens[0]] = (PortHandler*)PortHandler::GetPortHandler(tokens[0].c_str()); ports[tokens[0]]->SetBaudRate(std::atoi(tokens[1].c_str())); - port_default_joint[tokens[0]] = tokens[2]; + port_default_device[tokens[0]] = tokens[2]; } else if(session == "device info") { @@ -129,6 +129,49 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) } } } + 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]]); + } + } + } } } file.close(); @@ -139,9 +182,92 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) } } +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); + + // 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") + _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(); + } + 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; + Dynamixel *_dxl; + // load file model_name.device std::ifstream file(path.c_str()); if(file.is_open()) @@ -187,7 +313,7 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float continue; if(tokens[0] == "model_name") - dxl = new Dynamixel(id, tokens[1], protocol_version); + _dxl = new Dynamixel(id, tokens[1], protocol_version); } else if(_session == "type info") { @@ -196,20 +322,20 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float continue; if(tokens[0] == "current_ratio") - dxl->current_ratio = std::atof(tokens[1].c_str()); + _dxl->current_ratio = std::atof(tokens[1].c_str()); else if(tokens[0] == "velocity_ratio") - dxl->velocity_ratio = std::atof(tokens[1].c_str()); + _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()); + _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()); + _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()); + _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()); + _dxl->min_radian = std::atof(tokens[1].c_str()); else if(tokens[0] == "max_radian") - dxl->max_radian = std::atof(tokens[1].c_str()); + _dxl->max_radian = std::atof(tokens[1].c_str()); else if(tokens[0] == "torque_enable_item_name") _torque_enable_item_name = tokens[1]; @@ -250,33 +376,33 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float item->is_signed = true; else if(tokens[7] == "N") item->is_signed = false; - dxl->ctrl_table[tokens[1]] = item; + _dxl->ctrl_table[tokens[1]] = item; } } - dxl->port_name = port; + _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]; + 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; + 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; + return _dxl; } diff --git a/robotis_device/src/robotis_device/Sensor.cpp b/robotis_device/src/robotis_device/Sensor.cpp new file mode 100644 index 0000000..afe4f9f --- /dev/null +++ b/robotis_device/src/robotis_device/Sensor.cpp @@ -0,0 +1,21 @@ +/* + * Sensor.cpp + * + * Created on: 2016. 5. 11. + * Author: zerom + */ + +#include "robotis_device/Sensor.h" + +using namespace ROBOTIS; + +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(); + + bulk_read_items.clear(); +}