From 260c03b3aeedc1879664b5dddafd4adcbfb57fc6 Mon Sep 17 00:00:00 2001 From: s-changhyun Date: Wed, 28 Sep 2016 18:59:49 +0900 Subject: [PATCH] mode change debugging --- .../robotis_controller/robotis_controller.cpp | 32 +++++++- .../dynamixel/H54-100-B210-R-NR.device | 76 +++++++++++++++++++ 2 files changed, 107 insertions(+), 1 deletion(-) create mode 100644 robotis_device/devices/dynamixel/H54-100-B210-R-NR.device diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 89a4751..fc45481 100644 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -1519,6 +1519,11 @@ void RobotisController::setCtrlModuleCallback(const std_msgs::String::ConstPtr & { std::string _module_name_to_set = msg->data; + + + + + set_module_thread_ = boost::thread(boost::bind(&RobotisController::setCtrlModuleThread, this, _module_name_to_set)); } @@ -1532,7 +1537,7 @@ void RobotisController::setJointCtrlModuleCallback(const robotis_controller_msgs return; set_module_thread_ = boost::thread(boost::bind(&RobotisController::setJointCtrlModuleThread, this, msg)); - } +} bool RobotisController::getCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res) @@ -1751,6 +1756,18 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) // stop module std::list stop_modules; + ROS_INFO("----- Before -----"); + for (auto& d_it : robot_->dxls_) + { + std::string joint_name = d_it.first; + + Dynamixel *dxl = d_it.second; + double goal_position = dxl->dxl_state_->goal_position_; + + fprintf(stderr, "%s : %f \n", joint_name.c_str(), goal_position); + } + ROS_INFO("----- ----- -----"); + if (ctrl_module == "" || ctrl_module == "none") { // enqueue all modules in order to stop @@ -1956,6 +1973,19 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) if (current_module_msg.joint_name.size() == current_module_msg.module_name.size()) current_module_pub_.publish(current_module_msg); + + + ROS_INFO("----- After -----"); + for (auto& d_it : robot_->dxls_) + { + std::string joint_name = d_it.first; + + Dynamixel *dxl = d_it.second; + double goal_position = dxl->dxl_state_->goal_position_; + + fprintf(stderr, "%s : %f \n", joint_name.c_str(), goal_position); + } + ROS_INFO("----- ----- -----"); } void RobotisController::gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) diff --git a/robotis_device/devices/dynamixel/H54-100-B210-R-NR.device b/robotis_device/devices/dynamixel/H54-100-B210-R-NR.device new file mode 100644 index 0000000..4cb137a --- /dev/null +++ b/robotis_device/devices/dynamixel/H54-100-B210-R-NR.device @@ -0,0 +1,76 @@ +[device info] +model_name = H54-100-B210-R-NR +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 9.09201 +velocity_to_value_ratio = 2046.2777 +value_of_0_radian_position = 0 +value_of_min_radian_position = -250950 +value_of_max_radian_position = 250950 +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_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 + 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 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N