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)
177 strncpy(data->model, new_model,
sizeof(data->model)-1);
178 data->model[
sizeof(data->model)-1] = 0;
187 DynamixelServoInterface::model_number()
const
189 return data->model_number;
197 DynamixelServoInterface::maxlenof_model_number()
const
207 DynamixelServoInterface::set_model_number(
const uint32_t new_model_number)
209 data->model_number = new_model_number;
218 DynamixelServoInterface::cw_angle_limit()
const
220 return data->cw_angle_limit;
228 DynamixelServoInterface::maxlenof_cw_angle_limit()
const
238 DynamixelServoInterface::set_cw_angle_limit(
const uint32_t new_cw_angle_limit)
240 data->cw_angle_limit = new_cw_angle_limit;
249 DynamixelServoInterface::ccw_angle_limit()
const
251 return data->ccw_angle_limit;
259 DynamixelServoInterface::maxlenof_ccw_angle_limit()
const
269 DynamixelServoInterface::set_ccw_angle_limit(
const uint32_t new_ccw_angle_limit)
271 data->ccw_angle_limit = new_ccw_angle_limit;
280 DynamixelServoInterface::temperature_limit()
const
282 return data->temperature_limit;
290 DynamixelServoInterface::maxlenof_temperature_limit()
const
300 DynamixelServoInterface::set_temperature_limit(
const uint8_t new_temperature_limit)
302 data->temperature_limit = new_temperature_limit;
311 DynamixelServoInterface::max_torque()
const
313 return data->max_torque;
321 DynamixelServoInterface::maxlenof_max_torque()
const
331 DynamixelServoInterface::set_max_torque(
const uint32_t new_max_torque)
333 data->max_torque = new_max_torque;
342 DynamixelServoInterface::cw_margin()
const
344 return data->cw_margin;
352 DynamixelServoInterface::maxlenof_cw_margin()
const
362 DynamixelServoInterface::set_cw_margin(
const uint8_t new_cw_margin)
364 data->cw_margin = new_cw_margin;
373 DynamixelServoInterface::ccw_margin()
const
375 return data->ccw_margin;
383 DynamixelServoInterface::maxlenof_ccw_margin()
const
393 DynamixelServoInterface::set_ccw_margin(
const uint8_t new_ccw_margin)
395 data->ccw_margin = new_ccw_margin;
404 DynamixelServoInterface::cw_slope()
const
406 return data->cw_slope;
414 DynamixelServoInterface::maxlenof_cw_slope()
const
424 DynamixelServoInterface::set_cw_slope(
const uint8_t new_cw_slope)
426 data->cw_slope = new_cw_slope;
435 DynamixelServoInterface::ccw_slope()
const
437 return data->ccw_slope;
445 DynamixelServoInterface::maxlenof_ccw_slope()
const
455 DynamixelServoInterface::set_ccw_slope(
const uint8_t new_ccw_slope)
457 data->ccw_slope = new_ccw_slope;
466 DynamixelServoInterface::goal_position()
const
468 return data->goal_position;
476 DynamixelServoInterface::maxlenof_goal_position()
const
486 DynamixelServoInterface::set_goal_position(
const uint32_t new_goal_position)
488 data->goal_position = new_goal_position;
497 DynamixelServoInterface::goal_speed()
const
499 return data->goal_speed;
507 DynamixelServoInterface::maxlenof_goal_speed()
const
517 DynamixelServoInterface::set_goal_speed(
const uint32_t new_goal_speed)
519 data->goal_speed = new_goal_speed;
528 DynamixelServoInterface::torque_limit()
const
530 return data->torque_limit;
538 DynamixelServoInterface::maxlenof_torque_limit()
const
548 DynamixelServoInterface::set_torque_limit(
const uint32_t new_torque_limit)
550 data->torque_limit = new_torque_limit;
559 DynamixelServoInterface::position()
const
561 return data->position;
569 DynamixelServoInterface::maxlenof_position()
const
579 DynamixelServoInterface::set_position(
const uint32_t new_position)
581 data->position = new_position;
590 DynamixelServoInterface::speed()
const
600 DynamixelServoInterface::maxlenof_speed()
const
610 DynamixelServoInterface::set_speed(
const uint32_t new_speed)
612 data->speed = new_speed;
621 DynamixelServoInterface::load()
const
631 DynamixelServoInterface::maxlenof_load()
const
641 DynamixelServoInterface::set_load(
const uint32_t new_load)
643 data->load = new_load;
652 DynamixelServoInterface::voltage()
const
654 return data->voltage;
662 DynamixelServoInterface::maxlenof_voltage()
const
672 DynamixelServoInterface::set_voltage(
const uint8_t new_voltage)
674 data->voltage = new_voltage;
683 DynamixelServoInterface::temperature()
const
685 return data->temperature;
693 DynamixelServoInterface::maxlenof_temperature()
const
703 DynamixelServoInterface::set_temperature(
const uint8_t new_temperature)
705 data->temperature = new_temperature;
714 DynamixelServoInterface::punch()
const
724 DynamixelServoInterface::maxlenof_punch()
const
734 DynamixelServoInterface::set_punch(
const uint32_t new_punch)
736 data->punch = new_punch;
755 DynamixelServoInterface::alarm_shutdown()
const
757 return data->alarm_shutdown;
765 DynamixelServoInterface::maxlenof_alarm_shutdown()
const
785 DynamixelServoInterface::set_alarm_shutdown(
const uint8_t new_alarm_shutdown)
787 data->alarm_shutdown = new_alarm_shutdown;
806 DynamixelServoInterface::error()
const
816 DynamixelServoInterface::maxlenof_error()
const
836 DynamixelServoInterface::set_error(
const uint8_t new_error)
838 data->error = new_error;
847 DynamixelServoInterface::is_enable_prevent_alarm_shutdown()
const
849 return data->enable_prevent_alarm_shutdown;
857 DynamixelServoInterface::maxlenof_enable_prevent_alarm_shutdown()
const
867 DynamixelServoInterface::set_enable_prevent_alarm_shutdown(
const bool new_enable_prevent_alarm_shutdown)
869 data->enable_prevent_alarm_shutdown = new_enable_prevent_alarm_shutdown;
878 DynamixelServoInterface::angle()
const
888 DynamixelServoInterface::maxlenof_angle()
const
898 DynamixelServoInterface::set_angle(
const float new_angle)
900 data->angle = new_angle;
909 DynamixelServoInterface::is_enabled()
const
911 return data->enabled;
919 DynamixelServoInterface::maxlenof_enabled()
const
929 DynamixelServoInterface::set_enabled(
const bool new_enabled)
931 data->enabled = new_enabled;
940 DynamixelServoInterface::min_angle()
const
942 return data->min_angle;
950 DynamixelServoInterface::maxlenof_min_angle()
const
960 DynamixelServoInterface::set_min_angle(
const float new_min_angle)
962 data->min_angle = new_min_angle;
971 DynamixelServoInterface::max_angle()
const
973 return data->max_angle;
981 DynamixelServoInterface::maxlenof_max_angle()
const
991 DynamixelServoInterface::set_max_angle(
const float new_max_angle)
993 data->max_angle = new_max_angle;
1002 DynamixelServoInterface::max_velocity()
const
1004 return data->max_velocity;
1012 DynamixelServoInterface::maxlenof_max_velocity()
const
1022 DynamixelServoInterface::set_max_velocity(
const float new_max_velocity)
1024 data->max_velocity = new_max_velocity;
1025 data_changed =
true;
1033 DynamixelServoInterface::velocity()
const
1035 return data->velocity;
1043 DynamixelServoInterface::maxlenof_velocity()
const
1053 DynamixelServoInterface::set_velocity(
const float new_velocity)
1055 data->velocity = new_velocity;
1056 data_changed =
true;
1064 DynamixelServoInterface::mode()
const
1074 DynamixelServoInterface::maxlenof_mode()
const
1084 DynamixelServoInterface::set_mode(
const char * new_mode)
1086 strncpy(data->mode, new_mode,
sizeof(data->mode)-1);
1087 data->mode[
sizeof(data->mode)-1] = 0;
1088 data_changed =
true;
1099 DynamixelServoInterface::angle_margin()
const
1101 return data->angle_margin;
1109 DynamixelServoInterface::maxlenof_angle_margin()
const
1122 DynamixelServoInterface::set_angle_margin(
const float new_angle_margin)
1124 data->angle_margin = new_angle_margin;
1125 data_changed =
true;
1133 DynamixelServoInterface::is_autorecover_enabled()
const
1135 return data->autorecover_enabled;
1143 DynamixelServoInterface::maxlenof_autorecover_enabled()
const
1153 DynamixelServoInterface::set_autorecover_enabled(
const bool new_autorecover_enabled)
1155 data->autorecover_enabled = new_autorecover_enabled;
1156 data_changed =
true;
1165 DynamixelServoInterface::msgid()
const
1175 DynamixelServoInterface::maxlenof_msgid()
const
1186 DynamixelServoInterface::set_msgid(
const uint32_t new_msgid)
1188 data->msgid = new_msgid;
1189 data_changed =
true;
1198 DynamixelServoInterface::is_final()
const
1208 DynamixelServoInterface::maxlenof_final()
const
1219 DynamixelServoInterface::set_final(
const bool new_final)
1221 data->final = new_final;
1222 data_changed =
true;
1231 DynamixelServoInterface::error_code()
const
1241 DynamixelServoInterface::maxlenof_error_code()
const
1252 DynamixelServoInterface::set_error_code(
const ErrorCode new_error_code)
1254 data->error_code = new_error_code;
1255 data_changed =
true;
1260 DynamixelServoInterface::create_message(
const char *type)
const
1262 if ( strncmp(
"StopMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1264 }
else if ( strncmp(
"FlushMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1266 }
else if ( strncmp(
"GotoMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1267 return new GotoMessage();
1268 }
else if ( strncmp(
"TimedGotoMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1269 return new TimedGotoMessage();
1270 }
else if ( strncmp(
"SetModeMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1271 return new SetModeMessage();
1272 }
else if ( strncmp(
"SetSpeedMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1273 return new SetSpeedMessage();
1274 }
else if ( strncmp(
"SetEnabledMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1275 return new SetEnabledMessage();
1276 }
else if ( strncmp(
"SetVelocityMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1277 return new SetVelocityMessage();
1278 }
else if ( strncmp(
"SetMarginMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1279 return new SetMarginMessage();
1280 }
else if ( strncmp(
"SetComplianceValuesMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1281 return new SetComplianceValuesMessage();
1282 }
else if ( strncmp(
"SetGoalSpeedMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1283 return new SetGoalSpeedMessage();
1284 }
else if ( strncmp(
"SetTorqueLimitMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1285 return new SetTorqueLimitMessage();
1286 }
else if ( strncmp(
"SetPunchMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1287 return new SetPunchMessage();
1288 }
else if ( strncmp(
"GotoPositionMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1289 return new GotoPositionMessage();
1290 }
else if ( strncmp(
"SetAngleLimitsMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1291 return new SetAngleLimitsMessage();
1292 }
else if ( strncmp(
"ResetRawErrorMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1293 return new ResetRawErrorMessage();
1294 }
else if ( strncmp(
"SetPreventAlarmShutdownMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1295 return new SetPreventAlarmShutdownMessage();
1296 }
else if ( strncmp(
"SetAutorecoverEnabledMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1297 return new SetAutorecoverEnabledMessage();
1298 }
else if ( strncmp(
"RecoverMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1299 return new RecoverMessage();
1301 throw UnknownTypeException(
"The given type '%s' does not match any known "
1302 "message type for this interface type.", type);
1311 DynamixelServoInterface::copy_values(
const Interface *other)
1313 const DynamixelServoInterface *oi =
dynamic_cast<const DynamixelServoInterface *
>(other);
1315 throw TypeMismatchException(
"Can only copy values from interface of same type (%s vs. %s)",
1316 type(), other->type());
1318 memcpy(data, oi->data,
sizeof(DynamixelServoInterface_data_t));
1322 DynamixelServoInterface::enum_tostring(
const char *enumtype,
int val)
const
1324 if (strcmp(enumtype,
"ErrorCode") == 0) {
1325 return tostring_ErrorCode((ErrorCode)val);
1327 if (strcmp(enumtype,
"WorkingMode") == 0) {
1342 DynamixelServoInterface::StopMessage::StopMessage() : Message(
"StopMessage")
1347 data = (StopMessage_data_t *)
data_ptr;
1349 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
1353 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
1354 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
1371 data = (StopMessage_data_t *)
data_ptr;
1396 data_size =
sizeof(FlushMessage_data_t);
1399 data = (FlushMessage_data_t *)
data_ptr;
1401 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
1405 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
1406 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
1423 data = (FlushMessage_data_t *)
data_ptr;
1453 data = (GotoMessage_data_t *)
data_ptr;
1455 data->angle = ini_angle;
1456 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
1460 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
1461 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
1470 data = (GotoMessage_data_t *)
data_ptr;
1472 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
1476 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
1477 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
1495 data = (GotoMessage_data_t *)
data_ptr;
1527 data->angle = new_angle;
1553 data_size =
sizeof(TimedGotoMessage_data_t);
1556 data = (TimedGotoMessage_data_t *)
data_ptr;
1558 data->time_sec = ini_time_sec;
1559 data->angle = ini_angle;
1560 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
1564 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
1565 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
1572 data_size =
sizeof(TimedGotoMessage_data_t);
1575 data = (TimedGotoMessage_data_t *)
data_ptr;
1577 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
1581 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
1582 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
1601 data = (TimedGotoMessage_data_t *)
data_ptr;
1614 return data->time_sec;
1635 data->time_sec = new_time_sec;
1665 data->angle = new_angle;
1690 data_size =
sizeof(SetModeMessage_data_t);
1693 data = (SetModeMessage_data_t *)
data_ptr;
1695 data->mode = ini_mode;
1696 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
1700 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
1701 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
1707 data_size =
sizeof(SetModeMessage_data_t);
1710 data = (SetModeMessage_data_t *)
data_ptr;
1712 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
1716 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
1717 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
1735 data = (SetModeMessage_data_t *)
data_ptr;
1767 data->mode = new_mode;
1792 data_size =
sizeof(SetSpeedMessage_data_t);
1795 data = (SetSpeedMessage_data_t *)
data_ptr;
1797 data->speed = ini_speed;
1798 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
1802 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
1803 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
1809 data_size =
sizeof(SetSpeedMessage_data_t);
1812 data = (SetSpeedMessage_data_t *)
data_ptr;
1814 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
1818 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
1819 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
1837 data = (SetSpeedMessage_data_t *)
data_ptr;
1869 data->speed = new_speed;
1894 data_size =
sizeof(SetEnabledMessage_data_t);
1897 data = (SetEnabledMessage_data_t *)
data_ptr;
1899 data->enabled = ini_enabled;
1900 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
1904 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
1905 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
1911 data_size =
sizeof(SetEnabledMessage_data_t);
1914 data = (SetEnabledMessage_data_t *)
data_ptr;
1916 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
1920 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
1921 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
1939 data = (SetEnabledMessage_data_t *)
data_ptr;
1951 return data->enabled;
1971 data->enabled = new_enabled;
1996 data_size =
sizeof(SetVelocityMessage_data_t);
1999 data = (SetVelocityMessage_data_t *)
data_ptr;
2001 data->velocity = ini_velocity;
2002 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
2006 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
2007 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
2013 data_size =
sizeof(SetVelocityMessage_data_t);
2016 data = (SetVelocityMessage_data_t *)
data_ptr;
2018 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
2022 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
2023 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
2041 data = (SetVelocityMessage_data_t *)
data_ptr;
2053 return data->velocity;
2073 data->velocity = new_velocity;
2098 data_size =
sizeof(SetMarginMessage_data_t);
2101 data = (SetMarginMessage_data_t *)
data_ptr;
2103 data->angle_margin = ini_angle_margin;
2104 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
2108 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
2109 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
2115 data_size =
sizeof(SetMarginMessage_data_t);
2118 data = (SetMarginMessage_data_t *)
data_ptr;
2120 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
2124 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
2125 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
2143 data = (SetMarginMessage_data_t *)
data_ptr;
2158 return data->angle_margin;
2181 data->angle_margin = new_angle_margin;
2209 data_size =
sizeof(SetComplianceValuesMessage_data_t);
2212 data = (SetComplianceValuesMessage_data_t *)
data_ptr;
2214 data->cw_margin = ini_cw_margin;
2215 data->ccw_margin = ini_ccw_margin;
2216 data->cw_slope = ini_cw_slope;
2217 data->ccw_slope = ini_ccw_slope;
2218 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
2222 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
2223 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
2232 data_size =
sizeof(SetComplianceValuesMessage_data_t);
2235 data = (SetComplianceValuesMessage_data_t *)
data_ptr;
2237 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
2241 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
2242 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
2263 data = (SetComplianceValuesMessage_data_t *)
data_ptr;
2275 return data->cw_margin;
2295 data->cw_margin = new_cw_margin;
2305 return data->ccw_margin;
2325 data->ccw_margin = new_ccw_margin;
2335 return data->cw_slope;
2355 data->cw_slope = new_cw_slope;
2365 return data->ccw_slope;
2385 data->ccw_slope = new_ccw_slope;
2410 data_size =
sizeof(SetGoalSpeedMessage_data_t);
2413 data = (SetGoalSpeedMessage_data_t *)
data_ptr;
2415 data->goal_speed = ini_goal_speed;
2416 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
2420 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
2421 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
2427 data_size =
sizeof(SetGoalSpeedMessage_data_t);
2430 data = (SetGoalSpeedMessage_data_t *)
data_ptr;
2432 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
2436 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
2437 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
2455 data = (SetGoalSpeedMessage_data_t *)
data_ptr;
2467 return data->goal_speed;
2487 data->goal_speed = new_goal_speed;
2512 data_size =
sizeof(SetTorqueLimitMessage_data_t);
2515 data = (SetTorqueLimitMessage_data_t *)
data_ptr;
2517 data->torque_limit = ini_torque_limit;
2518 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
2522 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
2523 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
2529 data_size =
sizeof(SetTorqueLimitMessage_data_t);
2532 data = (SetTorqueLimitMessage_data_t *)
data_ptr;
2534 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
2538 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
2539 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
2557 data = (SetTorqueLimitMessage_data_t *)
data_ptr;
2569 return data->torque_limit;
2589 data->torque_limit = new_torque_limit;
2614 data_size =
sizeof(SetPunchMessage_data_t);
2617 data = (SetPunchMessage_data_t *)
data_ptr;
2619 data->punch = ini_punch;
2620 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
2624 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
2625 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
2631 data_size =
sizeof(SetPunchMessage_data_t);
2634 data = (SetPunchMessage_data_t *)
data_ptr;
2636 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
2640 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
2641 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
2659 data = (SetPunchMessage_data_t *)
data_ptr;
2691 data->punch = new_punch;
2716 data_size =
sizeof(GotoPositionMessage_data_t);
2719 data = (GotoPositionMessage_data_t *)
data_ptr;
2721 data->position = ini_position;
2722 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
2726 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
2727 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
2733 data_size =
sizeof(GotoPositionMessage_data_t);
2736 data = (GotoPositionMessage_data_t *)
data_ptr;
2738 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
2742 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
2743 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
2761 data = (GotoPositionMessage_data_t *)
data_ptr;
2773 return data->position;
2793 data->position = new_position;
2819 data_size =
sizeof(SetAngleLimitsMessage_data_t);
2822 data = (SetAngleLimitsMessage_data_t *)
data_ptr;
2824 data->angle_limit_cw = ini_angle_limit_cw;
2825 data->angle_limit_ccw = ini_angle_limit_ccw;
2826 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
2830 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
2831 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
2838 data_size =
sizeof(SetAngleLimitsMessage_data_t);
2841 data = (SetAngleLimitsMessage_data_t *)
data_ptr;
2843 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
2847 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
2848 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
2867 data = (SetAngleLimitsMessage_data_t *)
data_ptr;
2879 return data->angle_limit_cw;
2899 data->angle_limit_cw = new_angle_limit_cw;
2909 return data->angle_limit_ccw;
2929 data->angle_limit_ccw = new_angle_limit_ccw;
2952 data_size =
sizeof(ResetRawErrorMessage_data_t);
2955 data = (ResetRawErrorMessage_data_t *)
data_ptr;
2957 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
2961 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
2962 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
2979 data = (ResetRawErrorMessage_data_t *)
data_ptr;
3006 data_size =
sizeof(SetPreventAlarmShutdownMessage_data_t);
3009 data = (SetPreventAlarmShutdownMessage_data_t *)
data_ptr;
3011 data->enable_prevent_alarm_shutdown = ini_enable_prevent_alarm_shutdown;
3012 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
3016 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
3017 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
3023 data_size =
sizeof(SetPreventAlarmShutdownMessage_data_t);
3026 data = (SetPreventAlarmShutdownMessage_data_t *)
data_ptr;
3028 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
3032 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
3033 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
3051 data = (SetPreventAlarmShutdownMessage_data_t *)
data_ptr;
3063 return data->enable_prevent_alarm_shutdown;
3083 data->enable_prevent_alarm_shutdown = new_enable_prevent_alarm_shutdown;
3108 data_size =
sizeof(SetAutorecoverEnabledMessage_data_t);
3111 data = (SetAutorecoverEnabledMessage_data_t *)
data_ptr;
3113 data->autorecover_enabled = ini_autorecover_enabled;
3114 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
3118 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
3119 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
3125 data_size =
sizeof(SetAutorecoverEnabledMessage_data_t);
3128 data = (SetAutorecoverEnabledMessage_data_t *)
data_ptr;
3130 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
3134 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
3135 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
3153 data = (SetAutorecoverEnabledMessage_data_t *)
data_ptr;
3165 return data->autorecover_enabled;
3185 data->autorecover_enabled = new_autorecover_enabled;
3208 data_size =
sizeof(RecoverMessage_data_t);
3211 data = (RecoverMessage_data_t *)
data_ptr;
3213 enum_map_ErrorCode[(int)
ERROR_NONE] =
"ERROR_NONE";
3217 enum_map_WorkingMode[(int)
JOINT] =
"JOINT";
3218 enum_map_WorkingMode[(int)
WHEEL] =
"WHEEL";
3235 data = (RecoverMessage_data_t *)
data_ptr;
3265 const GotoMessage *m2 =
dynamic_cast<const GotoMessage *
>(message);
3269 const TimedGotoMessage *m3 =
dynamic_cast<const TimedGotoMessage *
>(message);
3273 const SetModeMessage *m4 =
dynamic_cast<const SetModeMessage *
>(message);
3277 const SetSpeedMessage *m5 =
dynamic_cast<const SetSpeedMessage *
>(message);
3281 const SetEnabledMessage *m6 =
dynamic_cast<const SetEnabledMessage *
>(message);
3285 const SetVelocityMessage *m7 =
dynamic_cast<const SetVelocityMessage *
>(message);
3289 const SetMarginMessage *m8 =
dynamic_cast<const SetMarginMessage *
>(message);
3293 const SetComplianceValuesMessage *m9 =
dynamic_cast<const SetComplianceValuesMessage *
>(message);
3297 const SetGoalSpeedMessage *m10 =
dynamic_cast<const SetGoalSpeedMessage *
>(message);
3298 if ( m10 != NULL ) {
3301 const SetTorqueLimitMessage *m11 =
dynamic_cast<const SetTorqueLimitMessage *
>(message);
3302 if ( m11 != NULL ) {
3305 const SetPunchMessage *m12 =
dynamic_cast<const SetPunchMessage *
>(message);
3306 if ( m12 != NULL ) {
3309 const GotoPositionMessage *m13 =
dynamic_cast<const GotoPositionMessage *
>(message);
3310 if ( m13 != NULL ) {
3313 const SetAngleLimitsMessage *m14 =
dynamic_cast<const SetAngleLimitsMessage *
>(message);
3314 if ( m14 != NULL ) {
3317 const ResetRawErrorMessage *m15 =
dynamic_cast<const ResetRawErrorMessage *
>(message);
3318 if ( m15 != NULL ) {
3321 const SetPreventAlarmShutdownMessage *m16 =
dynamic_cast<const SetPreventAlarmShutdownMessage *
>(message);
3322 if ( m16 != NULL ) {
3325 const SetAutorecoverEnabledMessage *m17 =
dynamic_cast<const SetAutorecoverEnabledMessage *
>(message);
3326 if ( m17 != NULL ) {
3329 const RecoverMessage *m18 =
dynamic_cast<const RecoverMessage *
>(message);
3330 if ( m18 != NULL ) {
3337 EXPORT_INTERFACE(DynamixelServoInterface)