diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h index aab8da0..6ee5c26 100755 --- a/robotis_controller/include/robotis_controller/robotis_controller.h +++ b/robotis_controller/include/robotis_controller/robotis_controller.h @@ -28,6 +28,7 @@ #include #include #include +#include #include #include #include @@ -38,6 +39,7 @@ #include "robotis_controller_msgs/GetJointModule.h" #include "robotis_controller_msgs/SetJointModule.h" #include "robotis_controller_msgs/SetModule.h" +#include "robotis_controller_msgs/LoadOffset.h" #include "robotis_device/robot.h" #include "robotis_framework_common/motion_module.h" @@ -64,6 +66,7 @@ private: bool init_pose_loaded_; bool is_timer_running_; + bool is_offset_enabled_; bool stop_timer_; pthread_t timer_thread_; ControllerMode controller_mode_; @@ -139,9 +142,11 @@ public: void setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); void setJointCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg); void setCtrlModuleCallback(const std_msgs::String::ConstPtr &msg); + void enableOffsetCallback(const std_msgs::Bool::ConstPtr &msg); bool getJointCtrlModuleService(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res); bool setJointCtrlModuleService(robotis_controller_msgs::SetJointModule::Request &req, robotis_controller_msgs::SetJointModule::Response &res); bool setCtrlModuleService(robotis_controller_msgs::SetModule::Request &req, robotis_controller_msgs::SetModule::Response &res); + bool loadOffsetService(robotis_controller_msgs::LoadOffset::Request &req, robotis_controller_msgs::LoadOffset::Response &res); void gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 000bd59..8a7ee7d 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -30,6 +30,7 @@ using namespace robotis_framework; RobotisController::RobotisController() : is_timer_running_(false), + is_offset_enabled_(true), stop_timer_(false), init_pose_loaded_(false), timer_thread_(0), @@ -142,7 +143,10 @@ void RobotisController::initializeSyncWrite() if ((dxl->present_position_item_ != 0) && (dxl->bulk_read_items_[i]->item_name_ == dxl->present_position_item_->item_name_)) { - dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(read_data) - dxl->dxl_state_->position_offset_; // remove offset + if(is_offset_enabled_) + dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(read_data) - dxl->dxl_state_->position_offset_; // remove offset + else + dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(read_data); dxl->dxl_state_->goal_position_ = dxl->dxl_state_->present_position_; port_to_sync_write_position_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); @@ -632,6 +636,8 @@ void RobotisController::msgQueueThread() &RobotisController::setControllerModeCallback, this); ros::Subscriber joint_states_sub = ros_node.subscribe("/robotis/set_joint_states", 10, &RobotisController::setJointStatesCallback, this); + ros::Subscriber enable_offset_sub = ros_node.subscribe("/robotis/enable_offset", 10, + &RobotisController::enableOffsetCallback, this); ros::Subscriber gazebo_joint_states_sub; if (gazebo_mode_ == true) @@ -664,6 +670,8 @@ void RobotisController::msgQueueThread() &RobotisController::setJointCtrlModuleService, this); ros::ServiceServer set_module_server = ros_node.advertiseService("/robotis/set_present_ctrl_modules", &RobotisController::setCtrlModuleService, this); + ros::ServiceServer load_offset_server = ros_node.advertiseService("/robotis/load_offset", + &RobotisController::loadOffsetService, this); ros::WallDuration duration(robot_->getControlCycle() / 1000.0); while(ros_node.ok()) @@ -926,13 +934,23 @@ void RobotisController::process() // change dxl_state if (dxl->present_position_item_ != 0 && item->item_name_ == dxl->present_position_item_->item_name_) - dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset + { + if(is_offset_enabled_) + dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset + else + dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data); + } else if (dxl->present_velocity_item_ != 0 && item->item_name_ == dxl->present_velocity_item_->item_name_) dxl->dxl_state_->present_velocity_ = dxl->convertValue2Velocity(data); else if (dxl->present_current_item_ != 0 && item->item_name_ == dxl->present_current_item_->item_name_) dxl->dxl_state_->present_torque_ = dxl->convertValue2Torque(data); else if (dxl->goal_position_item_ != 0 && item->item_name_ == dxl->goal_position_item_->item_name_) - dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset + { + if(is_offset_enabled_) + dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset + else + dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data); + } else if (dxl->goal_velocity_item_ != 0 && item->item_name_ == dxl->goal_velocity_item_->item_name_) dxl->dxl_state_->goal_velocity_ = dxl->convertValue2Velocity(data); else if (dxl->goal_current_item_ != 0 && item->item_name_ == dxl->goal_current_item_->item_name_) @@ -1146,13 +1164,23 @@ void RobotisController::process() // change dxl_state if (dxl->present_position_item_ != 0 && item->item_name_ == dxl->present_position_item_->item_name_) - dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset + { + if(is_offset_enabled_) + dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset + else + dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data); + } else if (dxl->present_velocity_item_ != 0 && item->item_name_ == dxl->present_velocity_item_->item_name_) dxl->dxl_state_->present_velocity_ = dxl->convertValue2Velocity(data); else if (dxl->present_current_item_ != 0 && item->item_name_ == dxl->present_current_item_->item_name_) dxl->dxl_state_->present_torque_ = dxl->convertValue2Torque(data); else if (dxl->goal_position_item_ != 0 && item->item_name_ == dxl->goal_position_item_->item_name_) - dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset + { + if(is_offset_enabled_) + dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset + else + dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data); + } else if (dxl->goal_velocity_item_ != 0 && item->item_name_ == dxl->goal_velocity_item_->item_name_) dxl->dxl_state_->goal_velocity_ = dxl->convertValue2Velocity(data); else if (dxl->goal_current_item_ != 0 && item->item_name_ == dxl->goal_current_item_->item_name_) @@ -1255,7 +1283,12 @@ void RobotisController::process() if (gazebo_mode_ == false) { // add offset - uint32_t pos_data = dxl->convertRadian2Value(dxl_state->goal_position_ + dxl_state->position_offset_); + uint32_t pos_data; + if(is_offset_enabled_) + pos_data= dxl->convertRadian2Value(dxl_state->goal_position_ + dxl_state->position_offset_); + else + pos_data= dxl->convertRadian2Value(dxl_state->goal_position_); + uint8_t sync_write_data[4] = { 0 }; sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); @@ -1266,7 +1299,7 @@ void RobotisController::process() port_to_sync_write_position_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data); // if position p gain value is changed -> sync write - if (dxl_state->position_p_gain_ != result_state->position_p_gain_) + if (result_state->position_p_gain_ != 65535 && dxl_state->position_p_gain_ != result_state->position_p_gain_) { dxl_state->position_p_gain_ = result_state->position_p_gain_; uint8_t sync_write_data[4] = { 0 }; @@ -1280,7 +1313,7 @@ void RobotisController::process() } // if position i gain value is changed -> sync write - if (dxl_state->position_i_gain_ != result_state->position_i_gain_) + if (result_state->position_i_gain_ != 65535 && 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 }; @@ -1294,7 +1327,7 @@ void RobotisController::process() } // if position d gain value is changed -> sync write - if (dxl_state->position_d_gain_ != result_state->position_d_gain_) + if (result_state->position_d_gain_ != 65535 && 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 }; @@ -1325,7 +1358,7 @@ void RobotisController::process() 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_) + if (result_state->velocity_p_gain_ != 65535 && 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 }; @@ -1339,7 +1372,7 @@ void RobotisController::process() } // if velocity i gain value is changed -> sync write - if (dxl_state->velocity_i_gain_ != result_state->velocity_i_gain_) + if (result_state->velocity_i_gain_ != 65535 && 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 }; @@ -1353,7 +1386,7 @@ void RobotisController::process() } // if velocity d gain value is changed -> sync write - if (dxl_state->velocity_d_gain_ != result_state->velocity_d_gain_) + if (result_state->velocity_d_gain_ != 65535 && 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 }; @@ -1653,7 +1686,12 @@ void RobotisController::setJointStatesCallback(const sensor_msgs::JointState::Co if (gazebo_mode_ == false) { // add offset - uint32_t pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_); + uint32_t pos_data; + if(is_offset_enabled_) + pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_); + else + pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_); + uint8_t sync_write_data[4] = { 0 }; sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); @@ -1697,6 +1735,11 @@ void RobotisController::setJointCtrlModuleCallback(const robotis_controller_msgs set_module_thread_ = boost::thread(boost::bind(&RobotisController::setJointCtrlModuleThread, this, msg)); } +void RobotisController::enableOffsetCallback(const std_msgs::Bool::ConstPtr &msg) +{ + is_offset_enabled_ = (bool)msg->data; +} + bool RobotisController::getJointCtrlModuleService(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res) { @@ -1748,6 +1791,14 @@ bool RobotisController::setCtrlModuleService(robotis_controller_msgs::SetModule: set_module_thread_.join(); + res.result = true; + return true; +} + +bool RobotisController::loadOffsetService(robotis_controller_msgs::LoadOffset::Request &req, robotis_controller_msgs::LoadOffset::Response &res) +{ + loadOffset((std::string)req.file_path); + res.result = true; return true; } @@ -1820,7 +1871,12 @@ void RobotisController::setJointCtrlModuleThread(const robotis_controller_msgs:: if(gazebo_mode_ == true) continue; - uint32_t _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_ + _dxl->dxl_state_->position_offset_); + uint32_t _pos_data; + if(is_offset_enabled_) + _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_ + _dxl->dxl_state_->position_offset_); + else + _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_); + uint8_t _sync_write_data[4]; _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data)); @@ -1858,7 +1914,12 @@ void RobotisController::setJointCtrlModuleThread(const robotis_controller_msgs:: if(_mode == PositionControl) { - uint32_t _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_ + _dxl->dxl_state_->position_offset_); + uint32_t _pos_data; + if(is_offset_enabled_) + _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_ + _dxl->dxl_state_->position_offset_); + else + _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_); + uint8_t _sync_write_data[4]; _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data)); @@ -2024,7 +2085,12 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) if (gazebo_mode_ == true) continue; - uint32_t pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_); + uint32_t pos_data; + if(is_offset_enabled_) + pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_); + else + pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_); + uint8_t sync_write_data[4] = { 0 }; sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); @@ -2062,7 +2128,12 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) if (mode == PositionControl) { - uint32_t pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_); + uint32_t pos_data; + if(is_offset_enabled_) + pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_); + else + pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_); + uint8_t sync_write_data[4] = { 0 }; sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); diff --git a/robotis_device/devices/dynamixel/XM540-W150.device b/robotis_device/devices/dynamixel/XM540-W150.device new file mode 100755 index 0000000..e11c017 --- /dev/null +++ b/robotis_device/devices/dynamixel/XM540-W150.device @@ -0,0 +1,89 @@ +[device info] +model_name = XM540-W150 +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 289.13672036 +velocity_to_value_ratio = 41.707853 + +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +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_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 + 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 | 7 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 16 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 20 | homing_offset | 4 | RW | EEPROM | -1044479 | 1044479 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 1023 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 1193 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 32767 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 1023 | N + 48 | max_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 52 | min_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 64 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 65 | LED | 1 | RW | RAM | 0 | 1 | N + 68 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 69 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 70 | hardware_error_status | 1 | R | RAM | 0 | 255 | N + 76 | velocity_i_gain | 2 | RW | RAM | 0 | 32767 | N + 78 | velocity_p_gain | 2 | RW | RAM | 0 | 32767 | N + 80 | position_d_gain | 2 | RW | RAM | 0 | 32767 | N + 82 | position_i_gain | 2 | RW | RAM | 0 | 32767 | N + 84 | position_p_gain | 2 | RW | RAM | 0 | 32767 | N + 88 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 90 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 98 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 100 | goal_pwm | 2 | RW | RAM | 0 | 885 | N + 102 | goal_current | 2 | RW | RAM | 0 | 1193 | N + 104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N + 108 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 112 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 116 | goal_position | 4 | RW | RAM | -1048575 | 1048575 | Y + 120 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 122 | moving | 1 | R | RAM | 0 | 1 | N + 123 | moving_status | 1 | R | RAM | 0 | 255 | N + 124 | present_pwm | 2 | R | RAM | 0 | 885 | N + 126 | present_current | 2 | R | RAM | 0 | 1193 | N + 128 | present_velocity | 4 | R | RAM | 0 | 1023 | N + 132 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 136 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 140 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 144 | present_input_voltage | 2 | R | RAM | 95 | 160 | N + 146 | present_temperature | 1 | R | RAM | 0 | 100 | N + 152 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 154 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 156 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 168 | indirect_address_1 | 2 | RW | RAM | 0 | 65535 | N + 224 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 578 | indirect_address_29 | 2 | RW | RAM | 0 | 65535 | N + 634 | indirect_data_29 | 1 | RW | RAM | 0 | 255 | N diff --git a/robotis_device/devices/dynamixel/XM540-W270.device b/robotis_device/devices/dynamixel/XM540-W270.device new file mode 100755 index 0000000..897f398 --- /dev/null +++ b/robotis_device/devices/dynamixel/XM540-W270.device @@ -0,0 +1,89 @@ +[device info] +model_name = XM540-W270 +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 156.133829 +velocity_to_value_ratio = 41.707853 + +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +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_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 + 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 | 7 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 16 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 20 | homing_offset | 4 | RW | EEPROM | -1044479 | 1044479 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 1023 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 1193 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 32767 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 1023 | N + 48 | max_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 52 | min_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 64 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 65 | LED | 1 | RW | RAM | 0 | 1 | N + 68 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 69 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 70 | hardware_error_status | 1 | R | RAM | 0 | 255 | N + 76 | velocity_i_gain | 2 | RW | RAM | 0 | 32767 | N + 78 | velocity_p_gain | 2 | RW | RAM | 0 | 32767 | N + 80 | position_d_gain | 2 | RW | RAM | 0 | 32767 | N + 82 | position_i_gain | 2 | RW | RAM | 0 | 32767 | N + 84 | position_p_gain | 2 | RW | RAM | 0 | 32767 | N + 88 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 90 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 98 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 100 | goal_pwm | 2 | RW | RAM | 0 | 885 | N + 102 | goal_current | 2 | RW | RAM | 0 | 1193 | N + 104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N + 108 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 112 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 116 | goal_position | 4 | RW | RAM | -1048575 | 1048575 | Y + 120 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 122 | moving | 1 | R | RAM | 0 | 1 | N + 123 | moving_status | 1 | R | RAM | 0 | 255 | N + 124 | present_pwm | 2 | R | RAM | 0 | 885 | N + 126 | present_current | 2 | R | RAM | 0 | 1193 | N + 128 | present_velocity | 4 | R | RAM | 0 | 1023 | N + 132 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 136 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 140 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 144 | present_input_voltage | 2 | R | RAM | 95 | 160 | N + 146 | present_temperature | 1 | R | RAM | 0 | 100 | N + 152 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 154 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 156 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 168 | indirect_address_1 | 2 | RW | RAM | 0 | 65535 | N + 224 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 578 | indirect_address_29 | 2 | RW | RAM | 0 | 65535 | N + 634 | indirect_data_29 | 1 | RW | RAM | 0 | 255 | N diff --git a/robotis_device/include/robotis_device/dynamixel_state.h b/robotis_device/include/robotis_device/dynamixel_state.h index b287bb3..05116c6 100755 --- a/robotis_device/include/robotis_device/dynamixel_state.h +++ b/robotis_device/include/robotis_device/dynamixel_state.h @@ -64,12 +64,12 @@ public: goal_position_(0.0), goal_velocity_(0.0), goal_torque_(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_p_gain_(65535), + position_i_gain_(65535), + position_d_gain_(65535), + velocity_p_gain_(65535), + velocity_i_gain_(65535), + velocity_d_gain_(65535), position_offset_(0) { bulk_read_table_.clear();