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 set_field(data->model, 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 set_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 set_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 set_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 set_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 set_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 set_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 set_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 set_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 set_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 set_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 set_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 set_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 set_field(data->position, new_position);
575 DynamixelServoInterface::speed()
const
585 DynamixelServoInterface::maxlenof_speed()
const
595 DynamixelServoInterface::set_speed(
const uint32_t new_speed)
597 set_field(data->speed, new_speed);
605 DynamixelServoInterface::load()
const
615 DynamixelServoInterface::maxlenof_load()
const
625 DynamixelServoInterface::set_load(
const uint32_t new_load)
627 set_field(data->load, 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 set_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 set_field(data->temperature, new_temperature);
695 DynamixelServoInterface::punch()
const
705 DynamixelServoInterface::maxlenof_punch()
const
715 DynamixelServoInterface::set_punch(
const uint32_t new_punch)
717 set_field(data->punch, 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 set_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)
817 set_field(data->error, 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 set_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)
877 set_field(data->angle, 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 set_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 set_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 set_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 set_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 set_field(data->velocity, new_velocity);
1035 DynamixelServoInterface::mode()
const
1045 DynamixelServoInterface::maxlenof_mode()
const
1055 DynamixelServoInterface::set_mode(
const char * new_mode)
1057 set_field(data->mode, 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 set_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 set_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)
1155 set_field(data->msgid, new_msgid);
1164 DynamixelServoInterface::is_final()
const
1174 DynamixelServoInterface::maxlenof_final()
const
1185 DynamixelServoInterface::set_final(
const bool new_final)
1187 set_field(data->final, new_final);
1196 DynamixelServoInterface::error_code()
const
1206 DynamixelServoInterface::maxlenof_error_code()
const
1217 DynamixelServoInterface::set_error_code(
const ErrorCode new_error_code)
1219 set_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;
1599 set_field(data->time_sec, new_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;
2037 set_field(data->velocity, new_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;
2145 set_field(data->angle_margin, new_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;
2259 set_field(data->cw_margin, new_cw_margin);
2269 return data->ccw_margin;
2289 set_field(data->ccw_margin, new_ccw_margin);
2299 return data->cw_slope;
2319 set_field(data->cw_slope, new_cw_slope);
2329 return data->ccw_slope;
2349 set_field(data->ccw_slope, new_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;
2451 set_field(data->goal_speed, new_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;
2553 set_field(data->torque_limit, new_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;
2757 set_field(data->position, new_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 set_field(data->angle_limit_cw, new_angle_limit_cw);
2873 return data->angle_limit_ccw;
2893 set_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 set_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 set_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 ) {
FlushMessage Fawkes BlackBoard Interface Message.
~FlushMessage()
Destructor.
virtual Message * clone() const
Clone this message.
FlushMessage()
Constructor.
GotoMessage Fawkes BlackBoard Interface Message.
virtual Message * clone() const
Clone this message.
void set_angle(const float new_angle)
Set angle value.
GotoMessage()
Constructor.
~GotoMessage()
Destructor.
float angle() const
Get angle value.
size_t maxlenof_angle() const
Get maximum length of angle value.
GotoPositionMessage Fawkes BlackBoard Interface Message.
void set_position(const uint32_t new_position)
Set position value.
~GotoPositionMessage()
Destructor.
size_t maxlenof_position() const
Get maximum length of position value.
uint32_t position() const
Get position value.
virtual Message * clone() const
Clone this message.
GotoPositionMessage()
Constructor.
RecoverMessage Fawkes BlackBoard Interface Message.
virtual Message * clone() const
Clone this message.
~RecoverMessage()
Destructor.
RecoverMessage()
Constructor.
ResetRawErrorMessage Fawkes BlackBoard Interface Message.
virtual Message * clone() const
Clone this message.
ResetRawErrorMessage()
Constructor.
~ResetRawErrorMessage()
Destructor.
SetAngleLimitsMessage Fawkes BlackBoard Interface Message.
uint32_t angle_limit_cw() const
Get angle_limit_cw value.
uint32_t angle_limit_ccw() const
Get angle_limit_ccw value.
size_t maxlenof_angle_limit_cw() const
Get maximum length of angle_limit_cw value.
void set_angle_limit_ccw(const uint32_t new_angle_limit_ccw)
Set angle_limit_ccw value.
~SetAngleLimitsMessage()
Destructor.
size_t maxlenof_angle_limit_ccw() const
Get maximum length of angle_limit_ccw value.
void set_angle_limit_cw(const uint32_t new_angle_limit_cw)
Set angle_limit_cw value.
virtual Message * clone() const
Clone this message.
SetAngleLimitsMessage()
Constructor.
SetAutorecoverEnabledMessage Fawkes BlackBoard Interface Message.
~SetAutorecoverEnabledMessage()
Destructor.
void set_autorecover_enabled(const bool new_autorecover_enabled)
Set autorecover_enabled value.
virtual Message * clone() const
Clone this message.
bool is_autorecover_enabled() const
Get autorecover_enabled value.
size_t maxlenof_autorecover_enabled() const
Get maximum length of autorecover_enabled value.
SetAutorecoverEnabledMessage()
Constructor.
SetComplianceValuesMessage Fawkes BlackBoard Interface Message.
void set_ccw_margin(const uint8_t new_ccw_margin)
Set ccw_margin value.
size_t maxlenof_ccw_slope() const
Get maximum length of ccw_slope value.
SetComplianceValuesMessage()
Constructor.
~SetComplianceValuesMessage()
Destructor.
uint8_t cw_margin() const
Get cw_margin value.
virtual Message * clone() const
Clone this message.
size_t maxlenof_ccw_margin() const
Get maximum length of ccw_margin value.
uint8_t cw_slope() const
Get cw_slope value.
size_t maxlenof_cw_slope() const
Get maximum length of cw_slope value.
uint8_t ccw_margin() const
Get ccw_margin value.
size_t maxlenof_cw_margin() const
Get maximum length of cw_margin value.
void set_cw_slope(const uint8_t new_cw_slope)
Set cw_slope value.
uint8_t ccw_slope() const
Get ccw_slope value.
void set_ccw_slope(const uint8_t new_ccw_slope)
Set ccw_slope value.
void set_cw_margin(const uint8_t new_cw_margin)
Set cw_margin value.
SetEnabledMessage Fawkes BlackBoard Interface Message.
~SetEnabledMessage()
Destructor.
bool is_enabled() const
Get enabled value.
SetEnabledMessage()
Constructor.
void set_enabled(const bool new_enabled)
Set enabled value.
size_t maxlenof_enabled() const
Get maximum length of enabled value.
virtual Message * clone() const
Clone this message.
SetGoalSpeedMessage Fawkes BlackBoard Interface Message.
uint32_t goal_speed() const
Get goal_speed value.
size_t maxlenof_goal_speed() const
Get maximum length of goal_speed value.
void set_goal_speed(const uint32_t new_goal_speed)
Set goal_speed value.
~SetGoalSpeedMessage()
Destructor.
virtual Message * clone() const
Clone this message.
SetGoalSpeedMessage()
Constructor.
SetMarginMessage Fawkes BlackBoard Interface Message.
SetMarginMessage()
Constructor.
size_t maxlenof_angle_margin() const
Get maximum length of angle_margin value.
virtual Message * clone() const
Clone this message.
void set_angle_margin(const float new_angle_margin)
Set angle_margin value.
float angle_margin() const
Get angle_margin value.
~SetMarginMessage()
Destructor.
SetModeMessage Fawkes BlackBoard Interface Message.
~SetModeMessage()
Destructor.
uint8_t mode() const
Get mode value.
size_t maxlenof_mode() const
Get maximum length of mode value.
void set_mode(const uint8_t new_mode)
Set mode value.
virtual Message * clone() const
Clone this message.
SetModeMessage()
Constructor.
SetPreventAlarmShutdownMessage Fawkes BlackBoard Interface Message.
size_t maxlenof_enable_prevent_alarm_shutdown() const
Get maximum length of enable_prevent_alarm_shutdown value.
bool is_enable_prevent_alarm_shutdown() const
Get enable_prevent_alarm_shutdown value.
SetPreventAlarmShutdownMessage()
Constructor.
~SetPreventAlarmShutdownMessage()
Destructor.
void set_enable_prevent_alarm_shutdown(const bool new_enable_prevent_alarm_shutdown)
Set enable_prevent_alarm_shutdown value.
virtual Message * clone() const
Clone this message.
SetPunchMessage Fawkes BlackBoard Interface Message.
size_t maxlenof_punch() const
Get maximum length of punch value.
~SetPunchMessage()
Destructor.
uint32_t punch() const
Get punch value.
virtual Message * clone() const
Clone this message.
SetPunchMessage()
Constructor.
void set_punch(const uint32_t new_punch)
Set punch value.
SetSpeedMessage Fawkes BlackBoard Interface Message.
virtual Message * clone() const
Clone this message.
SetSpeedMessage()
Constructor.
size_t maxlenof_speed() const
Get maximum length of speed value.
~SetSpeedMessage()
Destructor.
void set_speed(const uint16_t new_speed)
Set speed value.
uint16_t speed() const
Get speed value.
SetTorqueLimitMessage Fawkes BlackBoard Interface Message.
virtual Message * clone() const
Clone this message.
void set_torque_limit(const uint32_t new_torque_limit)
Set torque_limit value.
~SetTorqueLimitMessage()
Destructor.
SetTorqueLimitMessage()
Constructor.
size_t maxlenof_torque_limit() const
Get maximum length of torque_limit value.
uint32_t torque_limit() const
Get torque_limit value.
SetVelocityMessage Fawkes BlackBoard Interface Message.
~SetVelocityMessage()
Destructor.
float velocity() const
Get velocity value.
size_t maxlenof_velocity() const
Get maximum length of velocity value.
SetVelocityMessage()
Constructor.
virtual Message * clone() const
Clone this message.
void set_velocity(const float new_velocity)
Set velocity value.
StopMessage Fawkes BlackBoard Interface Message.
~StopMessage()
Destructor.
virtual Message * clone() const
Clone this message.
StopMessage()
Constructor.
TimedGotoMessage Fawkes BlackBoard Interface Message.
void set_time_sec(const float new_time_sec)
Set time_sec value.
TimedGotoMessage()
Constructor.
float time_sec() const
Get time_sec value.
~TimedGotoMessage()
Destructor.
void set_angle(const float new_angle)
Set angle value.
float angle() const
Get angle value.
size_t maxlenof_angle() const
Get maximum length of angle value.
virtual Message * clone() const
Clone this message.
size_t maxlenof_time_sec() const
Get maximum length of time_sec value.
DynamixelServoInterface Fawkes BlackBoard Interface.
ErrorCode
Error code to explain an error.
@ ERROR_NONE
No error occured.
@ ERROR_ANGLE_OUTOFRANGE
Desired angle is out of range.
@ ERROR_COMMUNICATION
Communication with device failed.
@ ERROR_UNSPECIFIC
Some unspecified error occured.
virtual bool message_valid(const Message *message) const
Check if message is valid and can be enqueued.
WorkingMode
Mode to be set for the servo.
@ JOINT
Joint mode to move in a range of -2.616 to +2.616 rad.
@ WHEEL
Wheel mode to use the servo in a continuously rotating mode.
Base class for all Fawkes BlackBoard interfaces.
const char * type() const
Get type of interface.
void * data_ptr
Pointer to local memory storage.
void set_field(FieldT &field, DataT &data)
Set a field, set data_changed to true and update data_changed accordingly.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
void add_fieldinfo(interface_fieldtype_t type, const char *name, size_t length, void *value, const char *enumtype=0, const interface_enum_map_t *enum_map=0)
Add an entry to the info list.
void * data_ptr
Pointer to memory that contains local data.
message_data_ts_t * data_ts
data timestamp aliasing pointer
unsigned int data_size
Size of memory needed to hold all data.
Fawkes library namespace.
@ IFT_UINT32
32 bit unsigned integer field
@ IFT_UINT16
16 bit unsigned integer field
@ IFT_UINT8
8 bit unsigned integer field
Timestamp data, must be present and first entries for each interface data structs!...