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_) == 0 ) {
1264 }
else if ( strncmp(
"FlushMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_) == 0 ) {
1266 }
else if ( strncmp(
"GotoMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_) == 0 ) {
1268 }
else if ( strncmp(
"TimedGotoMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_) == 0 ) {
1270 }
else if ( strncmp(
"SetModeMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_) == 0 ) {
1272 }
else if ( strncmp(
"SetSpeedMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_) == 0 ) {
1274 }
else if ( strncmp(
"SetEnabledMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_) == 0 ) {
1276 }
else if ( strncmp(
"SetVelocityMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_) == 0 ) {
1278 }
else if ( strncmp(
"SetMarginMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_) == 0 ) {
1280 }
else if ( strncmp(
"SetComplianceValuesMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_) == 0 ) {
1282 }
else if ( strncmp(
"SetGoalSpeedMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_) == 0 ) {
1284 }
else if ( strncmp(
"SetTorqueLimitMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_) == 0 ) {
1286 }
else if ( strncmp(
"SetPunchMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_) == 0 ) {
1288 }
else if ( strncmp(
"GotoPositionMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_) == 0 ) {
1290 }
else if ( strncmp(
"SetAngleLimitsMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_) == 0 ) {
1292 }
else if ( strncmp(
"ResetRawErrorMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_) == 0 ) {
1294 }
else if ( strncmp(
"SetPreventAlarmShutdownMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_) == 0 ) {
1296 }
else if ( strncmp(
"SetAutorecoverEnabledMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_) == 0 ) {
1298 }
else if ( strncmp(
"RecoverMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_) == 0 ) {
1302 "message type for this interface type.", type);
1311 DynamixelServoInterface::copy_values(
const Interface *other)
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;
3257 const StopMessage *m0 = dynamic_cast<const StopMessage *>(message);
3261 const FlushMessage *m1 = dynamic_cast<const FlushMessage *>(message);
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);
3289 const SetMarginMessage *m8 = dynamic_cast<const SetMarginMessage *>(message);
3298 if ( m10 != NULL ) {
3302 if ( m11 != NULL ) {
3305 const SetPunchMessage *m12 = dynamic_cast<const SetPunchMessage *>(message);
3306 if ( m12 != NULL ) {
3310 if ( m13 != NULL ) {
3314 if ( m14 != NULL ) {
3318 if ( m15 != NULL ) {
3322 if ( m16 != NULL ) {
3326 if ( m17 != NULL ) {
3329 const RecoverMessage *m18 = dynamic_cast<const RecoverMessage *>(message);
3330 if ( m18 != NULL ) {
virtual Message * clone() const
Clone this message.
SetPreventAlarmShutdownMessage()
Constructor.
ResetRawErrorMessage()
Constructor.
~TimedGotoMessage()
Destructor.
size_t maxlenof_angle() const
Get maximum length of angle value.
virtual Message * clone() const
Clone this message.
void * data_ptr
Pointer to memory that contains local data.
uint8_t cw_margin() const
Get cw_margin value.
uint32_t goal_speed() const
Get goal_speed value.
StopMessage()
Constructor.
SetVelocityMessage()
Constructor.
size_t maxlenof_angle_limit_cw() const
Get maximum length of angle_limit_cw value.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Joint mode to move in a range of -2.616 to +2.616 rad.
virtual Message * clone() const
Clone this message.
SetAutorecoverEnabledMessage Fawkes BlackBoard Interface Message.
~GotoPositionMessage()
Destructor.
void set_torque_limit(const uint32_t new_torque_limit)
Set torque_limit value.
void set_punch(const uint32_t new_punch)
Set punch value.
virtual Message * clone() const
Clone this message.
void set_angle_margin(const float new_angle_margin)
Set angle_margin value.
Communication with device failed.
virtual Message * clone() const
Clone this message.
void set_time_sec(const float new_time_sec)
Set time_sec value.
SetGoalSpeedMessage Fawkes BlackBoard Interface Message.
virtual Message * clone() const
Clone this message.
virtual Message * clone() const
Clone this message.
bool is_enabled() const
Get enabled value.
bool is_autorecover_enabled() const
Get autorecover_enabled value.
virtual Message * clone() const
Clone this message.
SetAngleLimitsMessage Fawkes BlackBoard Interface Message.
void set_angle(const float new_angle)
Set angle value.
~SetModeMessage()
Destructor.
SetAngleLimitsMessage()
Constructor.
Fawkes library namespace.
SetMarginMessage()
Constructor.
8 bit unsigned integer field
Timestamp data, must be present and first entries for each interface data structs!...
size_t maxlenof_cw_slope() const
Get maximum length of cw_slope value.
void set_cw_margin(const uint8_t new_cw_margin)
Set cw_margin value.
16 bit unsigned integer field
uint8_t cw_slope() const
Get cw_slope value.
void set_speed(const uint16_t new_speed)
Set speed value.
SetTorqueLimitMessage Fawkes BlackBoard Interface Message.
~SetPreventAlarmShutdownMessage()
Destructor.
RecoverMessage()
Constructor.
void set_position(const uint32_t new_position)
Set position value.
uint8_t mode() const
Get mode value.
~FlushMessage()
Destructor.
float time_sec() const
Get time_sec value.
~SetPunchMessage()
Destructor.
SetComplianceValuesMessage Fawkes BlackBoard Interface Message.
~SetEnabledMessage()
Destructor.
virtual Message * clone() const
Clone this message.
virtual Message * clone() const
Clone this message.
size_t maxlenof_mode() const
Get maximum length of mode value.
SetMarginMessage Fawkes BlackBoard Interface Message.
Base class for all Fawkes BlackBoard interfaces.
SetSpeedMessage()
Constructor.
size_t maxlenof_torque_limit() const
Get maximum length of torque_limit value.
virtual Message * clone() const
Clone this message.
size_t maxlenof_enable_prevent_alarm_shutdown() const
Get maximum length of enable_prevent_alarm_shutdown value.
SetEnabledMessage()
Constructor.
void set_angle_limit_cw(const uint32_t new_angle_limit_cw)
Set angle_limit_cw value.
ResetRawErrorMessage Fawkes BlackBoard Interface Message.
GotoPositionMessage()
Constructor.
StopMessage Fawkes BlackBoard Interface Message.
~SetAngleLimitsMessage()
Destructor.
Wheel mode to use the servo in a continuously rotating mode.
message_data_ts_t * data_ts
data timestamp aliasing pointer
virtual Message * clone() const
Clone this message.
~SetTorqueLimitMessage()
Destructor.
unsigned int data_size
Size of memory needed to hold all data.
size_t maxlenof_position() const
Get maximum length of position value.
SetModeMessage Fawkes BlackBoard Interface Message.
~SetVelocityMessage()
Destructor.
FlushMessage Fawkes BlackBoard Interface Message.
virtual Message * clone() const
Clone this message.
uint32_t angle_limit_cw() const
Get angle_limit_cw value.
uint32_t torque_limit() const
Get torque_limit value.
virtual Message * clone() const
Clone this message.
virtual Message * clone() const
Clone this message.
const char * type() const
Get type of interface.
void * data_ptr
Pointer to local memory storage.
FlushMessage()
Constructor.
virtual Message * clone() const
Clone this message.
void set_autorecover_enabled(const bool new_autorecover_enabled)
Set autorecover_enabled value.
SetAutorecoverEnabledMessage()
Constructor.
~SetMarginMessage()
Destructor.
SetModeMessage()
Constructor.
void set_velocity(const float new_velocity)
Set velocity value.
bool is_enable_prevent_alarm_shutdown() const
Get enable_prevent_alarm_shutdown value.
void set_goal_speed(const uint32_t new_goal_speed)
Set goal_speed value.
SetPunchMessage Fawkes BlackBoard Interface Message.
uint16_t speed() const
Get speed value.
void set_ccw_slope(const uint8_t new_ccw_slope)
Set ccw_slope value.
float angle() const
Get angle value.
uint32_t punch() const
Get punch value.
size_t maxlenof_enabled() const
Get maximum length of enabled value.
~GotoMessage()
Destructor.
size_t maxlenof_goal_speed() const
Get maximum length of goal_speed value.
TimedGotoMessage Fawkes BlackBoard Interface Message.
~StopMessage()
Destructor.
virtual Message * clone() const
Clone this message.
uint8_t ccw_margin() const
Get ccw_margin value.
SetGoalSpeedMessage()
Constructor.
~ResetRawErrorMessage()
Destructor.
~SetGoalSpeedMessage()
Destructor.
SetComplianceValuesMessage()
Constructor.
size_t maxlenof_angle_limit_ccw() const
Get maximum length of angle_limit_ccw value.
virtual Message * clone() const
Clone this message.
SetEnabledMessage Fawkes BlackBoard Interface Message.
size_t maxlenof_cw_margin() const
Get maximum length of cw_margin value.
Some unspecified error occured.
~SetComplianceValuesMessage()
Destructor.
size_t maxlenof_speed() const
Get maximum length of speed value.
size_t maxlenof_ccw_slope() const
Get maximum length of ccw_slope value.
GotoMessage()
Constructor.
~SetSpeedMessage()
Destructor.
virtual bool message_valid(const Message *message) const
Check if message is valid and can be enqueued.
size_t maxlenof_ccw_margin() const
Get maximum length of ccw_margin value.
TimedGotoMessage()
Constructor.
SetTorqueLimitMessage()
Constructor.
ErrorCode
Error code to explain an error.
virtual Message * clone() const
Clone this message.
float velocity() const
Get velocity value.
size_t maxlenof_angle() const
Get maximum length of angle value.
SetSpeedMessage Fawkes BlackBoard Interface Message.
void set_enable_prevent_alarm_shutdown(const bool new_enable_prevent_alarm_shutdown)
Set enable_prevent_alarm_shutdown value.
GotoPositionMessage Fawkes BlackBoard Interface Message.
GotoMessage Fawkes BlackBoard Interface Message.
void set_ccw_margin(const uint8_t new_ccw_margin)
Set ccw_margin value.
float angle_margin() const
Get angle_margin value.
void set_angle(const float new_angle)
Set angle value.
WorkingMode
Mode to be set for the servo.
uint32_t position() const
Get position value.
DynamixelServoInterface Fawkes BlackBoard Interface.
~SetAutorecoverEnabledMessage()
Destructor.
void set_cw_slope(const uint8_t new_cw_slope)
Set cw_slope value.
void set_enabled(const bool new_enabled)
Set enabled value.
void set_mode(const uint8_t new_mode)
Set mode value.
size_t maxlenof_angle_margin() const
Get maximum length of angle_margin value.
void set_angle_limit_ccw(const uint32_t new_angle_limit_ccw)
Set angle_limit_ccw value.
~RecoverMessage()
Destructor.
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.
uint8_t ccw_slope() const
Get ccw_slope value.
size_t maxlenof_punch() const
Get maximum length of punch value.
size_t maxlenof_autorecover_enabled() const
Get maximum length of autorecover_enabled value.
size_t maxlenof_time_sec() const
Get maximum length of time_sec value.
RecoverMessage Fawkes BlackBoard Interface Message.
float angle() const
Get angle value.
SetPunchMessage()
Constructor.
32 bit unsigned integer field
size_t maxlenof_velocity() const
Get maximum length of velocity value.
SetVelocityMessage Fawkes BlackBoard Interface Message.
uint32_t angle_limit_ccw() const
Get angle_limit_ccw value.
SetPreventAlarmShutdownMessage Fawkes BlackBoard Interface Message.
Desired angle is out of range.