Skip to content

Commit

Permalink
Merge pull request #29 from ROBOTIS-GIT/master
Browse files Browse the repository at this point in the history 
Merge for sync kinetic-devel and master branch
  • Loading branch information
@robotpilot
robotpilot authored Aug 30, 2016
2 parents 3142777 + c17bea0 commit 57b194a
Show file tree
Hide file tree
Showing 7 changed files with 55 additions and 27 deletions.
18 changes: 8 additions & 10 deletions robotis_controller/src/robotis_controller/robotis_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down Expand Up @@ -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);
}
}
Expand Down Expand Up @@ -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)
Expand All @@ -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);
Expand Down Expand Up @@ -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;
}
}
Expand Down
2 changes: 1 addition & 1 deletion robotis_device/devices/dynamixel/H42-20-S300-R.device
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion robotis_device/devices/dynamixel/H54-100-S500-R.device
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion robotis_device/devices/dynamixel/H54-200-B500-R.device
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion robotis_device/devices/dynamixel/H54-200-S500-R.device
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
26 changes: 13 additions & 13 deletions robotis_device/include/robotis_device/dynamixel_state.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::string, uint32_t> bulk_read_table_;

Expand All @@ -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();
}
Expand Down
30 changes: 30 additions & 0 deletions robotis_device/src/robotis_device/robot.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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())
{
Expand Down Expand Up @@ -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)
{
Expand Down Expand Up @@ -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;
Expand Down

0 comments on commit 57b194a

Please sign in to comment.