Go to the documentation of this file.
19 #include "../../include/dynamixel_workbench_toolbox/dynamixel_workbench.h"
44 result =
itemWrite(
id,
"Torque_Enable", (int32_t)onoff, log);
47 if (log != NULL) *log =
"[DynamixelWorkbench] Failed to change torque status!";
51 if (log != NULL) *log =
"[DynamixelWorkbench] Succeeded to change torque status!";
59 result =
torque(
id, 1, log);
68 result =
torque(
id, 0, log);
78 if (result ==
false)
return false;
83 if (log != NULL) *log =
"[DynamixelWorkbench] Failed to change ID!";
88 if (log != NULL) *log =
"[DynamixelWorkbench] Succeeded to change ID!";
97 if (result ==
false)
return false;
101 switch (new_baudrate)
158 switch (new_baudrate)
201 #if defined(__OPENCR__) || defined(__OPENCM904__)
209 if (log != NULL) *log =
"[DynamixelWorkbench] Failed to change Baud Rate!";
213 if (log != NULL) *log =
"[DynamixelWorkbench] Succeeded to change Baud Rate!";
224 if (!strncmp(
model_name,
"MX-28-2", strlen(
"MX-28-2")) ||
225 !strncmp(
model_name,
"MX-64-2", strlen(
"MX-64-2")) ||
226 !strncmp(
model_name,
"MX-106-2", strlen(
"MX-106-2")) ||
228 !strncmp(
model_name,
"XL330", strlen(
"XL330")) ||
229 !strncmp(
model_name,
"XL430", strlen(
"XL430")) ||
230 !strncmp(
model_name,
"XC430", strlen(
"XC430")) ||
234 result =
writeRegister(
id,
"Protocol_Version", version, log);
237 if (log != NULL) *log =
"[DynamixelWorkbench] Failed to set protocol version!";
244 if (log != NULL) *log =
"[DynamixelWorkbench] Succeeded to set protocol version!";
265 if (log != NULL) *log =
"[DynamixelWorkbench] Failed to change led status!";
269 if (log != NULL) *log =
"[DynamixelWorkbench] Succeeded to change led status!";
277 result =
led(
id, 1, log);
286 result =
led(
id, 0, log);
299 if (!strncmp(
model_name,
"MX-28-2", strlen(
"MX-28-2")) ||
300 !strncmp(
model_name,
"MX-64-2", strlen(
"MX-64-2")) ||
301 !strncmp(
model_name,
"MX-106-2", strlen(
"MX-106-2")) ||
303 !strncmp(
model_name,
"XL330", strlen(
"XL330")) ||
304 !strncmp(
model_name,
"XL430", strlen(
"XL430")) ||
305 !strncmp(
model_name,
"XC430", strlen(
"XC430")) ||
311 data = data & 0b00000100;
315 if (log != NULL) *log =
"[DynamixelWorkbench] Failed to set normal direction!";
320 if (log != NULL) *log =
"[DynamixelWorkbench] Succeeded to set normal direction!";
332 if (!strncmp(
model_name,
"MX-28-2", strlen(
"MX-28-2")) ||
333 !strncmp(
model_name,
"MX-64-2", strlen(
"MX-64-2")) ||
334 !strncmp(
model_name,
"MX-106-2", strlen(
"MX-106-2")) ||
336 !strncmp(
model_name,
"XL330", strlen(
"XL330")) ||
337 !strncmp(
model_name,
"XL430", strlen(
"XL430")) ||
338 !strncmp(
model_name,
"XC430", strlen(
"XC430")) ||
344 data = data | 0b00000001;
348 if (log != NULL) *log =
"[DynamixelWorkbench] Failed to set reverse direction!";
353 if (log != NULL) *log =
"[DynamixelWorkbench] Succeeded to set reverse direction!";
365 if (!strncmp(
model_name,
"MX-28-2", strlen(
"MX-28-2")) ||
366 !strncmp(
model_name,
"MX-64-2", strlen(
"MX-64-2")) ||
367 !strncmp(
model_name,
"MX-106-2", strlen(
"MX-106-2")) ||
369 !strncmp(
model_name,
"XL330", strlen(
"XL330")) ||
370 !strncmp(
model_name,
"XL430", strlen(
"XL430")) ||
371 !strncmp(
model_name,
"XC430", strlen(
"XC430")) ||
377 data = data & 0b00000001;
381 if (log != NULL) *log =
"[DynamixelWorkbench] Failed to set velocity based profile!";
386 if (log != NULL) *log =
"[DynamixelWorkbench] Succeeded to set velocity based profile!";
398 if (!strncmp(
model_name,
"MX-28-2", strlen(
"MX-28-2")) ||
399 !strncmp(
model_name,
"MX-64-2", strlen(
"MX-64-2")) ||
400 !strncmp(
model_name,
"MX-106-2", strlen(
"MX-106-2")) ||
402 !strncmp(
model_name,
"XL330", strlen(
"XL330")) ||
403 !strncmp(
model_name,
"XL430", strlen(
"XL430")) ||
404 !strncmp(
model_name,
"XC430", strlen(
"XC430")) ||
410 data = data | 0b00000100;
414 if (log != NULL) *log =
"[DynamixelWorkbench] Failed to set time based profile!";
419 if (log != NULL) *log =
"[DynamixelWorkbench] Succeeded to set time based profile!";
430 if (!strncmp(
model_name,
"MX-28-2", strlen(
"MX-28-2")) ||
431 !strncmp(
model_name,
"MX-64-2", strlen(
"MX-64-2")) ||
432 !strncmp(
model_name,
"MX-106-2", strlen(
"MX-106-2")) ||
434 !strncmp(
model_name,
"XL330", strlen(
"XL330")) ||
435 !strncmp(
model_name,
"XL430", strlen(
"XL430")) ||
436 !strncmp(
model_name,
"XC430", strlen(
"XC430")) ||
442 if (result ==
false)
return false;
444 result =
writeRegister(
id,
"Secondary_ID", secondary_id, log);
447 if (log != NULL) *log =
"[DynamixelWorkbench] Failed to set secondary ID!";
454 if (log != NULL) *log =
"[DynamixelWorkbench] Succeeded to set secondary ID!";
466 if (log != NULL) *log =
"[DynamixelWorkbench] Failed to set Position Control Mode!";
470 if (log != NULL) *log =
"[DynamixelWorkbench] Succeeded to set Position Control Mode!";
482 if (log != NULL) *log =
"[DynamixelWorkbench] Failed to set Velocity Control Mode!";
486 if (log != NULL) *log =
"[DynamixelWorkbench] Succeeded to set Velocity Control Mode!";
498 if (log != NULL) *log =
"[DynamixelWorkbench] Failed to set Current Control Mode!";
502 if (log != NULL) *log =
"[DynamixelWorkbench] Succeeded to set Current Control Mode!";
514 if (log != NULL) *log =
"[DynamixelWorkbench] Failed to set Torque Control Mode!";
518 if (log != NULL) *log =
"[DynamixelWorkbench] Succeeded to set Torque Control Mode!";
530 if (log != NULL) *log =
"[DynamixelWorkbench] Failed to set Extended Position Control Mode!";
534 if (log != NULL) *log =
"[DynamixelWorkbench] Succeeded to set Extended Position Control Mode!";
546 if (log != NULL) *log =
"[DynamixelWorkbench] Failed to set Multi-Turn Control Mode!";
550 if (log != NULL) *log =
"[DynamixelWorkbench] Succeeded to set Multi-Turn Control Mode!";
562 if (log != NULL) *log =
"[DynamixelWorkbench] Failed to set Current Based Position Control Mode!";
566 if (log != NULL) *log =
"[DynamixelWorkbench] Succeeded to set Current Based Position Control Mode!";
578 if (log != NULL) *log =
"[DynamixelWorkbench] Failed to set PWM Control Mode!";
582 if (log != NULL) *log =
"[DynamixelWorkbench] Succeeded to set PWM Control Mode!";
597 if (!strncmp(
model_name,
"MX-28-2", strlen(
"MX-28-2")) ||
598 !strncmp(
model_name,
"MX-64-2", strlen(
"MX-64-2")) ||
599 !strncmp(
model_name,
"MX-106-2", strlen(
"MX-106-2")) ||
600 !strncmp(
model_name,
"XL430", strlen(
"XL430")) ||
601 !strncmp(
model_name,
"XC430", strlen(
"XC430")) ||
623 if (!strncmp(
model_name,
"MX-28-2", strlen(
"MX-28-2")) ||
624 !strncmp(
model_name,
"MX-64-2", strlen(
"MX-64-2")) ||
625 !strncmp(
model_name,
"MX-106-2", strlen(
"MX-106-2")) ||
626 !strncmp(
model_name,
"XL430", strlen(
"XL430")) ||
627 !strncmp(
model_name,
"XC430", strlen(
"XC430")) ||
643 if (!strncmp(
model_name,
"XM", strlen(
"XM")) ||
646 !strncmp(
model_name,
"MX-64-2", strlen(
"MX-64-2")) ||
647 !strncmp(
model_name,
"MX-106-2", strlen(
"MX-106-2")) ||
655 if (!strncmp(
model_name,
"MX-64", strlen(
"MX-64")) ||
656 !strncmp(
model_name,
"MX-106", strlen(
"MX-106")) )
658 result =
writeRegister(
id,
"Torque_Control_Mode_Enable", 1, log);
663 if (!strncmp(
model_name,
"MX-64", strlen(
"MX-64")) ||
664 !strncmp(
model_name,
"MX-106", strlen(
"MX-106")) )
672 if (!strncmp(
model_name,
"MX-64-2", strlen(
"MX-64-2")) ||
673 !strncmp(
model_name,
"MX-106-2", strlen(
"MX-106-2")) ||
684 if (!strncmp(
model_name,
"MX-28-2", strlen(
"MX-28-2")) ||
685 !strncmp(
model_name,
"MX-64-2", strlen(
"MX-64-2")) ||
686 !strncmp(
model_name,
"MX-106-2", strlen(
"MX-106-2")) ||
699 if (!strncmp(
model_name,
"XL-320", strlen(
"XL-320")))
706 if (!strncmp(
model_name,
"XL-320", strlen(
"XL-320")))
713 if (!strncmp(
model_name,
"XL330", strlen(
"XL330")) ||
717 !strncmp(
model_name,
"MX-64-2", strlen(
"MX-64-2")) ||
718 !strncmp(
model_name,
"MX-106-2", strlen(
"MX-106-2")) ||
720 !strncmp(
model_name,
"PRO-PLUS", strlen(
"PRO-PLUS")))
727 if (!strncmp(
model_name,
"PRO", strlen(
"PRO")) ||
728 strncmp(
model_name,
"PRO-L42", strlen(
"PRO-L42")) )
735 if (!strncmp(
model_name,
"PRO", strlen(
"PRO")) ||
736 strncmp(
model_name,
"PRO-L42", strlen(
"PRO-L42")) )
743 if (!strncmp(
model_name,
"MX-64-2", strlen(
"MX-64-2")) ||
744 !strncmp(
model_name,
"MX-106-2", strlen(
"MX-106-2")) ||
745 !strncmp(
model_name,
"XL330", strlen(
"XL330")) ||
756 if (!strncmp(
model_name,
"MX-28-2", strlen(
"MX-28-2")) ||
757 !strncmp(
model_name,
"MX-64-2", strlen(
"MX-64-2")) ||
758 !strncmp(
model_name,
"MX-106-2", strlen(
"MX-106-2")) ||
759 !strncmp(
model_name,
"XL330", strlen(
"XL330")) ||
763 !strncmp(
model_name,
"XL430", strlen(
"XL430")) ||
764 !strncmp(
model_name,
"XC430", strlen(
"XC430")) ||
774 if (log != NULL) *log =
"[DynamixelWorkbench] Failed to set Operating Mode!";
778 if (log != NULL) *log =
"[DynamixelWorkbench] Succeeded to set Operating Mode!";
791 if (result ==
false)
return false;
794 if (result ==
false)
return false;
798 if (!strncmp(
model_name,
"MX-28-2", strlen(
"MX-28-2")) ||
799 !strncmp(
model_name,
"MX-64-2", strlen(
"MX-64-2")) ||
800 !strncmp(
model_name,
"MX-106-2", strlen(
"MX-106-2")) ||
801 !strncmp(
model_name,
"XL430", strlen(
"XL430")) ||
802 !strncmp(
model_name,
"XC430", strlen(
"XC430")) ||
807 result =
writeRegister(
id,
"Profile_Acceleration", acceleration, log);
808 result =
writeRegister(
id,
"Profile_Velocity", velocity, log);
810 else if (!strncmp(
model_name,
"MX-28", strlen(
"MX-28")) ||
811 !strncmp(
model_name,
"MX-64", strlen(
"MX-64")) ||
812 !strncmp(
model_name,
"MX-106", strlen(
"MX-106")))
815 result =
writeRegister(
id,
"Goal_Acceleration", acceleration, log);
824 if (!strncmp(
model_name,
"XL-320", strlen(
"XL-320")))
828 else if (!strncmp(
model_name,
"PRO-M42-10-S260-R-A", strlen(
"PRO-M42-10-S260-R-A")) ||
829 !strncmp(
model_name,
"PRO-M54-40-S250-R-A", strlen(
"PRO-M54-40-S250-R-A")) ||
830 !strncmp(
model_name,
"PRO-M54-60-S250-R-A", strlen(
"PRO-M54-60-S250-R-A")) ||
831 !strncmp(
model_name,
"PRO-H42-20-S300-R-A", strlen(
"PRO-H42-20-S300-R-A")) ||
832 !strncmp(
model_name,
"PRO-H54-100-S500-R-A", strlen(
"PRO-H54-100-S500-R-A")) ||
833 !strncmp(
model_name,
"PRO-H54-200-S500-R-A", strlen(
"PRO-H54-200-S500-R-A")) ||
834 !strncmp(
model_name,
"PRO-PLUS", strlen(
"PRO-PLUS")))
836 result =
writeRegister(
id,
"Profile_Acceleration", acceleration, log);
837 result =
writeRegister(
id,
"Profile_Velocity", velocity, log);
839 else if (!strncmp(
model_name,
"PRO-L", strlen(
"PRO-L")) ||
840 !strncmp(
model_name,
"PRO-M", strlen(
"PRO-M")) ||
841 !strncmp(
model_name,
"PRO-H", strlen(
"PRO-H")))
844 result =
writeRegister(
id,
"Goal_Acceleration", acceleration, log);
848 result =
writeRegister(
id,
"Profile_Acceleration", acceleration, log);
849 result =
writeRegister(
id,
"Profile_Velocity", velocity, log);
855 if (log != NULL) *log =
"[DynamixelWorkbench] Failed to set Joint Mode!";
860 if (result ==
false)
return false;
862 if (log != NULL) *log =
"[DynamixelWorkbench] Succeeded to set Joint Mode!";
874 if (result ==
false)
return false;
877 if (result ==
false)
return false;
881 if (!strncmp(
model_name,
"MX-28-2", strlen(
"MX-28-2")) ||
882 !strncmp(
model_name,
"MX-64-2", strlen(
"MX-64-2")) ||
883 !strncmp(
model_name,
"MX-106-2", strlen(
"MX-106-2")) ||
884 !strncmp(
model_name,
"XL430", strlen(
"XL430")) ||
885 !strncmp(
model_name,
"XC430", strlen(
"XC430")) ||
890 result =
writeRegister(
id,
"Profile_Acceleration", acceleration, log);
892 else if (!strncmp(
model_name,
"MX-28", strlen(
"MX-28")) ||
893 !strncmp(
model_name,
"MX-64", strlen(
"MX-64")) ||
894 !strncmp(
model_name,
"MX-106", strlen(
"MX-106")))
896 result =
writeRegister(
id,
"Goal_Acceleration", acceleration, log);
901 if (!strncmp(
model_name,
"PRO-PLUS", strlen(
"PRO-PLUS")))
903 result =
writeRegister(
id,
"Profile_Acceleration", acceleration, log);
905 else if (!strncmp(
model_name,
"PRO", strlen(
"PRO")))
907 result =
writeRegister(
id,
"Goal_Acceleration", acceleration, log);
911 result =
writeRegister(
id,
"Profile_Acceleration", acceleration, log);
917 if (log != NULL) *log =
"[DynamixelWorkbench] Failed to set Wheel Mode!";
922 if (result ==
false)
return false;
924 if (log != NULL) *log =
"[DynamixelWorkbench] Succeeded to set Wheel Mode!";
936 if (result ==
false)
return false;
939 if (result ==
false)
return false;
941 if (!strncmp(
model_name,
"MX-64-2", strlen(
"MX-64-2")) ||
942 !strncmp(
model_name,
"MX-106-2", strlen(
"MX-106-2")) ||
943 !strncmp(
model_name,
"XL330", strlen(
"XL330")) ||
954 if (log != NULL) *log =
"[DynamixelWorkbench] Failed to set Current Based Position Mode!";
959 if (result ==
false)
return false;
961 if (log != NULL) *log =
"[DynamixelWorkbench] Succeeded to set Current Based Position Wheel Mode!";
975 result =
itemWrite(
id,
"Goal_Position", value, log);
979 if (log != NULL) *log =
"[DynamixelWorkbench] Failed to set goal position!";
983 if (log != NULL) *log =
"[DynamixelWorkbench] Succeeded to set goal position!";
1003 bool result[2] = {
false,
false};
1008 if (result[0] ==
false)
1012 value = (-1) * value;
1016 if (result[1] ==
false)
1018 if (log != NULL) *log =
"[DynamixelWorkbench] Failed to set goal velocity!";
1023 if (log != NULL) *log =
"[DynamixelWorkbench] Succeeded to set goal velocity!";
1029 if (log != NULL) *log =
"[DynamixelWorkbench] Succeeded to set goal velocity!";
1036 if (result[0] ==
false)
1040 value = (-1) * value;
1044 if (result[1] ==
false)
1046 if (log != NULL) *log =
"[DynamixelWorkbench] Failed to set goal velocity!";
1051 if (log != NULL) *log =
"[DynamixelWorkbench] Succeeded to set goal velocity!";
1057 if (log != NULL) *log =
"[DynamixelWorkbench] Succeeded to set goal velocity!";
1062 if (log != NULL) *log =
"[DynamixelWorkbench] Failed to set goal velocity!";
1074 if (result ==
false)
1076 if (log != NULL) *log =
"[DynamixelWorkbench] Failed to set goal position!";
1080 if (log != NULL) *log =
"[DynamixelWorkbench] Succeeded to set goal position!";
1092 if (result ==
false)
1094 if (log != NULL) *log =
"[DynamixelWorkbench] Failed to set goal velocity!";
1098 if (log != NULL) *log =
"[DynamixelWorkbench] Succeeded to set goal velocity!";
1105 int32_t get_data = 0;
1107 result =
readRegister(
id,
"Present_Position", &get_data, log);
1108 if (result ==
false)
1110 if (log != NULL) *log =
"[DynamixelWorkbench] Failed to get present position data!";
1116 if (log != NULL) *log =
"[DynamixelWorkbench] Succeeded to get present position data!";
1123 int32_t get_data = 0;
1126 if (result ==
false)
1128 if (log != NULL) *log =
"[DynamixelWorkbench] Failed to get radian!";
1134 if (log != NULL) *log =
"[DynamixelWorkbench] Succeeded to get radian!";
1141 int32_t get_data = 0;
1144 if (result ==
false)
1146 if (log != NULL) *log =
"[DynamixelWorkbench] Failed to get velocity!";
1152 if (log != NULL) *log =
"[DynamixelWorkbench] Succeeded to get velocity!";
1159 int32_t get_data = 0;
1161 result =
readRegister(
id,
"Present_Speed", &get_data, log);
1162 if (result ==
false)
1164 if (log != NULL) *log =
"[DynamixelWorkbench] Failed to get present speed data!";
1169 if (log != NULL) *log =
"[DynamixelWorkbench] Succeeded to get present speed data!";
1175 int32_t position = 0;
1184 else if (radian < 0)
1207 else if (value < model_info->value_of_zero_radian_position)
1218 int32_t zero_position = (max_position + min_position)/2;
1222 value = (radian * (max_position - zero_position) / max_radian) + zero_position;
1224 else if (radian < 0)
1226 value = (radian * (min_position - zero_position) / min_radian) + zero_position;
1230 value = zero_position;
1239 int32_t zero_position = (max_position + min_position)/2;
1241 if (value > zero_position)
1243 radian = (float)(value - zero_position) * max_radian / (float)(max_position - zero_position);
1245 else if (value < zero_position)
1247 radian = (float)(value - zero_position) * min_radian / (float)(min_position - zero_position);
1256 const float RPM2RADPERSEC = 0.104719755f;
1263 if (strncmp(
getModelName(
id),
"AX", strlen(
"AX")) == 0 ||
1268 if (velocity == 0.0
f) value = 0;
1269 else if (velocity < 0.0
f) value = (velocity / (
model_info->
rpm * RPM2RADPERSEC));
1270 else if (velocity > 0.0
f) value = (velocity / (
model_info->
rpm * RPM2RADPERSEC)) + 1023;
1279 if (velocity == 0.0
f) value = 0;
1280 else if (velocity < 0.0
f) value = (velocity / (
model_info->
rpm * RPM2RADPERSEC));
1281 else if (velocity > 0.0
f) value = (velocity / (
model_info->
rpm * RPM2RADPERSEC)) + 1023;
1299 const float RPM2RADPERSEC = 0.104719755f;
1306 if (strncmp(
getModelName(
id),
"AX", strlen(
"AX")) == 0 ||
1311 if (value == 1023 || value == 0) velocity = 0.0f;
1312 else if (value > 0 && value < 1023) velocity = value *
model_info->
rpm * RPM2RADPERSEC;
1313 else if (value > 1023 && value < 2048) velocity = (value - 1023) *
model_info->
rpm * RPM2RADPERSEC * (-1.0f);
1322 if (value == 1023 || value == 0) velocity = 0.0f;
1323 else if (value > 0 && value < 1023) velocity = value *
model_info->
rpm * RPM2RADPERSEC;
1324 else if (value > 1023 && value < 2048) velocity = (value - 1023) *
model_info->
rpm * RPM2RADPERSEC * (-1.0f);
1339 float CURRENT_UNIT = 2.69f;
1346 return (current / CURRENT_UNIT);
1350 if (strncmp(
getModelName(
id),
"PRO-L", strlen(
"PRO-L")) == 0 ||
1351 strncmp(
getModelName(
id),
"PRO-M", strlen(
"PRO-M")) == 0 ||
1352 strncmp(
getModelName(
id),
"PRO-H", strlen(
"PRO-H")) == 0)
1354 CURRENT_UNIT = 16.11328f;
1355 return (current / CURRENT_UNIT);
1357 else if (strncmp(
getModelName(
id),
"PRO-PLUS", strlen(
"PRO-PLUS")) == 0)
1359 CURRENT_UNIT = 1.0f;
1360 return (current / CURRENT_UNIT);
1364 return (current / CURRENT_UNIT);
1368 return (current / CURRENT_UNIT);
1374 const float CURRENT_UNIT = 2.69f;
1376 value = current / CURRENT_UNIT;
1384 float CURRENT_UNIT = 2.69f;
1391 current = (int16_t)value * CURRENT_UNIT;
1396 if (strncmp(
getModelName(
id),
"PRO-L", strlen(
"PRO-L")) == 0 ||
1397 strncmp(
getModelName(
id),
"PRO-M", strlen(
"PRO-M")) == 0 ||
1398 strncmp(
getModelName(
id),
"PRO-H", strlen(
"PRO-H")) == 0)
1400 CURRENT_UNIT = 16.11328f;
1401 current = (int16_t)value * CURRENT_UNIT;
1404 else if (strncmp(
getModelName(
id),
"PRO-PLUS", strlen(
"PRO-PLUS")) == 0)
1406 CURRENT_UNIT = 1.0f;
1407 current = (int16_t)value * CURRENT_UNIT;
1412 current = (int16_t)value * CURRENT_UNIT;
1417 current = (int16_t)value * CURRENT_UNIT;
1425 const float CURRENT_UNIT = 2.69f;
1427 current = (int16_t)value * CURRENT_UNIT;
1435 const float LOAD_UNIT = 0.1f;
1437 if (value == 1023 || value == 0) load = 0.0f;
1438 else if (value > 0 && value < 1023) load = value * LOAD_UNIT;
1439 else if (value > 1023 && value < 2048) load = (value - 1023) * LOAD_UNIT * (-1.0
f);
bool setCurrentBasedPositionControlMode(uint8_t id, const char **log=NULL)
bool wheelMode(uint8_t id, int32_t acceleration=0, const char **log=NULL)
bool writeRegister(uint8_t id, uint16_t address, uint16_t length, uint8_t *data, const char **log=NULL)
bool setTimeBasedProfile(uint8_t id, const char **log=NULL)
float convertValue2Load(int16_t value)
bool torque(uint8_t id, int32_t onoff, const char **log=NULL)
bool setPacketHandler(float protocol_version, const char **log=NULL)
bool getRadian(uint8_t id, float *radian, const char **log=NULL)
bool changeID(uint8_t id, uint8_t new_id, const char **log=NULL)
float convertValue2Current(uint8_t id, int16_t value)
bool goalPosition(uint8_t id, int value, const char **log=NULL)
bool currentBasedPositionMode(uint8_t id, int32_t current=0, const char **log=NULL)
int64_t value_of_min_radian_position
bool itemWrite(uint8_t id, const char *item_name, int32_t data, const char **log=NULL)
bool getPresentVelocityData(uint8_t id, int32_t *data, const char **log=NULL)
bool setReverseDirection(uint8_t id, const char **log=NULL)
bool goalVelocity(uint8_t id, int value, const char **log=NULL)
float convertValue2Radian(uint8_t id, int32_t value)
bool led(uint8_t id, int32_t onoff, const char **log=NULL)
static const uint8_t TORQUE_CONTROL_MODE
bool setMultiTurnControlMode(uint8_t id, const char **log=NULL)
static const uint8_t CURRENT_CONTROL_MODE
bool getPresentPositionData(uint8_t id, int32_t *data, const char **log=NULL)
bool setOperatingMode(uint8_t id, uint8_t index, const char **log=NULL)
bool goalSpeed(uint8_t id, int value, const char **log=NULL)
const ModelInfo * getModelInfo(uint8_t id, const char **log=NULL)
bool setVelocityBasedProfile(uint8_t id, const char **log=NULL)
static const uint8_t EXTENDED_POSITION_CONTROL_MODE
static const uint8_t POSITION_CONTROL_MODE
static const uint8_t MULTI_TURN_MODE
static const char * model_name
bool ledOn(uint8_t id, const char **log=NULL)
int32_t convertVelocity2Value(uint8_t id, float velocity)
bool torqueOn(uint8_t id, const char **log=NULL)
static const uint8_t PWM_CONTROL_MODE
int64_t value_of_max_radian_position
static const uint8_t JOINT_MODE
float convertValue2Velocity(uint8_t id, int32_t value)
bool setSecondaryID(uint8_t id, uint8_t secondary_id, const char **log=NULL)
float getProtocolVersion(void)
static const uint8_t CURRENT_BASED_POSITION_CONTROL_MODE
int16_t convertCurrent2Value(uint8_t id, float current)
static const uint8_t WHEEL_MODE
static const ModelInfo * model_info
bool setVelocityControlMode(uint8_t id, const char **log=NULL)
int32_t convertRadian2Value(uint8_t id, float radian)
bool setTorqueControlMode(uint8_t id, const char **log=NULL)
bool setExtendedPositionControlMode(uint8_t id, const char **log=NULL)
int64_t value_of_zero_radian_position
bool itemRead(uint8_t id, const char *item_name, int32_t *data, const char **log=NULL)
bool setPositionControlMode(uint8_t id, const char **log=NULL)
const char * getModelName(uint8_t id, const char **log=NULL)
bool ledOff(uint8_t id, const char **log=NULL)
bool setNormalDirection(uint8_t id, const char **log=NULL)
bool changeProtocolVersion(uint8_t id, uint8_t version, const char **log=NULL)
static const uint8_t VELOCITY_CONTROL_MODE
bool getVelocity(uint8_t id, float *velocity, const char **log=NULL)
bool setPWMControlMode(uint8_t id, const char **log=NULL)
bool changeBaudrate(uint8_t id, uint32_t new_baudrate, const char **log=NULL)
bool setCurrentControlMode(uint8_t id, const char **log=NULL)
bool readRegister(uint8_t id, uint16_t address, uint16_t length, uint32_t *data, const char **log=NULL)
bool torqueOff(uint8_t id, const char **log=NULL)
bool jointMode(uint8_t id, int32_t velocity=0, int32_t acceleration=0, const char **log=NULL)