Skip to content

Commit 6e42165

Browse files
Woojin-CriveWoojin-Crive
committed
Added new Dynamixel models and improved error handling in the hardware interface
1 parent 5c46f10 commit 6e42165

File tree

6 files changed

+234
-58
lines changed

6 files changed

+234
-58
lines changed

‎param/dxl_model/dynamixel.model‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,7 @@ Number Name
99
1040 xh430_v350.model
1010
1050 xh430_v210.model
1111
1060 xl430_w250.model
12+
1061 xl431_t250.model
1213
1070 xc430_w150.model
1314
1070 xc430_w150.model
1415
1080 xc430_w240.model
@@ -46,3 +47,4 @@ Number Name
4647
35074 rh_p12_rn.model
4748
220 omy_hat.model
4849
230 omy_end.model
50+
536 sensorxel_joy.model

‎param/dxl_model/sensorxel_joy.model‎

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
[control table]
2+
Address Size Data Name
3+
0 2 Model Number
4+
2 1 ROBOT_Generation
5+
3 1 Board_Number
6+
5 1 Firmware_Version_Major
7+
6 1 Firmware_Version_Minor
8+
7 1 ID
9+
8 1 Baud Rate
10+
10 1 Boot_Version_Major
11+
11 1 Boot_Version_MInor
12+
12 4 Error_Code
13+
20 4 Micros
14+
24 4 Millis
15+
28 1 JOYSTICK TACT SWITCH
16+
29 2 JOYSTICK X VALUE
17+
31 2 JOYSTICK Y VALUE
18+
40 2 IMU ACC X
19+
42 2 IMU ACC Y
20+
44 2 IMU ACC Z
21+
46 2 IMU ACC SUM

‎param/dxl_model/xl431_t250.model‎

Lines changed: 69 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,69 @@
1+
[type info]
2+
name value
3+
value_of_zero_radian_position 2048
4+
value_of_max_radian_position 4095
5+
value_of_min_radian_position 0
6+
min_radian -3.14159265
7+
max_radian 3.14159265
8+
torque_constant 0.0009674796739867748
9+
10+
[control table]
11+
Address Size Data Name
12+
0 2 Model Number
13+
2 4 Model Information
14+
6 1 Firmware Version
15+
7 1 ID
16+
8 1 Baud Rate
17+
9 1 Return Delay Time
18+
10 1 Drive Mode
19+
11 1 Operating Mode
20+
12 1 Secondary(Shadow) ID
21+
13 1 Protocol Type
22+
20 4 Homing Offset
23+
24 4 Moving Threshold
24+
31 1 Temperature Limit
25+
32 2 Max Voltage Limit
26+
34 2 Min Voltage Limit
27+
36 2 PWM Limit
28+
38 2 Current Limit
29+
44 4 Velocity Limit
30+
48 4 Max Position Limit
31+
52 4 Min Position Limit
32+
62 1 PWM Slope
33+
63 1 Shutdown
34+
64 1 Torque Enable
35+
65 1 LED
36+
68 1 Status Return Level
37+
69 1 Registered Instruction
38+
70 1 Hardware Error Status
39+
76 2 Velocity I Gain
40+
78 2 Velocity P Gain
41+
80 2 Position D Gain
42+
82 2 Position I Gain
43+
84 2 Position P Gain
44+
88 2 Feedforward 2nd Gain
45+
90 2 Feedforward 1st Gain
46+
98 1 Bus Watchdog
47+
100 2 Goal PWM
48+
102 2 Goal Current
49+
104 4 Goal Velocity
50+
108 4 Profile Acceleration
51+
112 4 Profile Velocity
52+
116 4 Goal Position
53+
120 2 Realtime Tick
54+
122 1 Moving
55+
123 1 Moving Status
56+
124 2 Present PWM
57+
126 2 Present Current
58+
128 4 Present Velocity
59+
132 4 Present Position
60+
136 4 Velocity Trajectory
61+
140 4 Position Trajectory
62+
144 2 Present Input Voltage
63+
146 1 Present Temperature
64+
168 2 Indirect Address 1
65+
208 1 Indirect Data 1
66+
168 2 Indirect Address Write
67+
208 1 Indirect Data Write
68+
180 2 Indirect Address Read
69+
214 1 Indirect Data Read

‎src/dynamixel/dynamixel.cpp‎

Lines changed: 13 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -82,15 +82,25 @@ DxlError Dynamixel::InitDxlComm(
8282
fprintf(stderr, " - Ping succeeded. Dynamixel model number : %d\n", dxl_model_number);
8383
}
8484

85-
dxl_info_.ReadDxlModelFile(it_id, dxl_model_number);
85+
try {
86+
dxl_info_.ReadDxlModelFile(it_id, dxl_model_number);
87+
} catch (const std::exception& e) {
88+
fprintf(stderr, "Error reading model file for ID %d: %s\n", it_id, e.what());
89+
return DxlError::CANNOT_FIND_CONTROL_ITEM;
90+
}
8691
}
8792

8893
read_data_list_.clear();
8994
write_data_list_.clear();
9095

