dynamixel_item.cpp
Go to the documentation of this file.
1 /*******************************************************************************
2 * Copyright 2018 ROBOTIS CO., LTD.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *******************************************************************************/
16 
17 /* Authors: Taehun Lim (Darby), Ryan Shim */
18 
19 #include "../../include/dynamixel_workbench_toolbox/dynamixel_item.h"
20 
21 //=========================================================
22 // Servo register definitions
23 //=========================================================
24 
25 //_________________________________________________________
26 
27 static const char s_Acceleration_Limit[] = "Acceleration_Limit";
28 static const char s_Alarm_LED[] = "Alarm_LED";
29 static const char s_Baud_Rate[] = "Baud_Rate";
30 static const char s_Bus_Watchdog[] = "Bus_Watchdog";
31 static const char s_CCW_Angle_Limit[] = "CCW_Angle_Limit";
32 static const char s_CCW_Compliance_Margin[] = "CCW_Compliance_Margin";
33 static const char s_CCW_Compliance_Slope[] = "CCW_Compliance_Slope";
34 static const char s_Control_Mode[] = "Control_Mode";
35 static const char s_Current[] = "Current";
36 static const char s_Current_Limit[] = "Current_Limit";
37 static const char s_CW_Angle_Limit[] = "CW_Angle_Limit";
38 static const char s_CW_Compliance_Margin[] = "CW_Compliance_Margin";
39 static const char s_CW_Compliance_Slope[] = "CW_Compliance_Slope";
40 static const char s_D_gain[] = "D_gain";
41 static const char s_Drive_Mode[] = "Drive_Mode";
42 static const char s_External_Port_Mode_1[] = "External_Port_Mode_1";
43 static const char s_External_Port_Mode_2[] = "External_Port_Mode_2";
44 static const char s_External_Port_Mode_3[] = "External_Port_Mode_3";
45 static const char s_External_Port_Mode_4[] = "External_Port_Mode_4";
46 static const char s_External_Port_Data_1[] = "External_Port_Data_1";
47 static const char s_External_Port_Data_2[] = "External_Port_Data_2";
48 static const char s_External_Port_Data_3[] = "External_Port_Data_3";
49 static const char s_External_Port_Data_4[] = "External_Port_Data_4";
50 static const char s_Feedforward_1st_Gain[] = "Feedforward_1st_Gain";
51 static const char s_Feedforward_2nd_Gain[] = "Feedforward_2nd_Gain";
52 static const char s_Firmware_Version[] = "Firmware_Version";
53 static const char s_Goal_Acceleration[] = "Goal_Acceleration";
54 static const char s_Goal_Current[] = "Goal_Current";
55 static const char s_Goal_Position[] = "Goal_Position";
56 static const char s_Goal_PWM[] = "Goal_PWM";
57 static const char s_Goal_Torque[] = "Goal_Torque";
58 static const char s_Goal_Velocity[] = "Goal_Velocity";
59 static const char s_Hardware_Error_Status[] = "Hardware_Error_Status";
60 static const char s_Homing_Offset[] = "Homing_Offset";
61 static const char s_I_gain[] = "I_gain";
62 static const char s_ID[] = "ID";
63 static const char s_LED[] = "LED";
64 static const char s_LED_BLUE[] = "LED_BLUE";
65 static const char s_LED_GREEN[] = "LED_GREEN";
66 static const char s_LED_RED[] = "LED_RED";
67 static const char s_Lock[] = "Lock";
68 static const char s_Max_Position_Limit[] = "Max_Position_Limit";
69 static const char s_Max_Torque[] = "Max_Torque";
70 static const char s_Max_Voltage_Limit[] = "Max_Voltage_Limit";
71 static const char s_Min_Position_Limit[] = "Min_Position_Limit";
72 static const char s_Min_Voltage_Limit[] = "Min_Voltage_Limit";
73 static const char s_Model_Number[] = "Model_Number";
74 static const char s_Moving[] = "Moving";
75 static const char s_Moving_Speed[] = "Moving_Speed";
76 static const char s_Moving_Status[] = "Moving_Status";
77 static const char s_Moving_Threshold[] = "Moving_Threshold";
78 static const char s_Multi_Turn_Offset[] = "Multi_Turn_Offset";
79 static const char s_Operating_Mode[] = "Operating_Mode";
80 static const char s_P_gain[] = "P_gain";
81 static const char s_Position_D_Gain[] = "Position_D_Gain";
82 static const char s_Position_I_Gain[] = "Position_I_Gain";
83 static const char s_Position_P_Gain[] = "Position_P_Gain";
84 static const char s_Position_Trajectory[] = "Position_Trajectory";
85 static const char s_Present_Current[] = "Present_Current";
86 static const char s_Present_Input[] = "Present_Input";
87 static const char s_Present_Input_Voltage[] = "Present_Input_Voltage";
88 static const char s_Present_Load[] = "Present_Load";
89 static const char s_Present_Position[] = "Present_Position";
90 static const char s_Present_PWM[] = "Present_PWM";
91 static const char s_Present_Speed[] = "Present_Speed";
92 static const char s_Present_Temperature[] = "Present_Temperature";
93 static const char s_Present_Velocity[] = "Present_Velocity";
94 static const char s_Present_Voltage[] = "Present_Voltage";
95 static const char s_Profile_Acceleration[] = "Profile_Acceleration";
96 static const char s_Profile_Velocity[] = "Profile_Velocity";
97 static const char s_Protocol_Version[] = "Protocol_Version";
98 static const char s_Punch[] = "Punch";
99 static const char s_PWM_Limit[] = "PWM_Limit";
100 static const char s_Realtime_Tick[] = "Realtime_Tick";
101 static const char s_Registered[] = "Registered";
102 static const char s_Registered_Instruction[] = "Registered_Instruction";
103 static const char s_Resolution_Divider[] = "Resolution_Divider";
104 static const char s_Return_Delay_Time[] = "Return_Delay_Time";
105 static const char s_Secondary_ID[] = "Secondary_ID";
106 static const char s_Sensored_Current[] = "Sensored_Current";
107 static const char s_Shutdown[] = "Shutdown";
108 static const char s_Status_Return_Level[] = "Status_Return_Level";
109 static const char s_Temperature_Limit[] = "Temperature_Limit";
110 static const char s_Torque_Control_Mode_Enable[] = "Torque_Control_Mode_Enable";
111 static const char s_Torque_Enable[] = "Torque_Enable";
112 static const char s_Torque_Limit[] = "Torque_Limit";
113 static const char s_Velocity_I_Gain[] = "Velocity_I_Gain";
114 static const char s_Velocity_Limit[] = "Velocity_Limit";
115 static const char s_Velocity_P_Gain[] = "Velocity_P_Gain";
116 static const char s_Velocity_Trajectory[] = "Velocity_Trajectory";
117 
118 //_________________________________________________________
119 
120 //---------------------------------------------------------
121 // AX servos - (num == AX_12A || num == AX_12W || num == AX_18A)
122 //---------------------------------------------------------
123 static const ControlItem items_AX[]{
124  {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2},
125  {s_Firmware_Version, 2, sizeof(s_Firmware_Version) - 1, 1},
126  {s_ID, 3, sizeof(s_ID) - 1, 1},
127  {s_Baud_Rate, 4, sizeof(s_Baud_Rate) - 1, 1},
128  {s_Return_Delay_Time, 5, sizeof(s_Return_Delay_Time) - 1, 1},
129  {s_CW_Angle_Limit, 6, sizeof(s_CW_Angle_Limit) - 1, 2},
130  {s_CCW_Angle_Limit, 8, sizeof(s_CCW_Angle_Limit) - 1, 2},
131  {s_Temperature_Limit, 11, sizeof(s_Temperature_Limit) - 1, 1},
132  {s_Min_Voltage_Limit, 12, sizeof(s_Min_Voltage_Limit) - 1, 1},
133  {s_Max_Voltage_Limit, 13, sizeof(s_Max_Voltage_Limit) - 1, 1},
134  {s_Max_Torque, 14, sizeof(s_Max_Torque) - 1, 2},
135  {s_Status_Return_Level, 16, sizeof(s_Status_Return_Level) - 1, 1},
136  {s_Alarm_LED, 17, sizeof(s_Alarm_LED) - 1, 1},
137  {s_Shutdown, 18, sizeof(s_Shutdown) - 1, 1},
138 
139  {s_Torque_Enable, 24, sizeof(s_Torque_Enable) - 1, 1},
140  {s_LED, 25, sizeof(s_LED) - 1, 1},
141  {s_CW_Compliance_Margin, 26, sizeof(s_CW_Compliance_Margin) - 1, 1},
142  {s_CCW_Compliance_Margin, 27, sizeof(s_CCW_Compliance_Margin) - 1, 1},
143  {s_CW_Compliance_Slope, 28, sizeof(s_CW_Compliance_Slope) - 1, 1},
144  {s_CCW_Compliance_Slope, 29, sizeof(s_CCW_Compliance_Slope) - 1, 1},
145  {s_Goal_Position, 30, sizeof(s_Goal_Position) - 1, 2},
146  {s_Moving_Speed, 32, sizeof(s_Moving_Speed) - 1, 2},
147  {s_Torque_Limit, 34, sizeof(s_Torque_Limit) - 1, 2},
148  {s_Present_Position, 36, sizeof(s_Present_Position) - 1, 2},
149  {s_Present_Speed, 38, sizeof(s_Present_Speed) - 1, 2},
150  {s_Present_Load, 40, sizeof(s_Present_Load) - 1, 2},
151  {s_Present_Voltage, 42, sizeof(s_Present_Voltage) - 1, 1},
152  {s_Present_Temperature, 43, sizeof(s_Present_Temperature) - 1, 1},
153  {s_Registered, 44, sizeof(s_Registered) - 1, 1},
154  {s_Moving, 46, sizeof(s_Moving) - 1, 1},
155  {s_Lock, 47, sizeof(s_Lock) - 1, 1},
156  {s_Punch, 48, sizeof(s_Punch) - 1, 2}};
157 #define COUNT_AX_ITEMS (sizeof(items_AX) / sizeof(items_AX[0]))
158 
159 static const ModelInfo info_AX = {0.11,
160  0,
161  512,
162  1024,
163  -2.61799,
164  2.61799};
165 
166 //---------------------------------------------------------
167 // RX servos - (num == RX_10 || num == RX_24F || num == RX_28 || num == RX_64)
168 //---------------------------------------------------------
169 static const ControlItem items_RX[]{
170  {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2},
171  {s_Firmware_Version, 2, sizeof(s_Firmware_Version) - 1, 1},
172  {s_ID, 3, sizeof(s_ID) - 1, 1},
173  {s_Baud_Rate, 4, sizeof(s_Baud_Rate) - 1, 1},
174  {s_Return_Delay_Time, 5, sizeof(s_Return_Delay_Time) - 1, 1},
175  {s_CW_Angle_Limit, 6, sizeof(s_CW_Angle_Limit) - 1, 2},
176  {s_CCW_Angle_Limit, 8, sizeof(s_CCW_Angle_Limit) - 1, 2},
177  {s_Temperature_Limit, 11, sizeof(s_Temperature_Limit) - 1, 1},
178  {s_Min_Voltage_Limit, 12, sizeof(s_Min_Voltage_Limit) - 1, 1},
179  {s_Max_Voltage_Limit, 13, sizeof(s_Max_Voltage_Limit) - 1, 1},
180  {s_Max_Torque, 14, sizeof(s_Max_Torque) - 1, 2},
181  {s_Status_Return_Level, 16, sizeof(s_Status_Return_Level) - 1, 1},
182  {s_Alarm_LED, 17, sizeof(s_Alarm_LED) - 1, 1},
183  {s_Shutdown, 18, sizeof(s_Shutdown) - 1, 1},
184 
185  {s_Torque_Enable, 24, sizeof(s_Torque_Enable) - 1, 1},
186  {s_LED, 25, sizeof(s_LED) - 1, 1},
187  {s_CW_Compliance_Margin, 26, sizeof(s_CW_Compliance_Margin) - 1, 1},
188  {s_CCW_Compliance_Margin, 27, sizeof(s_CCW_Compliance_Margin) - 1, 1},
189  {s_CW_Compliance_Slope, 28, sizeof(s_CW_Compliance_Slope) - 1, 1},
190  {s_CCW_Compliance_Slope, 29, sizeof(s_CCW_Compliance_Slope) - 1, 1},
191  {s_Goal_Position, 30, sizeof(s_Goal_Position) - 1, 2},
192  {s_Moving_Speed, 32, sizeof(s_Moving_Speed) - 1, 2},
193  {s_Torque_Limit, 34, sizeof(s_Torque_Limit) - 1, 2},
194  {s_Present_Position, 36, sizeof(s_Present_Position) - 1, 2},
195  {s_Present_Speed, 38, sizeof(s_Present_Speed) - 1, 2},
196  {s_Present_Load, 40, sizeof(s_Present_Load) - 1, 2},
197  {s_Present_Voltage, 42, sizeof(s_Present_Voltage) - 1, 1},
198  {s_Present_Temperature, 43, sizeof(s_Present_Temperature) - 1, 1},
199  {s_Registered, 44, sizeof(s_Registered) - 1, 1},
200  {s_Moving, 46, sizeof(s_Moving) - 1, 1},
201  {s_Lock, 47, sizeof(s_Lock) - 1, 1},
202  {s_Punch, 48, sizeof(s_Punch) - 1, 2}};
203 
204 #define COUNT_RX_ITEMS (sizeof(items_RX) / sizeof(items_RX[0]))
205 
206 static const ModelInfo info_RX = {0.11,
207  0,
208  512,
209  1024,
210  -2.61799,
211  2.61799};
212 
213 //---------------------------------------------------------
214 // EX servos - (num == EX_106)
215 //---------------------------------------------------------
216 static const ControlItem items_EX[]{
217  {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2},
218  {s_Firmware_Version, 2, sizeof(s_Firmware_Version) - 1, 1},
219  {s_ID, 3, sizeof(s_ID) - 1, 1},
220  {s_Baud_Rate, 4, sizeof(s_Baud_Rate) - 1, 1},
221  {s_Return_Delay_Time, 5, sizeof(s_Return_Delay_Time) - 1, 1},
222  {s_CW_Angle_Limit, 6, sizeof(s_CW_Angle_Limit) - 1, 2},
223  {s_CCW_Angle_Limit, 8, sizeof(s_CCW_Angle_Limit) - 1, 2},
224  {s_Drive_Mode, 10, sizeof(s_Drive_Mode) - 1, 1},
225  {s_Temperature_Limit, 11, sizeof(s_Temperature_Limit) - 1, 1},
226  {s_Min_Voltage_Limit, 12, sizeof(s_Min_Voltage_Limit) - 1, 1},
227  {s_Max_Voltage_Limit, 13, sizeof(s_Max_Voltage_Limit) - 1, 1},
228  {s_Max_Torque, 14, sizeof(s_Max_Torque) - 1, 2},
229  {s_Status_Return_Level, 16, sizeof(s_Status_Return_Level) - 1, 1},
230  {s_Alarm_LED, 17, sizeof(s_Alarm_LED) - 1, 1},
231  {s_Shutdown, 18, sizeof(s_Shutdown) - 1, 1},
232 
233  {s_Torque_Enable, 24, sizeof(s_Torque_Enable) - 1, 1},
234  {s_LED, 25, sizeof(s_LED) - 1, 1},
235  {s_CW_Compliance_Margin, 26, sizeof(s_CW_Compliance_Margin) - 1, 1},
236  {s_CCW_Compliance_Margin, 27, sizeof(s_CCW_Compliance_Margin) - 1, 1},
237  {s_CW_Compliance_Slope, 28, sizeof(s_CW_Compliance_Slope) - 1, 1},
238  {s_CCW_Compliance_Slope, 29, sizeof(s_CCW_Compliance_Slope) - 1, 1},
239  {s_Goal_Position, 30, sizeof(s_Goal_Position) - 1, 2},
240  {s_Moving_Speed, 34, sizeof(s_Moving_Speed) - 1, 2},
241  {s_Torque_Limit, 35, sizeof(s_Torque_Limit) - 1, 2},
242  {s_Present_Position, 36, sizeof(s_Present_Position) - 1, 2},
243  {s_Present_Speed, 38, sizeof(s_Present_Speed) - 1, 2},
244  {s_Present_Load, 40, sizeof(s_Present_Load) - 1, 2},
245  {s_Present_Voltage, 42, sizeof(s_Present_Voltage) - 1, 1},
246  {s_Present_Temperature, 43, sizeof(s_Present_Temperature) - 1, 1},
247  {s_Registered, 44, sizeof(s_Registered) - 1, 1},
248  {s_Moving, 46, sizeof(s_Moving) - 1, 1},
249  {s_Lock, 47, sizeof(s_Lock) - 1, 1},
250  {s_Punch, 48, sizeof(s_Punch) - 1, 2},
251  {s_Sensored_Current, 56, sizeof(s_Sensored_Current) - 1, 2}};
252 
253 #define COUNT_EX_ITEMS (sizeof(items_EX) / sizeof(items_EX[0]))
254 
255 static const ModelInfo info_EX = {0.11,
256  0,
257  2048,
258  4096,
259  -2.18969008,
260  2.18969008};
261 
262 //---------------------------------------------------------
263 // MX Protocol 1 servos - (num == MX_12W || num == MX_28)
264 //---------------------------------------------------------
265 static const ControlItem items_MX[]{
266  {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2},
267  {s_Firmware_Version, 2, sizeof(s_Firmware_Version) - 1, 1},
268  {s_ID, 3, sizeof(s_ID) - 1, 1},
269  {s_Baud_Rate, 4, sizeof(s_Baud_Rate) - 1, 1},
270  {s_Return_Delay_Time, 5, sizeof(s_Return_Delay_Time) - 1, 1},
271  {s_CW_Angle_Limit, 6, sizeof(s_CW_Angle_Limit) - 1, 2},
272  {s_CCW_Angle_Limit, 8, sizeof(s_CCW_Angle_Limit) - 1, 2},
273  {s_Temperature_Limit, 11, sizeof(s_Temperature_Limit) - 1, 1},
274  {s_Min_Voltage_Limit, 12, sizeof(s_Min_Voltage_Limit) - 1, 1},
275  {s_Max_Voltage_Limit, 13, sizeof(s_Max_Voltage_Limit) - 1, 1},
276  {s_Max_Torque, 14, sizeof(s_Max_Torque) - 1, 2},
277  {s_Status_Return_Level, 16, sizeof(s_Status_Return_Level) - 1, 1},
278  {s_Alarm_LED, 17, sizeof(s_Alarm_LED) - 1, 1},
279  {s_Shutdown, 18, sizeof(s_Shutdown) - 1, 1},
280  {s_Multi_Turn_Offset, 20, sizeof(s_Multi_Turn_Offset) - 1, 2},
281  {s_Resolution_Divider, 22, sizeof(s_Resolution_Divider) - 1, 1},
282 
283  {s_Torque_Enable, 24, sizeof(s_Torque_Enable) - 1, 1},
284  {s_LED, 25, sizeof(s_LED) - 1, 1},
285  {s_D_gain, 26, sizeof(s_D_gain) - 1, 1},
286  {s_I_gain, 27, sizeof(s_I_gain) - 1, 1},
287  {s_P_gain, 28, sizeof(s_P_gain) - 1, 1},
288  {s_Goal_Position, 30, sizeof(s_Goal_Position) - 1, 2},
289  {s_Moving_Speed, 32, sizeof(s_Moving_Speed) - 1, 2},
290  {s_Torque_Limit, 34, sizeof(s_Torque_Limit) - 1, 2},
291  {s_Present_Position, 36, sizeof(s_Present_Position) - 1, 2},
292  {s_Present_Speed, 38, sizeof(s_Present_Speed) - 1, 2},
293  {s_Present_Load, 40, sizeof(s_Present_Load) - 1, 2},
294  {s_Present_Voltage, 42, sizeof(s_Present_Voltage) - 1, 1},
295  {s_Present_Temperature, 43, sizeof(s_Present_Temperature) - 1, 1},
296  {s_Registered, 44, sizeof(s_Registered) - 1, 1},
297  {s_Moving, 46, sizeof(s_Moving) - 1, 1},
298  {s_Lock, 47, sizeof(s_Lock) - 1, 1},
299  {s_Punch, 48, sizeof(s_Punch) - 1, 2},
300  {s_Goal_Acceleration, 73, sizeof(s_Goal_Acceleration) - 1, 1}};
301 
302 #define COUNT_MX_ITEMS (sizeof(items_MX) / sizeof(items_MX[0]))
303 
304 static const ModelInfo info_MX = {0.11,
305  0,
306  2048,
307  4096,
308  -3.14159265,
309  3.14159265};
310 
311 //---------------------------------------------------------
312 // MX Protocol 2 servos - (num == MX_28_2)
313 //---------------------------------------------------------
314 static const ControlItem items_MX2[]{
315  {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2},
316  {s_Firmware_Version, 6, sizeof(s_Firmware_Version) - 1, 1},
317  {s_ID, 7, sizeof(s_ID) - 1, 1},
318  {s_Baud_Rate, 8, sizeof(s_Baud_Rate) - 1, 1},
319  {s_Return_Delay_Time, 9, sizeof(s_Return_Delay_Time) - 1, 1},
320  {s_Drive_Mode, 10, sizeof(s_Drive_Mode) - 1, 1},
321  {s_Operating_Mode, 11, sizeof(s_Operating_Mode) - 1, 1},
322  {s_Secondary_ID, 12, sizeof(s_Secondary_ID) - 1, 1},
323  {s_Protocol_Version, 13, sizeof(s_Protocol_Version) - 1, 1},
324  {s_Homing_Offset, 20, sizeof(s_Homing_Offset) - 1, 4},
325  {s_Moving_Threshold, 24, sizeof(s_Moving_Threshold) - 1, 4},
326  {s_Temperature_Limit, 31, sizeof(s_Temperature_Limit) - 1, 1},
327  {s_Max_Voltage_Limit, 32, sizeof(s_Max_Voltage_Limit) - 1, 2},
328  {s_Min_Voltage_Limit, 34, sizeof(s_Min_Voltage_Limit) - 1, 2},
329  {s_PWM_Limit, 36, sizeof(s_PWM_Limit) - 1, 2},
330  {s_Acceleration_Limit, 40, sizeof(s_Acceleration_Limit) - 1, 4},
331  {s_Velocity_Limit, 44, sizeof(s_Velocity_Limit) - 1, 4},
332  {s_Max_Position_Limit, 48, sizeof(s_Max_Position_Limit) - 1, 4},
333  {s_Min_Position_Limit, 52, sizeof(s_Min_Position_Limit) - 1, 4},
334  {s_Shutdown, 63, sizeof(s_Shutdown) - 1, 1},
335 
336  {s_Torque_Enable, 64, sizeof(s_Torque_Enable) - 1, 1},
337  {s_LED, 65, sizeof(s_LED) - 1, 1},
338  {s_Status_Return_Level, 68, sizeof(s_Status_Return_Level) - 1, 1},
340  {s_Hardware_Error_Status, 70, sizeof(s_Hardware_Error_Status) - 1, 1},
341  {s_Velocity_I_Gain, 76, sizeof(s_Velocity_I_Gain) - 1, 2},
342  {s_Velocity_P_Gain, 78, sizeof(s_Velocity_P_Gain) - 1, 2},
343  {s_Position_D_Gain, 80, sizeof(s_Position_D_Gain) - 1, 2},
344  {s_Position_I_Gain, 82, sizeof(s_Position_I_Gain) - 1, 2},
345  {s_Position_P_Gain, 84, sizeof(s_Position_P_Gain) - 1, 2},
346  {s_Feedforward_2nd_Gain, 88, sizeof(s_Feedforward_2nd_Gain) - 1, 2},
347  {s_Feedforward_1st_Gain, 90, sizeof(s_Feedforward_1st_Gain) - 1, 2},
348  {s_Bus_Watchdog, 98, sizeof(s_Bus_Watchdog) - 1, 1},
349  {s_Goal_PWM, 100, sizeof(s_Goal_PWM) - 1, 2},
350  {s_Goal_Velocity, 104, sizeof(s_Goal_Velocity) - 1, 4},
351  {s_Profile_Acceleration, 108, sizeof(s_Profile_Acceleration) - 1, 4},
352  {s_Profile_Velocity, 112, sizeof(s_Profile_Velocity) - 1, 4},
353  {s_Goal_Position, 116, sizeof(s_Goal_Position) - 1, 4},
354  {s_Realtime_Tick, 120, sizeof(s_Realtime_Tick) - 1, 2},
355  {s_Moving, 122, sizeof(s_Moving) - 1, 1},
356  {s_Moving_Status, 123, sizeof(s_Moving_Status) - 1, 1},
357  {s_Present_PWM, 124, sizeof(s_Present_PWM) - 1, 2},
358  {s_Present_Load, 126, sizeof(s_Present_Load) - 1, 2},
359  {s_Present_Velocity, 128, sizeof(s_Present_Velocity) - 1, 4},
360  {s_Present_Position, 132, sizeof(s_Present_Position) - 1, 4},
361  {s_Velocity_Trajectory, 136, sizeof(s_Velocity_Trajectory) - 1, 4},
362  {s_Position_Trajectory, 140, sizeof(s_Position_Trajectory) - 1, 4},
363  {s_Present_Input_Voltage, 144, sizeof(s_Present_Input_Voltage) - 1, 2},
364  {s_Present_Temperature, 146, sizeof(s_Present_Temperature) - 1, 1}};
365 
366 #define COUNT_MX2_ITEMS (sizeof(items_MX2) / sizeof(items_MX2[0]))
367 
368 static const ModelInfo info_MX2 = {0.229,
369  0,
370  2048,
371  4096,
372  -3.14159265,
373  3.14159265};
374 
375 //---------------------------------------------------------
376 // EXT MX Protocol 1 servos - (num == MX_64 || num == MX_106)
377 //---------------------------------------------------------
378 static const ControlItem items_EXTMX[]{
379  {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2},
380  {s_Firmware_Version, 2, sizeof(s_Firmware_Version) - 1, 1},
381  {s_ID, 3, sizeof(s_ID) - 1, 1},
382  {s_Baud_Rate, 4, sizeof(s_Baud_Rate) - 1, 1},
383  {s_Return_Delay_Time, 5, sizeof(s_Return_Delay_Time) - 1, 1},
384  {s_CW_Angle_Limit, 6, sizeof(s_CW_Angle_Limit) - 1, 2},
385  {s_CCW_Angle_Limit, 8, sizeof(s_CCW_Angle_Limit) - 1, 2},
386  {s_Temperature_Limit, 11, sizeof(s_Temperature_Limit) - 1, 1},
387  {s_Min_Voltage_Limit, 12, sizeof(s_Min_Voltage_Limit) - 1, 1},
388  {s_Max_Voltage_Limit, 13, sizeof(s_Max_Voltage_Limit) - 1, 1},
389  {s_Max_Torque, 14, sizeof(s_Max_Torque) - 1, 2},
390  {s_Status_Return_Level, 16, sizeof(s_Status_Return_Level) - 1, 1},
391  {s_Alarm_LED, 17, sizeof(s_Alarm_LED) - 1, 1},
392  {s_Shutdown, 18, sizeof(s_Shutdown) - 1, 1},
393  {s_Multi_Turn_Offset, 20, sizeof(s_Multi_Turn_Offset) - 1, 2},
394  {s_Resolution_Divider, 22, sizeof(s_Resolution_Divider) - 1, 1},
395 
396  {s_Torque_Enable, 24, sizeof(s_Torque_Enable) - 1, 1},
397  {s_LED, 25, sizeof(s_LED) - 1, 1},
398  {s_D_gain, 26, sizeof(s_D_gain) - 1, 1},
399  {s_I_gain, 27, sizeof(s_I_gain) - 1, 1},
400  {s_P_gain, 28, sizeof(s_P_gain) - 1, 1},
401  {s_Goal_Position, 30, sizeof(s_Goal_Position) - 1, 2},
402  {s_Moving_Speed, 32, sizeof(s_Moving_Speed) - 1, 2},
403  {s_Torque_Limit, 34, sizeof(s_Torque_Limit) - 1, 2},
404  {s_Present_Position, 36, sizeof(s_Present_Position) - 1, 2},
405  {s_Present_Speed, 38, sizeof(s_Present_Speed) - 1, 2},
406  {s_Present_Load, 40, sizeof(s_Present_Load) - 1, 2},
407  {s_Present_Voltage, 42, sizeof(s_Present_Voltage) - 1, 1},
408  {s_Present_Temperature, 43, sizeof(s_Present_Temperature) - 1, 1},
409  {s_Registered, 44, sizeof(s_Registered) - 1, 1},
410  {s_Moving, 46, sizeof(s_Moving) - 1, 1},
411  {s_Lock, 47, sizeof(s_Lock) - 1, 1},
412  {s_Punch, 48, sizeof(s_Punch) - 1, 2},
413  {s_Current, 68, sizeof(s_Current) - 1, 2},
415  {s_Goal_Torque, 71, sizeof(s_Goal_Torque) - 1, 2},
416  {s_Goal_Acceleration, 73, sizeof(s_Goal_Acceleration) - 1, 1}};
417 
418 #define COUNT_EXTMX_ITEMS (sizeof(items_EXTMX) / sizeof(items_EXTMX[0]))
419 
420 static const ModelInfo info_EXTMX = {0.11,
421  0,
422  2048,
423  4096,
424  -3.14159265,
425  3.14159265};
426 
427 //---------------------------------------------------------
428 // EXT MX Protocol 2 Servos - (num == MX_64_2 || num == MX_106_2)
429 //---------------------------------------------------------
430 static const ControlItem items_EXTMX2[]{
431  {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2},
432  {s_Firmware_Version, 6, sizeof(s_Firmware_Version) - 1, 1},
433  {s_ID, 7, sizeof(s_ID) - 1, 1},
434  {s_Baud_Rate, 8, sizeof(s_Baud_Rate) - 1, 1},
435  {s_Return_Delay_Time, 9, sizeof(s_Return_Delay_Time) - 1, 1},
436  {s_Drive_Mode, 10, sizeof(s_Drive_Mode) - 1, 1},
437  {s_Operating_Mode, 11, sizeof(s_Operating_Mode) - 1, 1},
438  {s_Secondary_ID, 12, sizeof(s_Secondary_ID) - 1, 1},
439  {s_Protocol_Version, 13, sizeof(s_Protocol_Version) - 1, 1},
440  {s_Homing_Offset, 20, sizeof(s_Homing_Offset) - 1, 4},
441  {s_Moving_Threshold, 24, sizeof(s_Moving_Threshold) - 1, 4},
442  {s_Temperature_Limit, 31, sizeof(s_Temperature_Limit) - 1, 1},
443  {s_Max_Voltage_Limit, 32, sizeof(s_Max_Voltage_Limit) - 1, 2},
444  {s_Min_Voltage_Limit, 34, sizeof(s_Min_Voltage_Limit) - 1, 2},
445  {s_PWM_Limit, 36, sizeof(s_PWM_Limit) - 1, 2},
446  {s_Current_Limit, 38, sizeof(s_Current_Limit) - 1, 2},
447  {s_Acceleration_Limit, 40, sizeof(s_Acceleration_Limit) - 1, 4},
448  {s_Velocity_Limit, 44, sizeof(s_Velocity_Limit) - 1, 4},
449  {s_Max_Position_Limit, 48, sizeof(s_Max_Position_Limit) - 1, 4},
450  {s_Min_Position_Limit, 52, sizeof(s_Min_Position_Limit) - 1, 4},
451  {s_Shutdown, 63, sizeof(s_Shutdown) - 1, 1},
452 
453  {s_Torque_Enable, 64, sizeof(s_Torque_Enable) - 1, 1},
454  {s_LED, 65, sizeof(s_LED) - 1, 1},
455  {s_Status_Return_Level, 68, sizeof(s_Status_Return_Level) - 1, 1},
457  {s_Hardware_Error_Status, 70, sizeof(s_Hardware_Error_Status) - 1, 1},
458  {s_Velocity_I_Gain, 76, sizeof(s_Velocity_I_Gain) - 1, 2},
459  {s_Velocity_P_Gain, 78, sizeof(s_Velocity_P_Gain) - 1, 2},
460  {s_Position_D_Gain, 80, sizeof(s_Position_D_Gain) - 1, 2},
461  {s_Position_I_Gain, 82, sizeof(s_Position_I_Gain) - 1, 2},
462  {s_Position_P_Gain, 84, sizeof(s_Position_P_Gain) - 1, 2},
463  {s_Feedforward_2nd_Gain, 88, sizeof(s_Feedforward_2nd_Gain) - 1, 2},
464  {s_Feedforward_1st_Gain, 90, sizeof(s_Feedforward_1st_Gain) - 1, 2},
465  {s_Bus_Watchdog, 98, sizeof(s_Bus_Watchdog) - 1, 1},
466  {s_Goal_PWM, 100, sizeof(s_Goal_PWM) - 1, 2},
467  {s_Goal_Current, 102, sizeof(s_Goal_Current) - 1, 2},
468  {s_Goal_Velocity, 104, sizeof(s_Goal_Velocity) - 1, 4},
469  {s_Profile_Acceleration, 108, sizeof(s_Profile_Acceleration) - 1, 4},
470  {s_Profile_Velocity, 112, sizeof(s_Profile_Velocity) - 1, 4},
471  {s_Goal_Position, 116, sizeof(s_Goal_Position) - 1, 4},
472  {s_Realtime_Tick, 120, sizeof(s_Realtime_Tick) - 1, 2},
473  {s_Moving, 122, sizeof(s_Moving) - 1, 1},
474  {s_Moving_Status, 123, sizeof(s_Moving_Status) - 1, 1},
475  {s_Present_PWM, 124, sizeof(s_Present_PWM) - 1, 2},
476  {s_Present_Current, 126, sizeof(s_Present_Current) - 1, 2},
477  {s_Present_Velocity, 128, sizeof(s_Present_Velocity) - 1, 4},
478  {s_Present_Position, 132, sizeof(s_Present_Position) - 1, 4},
479  {s_Velocity_Trajectory, 136, sizeof(s_Velocity_Trajectory) - 1, 4},
480  {s_Position_Trajectory, 140, sizeof(s_Position_Trajectory) - 1, 4},
481  {s_Present_Input_Voltage, 144, sizeof(s_Present_Input_Voltage) - 1, 2},
482  {s_Present_Temperature, 146, sizeof(s_Present_Temperature) - 1, 1}};
483 
484 #define COUNT_EXTMX2_ITEMS (sizeof(items_EXTMX2) / sizeof(items_EXTMX2[0]))
485 
486 static const ModelInfo info_EXTMX2 = {0.229,
487  0,
488  2048,
489  4096,
490  -3.14159265,
491  3.14159265};
492 
493 //---------------------------------------------------------
494 // XL320 - (num == XL_320)
495 //---------------------------------------------------------
496 static const ControlItem items_XL320[]{
497  {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2},
498  {s_Firmware_Version, 2, sizeof(s_Firmware_Version) - 1, 1},
499  {s_ID, 3, sizeof(s_ID) - 1, 1},
500  {s_Baud_Rate, 4, sizeof(s_Baud_Rate) - 1, 1},
501  {s_Return_Delay_Time, 5, sizeof(s_Return_Delay_Time) - 1, 1},
502  {s_CW_Angle_Limit, 6, sizeof(s_CW_Angle_Limit) - 1, 2},
503  {s_CCW_Angle_Limit, 8, sizeof(s_CCW_Angle_Limit) - 1, 2},
504  {s_Control_Mode, 11, sizeof(s_Control_Mode) - 1, 1},
505  {s_Temperature_Limit, 12, sizeof(s_Temperature_Limit) - 1, 1},
506  {s_Min_Voltage_Limit, 13, sizeof(s_Min_Voltage_Limit) - 1, 1},
507  {s_Max_Voltage_Limit, 14, sizeof(s_Max_Voltage_Limit) - 1, 1},
508  {s_Max_Torque, 15, sizeof(s_Max_Torque) - 1, 2},
509  {s_Status_Return_Level, 17, sizeof(s_Status_Return_Level) - 1, 1},
510  {s_Shutdown, 18, sizeof(s_Shutdown) - 1, 1},
511 
512  {s_Torque_Enable, 24, sizeof(s_Torque_Enable) - 1, 1},
513  {s_LED, 25, sizeof(s_LED) - 1, 1},
514  {s_D_gain, 27, sizeof(s_D_gain) - 1, 1},
515  {s_I_gain, 28, sizeof(s_I_gain) - 1, 1},
516  {s_P_gain, 29, sizeof(s_P_gain) - 1, 1},
517  {s_Goal_Position, 30, sizeof(s_Goal_Position) - 1, 2},
518  {s_Moving_Speed, 32, sizeof(s_Moving_Speed) - 1, 2},
519  {s_Torque_Limit, 34, sizeof(s_Torque_Limit) - 1, 2},
520  {s_Present_Position, 37, sizeof(s_Present_Position) - 1, 2},
521  {s_Present_Speed, 39, sizeof(s_Present_Speed) - 1, 2},
522  {s_Present_Load, 41, sizeof(s_Present_Load) - 1, 2},
523  {s_Present_Voltage, 45, sizeof(s_Present_Voltage) - 1, 1},
524  {s_Present_Temperature, 46, sizeof(s_Present_Temperature) - 1, 1},
525  {s_Registered, 47, sizeof(s_Registered) - 1, 1},
526  {s_Moving, 49, sizeof(s_Moving) - 1, 1},
527  {s_Hardware_Error_Status, 50, sizeof(s_Hardware_Error_Status) - 1, 1},
528  {s_Punch, 51, sizeof(s_Punch) - 1, 2}};
529 
530 #define COUNT_XL320_ITEMS (sizeof(items_XL320) / sizeof(items_XL320[0]))
531 
532 static const ModelInfo info_XL320 = {0.11,
533  0,
534  512,
535  1024,
536  -2.61799,
537  2.61799};
538 
539 //---------------------------------------------------------
540 // XL - (num == XL430_W250, XL430_W250_2, XC430_W150, XC430_W240)
541 //---------------------------------------------------------
542 static const ControlItem items_XL[]{
543  {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2},
544  {s_Firmware_Version, 6, sizeof(s_Firmware_Version) - 1, 1},
545  {s_ID, 7, sizeof(s_ID) - 1, 1},
546  {s_Baud_Rate, 8, sizeof(s_Baud_Rate) - 1, 1},
547  {s_Return_Delay_Time, 9, sizeof(s_Return_Delay_Time) - 1, 1},
548  {s_Drive_Mode, 10, sizeof(s_Drive_Mode) - 1, 1},
549  {s_Operating_Mode, 11, sizeof(s_Operating_Mode) - 1, 1},
550  {s_Secondary_ID, 12, sizeof(s_Secondary_ID) - 1, 1},
551  {s_Protocol_Version, 13, sizeof(s_Protocol_Version) - 1, 1},
552  {s_Homing_Offset, 20, sizeof(s_Homing_Offset) - 1, 4},
553  {s_Moving_Threshold, 24, sizeof(s_Moving_Threshold) - 1, 4},
554  {s_Temperature_Limit, 31, sizeof(s_Temperature_Limit) - 1, 1},
555  {s_Max_Voltage_Limit, 32, sizeof(s_Max_Voltage_Limit) - 1, 2},
556  {s_Min_Voltage_Limit, 34, sizeof(s_Min_Voltage_Limit) - 1, 2},
557  {s_PWM_Limit, 36, sizeof(s_PWM_Limit) - 1, 2},
558  {s_Acceleration_Limit, 40, sizeof(s_Acceleration_Limit) - 1, 4},
559  {s_Velocity_Limit, 44, sizeof(s_Velocity_Limit) - 1, 4},
560  {s_Max_Position_Limit, 48, sizeof(s_Max_Position_Limit) - 1, 4},
561  {s_Min_Position_Limit, 52, sizeof(s_Min_Position_Limit) - 1, 4},
562  {s_Shutdown, 63, sizeof(s_Shutdown) - 1, 1},
563 
564  {s_Torque_Enable, 64, sizeof(s_Torque_Enable) - 1, 1},
565  {s_LED, 65, sizeof(s_LED) - 1, 1},
566  {s_Status_Return_Level, 68, sizeof(s_Status_Return_Level) - 1, 1},
568  {s_Hardware_Error_Status, 70, sizeof(s_Hardware_Error_Status) - 1, 1},
569  {s_Velocity_I_Gain, 76, sizeof(s_Velocity_I_Gain) - 1, 2},
570  {s_Velocity_P_Gain, 78, sizeof(s_Velocity_P_Gain) - 1, 2},
571  {s_Position_D_Gain, 80, sizeof(s_Position_D_Gain) - 1, 2},
572  {s_Position_I_Gain, 82, sizeof(s_Position_I_Gain) - 1, 2},
573  {s_Position_P_Gain, 84, sizeof(s_Position_P_Gain) - 1, 2},
574  {s_Feedforward_2nd_Gain, 88, sizeof(s_Feedforward_2nd_Gain) - 1, 2},
575  {s_Feedforward_1st_Gain, 90, sizeof(s_Feedforward_1st_Gain) - 1, 2},
576  {s_Bus_Watchdog, 98, sizeof(s_Bus_Watchdog) - 1, 1},
577  {s_Goal_PWM, 100, sizeof(s_Goal_PWM) - 1, 2},
578  {s_Goal_Velocity, 104, sizeof(s_Goal_Velocity) - 1, 4},
579  {s_Profile_Acceleration, 108, sizeof(s_Profile_Acceleration) - 1, 4},
580  {s_Profile_Velocity, 112, sizeof(s_Profile_Velocity) - 1, 4},
581  {s_Goal_Position, 116, sizeof(s_Goal_Position) - 1, 4},
582  {s_Realtime_Tick, 120, sizeof(s_Realtime_Tick) - 1, 2},
583  {s_Moving, 122, sizeof(s_Moving) - 1, 1},
584  {s_Moving_Status, 123, sizeof(s_Moving_Status) - 1, 1},
585  {s_Present_PWM, 124, sizeof(s_Present_PWM) - 1, 2},
586  {s_Present_Load, 126, sizeof(s_Present_Load) - 1, 2},
587  {s_Present_Velocity, 128, sizeof(s_Present_Velocity) - 1, 4},
588  {s_Present_Position, 132, sizeof(s_Present_Position) - 1, 4},
589  {s_Velocity_Trajectory, 136, sizeof(s_Velocity_Trajectory) - 1, 4},
590  {s_Position_Trajectory, 140, sizeof(s_Position_Trajectory) - 1, 4},
591  {s_Present_Input_Voltage, 144, sizeof(s_Present_Input_Voltage) - 1, 2},
592  {s_Present_Temperature, 146, sizeof(s_Present_Temperature) - 1, 1}};
593 
594 #define COUNT_XL_ITEMS (sizeof(items_XL) / sizeof(items_XL[0]))
595 
596 static const ModelInfo info_XL = {0.229,
597  0,
598  2048,
599  4096,
600  -3.14159265,
601  3.14159265};
602 
603 //---------------------------------------------------------
604 // XM - (num == XM430_W210 || num == XM430_W350)
605 //---------------------------------------------------------
606 static const ControlItem items_XM[]{
607  {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2},
608  {s_Firmware_Version, 6, sizeof(s_Firmware_Version) - 1, 1},
609  {s_ID, 7, sizeof(s_ID) - 1, 1},
610  {s_Baud_Rate, 8, sizeof(s_Baud_Rate) - 1, 1},
611  {s_Return_Delay_Time, 9, sizeof(s_Return_Delay_Time) - 1, 1},
612  {s_Drive_Mode, 10, sizeof(s_Drive_Mode) - 1, 1},
613  {s_Operating_Mode, 11, sizeof(s_Operating_Mode) - 1, 1},
614  {s_Secondary_ID, 12, sizeof(s_Secondary_ID) - 1, 1},
615  {s_Protocol_Version, 13, sizeof(s_Protocol_Version) - 1, 1},
616  {s_Homing_Offset, 20, sizeof(s_Homing_Offset) - 1, 4},
617  {s_Moving_Threshold, 24, sizeof(s_Moving_Threshold) - 1, 4},
618  {s_Temperature_Limit, 31, sizeof(s_Temperature_Limit) - 1, 1},
619  {s_Max_Voltage_Limit, 32, sizeof(s_Max_Voltage_Limit) - 1, 2},
620  {s_Min_Voltage_Limit, 34, sizeof(s_Min_Voltage_Limit) - 1, 2},
621  {s_PWM_Limit, 36, sizeof(s_PWM_Limit) - 1, 2},
622  {s_Current_Limit, 38, sizeof(s_Current_Limit) - 1, 2},
623  {s_Acceleration_Limit, 40, sizeof(s_Acceleration_Limit) - 1, 4},
624  {s_Velocity_Limit, 44, sizeof(s_Velocity_Limit) - 1, 4},
625  {s_Max_Position_Limit, 48, sizeof(s_Max_Position_Limit) - 1, 4},
626  {s_Min_Position_Limit, 52, sizeof(s_Min_Position_Limit) - 1, 4},
627  {s_Shutdown, 63, sizeof(s_Shutdown) - 1, 1},
628 
629  {s_Torque_Enable, 64, sizeof(s_Torque_Enable) - 1, 1},
630  {s_LED, 65, sizeof(s_LED) - 1, 1},
631  {s_Status_Return_Level, 68, sizeof(s_Status_Return_Level) - 1, 1},
633  {s_Hardware_Error_Status, 70, sizeof(s_Hardware_Error_Status) - 1, 1},
634  {s_Velocity_I_Gain, 76, sizeof(s_Velocity_I_Gain) - 1, 2},
635  {s_Velocity_P_Gain, 78, sizeof(s_Velocity_P_Gain) - 1, 2},
636  {s_Position_D_Gain, 80, sizeof(s_Position_D_Gain) - 1, 2},
637  {s_Position_I_Gain, 82, sizeof(s_Position_I_Gain) - 1, 2},
638  {s_Position_P_Gain, 84, sizeof(s_Position_P_Gain) - 1, 2},
639  {s_Feedforward_2nd_Gain, 88, sizeof(s_Feedforward_2nd_Gain) - 1, 2},
640  {s_Feedforward_1st_Gain, 90, sizeof(s_Feedforward_1st_Gain) - 1, 2},
641  {s_Bus_Watchdog, 98, sizeof(s_Bus_Watchdog) - 1, 1},
642  {s_Goal_PWM, 100, sizeof(s_Goal_PWM) - 1, 2},
643  {s_Goal_Current, 102, sizeof(s_Goal_Current) - 1, 2},
644  {s_Goal_Velocity, 104, sizeof(s_Goal_Velocity) - 1, 4},
645  {s_Profile_Acceleration, 108, sizeof(s_Profile_Acceleration) - 1, 4},
646  {s_Profile_Velocity, 112, sizeof(s_Profile_Velocity) - 1, 4},
647  {s_Goal_Position, 116, sizeof(s_Goal_Position) - 1, 4},
648  {s_Realtime_Tick, 120, sizeof(s_Realtime_Tick) - 1, 2},
649  {s_Moving, 122, sizeof(s_Moving) - 1, 1},
650  {s_Moving_Status, 123, sizeof(s_Moving_Status) - 1, 1},
651  {s_Present_PWM, 124, sizeof(s_Present_PWM) - 1, 2},
652  {s_Present_Current, 126, sizeof(s_Present_Current) - 1, 2},
653  {s_Present_Velocity, 128, sizeof(s_Present_Velocity) - 1, 4},
654  {s_Present_Position, 132, sizeof(s_Present_Position) - 1, 4},
655  {s_Velocity_Trajectory, 136, sizeof(s_Velocity_Trajectory) - 1, 4},
656  {s_Position_Trajectory, 140, sizeof(s_Position_Trajectory) - 1, 4},
657  {s_Present_Input_Voltage, 144, sizeof(s_Present_Input_Voltage) - 1, 2},
658  {s_Present_Temperature, 146, sizeof(s_Present_Temperature) - 1, 1}};
659 
660 #define COUNT_XM_ITEMS (sizeof(items_XM) / sizeof(items_XM[0]))
661 
662 static const ModelInfo info_XM = {0.229,
663  0,
664  2048,
665  4096,
666  -3.14159265,
667  3.14159265};
668 
669 //---------------------------------------------------------
670 // EXTXM - (num == XM540_W150 || num == XM540_W270)
671 //---------------------------------------------------------
672 static const ControlItem items_EXTXM[]{
673  {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2},
674  {s_Firmware_Version, 6, sizeof(s_Firmware_Version) - 1, 1},
675  {s_ID, 7, sizeof(s_ID) - 1, 1},
676  {s_Baud_Rate, 8, sizeof(s_Baud_Rate) - 1, 1},
677  {s_Return_Delay_Time, 9, sizeof(s_Return_Delay_Time) - 1, 1},
678  {s_Drive_Mode, 10, sizeof(s_Drive_Mode) - 1, 1},
679  {s_Operating_Mode, 11, sizeof(s_Operating_Mode) - 1, 1},
680  {s_Secondary_ID, 12, sizeof(s_Secondary_ID) - 1, 1},
681  {s_Protocol_Version, 13, sizeof(s_Protocol_Version) - 1, 1},
682  {s_Homing_Offset, 20, sizeof(s_Homing_Offset) - 1, 4},
683  {s_Moving_Threshold, 24, sizeof(s_Moving_Threshold) - 1, 4},
684  {s_Temperature_Limit, 31, sizeof(s_Temperature_Limit) - 1, 1},
685  {s_Max_Voltage_Limit, 32, sizeof(s_Max_Voltage_Limit) - 1, 2},
686  {s_Min_Voltage_Limit, 34, sizeof(s_Min_Voltage_Limit) - 1, 2},
687  {s_PWM_Limit, 36, sizeof(s_PWM_Limit) - 1, 2},
688  {s_Current_Limit, 38, sizeof(s_Current_Limit) - 1, 2},
689  {s_Acceleration_Limit, 40, sizeof(s_Acceleration_Limit) - 1, 4},
690  {s_Velocity_Limit, 44, sizeof(s_Velocity_Limit) - 1, 4},
691  {s_Max_Position_Limit, 48, sizeof(s_Max_Position_Limit) - 1, 4},
692  {s_Min_Position_Limit, 52, sizeof(s_Min_Position_Limit) - 1, 4},
693  {s_External_Port_Mode_1, 56, sizeof(s_External_Port_Mode_1) - 1, 1},
694  {s_External_Port_Mode_2, 57, sizeof(s_External_Port_Mode_2) - 1, 1},
695  {s_External_Port_Mode_3, 58, sizeof(s_External_Port_Mode_3) - 1, 1},
696  {s_Shutdown, 63, sizeof(s_Shutdown) - 1, 1},
697 
698  {s_Torque_Enable, 64, sizeof(s_Torque_Enable) - 1, 1},
699  {s_LED, 65, sizeof(s_LED) - 1, 1},
700  {s_Status_Return_Level, 68, sizeof(s_Status_Return_Level) - 1, 1},
702  {s_Hardware_Error_Status, 70, sizeof(s_Hardware_Error_Status) - 1, 1},
703  {s_Velocity_I_Gain, 76, sizeof(s_Velocity_I_Gain) - 1, 2},
704  {s_Velocity_P_Gain, 78, sizeof(s_Velocity_P_Gain) - 1, 2},
705  {s_Position_D_Gain, 80, sizeof(s_Position_D_Gain) - 1, 2},
706  {s_Position_I_Gain, 82, sizeof(s_Position_I_Gain) - 1, 2},
707  {s_Position_P_Gain, 84, sizeof(s_Position_P_Gain) - 1, 2},
708  {s_Feedforward_2nd_Gain, 88, sizeof(s_Feedforward_2nd_Gain) - 1, 2},
709  {s_Feedforward_1st_Gain, 90, sizeof(s_Feedforward_1st_Gain) - 1, 2},
710  {s_Bus_Watchdog, 98, sizeof(s_Bus_Watchdog) - 1, 1},
711  {s_Goal_PWM, 100, sizeof(s_Goal_PWM) - 1, 2},
712  {s_Goal_Current, 102, sizeof(s_Goal_Current) - 1, 2},
713  {s_Goal_Velocity, 104, sizeof(s_Goal_Velocity) - 1, 4},
714  {s_Profile_Acceleration, 108, sizeof(s_Profile_Acceleration) - 1, 4},
715  {s_Profile_Velocity, 112, sizeof(s_Profile_Velocity) - 1, 4},
716  {s_Goal_Position, 116, sizeof(s_Goal_Position) - 1, 4},
717  {s_Realtime_Tick, 120, sizeof(s_Realtime_Tick) - 1, 2},
718  {s_Moving, 122, sizeof(s_Moving) - 1, 1},
719  {s_Moving_Status, 123, sizeof(s_Moving_Status) - 1, 1},
720  {s_Present_PWM, 124, sizeof(s_Present_PWM) - 1, 2},
721  {s_Present_Current, 126, sizeof(s_Present_Current) - 1, 2},
722  {s_Present_Velocity, 128, sizeof(s_Present_Velocity) - 1, 4},
723  {s_Present_Position, 132, sizeof(s_Present_Position) - 1, 4},
724  {s_Velocity_Trajectory, 136, sizeof(s_Velocity_Trajectory) - 1, 4},
725  {s_Position_Trajectory, 140, sizeof(s_Position_Trajectory) - 1, 4},
726  {s_Present_Input_Voltage, 144, sizeof(s_Present_Input_Voltage) - 1, 2},
727  {s_Present_Temperature, 146, sizeof(s_Present_Temperature) - 1, 1},
728  {s_External_Port_Data_1, 152, sizeof(s_External_Port_Data_1) - 1, 2},
729  {s_External_Port_Data_2, 154, sizeof(s_External_Port_Data_2) - 1, 2},
730  {s_External_Port_Data_3, 156, sizeof(s_External_Port_Data_3) - 1, 2}};
731 
732 #define COUNT_EXTXM_ITEMS (sizeof(items_EXTXM) / sizeof(items_EXTXM[0]))
733 
734 static const ModelInfo info_EXTXM = {0.229,
735  0,
736  2048,
737  4096,
738  -3.14159265,
739  3.14159265};
740 
741 //---------------------------------------------------------
742 // XH - (num == XH430_V210 || num == XH430_V350 || num == XH430_W210 || num == XH430_W350)
743 //---------------------------------------------------------
744 static const ControlItem items_XH[]{
745  {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2},
746  {s_Firmware_Version, 6, sizeof(s_Firmware_Version) - 1, 1},
747  {s_ID, 7, sizeof(s_ID) - 1, 1},
748  {s_Baud_Rate, 8, sizeof(s_Baud_Rate) - 1, 1},
749  {s_Return_Delay_Time, 9, sizeof(s_Return_Delay_Time) - 1, 1},
750  {s_Drive_Mode, 10, sizeof(s_Drive_Mode) - 1, 1},
751  {s_Operating_Mode, 11, sizeof(s_Operating_Mode) - 1, 1},
752  {s_Secondary_ID, 12, sizeof(s_Secondary_ID) - 1, 1},
753  {s_Protocol_Version, 13, sizeof(s_Protocol_Version) - 1, 1},
754  {s_Homing_Offset, 20, sizeof(s_Homing_Offset) - 1, 4},
755  {s_Moving_Threshold, 24, sizeof(s_Moving_Threshold) - 1, 4},
756  {s_Temperature_Limit, 31, sizeof(s_Temperature_Limit) - 1, 1},
757  {s_Max_Voltage_Limit, 32, sizeof(s_Max_Voltage_Limit) - 1, 2},
758  {s_Min_Voltage_Limit, 34, sizeof(s_Min_Voltage_Limit) - 1, 2},
759  {s_PWM_Limit, 36, sizeof(s_PWM_Limit) - 1, 2},
760  {s_Current_Limit, 38, sizeof(s_Current_Limit) - 1, 2},
761  {s_Acceleration_Limit, 40, sizeof(s_Acceleration_Limit) - 1, 4},
762  {s_Velocity_Limit, 44, sizeof(s_Velocity_Limit) - 1, 4},
763  {s_Max_Position_Limit, 48, sizeof(s_Max_Position_Limit) - 1, 4},
764  {s_Min_Position_Limit, 52, sizeof(s_Min_Position_Limit) - 1, 4},
765  {s_Shutdown, 63, sizeof(s_Shutdown) - 1, 1},
766 
767  {s_Torque_Enable, 64, sizeof(s_Torque_Enable) - 1, 1},
768  {s_LED, 65, sizeof(s_LED) - 1, 1},
769  {s_Status_Return_Level, 68, sizeof(s_Status_Return_Level) - 1, 1},
771  {s_Hardware_Error_Status, 70, sizeof(s_Hardware_Error_Status) - 1, 1},
772  {s_Velocity_I_Gain, 76, sizeof(s_Velocity_I_Gain) - 1, 2},
773  {s_Velocity_P_Gain, 78, sizeof(s_Velocity_P_Gain) - 1, 2},
774  {s_Position_D_Gain, 80, sizeof(s_Position_D_Gain) - 1, 2},
775  {s_Position_I_Gain, 82, sizeof(s_Position_I_Gain) - 1, 2},
776  {s_Position_P_Gain, 84, sizeof(s_Position_P_Gain) - 1, 2},
777  {s_Feedforward_2nd_Gain, 88, sizeof(s_Feedforward_2nd_Gain) - 1, 2},
778  {s_Feedforward_1st_Gain, 90, sizeof(s_Feedforward_1st_Gain) - 1, 2},
779  {s_Bus_Watchdog, 98, sizeof(s_Bus_Watchdog) - 1, 1},
780  {s_Goal_PWM, 100, sizeof(s_Goal_PWM) - 1, 2},
781  {s_Goal_Current, 102, sizeof(s_Goal_Current) - 1, 2},
782  {s_Goal_Velocity, 104, sizeof(s_Goal_Velocity) - 1, 4},
783  {s_Profile_Acceleration, 108, sizeof(s_Profile_Acceleration) - 1, 4},
784  {s_Profile_Velocity, 112, sizeof(s_Profile_Velocity) - 1, 4},
785  {s_Goal_Position, 116, sizeof(s_Goal_Position) - 1, 4},
786  {s_Realtime_Tick, 120, sizeof(s_Realtime_Tick) - 1, 2},
787  {s_Moving, 122, sizeof(s_Moving) - 1, 1},
788  {s_Moving_Status, 123, sizeof(s_Moving_Status) - 1, 1},
789  {s_Present_PWM, 124, sizeof(s_Present_PWM) - 1, 2},
790  {s_Present_Current, 126, sizeof(s_Present_Current) - 1, 2},
791  {s_Present_Velocity, 128, sizeof(s_Present_Velocity) - 1, 4},
792  {s_Present_Position, 132, sizeof(s_Present_Position) - 1, 4},
793  {s_Velocity_Trajectory, 136, sizeof(s_Velocity_Trajectory) - 1, 4},
794  {s_Position_Trajectory, 140, sizeof(s_Position_Trajectory) - 1, 4},
795  {s_Present_Input_Voltage, 144, sizeof(s_Present_Input_Voltage) - 1, 2},
796  {s_Present_Temperature, 146, sizeof(s_Present_Temperature) - 1, 1}};
797 
798 #define COUNT_XH_ITEMS (sizeof(items_XH) / sizeof(items_XH[0]))
799 
800 static const ModelInfo info_XH = {0.229,
801  0,
802  2048,
803  4096,
804  -3.14159265,
805  3.14159265};
806 
807 //---------------------------------------------------------
808 // EXTXH - (num == XH540_W150 || num == XH540_W270 || num == XH540_V150 || num == XH540_W270)
809 //---------------------------------------------------------
810 static const ControlItem items_EXTXH[]{
811  {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2},
812  {s_Firmware_Version, 6, sizeof(s_Firmware_Version) - 1, 1},
813  {s_ID, 7, sizeof(s_ID) - 1, 1},
814  {s_Baud_Rate, 8, sizeof(s_Baud_Rate) - 1, 1},
815  {s_Return_Delay_Time, 9, sizeof(s_Return_Delay_Time) - 1, 1},
816  {s_Drive_Mode, 10, sizeof(s_Drive_Mode) - 1, 1},
817  {s_Operating_Mode, 11, sizeof(s_Operating_Mode) - 1, 1},
818  {s_Secondary_ID, 12, sizeof(s_Secondary_ID) - 1, 1},
819  {s_Protocol_Version, 13, sizeof(s_Protocol_Version) - 1, 1},
820  {s_Homing_Offset, 20, sizeof(s_Homing_Offset) - 1, 4},
821  {s_Moving_Threshold, 24, sizeof(s_Moving_Threshold) - 1, 4},
822  {s_Temperature_Limit, 31, sizeof(s_Temperature_Limit) - 1, 1},
823  {s_Max_Voltage_Limit, 32, sizeof(s_Max_Voltage_Limit) - 1, 2},
824  {s_Min_Voltage_Limit, 34, sizeof(s_Min_Voltage_Limit) - 1, 2},
825  {s_PWM_Limit, 36, sizeof(s_PWM_Limit) - 1, 2},
826  {s_Current_Limit, 38, sizeof(s_Current_Limit) - 1, 2},
827  {s_Acceleration_Limit, 40, sizeof(s_Acceleration_Limit) - 1, 4},
828  {s_Velocity_Limit, 44, sizeof(s_Velocity_Limit) - 1, 4},
829  {s_Max_Position_Limit, 48, sizeof(s_Max_Position_Limit) - 1, 4},
830  {s_Min_Position_Limit, 52, sizeof(s_Min_Position_Limit) - 1, 4},
831  {s_Shutdown, 63, sizeof(s_Shutdown) - 1, 1},
832 
833  {s_Torque_Enable, 64, sizeof(s_Torque_Enable) - 1, 1},
834  {s_LED, 65, sizeof(s_LED) - 1, 1},
835  {s_Status_Return_Level, 68, sizeof(s_Status_Return_Level) - 1, 1},
837  {s_Hardware_Error_Status, 70, sizeof(s_Hardware_Error_Status) - 1, 1},
838  {s_Velocity_I_Gain, 76, sizeof(s_Velocity_I_Gain) - 1, 2},
839  {s_Velocity_P_Gain, 78, sizeof(s_Velocity_P_Gain) - 1, 2},
840  {s_Position_D_Gain, 80, sizeof(s_Position_D_Gain) - 1, 2},
841  {s_Position_I_Gain, 82, sizeof(s_Position_I_Gain) - 1, 2},
842  {s_Position_P_Gain, 84, sizeof(s_Position_P_Gain) - 1, 2},
843  {s_Feedforward_2nd_Gain, 88, sizeof(s_Feedforward_2nd_Gain) - 1, 2},
844  {s_Feedforward_1st_Gain, 90, sizeof(s_Feedforward_1st_Gain) - 1, 2},
845  {s_Bus_Watchdog, 98, sizeof(s_Bus_Watchdog) - 1, 1},
846  {s_Goal_PWM, 100, sizeof(s_Goal_PWM) - 1, 2},
847  {s_Goal_Current, 102, sizeof(s_Goal_Current) - 1, 2},
848  {s_Goal_Velocity, 104, sizeof(s_Goal_Velocity) - 1, 4},
849  {s_Profile_Acceleration, 108, sizeof(s_Profile_Acceleration) - 1, 4},
850  {s_Profile_Velocity, 112, sizeof(s_Profile_Velocity) - 1, 4},
851  {s_Goal_Position, 116, sizeof(s_Goal_Position) - 1, 4},
852  {s_Realtime_Tick, 120, sizeof(s_Realtime_Tick) - 1, 2},
853  {s_Moving, 122, sizeof(s_Moving) - 1, 1},
854  {s_Moving_Status, 123, sizeof(s_Moving_Status) - 1, 1},
855  {s_Present_PWM, 124, sizeof(s_Present_PWM) - 1, 2},
856  {s_Present_Current, 126, sizeof(s_Present_Current) - 1, 2},
857  {s_Present_Velocity, 128, sizeof(s_Present_Velocity) - 1, 4},
858  {s_Present_Position, 132, sizeof(s_Present_Position) - 1, 4},
859  {s_Velocity_Trajectory, 136, sizeof(s_Velocity_Trajectory) - 1, 4},
860  {s_Position_Trajectory, 140, sizeof(s_Position_Trajectory) - 1, 4},
861  {s_Present_Input_Voltage, 144, sizeof(s_Present_Input_Voltage) - 1, 2},
862  {s_Present_Temperature, 146, sizeof(s_Present_Temperature) - 1, 1}};
863 
864 #define COUNT_EXTXH_ITEMS (sizeof(items_EXTXH) / sizeof(items_EXTXH[0]))
865 
866 static const ModelInfo info_EXTXH = {0.229,
867  0,
868  2048,
869  4096,
870  -3.14159265,
871  3.14159265};
872 
873 //---------------------------------------------------------
874 // XW - (num == XW540_T260 || XW540_T140)
875 //---------------------------------------------------------
876 static const ControlItem items_XW[]{
877  {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2},
878  {s_Firmware_Version, 6, sizeof(s_Firmware_Version) - 1, 1},
879  {s_ID, 7, sizeof(s_ID) - 1, 1},
880  {s_Baud_Rate, 8, sizeof(s_Baud_Rate) - 1, 1},
881  {s_Return_Delay_Time, 9, sizeof(s_Return_Delay_Time) - 1, 1},
882  {s_Drive_Mode, 10, sizeof(s_Drive_Mode) - 1, 1},
883  {s_Operating_Mode, 11, sizeof(s_Operating_Mode) - 1, 1},
884  {s_Secondary_ID, 12, sizeof(s_Secondary_ID) - 1, 1},
885  {s_Protocol_Version, 13, sizeof(s_Protocol_Version) - 1, 1},
886  {s_Homing_Offset, 20, sizeof(s_Homing_Offset) - 1, 4},
887  {s_Moving_Threshold, 24, sizeof(s_Moving_Threshold) - 1, 4},
888  {s_Temperature_Limit, 31, sizeof(s_Temperature_Limit) - 1, 1},
889  {s_Max_Voltage_Limit, 32, sizeof(s_Max_Voltage_Limit) - 1, 2},
890  {s_Min_Voltage_Limit, 34, sizeof(s_Min_Voltage_Limit) - 1, 2},
891  {s_PWM_Limit, 36, sizeof(s_PWM_Limit) - 1, 2},
892  {s_Current_Limit, 38, sizeof(s_Current_Limit) - 1, 2},
893  {s_Acceleration_Limit, 40, sizeof(s_Acceleration_Limit) - 1, 4},
894  {s_Velocity_Limit, 44, sizeof(s_Velocity_Limit) - 1, 4},
895  {s_Max_Position_Limit, 48, sizeof(s_Max_Position_Limit) - 1, 4},
896  {s_Min_Position_Limit, 52, sizeof(s_Min_Position_Limit) - 1, 4},
897  {s_Shutdown, 63, sizeof(s_Shutdown) - 1, 1},
898 
899  {s_Torque_Enable, 64, sizeof(s_Torque_Enable) - 1, 1},
900  {s_Status_Return_Level, 68, sizeof(s_Status_Return_Level) - 1, 1},
902  {s_Hardware_Error_Status, 70, sizeof(s_Hardware_Error_Status) - 1, 1},
903  {s_Velocity_I_Gain, 76, sizeof(s_Velocity_I_Gain) - 1, 2},
904  {s_Velocity_P_Gain, 78, sizeof(s_Velocity_P_Gain) - 1, 2},
905  {s_Position_D_Gain, 80, sizeof(s_Position_D_Gain) - 1, 2},
906  {s_Position_I_Gain, 82, sizeof(s_Position_I_Gain) - 1, 2},
907  {s_Position_P_Gain, 84, sizeof(s_Position_P_Gain) - 1, 2},
908  {s_Feedforward_2nd_Gain, 88, sizeof(s_Feedforward_2nd_Gain) - 1, 2},
909  {s_Feedforward_1st_Gain, 90, sizeof(s_Feedforward_1st_Gain) - 1, 2},
910  {s_Bus_Watchdog, 98, sizeof(s_Bus_Watchdog) - 1, 1},
911  {s_Goal_PWM, 100, sizeof(s_Goal_PWM) - 1, 2},
912  {s_Goal_Current, 102, sizeof(s_Goal_Current) - 1, 2},
913  {s_Goal_Velocity, 104, sizeof(s_Goal_Velocity) - 1, 4},
914  {s_Profile_Acceleration, 108, sizeof(s_Profile_Acceleration) - 1, 4},
915  {s_Profile_Velocity, 112, sizeof(s_Profile_Velocity) - 1, 4},
916  {s_Goal_Position, 116, sizeof(s_Goal_Position) - 1, 4},
917  {s_Realtime_Tick, 120, sizeof(s_Realtime_Tick) - 1, 2},
918  {s_Moving, 122, sizeof(s_Moving) - 1, 1},
919  {s_Moving_Status, 123, sizeof(s_Moving_Status) - 1, 1},
920  {s_Present_PWM, 124, sizeof(s_Present_PWM) - 1, 2},
921  {s_Present_Current, 126, sizeof(s_Present_Current) - 1, 2},
922  {s_Present_Velocity, 128, sizeof(s_Present_Velocity) - 1, 4},
923  {s_Present_Position, 132, sizeof(s_Present_Position) - 1, 4},
924  {s_Velocity_Trajectory, 136, sizeof(s_Velocity_Trajectory) - 1, 4},
925  {s_Position_Trajectory, 140, sizeof(s_Position_Trajectory) - 1, 4},
926  {s_Present_Input_Voltage, 144, sizeof(s_Present_Input_Voltage) - 1, 2},
927  {s_Present_Temperature, 146, sizeof(s_Present_Temperature) - 1, 1}};
928 
929 #define COUNT_XW_ITEMS (sizeof(items_XW) / sizeof(items_XW[0]))
930 
931 static const ModelInfo info_XW = {0.229,
932  0,
933  2048,
934  4096,
935  -3.14159265,
936  3.14159265};
937 
938 //---------------------------------------------------------
939 // PRO - (num == PRO_L42_10_S300_R)
940 //---------------------------------------------------------
941 static const ControlItem items_PRO[]{
942  {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2},
943  {s_Firmware_Version, 6, sizeof(s_Firmware_Version) - 1, 1},
944  {s_ID, 7, sizeof(s_ID) - 1, 1},
945  {s_Baud_Rate, 8, sizeof(s_Baud_Rate) - 1, 1},
946  {s_Return_Delay_Time, 9, sizeof(s_Return_Delay_Time) - 1, 1},
947  {s_Operating_Mode, 11, sizeof(s_Operating_Mode) - 1, 1},
948  {s_Moving_Threshold, 17, sizeof(s_Moving_Threshold) - 1, 4},
949  {s_Temperature_Limit, 21, sizeof(s_Temperature_Limit) - 1, 1},
950  {s_Max_Voltage_Limit, 22, sizeof(s_Max_Voltage_Limit) - 1, 2},
951  {s_Min_Voltage_Limit, 24, sizeof(s_Min_Voltage_Limit) - 1, 2},
952  {s_Acceleration_Limit, 26, sizeof(s_Acceleration_Limit) - 1, 4},
953  {s_Torque_Limit, 30, sizeof(s_Torque_Limit) - 1, 2},
954  {s_Velocity_Limit, 32, sizeof(s_Velocity_Limit) - 1, 4},
955  {s_Max_Position_Limit, 36, sizeof(s_Max_Position_Limit) - 1, 4},
956  {s_Min_Position_Limit, 40, sizeof(s_Min_Position_Limit) - 1, 4},
957  {s_External_Port_Mode_1, 44, sizeof(s_External_Port_Mode_1) - 1, 1},
958  {s_External_Port_Mode_2, 45, sizeof(s_External_Port_Mode_2) - 1, 1},
959  {s_External_Port_Mode_3, 46, sizeof(s_External_Port_Mode_3) - 1, 1},
960  {s_External_Port_Mode_4, 47, sizeof(s_External_Port_Mode_4) - 1, 1},
961  {s_Shutdown, 48, sizeof(s_Shutdown) - 1, 1},
962 
963  {s_Torque_Enable, 562, sizeof(s_Torque_Enable) - 1, 1},
964  {s_LED_RED, 563, sizeof(s_LED_RED) - 1, 1},
965  {s_LED_GREEN, 564, sizeof(s_LED_GREEN) - 1, 1},
966  {s_LED_BLUE, 565, sizeof(s_LED_BLUE) - 1, 1},
967  {s_Velocity_I_Gain, 586, sizeof(s_Velocity_I_Gain) - 1, 2},
968  {s_Velocity_P_Gain, 588, sizeof(s_Velocity_P_Gain) - 1, 2},
969  {s_Position_P_Gain, 594, sizeof(s_Position_P_Gain) - 1, 2},
970  {s_Goal_Position, 596, sizeof(s_Goal_Position) - 1, 4},
971  {s_Goal_Velocity, 600, sizeof(s_Goal_Velocity) - 1, 4},
972  {s_Goal_Torque, 604, sizeof(s_Goal_Torque) - 1, 2},
973  {s_Goal_Acceleration, 606, sizeof(s_Goal_Acceleration) - 1, 4},
974  {s_Moving, 610, sizeof(s_Moving) - 1, 1},
975  {s_Present_Position, 611, sizeof(s_Present_Position) - 1, 4},
976  {s_Present_Velocity, 615, sizeof(s_Present_Velocity) - 1, 4},
977  {s_Present_Current, 621, sizeof(s_Present_Current) - 1, 2},
978  {s_Present_Input_Voltage, 623, sizeof(s_Present_Input_Voltage) - 1, 2},
979  {s_Present_Temperature, 625, sizeof(s_Present_Temperature) - 1, 1},
980  {s_External_Port_Data_1, 626, sizeof(s_External_Port_Data_1) - 1, 2},
981  {s_External_Port_Data_2, 628, sizeof(s_External_Port_Data_2) - 1, 2},
982  {s_External_Port_Data_3, 630, sizeof(s_External_Port_Data_3) - 1, 2},
983  {s_External_Port_Data_4, 632, sizeof(s_External_Port_Data_4) - 1, 2},
984  {s_Registered_Instruction, 890, sizeof(s_Registered_Instruction) - 1, 1},
985  {s_Status_Return_Level, 891, sizeof(s_Status_Return_Level) - 1, 1},
986  {s_Hardware_Error_Status, 892, sizeof(s_Hardware_Error_Status) - 1, 1}};
987 
988 #define COUNT_PRO_ITEMS (sizeof(items_PRO) / sizeof(items_PRO[0]))
989 
990 static const ModelInfo info_PRO = {0.114,
991  -2048,
992  0,
993  2048,
994  -3.14159265,
995  3.14159265};
996 
997 //---------------------------------------------------------
998 // EXT PRO - All Other Pros...
999 //---------------------------------------------------------
1000 static const ControlItem items_EXTPRO[]{
1001  {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2},
1002  {s_Firmware_Version, 6, sizeof(s_Firmware_Version) - 1, 1},
1003  {s_ID, 7, sizeof(s_ID) - 1, 1},
1004  {s_Baud_Rate, 8, sizeof(s_Baud_Rate) - 1, 1},
1005  {s_Return_Delay_Time, 9, sizeof(s_Return_Delay_Time) - 1, 1},
1006  {s_Operating_Mode, 11, sizeof(s_Operating_Mode) - 1, 1},
1007  {s_Homing_Offset, 13, sizeof(s_Homing_Offset) - 1, 4},
1008  {s_Moving_Threshold, 17, sizeof(s_Moving_Threshold) - 1, 4},
1009  {s_Temperature_Limit, 21, sizeof(s_Temperature_Limit) - 1, 1},
1010  {s_Max_Voltage_Limit, 22, sizeof(s_Max_Voltage_Limit) - 1, 2},
1011  {s_Min_Voltage_Limit, 24, sizeof(s_Min_Voltage_Limit) - 1, 2},
1012  {s_Acceleration_Limit, 26, sizeof(s_Acceleration_Limit) - 1, 4},
1013  {s_Torque_Limit, 30, sizeof(s_Torque_Limit) - 1, 2},
1014  {s_Velocity_Limit, 32, sizeof(s_Velocity_Limit) - 1, 4},
1015  {s_Max_Position_Limit, 36, sizeof(s_Max_Position_Limit) - 1, 4},
1016  {s_Min_Position_Limit, 40, sizeof(s_Min_Position_Limit) - 1, 4},
1017  {s_External_Port_Mode_1, 44, sizeof(s_External_Port_Mode_1) - 1, 1},
1018  {s_External_Port_Mode_2, 45, sizeof(s_External_Port_Mode_2) - 1, 1},
1019  {s_External_Port_Mode_3, 46, sizeof(s_External_Port_Mode_3) - 1, 1},
1020  {s_External_Port_Mode_4, 47, sizeof(s_External_Port_Mode_4) - 1, 1},
1021  {s_Shutdown, 48, sizeof(s_Shutdown) - 1, 1},
1022 
1023  {s_Torque_Enable, 562, sizeof(s_Torque_Enable) - 1, 1},
1024  {s_LED_RED, 563, sizeof(s_LED_RED) - 1, 1},
1025  {s_LED_GREEN, 564, sizeof(s_LED_GREEN) - 1, 1},
1026  {s_LED_BLUE, 565, sizeof(s_LED_BLUE) - 1, 1},
1027  {s_Velocity_I_Gain, 586, sizeof(s_Velocity_I_Gain) - 1, 2},
1028  {s_Velocity_P_Gain, 588, sizeof(s_Velocity_P_Gain) - 1, 2},
1029  {s_Position_P_Gain, 594, sizeof(s_Position_P_Gain) - 1, 2},
1030  {s_Goal_Position, 596, sizeof(s_Goal_Position) - 1, 4},
1031  {s_Goal_Velocity, 600, sizeof(s_Goal_Velocity) - 1, 4},
1032  {s_Goal_Torque, 604, sizeof(s_Goal_Torque) - 1, 2},
1033  {s_Goal_Acceleration, 606, sizeof(s_Goal_Acceleration) - 1, 4},
1034  {s_Moving, 610, sizeof(s_Moving) - 1, 1},
1035  {s_Present_Position, 611, sizeof(s_Present_Position) - 1, 4},
1036  {s_Present_Velocity, 615, sizeof(s_Present_Velocity) - 1, 4},
1037  {s_Present_Current, 621, sizeof(s_Present_Current) - 1, 2},
1038  {s_Present_Input_Voltage, 623, sizeof(s_Present_Input_Voltage) - 1, 2},
1039  {s_Present_Temperature, 625, sizeof(s_Present_Temperature) - 1, 1},
1040  {s_External_Port_Data_1, 626, sizeof(s_External_Port_Data_1) - 1, 2},
1041  {s_External_Port_Data_2, 628, sizeof(s_External_Port_Data_2) - 1, 2},
1042  {s_External_Port_Data_3, 630, sizeof(s_External_Port_Data_3) - 1, 2},
1043  {s_External_Port_Data_4, 632, sizeof(s_External_Port_Data_4) - 1, 2},
1044  {s_Registered_Instruction, 890, sizeof(s_Registered_Instruction) - 1, 1},
1045  {s_Status_Return_Level, 891, sizeof(s_Status_Return_Level) - 1, 1},
1046  {s_Hardware_Error_Status, 892, sizeof(s_Hardware_Error_Status) - 1, 1}};
1047 
1048 #define COUNT_EXTPRO_ITEMS (sizeof(items_EXTPRO) / sizeof(items_EXTPRO[0]))
1049 
1050 static const ModelInfo info_EXTPRO[] = {
1051  {0.00249657, -144197, 0, 144197, -3.14159265, 3.14159265}, // PRO_L54_30_S400_R
1052  {0.00199234, -180692, 0, 180692, -3.14159265, 3.14159265}, // PRO_L54_30_S500_R, PRO_L54_50_S500_R
1053  {0.00346667, -103846, 0, 103846, -3.14159265, 3.14159265}, // PRO_L54_50_S290_R
1054  {0.00389076, -131593, 0, 131593, -3.14159265, 3.14159265}, // PRO_M42_10_S260_R
1055  {0.00397746, -125708, 0, 125708, -3.14159265, 3.14159265}, // PRO_M54_40_S250_R, PRO_M54_60_S250_R
1056  {0.00329218, -151875, 0, 151875, -3.14159265, 3.14159265}, // PRO_H42_20_S300_R
1057  {0.00199234, -250961, 0, 250961, -3.14159265, 3.14159265}}; // PRO_H54_100_S500_R, PRO_H54_200_S500_R
1058 
1059 //---------------------------------------------------------
1060 // EXT PRO (A Firmware_Version)
1061 //---------------------------------------------------------
1063  {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2},
1064  {s_Firmware_Version, 6, sizeof(s_Firmware_Version) - 1, 1},
1065  {s_ID, 7, sizeof(s_ID) - 1, 1},
1066  {s_Baud_Rate, 8, sizeof(s_Baud_Rate) - 1, 1},
1067  {s_Return_Delay_Time, 9, sizeof(s_Return_Delay_Time) - 1, 1},
1068  {s_Operating_Mode, 11, sizeof(s_Operating_Mode) - 1, 1},
1069  {s_Homing_Offset, 20, sizeof(s_Homing_Offset) - 1, 4},
1070  {s_Moving_Threshold, 24, sizeof(s_Moving_Threshold) - 1, 4},
1071  {s_Temperature_Limit, 31, sizeof(s_Temperature_Limit) - 1, 1},
1072  {s_Max_Voltage_Limit, 32, sizeof(s_Max_Voltage_Limit) - 1, 2},
1073  {s_Min_Voltage_Limit, 34, sizeof(s_Min_Voltage_Limit) - 1, 2},
1074  {s_Current_Limit, 38, sizeof(s_Current_Limit) - 1, 2},
1075  {s_Acceleration_Limit, 40, sizeof(s_Acceleration_Limit) - 1, 4},
1076  {s_Velocity_Limit, 44, sizeof(s_Velocity_Limit) - 1, 4},
1077  {s_Max_Position_Limit, 48, sizeof(s_Max_Position_Limit) - 1, 4},
1078  {s_Min_Position_Limit, 52, sizeof(s_Min_Position_Limit) - 1, 4},
1079  {s_External_Port_Mode_1, 56, sizeof(s_External_Port_Mode_1) - 1, 1},
1080  {s_External_Port_Mode_2, 57, sizeof(s_External_Port_Mode_2) - 1, 1},
1081  {s_External_Port_Mode_3, 58, sizeof(s_External_Port_Mode_3) - 1, 1},
1082  {s_External_Port_Mode_4, 59, sizeof(s_External_Port_Mode_4) - 1, 1},
1083  {s_Shutdown, 63, sizeof(s_Shutdown) - 1, 1},
1084 
1085  {s_Torque_Enable, 512, sizeof(s_Torque_Enable) - 1, 1},
1086  {s_LED_RED, 513, sizeof(s_LED_RED) - 1, 1},
1087  {s_LED_GREEN, 514, sizeof(s_LED_GREEN) - 1, 1},
1088  {s_LED_BLUE, 515, sizeof(s_LED_BLUE) - 1, 1},
1089  {s_Velocity_I_Gain, 524, sizeof(s_Velocity_I_Gain) - 1, 2},
1090  {s_Velocity_P_Gain, 526, sizeof(s_Velocity_P_Gain) - 1, 2},
1091  {s_Position_D_Gain, 528, sizeof(s_Position_D_Gain) - 1, 2},
1092  {s_Position_P_Gain, 532, sizeof(s_Position_P_Gain) - 1, 2},
1093  {s_Position_I_Gain, 530, sizeof(s_Position_I_Gain) - 1, 2},
1094  {s_Goal_Position, 564, sizeof(s_Goal_Position) - 1, 4},
1095  {s_Goal_Velocity, 552, sizeof(s_Goal_Velocity) - 1, 4},
1096  {s_Goal_Current, 604, sizeof(s_Goal_Current) - 1, 2},
1097  {s_Profile_Acceleration, 556, sizeof(s_Profile_Acceleration) - 1, 4},
1098  {s_Profile_Velocity, 560, sizeof(s_Profile_Velocity) - 1, 4},
1099  {s_Moving, 570, sizeof(s_Moving) - 1, 1},
1100  {s_Present_Position, 580, sizeof(s_Present_Position) - 1, 4},
1101  {s_Present_Velocity, 576, sizeof(s_Present_Velocity) - 1, 4},
1102  {s_Present_Current, 574, sizeof(s_Present_Current) - 1, 2},
1103  {s_Present_Input_Voltage, 592, sizeof(s_Present_Input_Voltage) - 1, 2},
1104  {s_Present_Temperature, 594, sizeof(s_Present_Temperature) - 1, 1},
1105  {s_External_Port_Data_1, 600, sizeof(s_External_Port_Data_1) - 1, 2},
1106  {s_External_Port_Data_2, 602, sizeof(s_External_Port_Data_2) - 1, 2},
1107  {s_External_Port_Data_3, 604, sizeof(s_External_Port_Data_3) - 1, 2},
1108  {s_External_Port_Data_4, 606, sizeof(s_External_Port_Data_4) - 1, 2}};
1109 
1110 #define COUNT_EXTPRO_A_ITEMS (sizeof(items_EXTPRO_A) / sizeof(items_EXTPRO_A[0]))
1111 
1112 static const ModelInfo info_EXTPRO_A[] = {
1113  {0.00389076, -131593, 0, 131593, -3.14159265, 3.14159265}, // PRO_M42_10_S260_R_A
1114  {0.00397746, -125708, 0, 125708, -3.14159265, 3.14159265}, // PRO_M54_40_S250_R_A, PRO_M54_60_S250_R_A
1115  {0.00329218, -151875, 0, 151875, -3.14159265, 3.14159265}, // PRO_H42_20_S300_R_A
1116  {0.00199234, -250961, 0, 250961, -3.14159265, 3.14159265}}; // PRO_H54_100_S500_R_A, PRO_H54_200_S500_R_A
1117 
1118 //---------------------------------------------------------
1119 // PRO PLUS - (num == PRO_H42P_020_S300_R, PRO_H54P_100_S500_R, PRO_H54P_200_S500_R)
1120 //---------------------------------------------------------
1122  {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2},
1123  {s_Firmware_Version, 6, sizeof(s_Firmware_Version) - 1, 1},
1124  {s_ID, 7, sizeof(s_ID) - 1, 1},
1125  {s_Baud_Rate, 8, sizeof(s_Baud_Rate) - 1, 1},
1126  {s_Return_Delay_Time, 9, sizeof(s_Return_Delay_Time) - 1, 1},
1127  {s_Drive_Mode, 10, sizeof(s_Drive_Mode) - 1, 1},
1128  {s_Operating_Mode, 11, sizeof(s_Operating_Mode) - 1, 1},
1129  {s_Secondary_ID, 12, sizeof(s_Secondary_ID) - 1, 1},
1130  {s_Homing_Offset, 20, sizeof(s_Homing_Offset) - 1, 4},
1131  {s_Moving_Threshold, 24, sizeof(s_Moving_Threshold) - 1, 4},
1132  {s_Temperature_Limit, 31, sizeof(s_Temperature_Limit) - 1, 1},
1133  {s_Max_Voltage_Limit, 32, sizeof(s_Max_Voltage_Limit) - 1, 2},
1134  {s_Min_Voltage_Limit, 34, sizeof(s_Min_Voltage_Limit) - 1, 2},
1135  {s_PWM_Limit, 36, sizeof(s_PWM_Limit) - 1, 2},
1136  {s_Current_Limit, 38, sizeof(s_Current_Limit) - 1, 2},
1137  {s_Acceleration_Limit, 40, sizeof(s_Acceleration_Limit) - 1, 4},
1138  {s_Velocity_Limit, 44, sizeof(s_Velocity_Limit) - 1, 4},
1139  {s_Max_Position_Limit, 48, sizeof(s_Max_Position_Limit) - 1, 4},
1140  {s_Min_Position_Limit, 52, sizeof(s_Min_Position_Limit) - 1, 4},
1141  {s_External_Port_Mode_1, 56, sizeof(s_External_Port_Mode_1) - 1, 1},
1142  {s_External_Port_Mode_2, 57, sizeof(s_External_Port_Mode_2) - 1, 1},
1143  {s_External_Port_Mode_3, 58, sizeof(s_External_Port_Mode_3) - 1, 1},
1144  {s_External_Port_Mode_4, 59, sizeof(s_External_Port_Mode_4) - 1, 1},
1145  {s_Shutdown, 63, sizeof(s_Shutdown) - 1, 1},
1146 
1147  {s_Torque_Enable, 512, sizeof(s_Torque_Enable) - 1, 1},
1148  {s_LED_RED, 513, sizeof(s_LED_RED) - 1, 1},
1149  {s_LED_GREEN, 514, sizeof(s_LED_GREEN) - 1, 1},
1150  {s_LED_BLUE, 515, sizeof(s_LED_BLUE) - 1, 1},
1151  {s_Status_Return_Level, 516, sizeof(s_Status_Return_Level) - 1, 1},
1152  {s_Registered_Instruction, 517, sizeof(s_Registered_Instruction) - 1, 1},
1153  {s_Hardware_Error_Status, 518, sizeof(s_Hardware_Error_Status) - 1, 1},
1154  {s_Velocity_I_Gain, 524, sizeof(s_Velocity_I_Gain) - 1, 2},
1155  {s_Velocity_P_Gain, 526, sizeof(s_Velocity_P_Gain) - 1, 2},
1156  {s_Position_D_Gain, 528, sizeof(s_Position_D_Gain) - 1, 2},
1157  {s_Position_I_Gain, 530, sizeof(s_Position_I_Gain) - 1, 2},
1158  {s_Position_P_Gain, 532, sizeof(s_Position_P_Gain) - 1, 2},
1159  {s_Feedforward_2nd_Gain, 536, sizeof(s_Feedforward_2nd_Gain) - 1, 2},
1160  {s_Feedforward_1st_Gain, 538, sizeof(s_Feedforward_1st_Gain) - 1, 2},
1161  {s_Bus_Watchdog, 546, sizeof(s_Bus_Watchdog) - 1, 1},
1162  {s_Goal_PWM, 548, sizeof(s_Goal_PWM) - 1, 2},
1163  {s_Goal_Current, 550, sizeof(s_Goal_Current) - 1, 2},
1164  {s_Goal_Velocity, 552, sizeof(s_Goal_Velocity) - 1, 4},
1165  {s_Profile_Acceleration, 556, sizeof(s_Profile_Acceleration) - 1, 4},
1166  {s_Profile_Velocity, 560, sizeof(s_Profile_Velocity) - 1, 4},
1167  {s_Goal_Position, 564, sizeof(s_Goal_Position) - 1, 4},
1168  {s_Realtime_Tick, 568, sizeof(s_Realtime_Tick) - 1, 2},
1169  {s_Moving, 570, sizeof(s_Moving) - 1, 1},
1170  {s_Moving_Status, 571, sizeof(s_Moving) - 1, 1},
1171  {s_Present_PWM, 572, sizeof(s_Present_PWM) - 1, 2},
1172  {s_Present_Current, 574, sizeof(s_Present_Current) - 1, 2},
1173  {s_Present_Velocity, 576, sizeof(s_Present_Velocity) - 1, 4},
1174  {s_Present_Position, 580, sizeof(s_Present_Position) - 1, 4},
1175  {s_Velocity_Trajectory, 584, sizeof(s_Velocity_Trajectory) - 1, 4},
1176  {s_Position_Trajectory, 588, sizeof(s_Position_Trajectory) - 1, 4},
1177  {s_Present_Input_Voltage, 592, sizeof(s_Present_Input_Voltage) - 1, 2},
1178  {s_Present_Temperature, 594, sizeof(s_Present_Temperature) - 1, 1},
1179  {s_External_Port_Data_1, 600, sizeof(s_External_Port_Data_1) - 1, 2},
1180  {s_External_Port_Data_2, 602, sizeof(s_External_Port_Data_2) - 1, 2},
1181  {s_External_Port_Data_3, 604, sizeof(s_External_Port_Data_3) - 1, 2},
1182  {s_External_Port_Data_4, 606, sizeof(s_External_Port_Data_4) - 1, 2}};
1183 
1184 #define COUNT_EXTPRO_PLUS_ITEMS (sizeof(items_PRO_PLUS) / sizeof(items_PRO_PLUS[0]))
1185 
1186 static const ModelInfo info_PRO_PLUS[] = {
1187  {0.01, -251173, 0, 251173, -3.14159265, 3.14159265}, // PRO_PLUS_M42P_010_S260_R
1188  {0.01, -251173, 0, 251173, -3.14159265, 3.14159265}, // PRO_PLUS_M54P_040_S250_R
1189  {0.01, -262931, 0, 262931, -3.14159265, 3.14159265}, // PRO_PLUS_M54P_060_S250_R
1190  {0.01, -303454, 0, 303454, -3.14159265, 3.14159265}, // PRO_PLUS_H42P_020_S300_R
1191  {0.01, -501433, 0, 501433, -3.14159265, 3.14159265}, // PRO_PLUS_H54P_100_S500_R
1192  {0.01, -501433, 0, 501433, -3.14159265, 3.14159265}}; // PRO_PLUS_H54P_200_S500_R
1193 
1194 //---------------------------------------------------------
1195 // Gripper - (num == RH_P12_RN)
1196 //---------------------------------------------------------
1198  {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2},
1199  {s_Firmware_Version, 6, sizeof(s_Firmware_Version) - 1, 1},
1200  {s_ID, 7, sizeof(s_ID) - 1, 1},
1201  {s_Baud_Rate, 8, sizeof(s_Baud_Rate) - 1, 1},
1202  {s_Return_Delay_Time, 9, sizeof(s_Return_Delay_Time) - 1, 1},
1203  {s_Operating_Mode, 11, sizeof(s_Operating_Mode) - 1, 1},
1204  {s_Homing_Offset, 13, sizeof(s_Homing_Offset) - 1, 4},
1205  {s_Moving_Threshold, 17, sizeof(s_Moving_Threshold) - 1, 4},
1206  {s_Temperature_Limit, 21, sizeof(s_Temperature_Limit) - 1, 1},
1207  {s_Max_Voltage_Limit, 22, sizeof(s_Max_Voltage_Limit) - 1, 2},
1208  {s_Min_Voltage_Limit, 24, sizeof(s_Min_Voltage_Limit) - 1, 2},
1209  {s_Acceleration_Limit, 26, sizeof(s_Acceleration_Limit) - 1, 4},
1210  {s_Torque_Limit, 30, sizeof(s_Torque_Limit) - 1, 2},
1211  {s_Velocity_Limit, 32, sizeof(s_Velocity_Limit) - 1, 4},
1212  {s_Max_Position_Limit, 36, sizeof(s_Max_Position_Limit) - 1, 4},
1213  {s_Min_Position_Limit, 40, sizeof(s_Min_Position_Limit) - 1, 4},
1214  {s_External_Port_Mode_1, 44, sizeof(s_External_Port_Mode_1) - 1, 1},
1215  {s_External_Port_Mode_2, 45, sizeof(s_External_Port_Mode_2) - 1, 1},
1216  {s_External_Port_Mode_3, 46, sizeof(s_External_Port_Mode_3) - 1, 1},
1217  {s_External_Port_Mode_4, 47, sizeof(s_External_Port_Mode_4) - 1, 1},
1218  {s_Shutdown, 48, sizeof(s_Shutdown) - 1, 1},
1219 
1220  {s_Torque_Enable, 562, sizeof(s_Torque_Enable) - 1, 1},
1221  {s_LED_RED, 563, sizeof(s_LED_RED) - 1, 1},
1222  {s_LED_GREEN, 564, sizeof(s_LED_GREEN) - 1, 1},
1223  {s_LED_BLUE, 565, sizeof(s_LED_BLUE) - 1, 1},
1224  {s_Velocity_I_Gain, 586, sizeof(s_Velocity_I_Gain) - 1, 2},
1225  {s_Velocity_P_Gain, 588, sizeof(s_Velocity_P_Gain) - 1, 2},
1226  {s_Position_P_Gain, 594, sizeof(s_Position_P_Gain) - 1, 2},
1227  {s_Goal_Position, 596, sizeof(s_Goal_Position) - 1, 4},
1228  {s_Goal_Velocity, 600, sizeof(s_Goal_Velocity) - 1, 4},
1229  {s_Goal_Torque, 604, sizeof(s_Goal_Torque) - 1, 2},
1230  {s_Goal_Acceleration, 606, sizeof(s_Goal_Acceleration) - 1, 4},
1231  {s_Moving, 610, sizeof(s_Moving) - 1, 1},
1232  {s_Present_Position, 611, sizeof(s_Present_Position) - 1, 4},
1233  {s_Present_Velocity, 615, sizeof(s_Present_Velocity) - 1, 4},
1234  {s_Present_Current, 621, sizeof(s_Present_Current) - 1, 2},
1235  {s_Present_Input_Voltage, 623, sizeof(s_Present_Input_Voltage) - 1, 2},
1236  {s_Present_Temperature, 625, sizeof(s_Present_Temperature) - 1, 1},
1237  {s_External_Port_Data_1, 626, sizeof(s_External_Port_Data_1) - 1, 2},
1238  {s_External_Port_Data_2, 628, sizeof(s_External_Port_Data_2) - 1, 2},
1239  {s_External_Port_Data_3, 630, sizeof(s_External_Port_Data_3) - 1, 2},
1240  {s_External_Port_Data_4, 632, sizeof(s_External_Port_Data_4) - 1, 2},
1241  {s_Registered_Instruction, 890, sizeof(s_Registered_Instruction) - 1, 1},
1242  {s_Status_Return_Level, 891, sizeof(s_Status_Return_Level) - 1, 1},
1243  {s_Hardware_Error_Status, 892, sizeof(s_Hardware_Error_Status) - 1, 1}};
1244 #define COUNT_Gripper_ITEMS (sizeof(items_Gripper) / sizeof(items_Gripper[0]))
1245 
1246 static const ModelInfo info_Gripper = {0.01,
1247  0,
1248  0,
1249  1150,
1250  0,
1251  1.7631835937};
1252 
1253 //---------------------------------------------------------
1254 // Gripper A Firmware - (num == RH_P12_RN_A)
1255 //---------------------------------------------------------
1257  {s_Model_Number, 0, sizeof(s_Model_Number) - 1, 2},
1258  {s_Firmware_Version, 6, sizeof(s_Firmware_Version) - 1, 1},
1259  {s_ID, 7, sizeof(s_ID) - 1, 1},
1260  {s_Baud_Rate, 8, sizeof(s_Baud_Rate) - 1, 1},
1261  {s_Return_Delay_Time, 9, sizeof(s_Return_Delay_Time) - 1, 1},
1262  {s_Drive_Mode, 10, sizeof(s_Drive_Mode) - 1, 1},
1263  {s_Operating_Mode, 11, sizeof(s_Operating_Mode) - 1, 1},
1264  {s_Secondary_ID, 12, sizeof(s_Secondary_ID) - 1, 1},
1265  {s_Homing_Offset, 20, sizeof(s_Homing_Offset) - 1, 4},
1266  {s_Moving_Threshold, 24, sizeof(s_Moving_Threshold) - 1, 4},
1267  {s_Temperature_Limit, 31, sizeof(s_Temperature_Limit) - 1, 1},
1268  {s_Max_Voltage_Limit, 32, sizeof(s_Max_Voltage_Limit) - 1, 2},
1269  {s_Min_Voltage_Limit, 34, sizeof(s_Min_Voltage_Limit) - 1, 2},
1270  {s_PWM_Limit, 36, sizeof(s_PWM_Limit) - 1, 2},
1271  {s_Current_Limit, 38, sizeof(s_Current_Limit) - 1, 2},
1272  {s_Acceleration_Limit, 40, sizeof(s_Acceleration_Limit) - 1, 4},
1273  {s_Velocity_Limit, 44, sizeof(s_Velocity_Limit) - 1, 4},
1274  {s_Max_Position_Limit, 48, sizeof(s_Max_Position_Limit) - 1, 4},
1275  {s_Min_Position_Limit, 52, sizeof(s_Min_Position_Limit) - 1, 4},
1276  {s_External_Port_Mode_1, 56, sizeof(s_External_Port_Mode_1) - 1, 1},
1277  {s_External_Port_Mode_2, 57, sizeof(s_External_Port_Mode_2) - 1, 1},
1278  {s_External_Port_Mode_3, 58, sizeof(s_External_Port_Mode_3) - 1, 1},
1279  {s_External_Port_Mode_4, 59, sizeof(s_External_Port_Mode_4) - 1, 1},
1280  {s_Shutdown, 63, sizeof(s_Shutdown) - 1, 1},
1281 
1282  {s_Torque_Enable, 512, sizeof(s_Torque_Enable) - 1, 1},
1283  {s_LED_RED, 513, sizeof(s_LED_RED) - 1, 1},
1284  {s_LED_GREEN, 514, sizeof(s_LED_GREEN) - 1, 1},
1285  {s_LED_BLUE, 515, sizeof(s_LED_BLUE) - 1, 1},
1286  {s_Status_Return_Level, 516, sizeof(s_Status_Return_Level) - 1, 1},
1287  {s_Registered_Instruction, 517, sizeof(s_Registered_Instruction) - 1, 1},
1288  {s_Hardware_Error_Status, 518, sizeof(s_Hardware_Error_Status) - 1, 1},
1289  {s_Velocity_I_Gain, 524, sizeof(s_Velocity_I_Gain) - 1, 2},
1290  {s_Velocity_P_Gain, 526, sizeof(s_Velocity_P_Gain) - 1, 2},
1291  {s_Position_D_Gain, 528, sizeof(s_Position_D_Gain) - 1, 2},
1292  {s_Position_I_Gain, 530, sizeof(s_Position_I_Gain) - 1, 2},
1293  {s_Position_P_Gain, 532, sizeof(s_Position_P_Gain) - 1, 2},
1294  {s_Feedforward_2nd_Gain, 536, sizeof(s_Feedforward_2nd_Gain) - 1, 2},
1295  {s_Feedforward_1st_Gain, 538, sizeof(s_Feedforward_1st_Gain) - 1, 2},
1296  {s_Bus_Watchdog, 546, sizeof(s_Bus_Watchdog) - 1, 1},
1297  {s_Goal_PWM, 548, sizeof(s_Goal_PWM) - 1, 2},
1298  {s_Goal_Current, 550, sizeof(s_Goal_Current) - 1, 2},
1299  {s_Goal_Velocity, 552, sizeof(s_Goal_Velocity) - 1, 4},
1300  {s_Profile_Acceleration, 556, sizeof(s_Profile_Acceleration) - 1, 4},
1301  {s_Profile_Velocity, 560, sizeof(s_Profile_Velocity) - 1, 4},
1302  {s_Goal_Position, 564, sizeof(s_Goal_Position) - 1, 4},
1303  {s_Realtime_Tick, 568, sizeof(s_Realtime_Tick) - 1, 2},
1304  {s_Moving, 570, sizeof(s_Moving) - 1, 1},
1305  {s_Moving_Status, 571, sizeof(s_Moving) - 1, 1},
1306  {s_Present_PWM, 572, sizeof(s_Present_PWM) - 1, 2},
1307  {s_Present_Current, 574, sizeof(s_Present_Current) - 1, 2},
1308  {s_Present_Velocity, 576, sizeof(s_Present_Velocity) - 1, 4},
1309  {s_Present_Position, 580, sizeof(s_Present_Position) - 1, 4},
1310  {s_Velocity_Trajectory, 584, sizeof(s_Velocity_Trajectory) - 1, 4},
1311  {s_Position_Trajectory, 588, sizeof(s_Position_Trajectory) - 1, 4},
1312  {s_Present_Input_Voltage, 592, sizeof(s_Present_Input_Voltage) - 1, 2},
1313  {s_Present_Temperature, 594, sizeof(s_Present_Temperature) - 1, 1},
1314  {s_External_Port_Data_1, 600, sizeof(s_External_Port_Data_1) - 1, 2},
1315  {s_External_Port_Data_2, 602, sizeof(s_External_Port_Data_2) - 1, 2},
1316  {s_External_Port_Data_3, 604, sizeof(s_External_Port_Data_3) - 1, 2},
1317  {s_External_Port_Data_4, 606, sizeof(s_External_Port_Data_4) - 1, 2}};
1318 #define COUNT_EXTGripper_ITEMS (sizeof(items_EXTGripper) / sizeof(items_EXTGripper[0]))
1319 
1320 static const ModelInfo info_EXTGripper = {0.01,
1321  0,
1322  0,
1323  1150,
1324  0,
1325  1.7631835937};
1326 
1327 //=========================================================
1328 // Get Servo control table for the specified servo type
1329 //=========================================================
1330 static uint8_t the_number_of_item = 0;
1331 const ControlItem *DynamixelItem::getControlTable(uint16_t model_number)
1332 {
1333  uint16_t num = model_number;
1334 
1335  const ControlItem *control_table;
1336  if (num == AX_12A || num == AX_12W || num == AX_18A)
1337  {
1338  control_table = items_AX;
1340  }
1341  else if (num == RX_10 || num == RX_24F || num == RX_28 || num == RX_64)
1342  {
1343  control_table = items_RX;
1345  }
1346  else if (num == EX_106)
1347  {
1348  control_table = items_EX;
1350  }
1351  else if (num == MX_12W || num == MX_28)
1352  {
1353  control_table = items_MX;
1355  }
1356  else if (num == MX_64 || num == MX_106)
1357  {
1358  control_table = items_EXTMX;
1360  }
1361  else if (num == MX_28_2)
1362  {
1363  control_table = items_MX2;
1365  }
1366  else if (num == MX_64_2 || num == MX_106_2)
1367  {
1368  control_table = items_EXTMX2;
1370  }
1371  else if (num == XL_320)
1372  {
1373  control_table = items_XL320;
1375  }
1376  else if (num == XL430_W250 || num == XL430_W250_2 || num == XC430_W150 || num == XC430_W240)
1377  {
1378  control_table = items_XL;
1380  }
1381  else if (num == XM430_W210 || num == XM430_W350)
1382  {
1383  control_table = items_XM;
1385  }
1386  else if (num == XM540_W150 || num == XM540_W270)
1387  {
1388  control_table = items_EXTXM;
1390  }
1391  else if (num == XH430_V210 || num == XH430_V350 || num == XH430_W210 || num == XH430_W350)
1392  {
1393  control_table = items_XH;
1395  }
1396  else if (num == XH540_W150 || num == XH540_W270 || num == XH540_V150 || num == XH540_V270)
1397  {
1398  control_table = items_EXTXH;
1400  }
1401  else if (num == XW540_T260 || num == XW540_T140)
1402  {
1403  control_table = items_XW;
1405  }
1406  else if (num == PRO_L42_10_S300_R)
1407  {
1408  control_table = items_PRO;
1410  }
1411  else if (num == PRO_L54_30_S400_R || num == PRO_L54_30_S500_R || num == PRO_L54_50_S290_R || num == PRO_L54_50_S500_R ||
1412  num == PRO_M42_10_S260_R || num == PRO_M54_40_S250_R || num == PRO_M54_60_S250_R ||
1413  num == PRO_H42_20_S300_R || num == PRO_H54_100_S500_R || num == PRO_H54_200_S500_R)
1414  {
1415  control_table = items_EXTPRO;
1417  }
1418  else if (num == PRO_M42_10_S260_R_A || num == PRO_M54_40_S250_R_A || num == PRO_M54_60_S250_R_A ||
1420  {
1421  control_table = items_EXTPRO_A;
1423  }
1424  else if (num == PRO_PLUS_M42P_010_S260_R || num == PRO_PLUS_M54P_040_S250_R || num == PRO_PLUS_M54P_060_S250_R ||
1426  {
1427  control_table = items_PRO_PLUS;
1429  }
1430  else if (num == RH_P12_RN)
1431  {
1432  control_table = items_Gripper;
1434  }
1435  else if (num == RH_P12_RN_A)
1436  {
1437  control_table = items_EXTGripper;
1439  }
1440  else
1441  {
1442  control_table = NULL;
1443  the_number_of_item = 0;
1444  }
1445 
1446  return control_table;
1447 }
1448 
1449 const ModelInfo *DynamixelItem::getModelInfo(uint16_t model_number)
1450 {
1451  uint16_t num = model_number;
1452 
1453  const ModelInfo *info;
1454 
1455  if (num == AX_12A || num == AX_12W || num == AX_18A)
1456  {
1457  info = &info_AX;
1458  }
1459  else if (num == RX_10 || num == RX_24F || num == RX_28 || num == RX_64)
1460  {
1461  info = &info_RX;
1462  }
1463  else if (num == EX_106)
1464  {
1465  info = &info_EX;
1466  }
1467  else if (num == MX_12W || num == MX_28)
1468  {
1469  info = &info_MX;
1470  }
1471  else if (num == MX_64 || num == MX_106)
1472  {
1473  info = &info_EXTMX;
1474  }
1475  else if (num == MX_28_2)
1476  {
1477  info = &info_MX2;
1478  }
1479  else if (num == MX_64_2 || num == MX_106_2)
1480  {
1481  info = &info_EXTMX2;
1482  }
1483  else if (num == XL_320)
1484  {
1485  info = &info_XL320;
1486  }
1487  else if (num == XL430_W250 || num == XL430_W250_2 || num == XC430_W150 || num == XC430_W240)
1488  {
1489  info = &info_XL;
1490  }
1491  else if (num == XM430_W210 || num == XM430_W350)
1492  {
1493  info = &info_XM;
1494  }
1495  else if (num == XM540_W150 || num == XM540_W270)
1496  {
1497  info = &info_EXTXM;
1498  }
1499  else if (num == XH430_V210 || num == XH430_V350 || num == XH430_W210 || num == XH430_W350)
1500  {
1501  info = &info_XH;
1502  }
1503  else if (num == XH540_W150 || num == XH540_W270 || num == XH540_V150 || num == XH540_V270)
1504  {
1505  info = &info_EXTXH;
1506  }
1507  else if (num == XW540_T260 || num == XW540_T140)
1508  {
1509  info = &info_XW;
1510  }
1511  else if (num == PRO_L42_10_S300_R)
1512  {
1513  info = &info_PRO;
1514  }
1515  else if (num == PRO_L54_30_S400_R)
1516  {
1517  info = &info_EXTPRO[0];
1518  }
1519  else if (num == PRO_L54_30_S500_R || num == PRO_L54_50_S500_R)
1520  {
1521  info = &info_EXTPRO[1];
1522  }
1523  else if (num == PRO_L54_50_S290_R)
1524  {
1525  info = &info_EXTPRO[2];
1526  }
1527  else if (num == PRO_M42_10_S260_R)
1528  {
1529  info = &info_EXTPRO[3];
1530  }
1531  else if (num == PRO_M54_40_S250_R || num == PRO_M54_60_S250_R)
1532  {
1533  info = &info_EXTPRO[4];
1534  }
1535  else if (num == PRO_H42_20_S300_R)
1536  {
1537  info = &info_EXTPRO[5];
1538  }
1539  else if (num == PRO_H54_100_S500_R || num == PRO_H54_200_S500_R)
1540  {
1541  info = &info_EXTPRO[6];
1542  }
1543  else if (num == PRO_M42_10_S260_R_A)
1544  {
1545  info = &info_EXTPRO_A[0];
1546  }
1547  else if (num == PRO_M54_40_S250_R_A || num == PRO_M54_60_S250_R_A)
1548  {
1549  info = &info_EXTPRO_A[1];
1550  }
1551  else if (num == PRO_H42_20_S300_R_A)
1552  {
1553  info = &info_EXTPRO_A[2];
1554  }
1555  else if (num == PRO_H54_100_S500_R_A || num == PRO_H54_200_S500_R_A)
1556  {
1557  info = &info_EXTPRO_A[3];
1558  }
1559  else if (num == PRO_PLUS_M42P_010_S260_R)
1560  {
1561  info = &info_PRO_PLUS[0];
1562  }
1563  else if (num == PRO_PLUS_M54P_040_S250_R)
1564  {
1565  info = &info_PRO_PLUS[1];
1566  }
1567  else if (num == PRO_PLUS_M54P_060_S250_R)
1568  {
1569  info = &info_PRO_PLUS[2];
1570  }
1571  else if (num == PRO_PLUS_H42P_020_S300_R)
1572  {
1573  info = &info_PRO_PLUS[3];
1574  }
1575  else if (num == PRO_PLUS_H54P_100_S500_R)
1576  {
1577  info = &info_PRO_PLUS[4];
1578  }
1579  else if (num == PRO_PLUS_H54P_200_S500_R)
1580  {
1581  info = &info_PRO_PLUS[5];
1582  }
1583  else if (num == RH_P12_RN)
1584  {
1585  info = &info_Gripper;
1586  }
1587  else if (num == RH_P12_RN_A)
1588  {
1589  info = &info_EXTGripper;
1590  }
1591  else
1592  {
1593  info = NULL;
1594  }
1595 
1596  return info;
1597 }
1598 
1600 {
1601  return the_number_of_item;
1602 }
static const char s_Goal_PWM[]
static const char s_External_Port_Data_2[]
const ControlItem * getControlTable(uint16_t model_number)
static const ControlItem items_XL[]
static const char s_Firmware_Version[]
static const char s_Goal_Torque[]
#define COUNT_EXTGripper_ITEMS
#define COUNT_XL320_ITEMS
static const ControlItem items_XM[]
static const ModelInfo info_Gripper
#define COUNT_Gripper_ITEMS
static const char s_External_Port_Mode_1[]
static const char s_Velocity_P_Gain[]
static const char s_Feedforward_2nd_Gain[]
#define COUNT_EXTPRO_ITEMS
static const ControlItem items_EXTMX[]
#define COUNT_EXTXM_ITEMS
#define COUNT_XW_ITEMS
static const ControlItem items_XW[]
static const ModelInfo info_RX
static const char s_Drive_Mode[]
static const char s_Velocity_Trajectory[]
static const char s_LED_RED[]
static const ModelInfo info_XW
static const char s_Present_Voltage[]
#define MX_28
#define RH_P12_RN_A
static const char s_Max_Torque[]
#define COUNT_EXTXH_ITEMS
#define PRO_H54_100_S500_R
static const char s_External_Port_Data_4[]
#define COUNT_PRO_ITEMS
static const ModelInfo info_EXTPRO[]
#define PRO_M54_60_S250_R_A
#define XH540_V150
#define XM540_W270
#define XH540_W150
#define XM430_W350
static const ControlItem items_MX2[]
#define RX_64
static const char s_PWM_Limit[]
#define COUNT_XM_ITEMS
static const char s_I_gain[]
static const char s_Torque_Enable[]
#define COUNT_MX_ITEMS
static const ModelInfo info_XM
#define COUNT_EXTMX_ITEMS
static const char s_CW_Angle_Limit[]
static const ControlItem items_EXTXH[]
#define PRO_M42_10_S260_R_A
static const char s_Present_Current[]
static const char s_Shutdown[]
#define RX_24F
#define AX_12A
static const ModelInfo info_EXTXM
static const char s_Sensored_Current[]
#define PRO_M54_40_S250_R
static const ModelInfo info_PRO
static const char s_Acceleration_Limit[]
static const char s_LED[]
#define MX_64_2
#define COUNT_AX_ITEMS
static const ControlItem items_EXTGripper[]
static const char s_Moving[]
static const char s_Moving_Status[]
static const ModelInfo info_PRO_PLUS[]
#define XC430_W240
#define PRO_H54_100_S500_R_A
static const char s_CW_Compliance_Margin[]
uint8_t getTheNumberOfControlItem()
static const ModelInfo info_XH
static const char s_Return_Delay_Time[]
const ModelInfo * getModelInfo(uint16_t model_number)
#define PRO_PLUS_H54P_100_S500_R
static const char s_LED_BLUE[]
static const ModelInfo info_XL
static const char s_Alarm_LED[]
#define MX_106_2
#define XH430_W210
static const char s_Present_Speed[]
static const ControlItem items_Gripper[]
static const char s_Status_Return_Level[]
#define COUNT_RX_ITEMS
static const char s_Current_Limit[]
static const ControlItem items_EXTMX2[]
static const ControlItem items_PRO[]
static const char s_D_gain[]
#define XW540_T140
static const char s_Registered[]
#define PRO_L54_50_S290_R
#define RX_28
#define PRO_PLUS_M42P_010_S260_R
static const char s_Present_Temperature[]
static const char s_Multi_Turn_Offset[]
static const char s_Present_Load[]
static const char s_External_Port_Mode_3[]
#define AX_12W
static const char s_Moving_Threshold[]
#define COUNT_XH_ITEMS
static const char s_CCW_Angle_Limit[]
static const char s_External_Port_Data_1[]
static const char s_CW_Compliance_Slope[]
#define XM540_W150
#define AX_18A
#define XH540_V270
#define PRO_PLUS_M54P_060_S250_R
#define MX_12W
static const char s_CCW_Compliance_Margin[]
static const ModelInfo info_EXTGripper
#define XL_320
#define COUNT_EXTPRO_PLUS_ITEMS
static const char s_LED_GREEN[]
#define XL430_W250_2
static const ControlItem items_XL320[]
static const char s_Temperature_Limit[]
static const char s_Position_I_Gain[]
static const char s_Operating_Mode[]
static const ControlItem items_EXTPRO_A[]
static const ModelInfo info_MX
static const char s_Max_Voltage_Limit[]
static const char s_Protocol_Version[]
static const char s_External_Port_Mode_4[]
#define PRO_L54_50_S500_R
static const char s_Current[]
#define PRO_H42_20_S300_R
static const char s_Position_D_Gain[]
static const ControlItem items_EXTPRO[]
static const char s_Torque_Limit[]
static const char s_Present_Input_Voltage[]
static const char s_Goal_Position[]
static const ModelInfo info_AX
#define PRO_PLUS_H54P_200_S500_R
static const ModelInfo info_MX2
static const char s_Hardware_Error_Status[]
#define RX_10
static const char s_Position_P_Gain[]
static const ModelInfo info_EXTXH
static const char s_Min_Position_Limit[]
#define COUNT_XL_ITEMS
static const char s_Present_Velocity[]
static const ModelInfo info_EX
static uint8_t the_number_of_item
#define XH540_W270
#define MX_64
static const char s_Baud_Rate[]
static const char s_CCW_Compliance_Slope[]
static const char s_Lock[]
#define XH430_W350
#define MX_28_2
#define PRO_M42_10_S260_R
static const char s_Registered_Instruction[]
#define XL430_W250
#define COUNT_EX_ITEMS
#define PRO_M54_40_S250_R_A
static const ControlItem items_XH[]
#define PRO_H54_200_S500_R_A
static const char s_External_Port_Data_3[]
static const char s_Present_Position[]
static const ModelInfo info_XL320
#define EX_106
static const char s_Velocity_I_Gain[]
#define XH430_V210
#define MX_106
#define XH430_V350
static const char s_Goal_Current[]
#define PRO_L54_30_S400_R
static const char s_Control_Mode[]
static const ControlItem items_PRO_PLUS[]
#define RH_P12_RN
static const char s_Max_Position_Limit[]
#define PRO_M54_60_S250_R
static const char s_Feedforward_1st_Gain[]
static const char s_Torque_Control_Mode_Enable[]
static const char s_Min_Voltage_Limit[]
#define PRO_H54_200_S500_R
#define COUNT_EXTPRO_A_ITEMS
static const ControlItem items_AX[]
static const char s_Goal_Acceleration[]
static const char s_Moving_Speed[]
static const char s_Present_Input[]
static const char s_Punch[]
static const char s_P_gain[]
static const char s_Resolution_Divider[]
#define COUNT_MX2_ITEMS
static const ControlItem items_EX[]
#define XM430_W210
static const ControlItem items_MX[]
static const ModelInfo info_EXTPRO_A[]
static const ControlItem items_EXTXM[]
#define PRO_H42_20_S300_R_A
static const char s_Realtime_Tick[]
static const char s_Goal_Velocity[]
static const char s_Profile_Velocity[]
static const ControlItem items_RX[]
static const char s_Secondary_ID[]
static const char s_External_Port_Mode_2[]
#define PRO_L42_10_S300_R
static const char s_Model_Number[]
static const char s_Homing_Offset[]
static const char s_ID[]
#define PRO_PLUS_H42P_020_S300_R
static const char s_Profile_Acceleration[]
#define PRO_PLUS_M54P_040_S250_R
static const char s_Velocity_Limit[]
#define XW540_T260
#define XC430_W150
static const ModelInfo info_EXTMX2
static const char s_Bus_Watchdog[]
static const char s_Present_PWM[]
#define COUNT_EXTMX2_ITEMS
static const ModelInfo info_EXTMX
static const char s_Position_Trajectory[]
#define PRO_L54_30_S500_R


dynamixel_workbench_toolbox
Author(s): Darby Lim , Ryan Shim
autogenerated on Mon Sep 28 2020 03:37:05