diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 1bea26e..acbe656 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) @@ -1049,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); @@ -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; } } 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 @@ public: 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 @@ public: 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;