24 #include <interfaces/DynamixelServoInterface.h>
26 #include <core/exceptions/software.h>
46 DynamixelServoInterface::DynamixelServoInterface() : Interface()
48 data_size =
sizeof(DynamixelServoInterface_data_t);
49 data_ptr = malloc(data_size);
50 data = (DynamixelServoInterface_data_t *)data_ptr;
51 data_ts = (interface_data_ts_t *)data_ptr;
52 memset(data_ptr, 0, data_size);
53 enum_map_ErrorCode[(int)ERROR_NONE] =
"ERROR_NONE";
54 enum_map_ErrorCode[(int)ERROR_UNSPECIFIC] =
"ERROR_UNSPECIFIC";
55 enum_map_ErrorCode[(int)ERROR_COMMUNICATION] =
"ERROR_COMMUNICATION";
56 enum_map_ErrorCode[(int)ERROR_ANGLE_OUTOFRANGE] =
"ERROR_ANGLE_OUTOFRANGE";
57 enum_map_WorkingMode[(int)JOINT] =
"JOINT";
58 enum_map_WorkingMode[(int)WHEEL] =
"WHEEL";
59 add_fieldinfo(IFT_STRING,
"model", 8, data->model);
60 add_fieldinfo(IFT_UINT32,
"model_number", 1, &data->model_number);
61 add_fieldinfo(IFT_UINT32,
"cw_angle_limit", 1, &data->cw_angle_limit);
62 add_fieldinfo(IFT_UINT32,
"ccw_angle_limit", 1, &data->ccw_angle_limit);
63 add_fieldinfo(IFT_UINT8,
"temperature_limit", 1, &data->temperature_limit);
64 add_fieldinfo(IFT_UINT32,
"max_torque", 1, &data->max_torque);
65 add_fieldinfo(IFT_UINT8,
"cw_margin", 1, &data->cw_margin);
66 add_fieldinfo(IFT_UINT8,
"ccw_margin", 1, &data->ccw_margin);
67 add_fieldinfo(IFT_UINT8,
"cw_slope", 1, &data->cw_slope);
68 add_fieldinfo(IFT_UINT8,
"ccw_slope", 1, &data->ccw_slope);
69 add_fieldinfo(IFT_UINT32,
"goal_position", 1, &data->goal_position);
70 add_fieldinfo(IFT_UINT32,
"goal_speed", 1, &data->goal_speed);
71 add_fieldinfo(IFT_UINT32,
"torque_limit", 1, &data->torque_limit);
72 add_fieldinfo(IFT_UINT32,
"position", 1, &data->position);
73 add_fieldinfo(IFT_UINT32,
"speed", 1, &data->speed);
74 add_fieldinfo(IFT_UINT32,
"load", 1, &data->load);
75 add_fieldinfo(IFT_UINT8,
"voltage", 1, &data->voltage);
76 add_fieldinfo(IFT_UINT8,
"temperature", 1, &data->temperature);
77 add_fieldinfo(IFT_UINT32,
"punch", 1, &data->punch);
78 add_fieldinfo(IFT_UINT8,
"alarm_shutdown", 1, &data->alarm_shutdown);
79 add_fieldinfo(IFT_UINT8,
"error", 1, &data->error);
80 add_fieldinfo(IFT_BOOL,
"enable_prevent_alarm_shutdown", 1, &data->enable_prevent_alarm_shutdown);
81 add_fieldinfo(IFT_FLOAT,
"angle", 1, &data->angle);
82 add_fieldinfo(IFT_BOOL,
"enabled", 1, &data->enabled);
83 add_fieldinfo(IFT_FLOAT,
"min_angle", 1, &data->min_angle);
84 add_fieldinfo(IFT_FLOAT,
"max_angle", 1, &data->max_angle);
85 add_fieldinfo(IFT_FLOAT,
"max_velocity", 1, &data->max_velocity);
86 add_fieldinfo(IFT_FLOAT,
"velocity", 1, &data->velocity);
87 add_fieldinfo(IFT_STRING,
"mode", 5, data->mode);
88 add_fieldinfo(IFT_FLOAT,
"angle_margin", 1, &data->angle_margin);
89 add_fieldinfo(IFT_BOOL,
"autorecover_enabled", 1, &data->autorecover_enabled);
90 add_fieldinfo(IFT_UINT32,
"msgid", 1, &data->msgid);
91 add_fieldinfo(IFT_BOOL,
"final", 1, &data->final);
92 add_fieldinfo(IFT_ENUM,
"error_code", 1, &data->error_code,
"ErrorCode", &enum_map_ErrorCode);
93 add_messageinfo(
"StopMessage");
94 add_messageinfo(
"FlushMessage");
95 add_messageinfo(
"GotoMessage");
96 add_messageinfo(
"TimedGotoMessage");
97 add_messageinfo(
"SetModeMessage");
98 add_messageinfo(
"SetSpeedMessage");
99 add_messageinfo(
"SetEnabledMessage");
100 add_messageinfo(
"SetVelocityMessage");
101 add_messageinfo(
"SetMarginMessage");
102 add_messageinfo(
"SetComplianceValuesMessage");
103 add_messageinfo(
"SetGoalSpeedMessage");
104 add_messageinfo(
"SetTorqueLimitMessage");
105 add_messageinfo(
"SetPunchMessage");
106 add_messageinfo(
"GotoPositionMessage");
107 add_messageinfo(
"SetAngleLimitsMessage");
108 add_messageinfo(
"ResetRawErrorMessage");
109 add_messageinfo(
"SetPreventAlarmShutdownMessage");
110 add_messageinfo(
"SetAutorecoverEnabledMessage");
111 add_messageinfo(
"RecoverMessage");
112 unsigned char tmp_hash[] = {0x18, 0x72, 0x74, 0xa6, 0x80, 0xfa, 0x62, 0xa2, 0x56, 0x91, 0x21, 0xfc, 0x48, 0xd5, 0xe0, 0x5f};
117 DynamixelServoInterface::~DynamixelServoInterface()
126 DynamixelServoInterface::tostring_ErrorCode(
ErrorCode value)
const
129 case ERROR_NONE:
return "ERROR_NONE";
130 case ERROR_UNSPECIFIC:
return "ERROR_UNSPECIFIC";
131 case ERROR_COMMUNICATION:
return "ERROR_COMMUNICATION";
132 case ERROR_ANGLE_OUTOFRANGE:
return "ERROR_ANGLE_OUTOFRANGE";
133 default:
return "UNKNOWN";
141 DynamixelServoInterface::tostring_WorkingMode(
WorkingMode value)
const
144 case JOINT:
return "JOINT";
145 case WHEEL:
return "WHEEL";
146 default:
return "UNKNOWN";
155 DynamixelServoInterface::model()
const
165 DynamixelServoInterface::maxlenof_model()
const
175 DynamixelServoInterface::set_model(
const char * new_model)
185 DynamixelServoInterface::model_number()
const
187 return data->model_number;
195 DynamixelServoInterface::maxlenof_model_number()
const
205 DynamixelServoInterface::set_model_number(
const uint32_t new_model_number)
207 data_changed |=
change_field(data->model_number, new_model_number);
215 DynamixelServoInterface::cw_angle_limit()
const
217 return data->cw_angle_limit;
225 DynamixelServoInterface::maxlenof_cw_angle_limit()
const
235 DynamixelServoInterface::set_cw_angle_limit(
const uint32_t new_cw_angle_limit)
237 data_changed |=
change_field(data->cw_angle_limit, new_cw_angle_limit);
245 DynamixelServoInterface::ccw_angle_limit()
const
247 return data->ccw_angle_limit;
255 DynamixelServoInterface::maxlenof_ccw_angle_limit()
const
265 DynamixelServoInterface::set_ccw_angle_limit(
const uint32_t new_ccw_angle_limit)
267 data_changed |=
change_field(data->ccw_angle_limit, new_ccw_angle_limit);
275 DynamixelServoInterface::temperature_limit()
const
277 return data->temperature_limit;
285 DynamixelServoInterface::maxlenof_temperature_limit()
const
295 DynamixelServoInterface::set_temperature_limit(
const uint8_t new_temperature_limit)
297 data_changed |=
change_field(data->temperature_limit, new_temperature_limit);
305 DynamixelServoInterface::max_torque()
const
307 return data->max_torque;
315 DynamixelServoInterface::maxlenof_max_torque()
const
325 DynamixelServoInterface::set_max_torque(
const uint32_t new_max_torque)
327 data_changed |=
change_field(data->max_torque, new_max_torque);
335 DynamixelServoInterface::cw_margin()
const
337 return data->cw_margin;
345 DynamixelServoInterface::maxlenof_cw_margin()
const
355 DynamixelServoInterface::set_cw_margin(
const uint8_t new_cw_margin)
357 data_changed |=
change_field(data->cw_margin, new_cw_margin);
365 DynamixelServoInterface::ccw_margin()
const
367 return data->ccw_margin;
375 DynamixelServoInterface::maxlenof_ccw_margin()
const
385 DynamixelServoInterface::set_ccw_margin(
const uint8_t new_ccw_margin)
387 data_changed |=
change_field(data->ccw_margin, new_ccw_margin);
395 DynamixelServoInterface::cw_slope()
const
397 return data->cw_slope;
405 DynamixelServoInterface::maxlenof_cw_slope()
const
415 DynamixelServoInterface::set_cw_slope(
const uint8_t new_cw_slope)
417 data_changed |=
change_field(data->cw_slope, new_cw_slope);
425 DynamixelServoInterface::ccw_slope()
const
427 return data->ccw_slope;
435 DynamixelServoInterface::maxlenof_ccw_slope()
const
445 DynamixelServoInterface::set_ccw_slope(
const uint8_t new_ccw_slope)
447 data_changed |=
change_field(data->ccw_slope, new_ccw_slope);
455 DynamixelServoInterface::goal_position()
const
457 return data->goal_position;
465 DynamixelServoInterface::maxlenof_goal_position()
const
475 DynamixelServoInterface::set_goal_position(
const uint32_t new_goal_position)
477 data_changed |=
change_field(data->goal_position, new_goal_position);
485 DynamixelServoInterface::goal_speed()
const
487 return data->goal_speed;
495 DynamixelServoInterface::maxlenof_goal_speed()
const
505 DynamixelServoInterface::set_goal_speed(
const uint32_t new_goal_speed)
507 data_changed |=
change_field(data->goal_speed, new_goal_speed);
515 DynamixelServoInterface::torque_limit()
const
517 return data->torque_limit;
525 DynamixelServoInterface::maxlenof_torque_limit()
const
535 DynamixelServoInterface::set_torque_limit(
const uint32_t new_torque_limit)
537 data_changed |=
change_field(data->torque_limit, new_torque_limit);
545 DynamixelServoInterface::position()
const
547 return data->position;
555 DynamixelServoInterface::maxlenof_position()
const
565 DynamixelServoInterface::set_position(
const uint32_t new_position)
567 data_changed |=
change_field(data->position, new_position);
575 DynamixelServoInterface::speed()
const
585 DynamixelServoInterface::maxlenof_speed()
const
595 DynamixelServoInterface::set_speed(
const uint32_t new_speed)
605 DynamixelServoInterface::load()
const
615 DynamixelServoInterface::maxlenof_load()
const
625 DynamixelServoInterface::set_load(
const uint32_t new_load)
635 DynamixelServoInterface::voltage()
const
637 return data->voltage;
645 DynamixelServoInterface::maxlenof_voltage()
const
655 DynamixelServoInterface::set_voltage(
const uint8_t new_voltage)
657 data_changed |=
change_field(data->voltage, new_voltage);
665 DynamixelServoInterface::temperature()
const
667 return data->temperature;
675 DynamixelServoInterface::maxlenof_temperature()
const
685 DynamixelServoInterface::set_temperature(
const uint8_t new_temperature)
687 data_changed |=
change_field(data->temperature, new_temperature);
695 DynamixelServoInterface::punch()
const
705 DynamixelServoInterface::maxlenof_punch()
const
715 DynamixelServoInterface::set_punch(
const uint32_t new_punch)
735 DynamixelServoInterface::alarm_shutdown()
const
737 return data->alarm_shutdown;
745 DynamixelServoInterface::maxlenof_alarm_shutdown()
const
765 DynamixelServoInterface::set_alarm_shutdown(
const uint8_t new_alarm_shutdown)
767 data_changed |=
change_field(data->alarm_shutdown, new_alarm_shutdown);
785 DynamixelServoInterface::error()
const
795 DynamixelServoInterface::maxlenof_error()
const
815 DynamixelServoInterface::set_error(
const uint8_t new_error)
825 DynamixelServoInterface::is_enable_prevent_alarm_shutdown()
const
827 return data->enable_prevent_alarm_shutdown;
835 DynamixelServoInterface::maxlenof_enable_prevent_alarm_shutdown()
const
845 DynamixelServoInterface::set_enable_prevent_alarm_shutdown(
const bool new_enable_prevent_alarm_shutdown)
847 data_changed |=
change_field(data->enable_prevent_alarm_shutdown, new_enable_prevent_alarm_shutdown);
855 DynamixelServoInterface::angle()
const
865 DynamixelServoInterface::maxlenof_angle()
const
875 DynamixelServoInterface::set_angle(
const float new_angle)
885 DynamixelServoInterface::is_enabled()
const
887 return data->enabled;
895 DynamixelServoInterface::maxlenof_enabled()
const
905 DynamixelServoInterface::set_enabled(
const bool new_enabled)
907 data_changed |=
change_field(data->enabled, new_enabled);
915 DynamixelServoInterface::min_angle()
const
917 return data->min_angle;
925 DynamixelServoInterface::maxlenof_min_angle()
const
935 DynamixelServoInterface::set_min_angle(
const float new_min_angle)
937 data_changed |=
change_field(data->min_angle, new_min_angle);
945 DynamixelServoInterface::max_angle()
const
947 return data->max_angle;
955 DynamixelServoInterface::maxlenof_max_angle()
const
965 DynamixelServoInterface::set_max_angle(
const float new_max_angle)
967 data_changed |=
change_field(data->max_angle, new_max_angle);
975 DynamixelServoInterface::max_velocity()
const
977 return data->max_velocity;
985 DynamixelServoInterface::maxlenof_max_velocity()
const
995 DynamixelServoInterface::set_max_velocity(
const float new_max_velocity)
997 data_changed |=
change_field(data->max_velocity, new_max_velocity);
1005 DynamixelServoInterface::velocity()
const
1007 return data->velocity;
1015 DynamixelServoInterface::maxlenof_velocity()
const
1025 DynamixelServoInterface::set_velocity(
const float new_velocity)
1027 data_changed |=
change_field(data->velocity, new_velocity);
1035 DynamixelServoInterface::mode()
const
1045 DynamixelServoInterface::maxlenof_mode()
const
1055 DynamixelServoInterface::set_mode(
const char * new_mode)
1068 DynamixelServoInterface::angle_margin()
const
1070 return data->angle_margin;
1078 DynamixelServoInterface::maxlenof_angle_margin()
const
1091 DynamixelServoInterface::set_angle_margin(
const float new_angle_margin)
1093 data_changed |=
change_field(data->angle_margin, new_angle_margin);
1101 DynamixelServoInterface::is_autorecover_enabled()
const
1103 return data->autorecover_enabled;
1111 DynamixelServoInterface::maxlenof_autorecover_enabled()
const
1121 DynamixelServoInterface::set_autorecover_enabled(
const bool new_autorecover_enabled)
1123 data_changed |=
change_field(data->autorecover_enabled, new_autorecover_enabled);
1132 DynamixelServoInterface::msgid()
const
1142 DynamixelServoInterface::maxlenof_msgid()
const
1153 DynamixelServoInterface::set_msgid(
const uint32_t new_msgid)
1164 DynamixelServoInterface::is_final()
const
1174 DynamixelServoInterface::maxlenof_final()
const
1185 DynamixelServoInterface::set_final(
const bool new_final)
1196 DynamixelServoInterface::error_code()
const
1206 DynamixelServoInterface::maxlenof_error_code()
const
1217 DynamixelServoInterface::set_error_code(
const ErrorCode new_error_code)
1219 data_changed |=
change_field(data->error_code, new_error_code);
1224 DynamixelServoInterface::create_message(
const char *type)
const
1226 if ( strncmp(
"StopMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1228 }
else if ( strncmp(
"FlushMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1230 }
else if ( strncmp(
"GotoMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1232 }
else if ( strncmp(
"TimedGotoMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1234 }
else if ( strncmp(
"SetModeMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1236 }
else if ( strncmp(
"SetSpeedMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1238 }
else if ( strncmp(
"SetEnabledMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1240 }
else if ( strncmp(
"SetVelocityMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1242 }
else if ( strncmp(
"SetMarginMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1244 }
else if ( strncmp(
"SetComplianceValuesMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1246 }
else if ( strncmp(
"SetGoalSpeedMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1248 }
else if ( strncmp(
"SetTorqueLimitMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1250 }
else if ( strncmp(
"SetPunchMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1252 }
else if ( strncmp(
"GotoPositionMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1254 }
else if ( strncmp(
"SetAngleLimitsMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1256 }
else if ( strncmp(
"ResetRawErrorMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1258 }
else if ( strncmp(
"SetPreventAlarmShutdownMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1260 }
else if ( strncmp(
"SetAutorecoverEnabledMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1262 }
else if ( strncmp(
"RecoverMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1266 "message type for this interface type.", type);
1275 DynamixelServoInterface::copy_values(
const Interface *other)
1280 type(), other->
type());
1282 memcpy(data, oi->data,
sizeof(DynamixelServoInterface_data_t));
1286 DynamixelServoInterface::enum_tostring(
const char *enumtype,
int val)
const
1288 if (strcmp(enumtype,
"ErrorCode") == 0) {
1289 return tostring_ErrorCode((
ErrorCode)val);
1291 if (strcmp(enumtype,
"WorkingMode") == 0) {
1306 DynamixelServoInterface::StopMessage::StopMessage() :
Message(
"StopMessage")
1311 data = (StopMessage_data_t *)
data_ptr;
1313 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
1317 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
1318 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
1335 data = (StopMessage_data_t *)
data_ptr;
1360 data_size =
sizeof(FlushMessage_data_t);
1363 data = (FlushMessage_data_t *)
data_ptr;
1365 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
1369 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
1370 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
1387 data = (FlushMessage_data_t *)
data_ptr;
1417 data = (GotoMessage_data_t *)
data_ptr;
1419 data->angle = ini_angle;
1420 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
1424 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
1425 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
1434 data = (GotoMessage_data_t *)
data_ptr;
1436 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
1440 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
1441 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
1459 data = (GotoMessage_data_t *)
data_ptr;
1517 data_size =
sizeof(TimedGotoMessage_data_t);
1520 data = (TimedGotoMessage_data_t *)
data_ptr;
1522 data->time_sec = ini_time_sec;
1523 data->angle = ini_angle;
1524 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
1528 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
1529 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
1536 data_size =
sizeof(TimedGotoMessage_data_t);
1539 data = (TimedGotoMessage_data_t *)
data_ptr;
1541 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
1545 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
1546 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
1565 data = (TimedGotoMessage_data_t *)
data_ptr;
1578 return data->time_sec;
1654 data_size =
sizeof(SetModeMessage_data_t);
1657 data = (SetModeMessage_data_t *)
data_ptr;
1659 data->mode = ini_mode;
1660 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
1664 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
1665 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
1671 data_size =
sizeof(SetModeMessage_data_t);
1674 data = (SetModeMessage_data_t *)
data_ptr;
1676 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
1680 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
1681 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
1699 data = (SetModeMessage_data_t *)
data_ptr;
1756 data_size =
sizeof(SetSpeedMessage_data_t);
1759 data = (SetSpeedMessage_data_t *)
data_ptr;
1761 data->speed = ini_speed;
1762 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
1766 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
1767 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
1773 data_size =
sizeof(SetSpeedMessage_data_t);
1776 data = (SetSpeedMessage_data_t *)
data_ptr;
1778 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
1782 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
1783 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
1801 data = (SetSpeedMessage_data_t *)
data_ptr;
1858 data_size =
sizeof(SetEnabledMessage_data_t);
1861 data = (SetEnabledMessage_data_t *)
data_ptr;
1863 data->enabled = ini_enabled;
1864 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
1868 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
1869 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
1875 data_size =
sizeof(SetEnabledMessage_data_t);
1878 data = (SetEnabledMessage_data_t *)
data_ptr;
1880 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
1884 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
1885 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
1903 data = (SetEnabledMessage_data_t *)
data_ptr;
1915 return data->enabled;
1960 data_size =
sizeof(SetVelocityMessage_data_t);
1963 data = (SetVelocityMessage_data_t *)
data_ptr;
1965 data->velocity = ini_velocity;
1966 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
1970 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
1971 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
1977 data_size =
sizeof(SetVelocityMessage_data_t);
1980 data = (SetVelocityMessage_data_t *)
data_ptr;
1982 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
1986 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
1987 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
2005 data = (SetVelocityMessage_data_t *)
data_ptr;
2017 return data->velocity;
2062 data_size =
sizeof(SetMarginMessage_data_t);
2065 data = (SetMarginMessage_data_t *)
data_ptr;
2067 data->angle_margin = ini_angle_margin;
2068 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
2072 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
2073 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
2079 data_size =
sizeof(SetMarginMessage_data_t);
2082 data = (SetMarginMessage_data_t *)
data_ptr;
2084 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
2088 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
2089 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
2107 data = (SetMarginMessage_data_t *)
data_ptr;
2122 return data->angle_margin;
2173 data_size =
sizeof(SetComplianceValuesMessage_data_t);
2176 data = (SetComplianceValuesMessage_data_t *)
data_ptr;
2178 data->cw_margin = ini_cw_margin;
2179 data->ccw_margin = ini_ccw_margin;
2180 data->cw_slope = ini_cw_slope;
2181 data->ccw_slope = ini_ccw_slope;
2182 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
2186 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
2187 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
2196 data_size =
sizeof(SetComplianceValuesMessage_data_t);
2199 data = (SetComplianceValuesMessage_data_t *)
data_ptr;
2201 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
2205 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
2206 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
2227 data = (SetComplianceValuesMessage_data_t *)
data_ptr;
2239 return data->cw_margin;
2269 return data->ccw_margin;
2299 return data->cw_slope;
2329 return data->ccw_slope;
2374 data_size =
sizeof(SetGoalSpeedMessage_data_t);
2377 data = (SetGoalSpeedMessage_data_t *)
data_ptr;
2379 data->goal_speed = ini_goal_speed;
2380 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
2384 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
2385 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
2391 data_size =
sizeof(SetGoalSpeedMessage_data_t);
2394 data = (SetGoalSpeedMessage_data_t *)
data_ptr;
2396 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
2400 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
2401 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
2419 data = (SetGoalSpeedMessage_data_t *)
data_ptr;
2431 return data->goal_speed;
2476 data_size =
sizeof(SetTorqueLimitMessage_data_t);
2479 data = (SetTorqueLimitMessage_data_t *)
data_ptr;
2481 data->torque_limit = ini_torque_limit;
2482 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
2486 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
2487 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
2493 data_size =
sizeof(SetTorqueLimitMessage_data_t);
2496 data = (SetTorqueLimitMessage_data_t *)
data_ptr;
2498 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
2502 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
2503 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
2521 data = (SetTorqueLimitMessage_data_t *)
data_ptr;
2533 return data->torque_limit;
2578 data_size =
sizeof(SetPunchMessage_data_t);
2581 data = (SetPunchMessage_data_t *)
data_ptr;
2583 data->punch = ini_punch;
2584 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
2588 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
2589 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
2595 data_size =
sizeof(SetPunchMessage_data_t);
2598 data = (SetPunchMessage_data_t *)
data_ptr;
2600 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
2604 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
2605 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
2623 data = (SetPunchMessage_data_t *)
data_ptr;
2680 data_size =
sizeof(GotoPositionMessage_data_t);
2683 data = (GotoPositionMessage_data_t *)
data_ptr;
2685 data->position = ini_position;
2686 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
2690 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
2691 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
2697 data_size =
sizeof(GotoPositionMessage_data_t);
2700 data = (GotoPositionMessage_data_t *)
data_ptr;
2702 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
2706 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
2707 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
2725 data = (GotoPositionMessage_data_t *)
data_ptr;
2737 return data->position;
2783 data_size =
sizeof(SetAngleLimitsMessage_data_t);
2786 data = (SetAngleLimitsMessage_data_t *)
data_ptr;
2788 data->angle_limit_cw = ini_angle_limit_cw;
2789 data->angle_limit_ccw = ini_angle_limit_ccw;
2790 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
2794 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
2795 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
2802 data_size =
sizeof(SetAngleLimitsMessage_data_t);
2805 data = (SetAngleLimitsMessage_data_t *)
data_ptr;
2807 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
2811 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
2812 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
2831 data = (SetAngleLimitsMessage_data_t *)
data_ptr;
2843 return data->angle_limit_cw;
2863 change_field(data->angle_limit_cw, new_angle_limit_cw);
2873 return data->angle_limit_ccw;
2893 change_field(data->angle_limit_ccw, new_angle_limit_ccw);
2916 data_size =
sizeof(ResetRawErrorMessage_data_t);
2919 data = (ResetRawErrorMessage_data_t *)
data_ptr;
2921 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
2925 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
2926 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
2943 data = (ResetRawErrorMessage_data_t *)
data_ptr;
2970 data_size =
sizeof(SetPreventAlarmShutdownMessage_data_t);
2973 data = (SetPreventAlarmShutdownMessage_data_t *)
data_ptr;
2975 data->enable_prevent_alarm_shutdown = ini_enable_prevent_alarm_shutdown;
2976 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
2980 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
2981 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
2987 data_size =
sizeof(SetPreventAlarmShutdownMessage_data_t);
2990 data = (SetPreventAlarmShutdownMessage_data_t *)
data_ptr;
2992 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
2996 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
2997 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
3015 data = (SetPreventAlarmShutdownMessage_data_t *)
data_ptr;
3027 return data->enable_prevent_alarm_shutdown;
3047 change_field(data->enable_prevent_alarm_shutdown, new_enable_prevent_alarm_shutdown);
3072 data_size =
sizeof(SetAutorecoverEnabledMessage_data_t);
3075 data = (SetAutorecoverEnabledMessage_data_t *)
data_ptr;
3077 data->autorecover_enabled = ini_autorecover_enabled;
3078 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
3082 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
3083 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
3089 data_size =
sizeof(SetAutorecoverEnabledMessage_data_t);
3092 data = (SetAutorecoverEnabledMessage_data_t *)
data_ptr;
3094 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
3098 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
3099 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
3117 data = (SetAutorecoverEnabledMessage_data_t *)
data_ptr;
3129 return data->autorecover_enabled;
3149 change_field(data->autorecover_enabled, new_autorecover_enabled);
3172 data_size =
sizeof(RecoverMessage_data_t);
3175 data = (RecoverMessage_data_t *)
data_ptr;
3177 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
3181 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
3182 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
3199 data = (RecoverMessage_data_t *)
data_ptr;
3262 if ( m10 != NULL ) {
3266 if ( m11 != NULL ) {
3270 if ( m12 != NULL ) {
3274 if ( m13 != NULL ) {
3278 if ( m14 != NULL ) {
3282 if ( m15 != NULL ) {
3286 if ( m16 != NULL ) {
3290 if ( m17 != NULL ) {
3294 if ( m18 != NULL ) {