Skip to content

Commit 260c03b

Browse files
s-changhyuns-changhyun
committed
mode change debugging
1 parent d0de317 commit 260c03b

File tree

2 files changed

+107
-1
lines changed

2 files changed

+107
-1
lines changed

‎robotis_controller/src/robotis_controller/robotis_controller.cpp‎

Lines changed: 31 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1519,6 +1519,11 @@ void RobotisController::setCtrlModuleCallback(const std_msgs::String::ConstPtr &
15191519
{
15201520
std::string _module_name_to_set = msg->data;
15211521

1522+
1523+
1524+
1525+
1526+
15221527
set_module_thread_ = boost::thread(boost::bind(&RobotisController::setCtrlModuleThread, this, _module_name_to_set));
15231528
}
15241529

@@ -1532,7 +1537,7 @@ void RobotisController::setJointCtrlModuleCallback(const robotis_controller_msgs
15321537
return;
15331538

15341539
set_module_thread_ = boost::thread(boost::bind(&RobotisController::setJointCtrlModuleThread, this, msg));
1535-
}
1540+
}
15361541

15371542
bool RobotisController::getCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req,
15381543
robotis_controller_msgs::GetJointModule::Response &res)
@@ -1751,6 +1756,18 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module)
17511756
// stop module
17521757
std::list<MotionModule *> stop_modules;
17531758

1759+
ROS_INFO("----- Before -----");
1760+
for (auto& d_it : robot_->dxls_)
1761+
{
1762+
std::string joint_name = d_it.first;
1763+
1764+
Dynamixel *dxl = d_it.second;
1765+
double goal_position = dxl->dxl_state_->goal_position_;
1766+
1767+
fprintf(stderr, "%s : %f \n", joint_name.c_str(), goal_position);
1768+
}
1769+
ROS_INFO("----- ----- -----");
1770+
17541771
if (ctrl_module == "" || ctrl_module == "none")
17551772
{
17561773
// enqueue all modules in order to stop
@@ -1956,6 +1973,19 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module)
19561973

19571974
if (current_module_msg.joint_name.size() == current_module_msg.module_name.size())
19581975
current_module_pub_.publish(current_module_msg);
1976+
1977+
1978+
ROS_INFO("----- After -----");
1979+
for (auto& d_it : robot_->dxls_)
1980+
{
1981+
std::string joint_name = d_it.first;
1982+
1983+
Dynamixel *dxl = d_it.second;
1984+
double goal_position = dxl->dxl_state_->goal_position_;
1985+
1986+
fprintf(stderr, "%s : %f \n", joint_name.c_str(), goal_position);
1987+
}
1988+
ROS_INFO("----- ----- -----");
19591989
}
19601990

19611991
void RobotisController::gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg)

‎robotis_device/devices/dynamixel/H54-100-B210-R-NR.device‎

Lines changed: 76 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,76 @@
1+
[device info]
2+
model_name = H54-100-B210-R-NR
3+
device_type = dynamixel
4+
5+
[type info]
6+
torque_to_current_value_ratio = 9.09201
7+
velocity_to_value_ratio = 2046.2777
8+
value_of_0_radian_position = 0
9+
value_of_min_radian_position = -250950
10+
value_of_max_radian_position = 250950
11+
min_radian = -3.14159265
12+
max_radian = 3.14159265
13+
14+
torque_enable_item_name = torque_enable
15+
present_position_item_name = present_position
16+
present_velocity_item_name = present_velocity
17+
present_current_item_name = present_current
18+
goal_position_item_name = goal_position
19+
goal_velocity_item_name = goal_velocity
20+
goal_current_item_name = goal_torque
21+
position_d_gain_item_name =
22+
position_i_gain_item_name =
23+
position_p_gain_item_name = position_p_gain
24+
velocity_d_gain_item_name =
25+
velocity_i_gain_item_name = velocity_i_gain
26+
velocity_p_gain_item_name = velocity_p_gain
27+
28+
[control table]
29+
# addr | item name | length | access | memory | min value | max value | signed
30+
0 | model_number | 2 | R | EEPROM | 0 | 65535 | N
31+
6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N
32+
7 | ID | 1 | RW | EEPROM | 0 | 252 | N
33+
8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N
34+
9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N
35+
11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N
36+
13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y
37+
17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N
38+
21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N
39+
22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N
40+
24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N
41+
26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N
42+
30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N
43+
32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N
44+
36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y
45+
40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y
46+
44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N
47+
45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N
48+
46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N
49+
47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N
50+
48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N
51+
49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N
52+
562 | torque_enable | 1 | RW | RAM | 0 | 1 | N
53+
563 | LED_RED | 1 | RW | RAM | 0 | 255 | N
54+
564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N
55+
565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N
56+
586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N
57+
588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N
58+
594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N
59+
596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y
60+
600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y
61+
604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y
62+
606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N
63+
610 | is_moving | 1 | R | RAM | 0 | 1 | N
64+
611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y
65+
615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y
66+
621 | present_current | 2 | R | RAM | -32768 | 32767 | Y
67+
623 | present_voltage | 2 | R | RAM | 0 | 500 | N
68+
625 | present_temperature | 1 | R | RAM | 0 | 200 | N
69+
626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N
70+
628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N
71+
630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N
72+
632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N
73+
634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N
74+
890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N
75+
891 | status_return_level | 1 | RW | RAM | 0 | 2 | N
76+
892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N

0 commit comments

Comments
 (0)