9196
for (auto it_id : id_arr) {
92-
if (dxl_info_.CheckDxlControlItem(it_id, "Torque Enable")) {
93-
torque_state_[it_id] = TORQUE_OFF;
97+
try {
98+
if (dxl_info_.CheckDxlControlItem(it_id, "Torque Enable")) {
99+
torque_state_[it_id] = TORQUE_OFF;
100+
}
101+
} catch (const std::exception& e) {
102+
fprintf(stderr, "Error checking control item for ID %d: %s\n", it_id, e.what());
103+
return DxlError::CANNOT_FIND_CONTROL_ITEM;
94104
}
95105
}
96106

‎src/dynamixel/dynamixel_info.cpp‎

Lines changed: 47 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -60,13 +60,13 @@ void DynamixelInfo::ReadDxlModelFile(uint8_t id, uint16_t model_num)
6060
path += it->second;
6161
} else {
6262
fprintf(stderr, "[ERROR] CANNOT FIND THE DXL MODEL FROM FILE LIST.\n");
63-
return;
63+
throw std::runtime_error("Cannot find the DXL model from file list");
6464
}
6565

6666
std::ifstream open_file(path);
6767
if (open_file.is_open() != 1) {
6868
fprintf(stderr, "[ERROR] CANNOT FIND DXL [%s] MODEL FILE.\n", path.c_str());
69-
exit(-1);
69+
throw std::runtime_error("Cannot find DXL model file");
7070
}
7171

7272
DxlInfo temp_dxl_info;
@@ -83,18 +83,28 @@ void DynamixelInfo::ReadDxlModelFile(uint8_t id, uint16_t model_num)
8383
std::vector<std::string> strs;
8484
boost::split(strs, line, boost::is_any_of("\t"));
8585

86-
if (strs.at(0) == "value_of_zero_radian_position") {
87-
temp_dxl_info.value_of_zero_radian_position = static_cast<int32_t>(stoi(strs.at(1)));
88-
} else if (strs.at(0) == "value_of_max_radian_position") {
89-
temp_dxl_info.value_of_max_radian_position = static_cast<int32_t>(stoi(strs.at(1)));
90-
} else if (strs.at(0) == "value_of_min_radian_position") {
91-
temp_dxl_info.value_of_min_radian_position = static_cast<int32_t>(stoi(strs.at(1)));
92-
} else if (strs.at(0) == "min_radian") {
93-
temp_dxl_info.min_radian = static_cast<double>(stod(strs.at(1)));
94-
} else if (strs.at(0) == "max_radian") {
95-
temp_dxl_info.max_radian = static_cast<double>(stod(strs.at(1)));
96-
} else if (strs.at(0) == "torque_constant") {
97-
temp_dxl_info.torque_constant = static_cast<double>(stod(strs.at(1)));
86+
if (strs.size() < 2) {
87+
continue;
88+
}
89+
90+
try {
91+
if (strs.at(0) == "value_of_zero_radian_position") {
92+
temp_dxl_info.value_of_zero_radian_position = static_cast<int32_t>(stoi(strs.at(1)));
93+
} else if (strs.at(0) == "value_of_max_radian_position") {
94+
temp_dxl_info.value_of_max_radian_position = static_cast<int32_t>(stoi(strs.at(1)));
95+
} else if (strs.at(0) == "value_of_min_radian_position") {
96+
temp_dxl_info.value_of_min_radian_position = static_cast<int32_t>(stoi(strs.at(1)));
97+
} else if (strs.at(0) == "min_radian") {
98+
temp_dxl_info.min_radian = static_cast<double>(stod(strs.at(1)));
99+
} else if (strs.at(0) == "max_radian") {
100+
temp_dxl_info.max_radian = static_cast<double>(stod(strs.at(1)));
101+
} else if (strs.at(0) == "torque_constant") {
102+
temp_dxl_info.torque_constant = static_cast<double>(stod(strs.at(1)));
103+
}
104+
} catch (const std::exception& e) {
105+
std::string error_msg = "Error processing line in model file: " + line + "\nError: " + e.what();
106+
// fprintf(stderr, "[WARN] %s\n", error_msg.c_str());
107+
throw std::runtime_error(error_msg);
98108
}
99109
}
100110

@@ -108,11 +118,29 @@ void DynamixelInfo::ReadDxlModelFile(uint8_t id, uint16_t model_num)
108118
std::vector<std::string> strs;
109119
boost::split(strs, line, boost::is_any_of("\t"));
110120

111-
ControlItem temp;
112-
temp.address = static_cast<uint16_t>(stoi(strs.at(0)));
113-
temp.size = static_cast<uint8_t>(stoi(strs.at(1)));
114-
temp.item_name = strs.at(2);
115-
temp_dxl_info.item.push_back(temp);
121+
if (strs.size() < 3) {
122+
std::string error_msg = "Malformed control table line: " + line;
123+
// fprintf(stderr, "[ERROR] %s\n", error_msg.c_str());
124+
throw std::runtime_error(error_msg);
125+
}
126+
127+
try {
128+
ControlItem temp;
129+
temp.address = static_cast<uint16_t>(stoi(strs.at(0)));
130+
temp.size = static_cast<uint8_t>(stoi(strs.at(1)));
131+
temp.item_name = strs.at(2);
132+
temp_dxl_info.item.push_back(temp);
133+
} catch (const std::exception& e) {
134+
std::string error_msg = "Error processing control table line: " + line + "\nError: " + e.what();
135+
// fprintf(stderr, "[ERROR] %s\n", error_msg.c_str());
136+
throw std::runtime_error(error_msg);
137+
}
138+
}
139+
140+
if (temp_dxl_info.item.empty()) {
141+
std::string error_msg = "No control table items found in model file for ID " + std::to_string(id);
142+
// fprintf(stderr, "[ERROR] %s\n", error_msg.c_str());
143+
throw std::runtime_error(error_msg);
116144
}
117145

118146
dxl_info_[id] = temp_dxl_info;

0 commit comments

Comments
 (0)