From 0b212111274038fe65a06aed86cfcfde24edf67c Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Fri, 12 Aug 2016 16:47:19 +0900 Subject: [PATCH] - added velocity p/i/d gain and position i/d gain sync_write code. --- .../robotis_controller/robotis_controller.h | 5 + .../robotis_controller/robotis_controller.cpp | 242 +++++++++++++++++- .../devices/dynamixel/GRIPPER.device | 3 + .../devices/dynamixel/H42-20-S300-R.device | 3 + .../devices/dynamixel/H54-100-S500-R.device | 3 + .../devices/dynamixel/H54-200-B500-R.device | 3 + .../devices/dynamixel/H54-200-S500-R.device | 3 + .../devices/dynamixel/L42-10-S300-R.device | 3 + .../devices/dynamixel/L54-30-S400-R.device | 3 + .../devices/dynamixel/L54-30-S500-R.device | 3 + .../devices/dynamixel/L54-50-S290-R.device | 3 + .../devices/dynamixel/L54-50-S500-R.device | 3 + .../devices/dynamixel/M42-10-S260-R.device | 3 + .../devices/dynamixel/M54-40-S250-R.device | 3 + .../devices/dynamixel/M54-60-S250-R.device | 3 + .../devices/dynamixel/MX-106.device | 3 + robotis_device/devices/dynamixel/MX-28.device | 3 + robotis_device/devices/dynamixel/MX-64.device | 3 + .../devices/dynamixel/XM-430-W210.device | 3 + .../devices/dynamixel/XM-430-W350.device | 3 + .../devices/dynamixel/XM-430.device | 3 + .../include/robotis_device/dynamixel.h | 5 + .../include/robotis_device/dynamixel_state.h | 10 + .../src/robotis_device/dynamixel.cpp | 7 +- 24 files changed, 319 insertions(+), 7 deletions(-) diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h index ae91370..65d7821 100644 --- a/robotis_controller/include/robotis_controller/robotis_controller.h +++ b/robotis_controller/include/robotis_controller/robotis_controller.h @@ -109,6 +109,11 @@ public: std::map port_to_sync_write_velocity_; std::map port_to_sync_write_current_; std::map port_to_sync_write_position_p_gain_; + std::map port_to_sync_write_position_i_gain_; + std::map port_to_sync_write_position_d_gain_; + std::map port_to_sync_write_velocity_p_gain_; + std::map port_to_sync_write_velocity_i_gain_; + std::map port_to_sync_write_velocity_d_gain_; /* publisher */ ros::Publisher goal_joint_state_pub_; diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 0029e4f..35c81b1 100644 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -93,11 +93,36 @@ void RobotisController::initializeSyncWrite() if (it.second != NULL) it.second->clearParam(); } + for (auto& it : port_to_sync_write_position_i_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_position_d_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } for (auto& it : port_to_sync_write_velocity_) { if (it.second != NULL) it.second->clearParam(); } + for (auto& it : port_to_sync_write_velocity_p_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_velocity_i_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_velocity_d_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } for (auto& it : port_to_sync_write_current_) { if (it.second != NULL) @@ -141,12 +166,37 @@ void RobotisController::initializeSyncWrite() { dxl->dxl_state_->position_p_gain_ = read_data; } + else if ((dxl->position_i_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->position_i_gain_item_->item_name_)) + { + dxl->dxl_state_->position_i_gain_ = read_data; + } + else if ((dxl->position_d_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->position_d_gain_item_->item_name_)) + { + dxl->dxl_state_->position_d_gain_ = read_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_; } + else if ((dxl->velocity_p_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->velocity_p_gain_item_->item_name_)) + { + dxl->dxl_state_->velocity_p_gain_ = read_data; + } + else if ((dxl->velocity_i_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->velocity_i_gain_item_->item_name_)) + { + dxl->dxl_state_->velocity_i_gain_ = read_data; + } + else if ((dxl->velocity_d_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->velocity_d_gain_item_->item_name_)) + { + dxl->dxl_state_->velocity_d_gain_ = read_data; + } else if ((dxl->present_current_item_ != 0) && (dxl->bulk_read_items_[i]->item_name_ == dxl->present_current_item_->item_name_)) { @@ -210,6 +260,24 @@ bool RobotisController::initialize(const std::string robot_file_path, const std: default_device->position_p_gain_item_->data_length_); } + if (default_device->position_i_gain_item_ != 0) + { + port_to_sync_write_position_i_gain_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->position_i_gain_item_->address_, + default_device->position_i_gain_item_->data_length_); + } + + if (default_device->position_d_gain_item_ != 0) + { + port_to_sync_write_position_d_gain_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->position_d_gain_item_->address_, + default_device->position_d_gain_item_->data_length_); + } + if (default_device->goal_velocity_item_ != 0) { port_to_sync_write_velocity_[port_name] @@ -219,6 +287,33 @@ bool RobotisController::initialize(const std::string robot_file_path, const std: default_device->goal_velocity_item_->data_length_); } + if (default_device->velocity_p_gain_item_ != 0) + { + port_to_sync_write_velocity_p_gain_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->velocity_p_gain_item_->address_, + default_device->velocity_p_gain_item_->data_length_); + } + + if (default_device->velocity_i_gain_item_ != 0) + { + port_to_sync_write_velocity_i_gain_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->velocity_i_gain_item_->address_, + default_device->velocity_i_gain_item_->data_length_); + } + + if (default_device->velocity_d_gain_item_ != 0) + { + port_to_sync_write_velocity_d_gain_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->velocity_d_gain_item_->address_, + default_device->velocity_d_gain_item_->data_length_); + } + if (default_device->goal_current_item_ != 0) { port_to_sync_write_current_[port_name] @@ -687,11 +782,36 @@ void RobotisController::stopTimer() if (it.second != NULL) it.second->clearParam(); } + for (auto& it : port_to_sync_write_position_i_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_position_d_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } for (auto& it : port_to_sync_write_velocity_) { if (it.second != NULL) it.second->clearParam(); } + for (auto& it : port_to_sync_write_velocity_p_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_velocity_i_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_velocity_d_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } for (auto& it : port_to_sync_write_current_) { if (it.second != NULL) @@ -937,14 +1057,42 @@ void RobotisController::process() if (dxl_state->position_p_gain_ != result_state->position_p_gain_) { dxl_state->position_p_gain_ = result_state->position_p_gain_; - uint8_t sync_write_p_gain[4] = { 0 }; - sync_write_p_gain[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->position_p_gain_)); - sync_write_p_gain[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->position_p_gain_)); - sync_write_p_gain[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->position_p_gain_)); - sync_write_p_gain[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->position_p_gain_)); + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->position_p_gain_)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->position_p_gain_)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->position_p_gain_)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->position_p_gain_)); if (port_to_sync_write_position_p_gain_[dxl->port_name_] != NULL) - port_to_sync_write_position_p_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_p_gain); + port_to_sync_write_position_p_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + } + + // if position i gain value is changed -> sync write + if (dxl_state->position_i_gain_ != result_state->position_i_gain_) + { + dxl_state->position_i_gain_ = result_state->position_i_gain_; + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->position_i_gain_)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->position_i_gain_)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->position_i_gain_)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->position_i_gain_)); + + if (port_to_sync_write_position_i_gain_[dxl->port_name_] != NULL) + port_to_sync_write_position_i_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + } + + // if position d gain value is changed -> sync write + if (dxl_state->position_d_gain_ != result_state->position_d_gain_) + { + dxl_state->position_d_gain_ = result_state->position_d_gain_; + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->position_d_gain_)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->position_d_gain_)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->position_d_gain_)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->position_d_gain_)); + + if (port_to_sync_write_position_d_gain_[dxl->port_name_] != NULL) + port_to_sync_write_position_d_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); } } } @@ -963,6 +1111,48 @@ void RobotisController::process() if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) port_to_sync_write_velocity_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data); + + // if velocity p gain gain value is changed -> sync write + if (dxl_state->velocity_p_gain_ != result_state->velocity_p_gain_) + { + dxl_state->velocity_p_gain_ = result_state->velocity_p_gain_; + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_p_gain_)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_p_gain_)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_p_gain_)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_p_gain_)); + + if (port_to_sync_write_velocity_p_gain_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_p_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + } + + // if velocity i gain value is changed -> sync write + if (dxl_state->velocity_i_gain_ != result_state->velocity_i_gain_) + { + dxl_state->velocity_i_gain_ = result_state->velocity_i_gain_; + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_i_gain_)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_i_gain_)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_i_gain_)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_i_gain_)); + + if (port_to_sync_write_velocity_i_gain_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_i_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + } + + // if velocity d gain value is changed -> sync write + if (dxl_state->velocity_d_gain_ != result_state->velocity_d_gain_) + { + dxl_state->velocity_d_gain_ = result_state->velocity_d_gain_; + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_d_gain_)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_d_gain_)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_d_gain_)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_d_gain_)); + + if (port_to_sync_write_velocity_d_gain_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_d_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + } } } else if ((*module_it)->getControlMode() == TorqueControl) @@ -1004,6 +1194,46 @@ void RobotisController::process() it.second->clearParam(); } } + if (port_to_sync_write_position_i_gain_.size() > 0) + { + for (auto& it : port_to_sync_write_position_i_gain_) + { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_position_d_gain_.size() > 0) + { + for (auto& it : port_to_sync_write_position_d_gain_) + { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_velocity_p_gain_.size() > 0) + { + for (auto& it : port_to_sync_write_velocity_p_gain_) + { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_velocity_i_gain_.size() > 0) + { + for (auto& it : port_to_sync_write_velocity_i_gain_) + { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_velocity_d_gain_.size() > 0) + { + for (auto& it : port_to_sync_write_velocity_d_gain_) + { + it.second->txPacket(); + it.second->clearParam(); + } + } for (auto& it : port_to_sync_write_position_) { if (it.second != NULL) diff --git a/robotis_device/devices/dynamixel/GRIPPER.device b/robotis_device/devices/dynamixel/GRIPPER.device index e95ff17..4267c99 100644 --- a/robotis_device/devices/dynamixel/GRIPPER.device +++ b/robotis_device/devices/dynamixel/GRIPPER.device @@ -19,6 +19,9 @@ 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 diff --git a/robotis_device/devices/dynamixel/H42-20-S300-R.device b/robotis_device/devices/dynamixel/H42-20-S300-R.device index e348f42..95686ff 100644 --- a/robotis_device/devices/dynamixel/H42-20-S300-R.device +++ b/robotis_device/devices/dynamixel/H42-20-S300-R.device @@ -21,6 +21,9 @@ 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 diff --git a/robotis_device/devices/dynamixel/H54-100-S500-R.device b/robotis_device/devices/dynamixel/H54-100-S500-R.device index cdf81d0..a251ddd 100644 --- a/robotis_device/devices/dynamixel/H54-100-S500-R.device +++ b/robotis_device/devices/dynamixel/H54-100-S500-R.device @@ -21,6 +21,9 @@ 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 diff --git a/robotis_device/devices/dynamixel/H54-200-B500-R.device b/robotis_device/devices/dynamixel/H54-200-B500-R.device index d002f97..20a280a 100644 --- a/robotis_device/devices/dynamixel/H54-200-B500-R.device +++ b/robotis_device/devices/dynamixel/H54-200-B500-R.device @@ -21,6 +21,9 @@ 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 diff --git a/robotis_device/devices/dynamixel/H54-200-S500-R.device b/robotis_device/devices/dynamixel/H54-200-S500-R.device index 7e18900..5b7dc76 100644 --- a/robotis_device/devices/dynamixel/H54-200-S500-R.device +++ b/robotis_device/devices/dynamixel/H54-200-S500-R.device @@ -21,6 +21,9 @@ 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 diff --git a/robotis_device/devices/dynamixel/L42-10-S300-R.device b/robotis_device/devices/dynamixel/L42-10-S300-R.device index 8a4ca5a..84e556d 100644 --- a/robotis_device/devices/dynamixel/L42-10-S300-R.device +++ b/robotis_device/devices/dynamixel/L42-10-S300-R.device @@ -19,6 +19,9 @@ 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 diff --git a/robotis_device/devices/dynamixel/L54-30-S400-R.device b/robotis_device/devices/dynamixel/L54-30-S400-R.device index d684e67..9dc5d36 100644 --- a/robotis_device/devices/dynamixel/L54-30-S400-R.device +++ b/robotis_device/devices/dynamixel/L54-30-S400-R.device @@ -19,6 +19,9 @@ 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 diff --git a/robotis_device/devices/dynamixel/L54-30-S500-R.device b/robotis_device/devices/dynamixel/L54-30-S500-R.device index 68848a2..fa15941 100644 --- a/robotis_device/devices/dynamixel/L54-30-S500-R.device +++ b/robotis_device/devices/dynamixel/L54-30-S500-R.device @@ -19,6 +19,9 @@ 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 diff --git a/robotis_device/devices/dynamixel/L54-50-S290-R.device b/robotis_device/devices/dynamixel/L54-50-S290-R.device index 3cbd843..8c2f1a2 100644 --- a/robotis_device/devices/dynamixel/L54-50-S290-R.device +++ b/robotis_device/devices/dynamixel/L54-50-S290-R.device @@ -19,6 +19,9 @@ 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 diff --git a/robotis_device/devices/dynamixel/L54-50-S500-R.device b/robotis_device/devices/dynamixel/L54-50-S500-R.device index bfbf0c6..991e159 100644 --- a/robotis_device/devices/dynamixel/L54-50-S500-R.device +++ b/robotis_device/devices/dynamixel/L54-50-S500-R.device @@ -19,6 +19,9 @@ 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 diff --git a/robotis_device/devices/dynamixel/M42-10-S260-R.device b/robotis_device/devices/dynamixel/M42-10-S260-R.device index f21bb44..4c25631 100644 --- a/robotis_device/devices/dynamixel/M42-10-S260-R.device +++ b/robotis_device/devices/dynamixel/M42-10-S260-R.device @@ -19,6 +19,9 @@ 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 diff --git a/robotis_device/devices/dynamixel/M54-40-S250-R.device b/robotis_device/devices/dynamixel/M54-40-S250-R.device index d9424ad..76796b1 100644 --- a/robotis_device/devices/dynamixel/M54-40-S250-R.device +++ b/robotis_device/devices/dynamixel/M54-40-S250-R.device @@ -19,6 +19,9 @@ 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 diff --git a/robotis_device/devices/dynamixel/M54-60-S250-R.device b/robotis_device/devices/dynamixel/M54-60-S250-R.device index 284cac9..a6cfa2a 100644 --- a/robotis_device/devices/dynamixel/M54-60-S250-R.device +++ b/robotis_device/devices/dynamixel/M54-60-S250-R.device @@ -19,6 +19,9 @@ 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 diff --git a/robotis_device/devices/dynamixel/MX-106.device b/robotis_device/devices/dynamixel/MX-106.device index 23b16f1..4b8c63d 100644 --- a/robotis_device/devices/dynamixel/MX-106.device +++ b/robotis_device/devices/dynamixel/MX-106.device @@ -19,6 +19,9 @@ goal_current_item_name = position_d_gain_item_name = position_d_gain position_i_gain_item_name = position_i_gain position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = +velocity_p_gain_item_name = [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/MX-28.device b/robotis_device/devices/dynamixel/MX-28.device index ebcad6e..ff67f2a 100644 --- a/robotis_device/devices/dynamixel/MX-28.device +++ b/robotis_device/devices/dynamixel/MX-28.device @@ -19,6 +19,9 @@ goal_current_item_name = position_d_gain_item_name = position_d_gain position_i_gain_item_name = position_i_gain position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = +velocity_p_gain_item_name = [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/MX-64.device b/robotis_device/devices/dynamixel/MX-64.device index 2988c83..06c0a4a 100644 --- a/robotis_device/devices/dynamixel/MX-64.device +++ b/robotis_device/devices/dynamixel/MX-64.device @@ -19,6 +19,9 @@ goal_current_item_name = position_d_gain_item_name = position_d_gain position_i_gain_item_name = position_i_gain position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = +velocity_p_gain_item_name = [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/XM-430-W210.device b/robotis_device/devices/dynamixel/XM-430-W210.device index 178fc4f..f42ae57 100644 --- a/robotis_device/devices/dynamixel/XM-430-W210.device +++ b/robotis_device/devices/dynamixel/XM-430-W210.device @@ -22,6 +22,9 @@ 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 +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 diff --git a/robotis_device/devices/dynamixel/XM-430-W350.device b/robotis_device/devices/dynamixel/XM-430-W350.device index ed6c17f..ee14157 100644 --- a/robotis_device/devices/dynamixel/XM-430-W350.device +++ b/robotis_device/devices/dynamixel/XM-430-W350.device @@ -22,6 +22,9 @@ 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 +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 diff --git a/robotis_device/devices/dynamixel/XM-430.device b/robotis_device/devices/dynamixel/XM-430.device index 67d523f..c3a7bad 100644 --- a/robotis_device/devices/dynamixel/XM-430.device +++ b/robotis_device/devices/dynamixel/XM-430.device @@ -21,6 +21,9 @@ 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 +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 diff --git a/robotis_device/include/robotis_device/dynamixel.h b/robotis_device/include/robotis_device/dynamixel.h index 349020a..adff01b 100644 --- a/robotis_device/include/robotis_device/dynamixel.h +++ b/robotis_device/include/robotis_device/dynamixel.h @@ -73,6 +73,11 @@ public: ControlTableItem *goal_velocity_item_; ControlTableItem *goal_current_item_; ControlTableItem *position_p_gain_item_; + ControlTableItem *position_i_gain_item_; + ControlTableItem *position_d_gain_item_; + ControlTableItem *velocity_p_gain_item_; + ControlTableItem *velocity_i_gain_item_; + ControlTableItem *velocity_d_gain_item_; Dynamixel(int id, std::string model_name, float protocol_version); diff --git a/robotis_device/include/robotis_device/dynamixel_state.h b/robotis_device/include/robotis_device/dynamixel_state.h index a318a1f..5f4ae36 100644 --- a/robotis_device/include/robotis_device/dynamixel_state.h +++ b/robotis_device/include/robotis_device/dynamixel_state.h @@ -60,6 +60,11 @@ public: 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_; std::map bulk_read_table_; @@ -74,6 +79,11 @@ public: 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) { bulk_read_table_.clear(); diff --git a/robotis_device/src/robotis_device/dynamixel.cpp b/robotis_device/src/robotis_device/dynamixel.cpp index ba316b2..cdd5470 100644 --- a/robotis_device/src/robotis_device/dynamixel.cpp +++ b/robotis_device/src/robotis_device/dynamixel.cpp @@ -55,7 +55,12 @@ Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version) goal_position_item_(0), goal_velocity_item_(0), goal_current_item_(0), - position_p_gain_item_(0) + position_p_gain_item_(0), + position_i_gain_item_(0), + position_d_gain_item_(0), + velocity_p_gain_item_(0), + velocity_i_gain_item_(0), + velocity_d_gain_item_(0) { this->id_ = id; this->model_name_ = model_name;