BRSRI/bellande_humanoid_robot_intelligence_control_system_framework
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge pull request #28 from ROBOTIS-GIT/develop

Browse Source 
merge (develop branch -> master branch)
This commit is contained in:
ROBOTIS-zerom
2016-08-22 16:23:42 +09:00
committed by GitHub
parent 3e87cb1566 adaaaea42b
commit c17bea094f
7 changed files with 55 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
robotis_controller/src/robotis_controller/robotis_controller.cpp
View File

@ -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;
}
}

2
robotis_device/devices/dynamixel/H42-20-S300-R.device
View File

@ -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

2
robotis_device/devices/dynamixel/H54-100-S500-R.device
View File

@ -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

2
robotis_device/devices/dynamixel/H54-200-B500-R.device
View File

@ -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

2
robotis_device/devices/dynamixel/H54-200-S500-R.device
View File

@ -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

26
robotis_device/include/robotis_device/dynamixel_state.h
View File

@ -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<std::string, uint32_t> 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();
}

30
robotis_device/src/robotis_device/robot.cpp
View File

@ -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;