Fawkes API Fawkes Development Version
DynamixelServoInterface.cpp
1
2/***************************************************************************
3 * DynamixelServoInterface.cpp - Fawkes BlackBoard Interface - DynamixelServoInterface
4 *
5 * Templated created: Thu Oct 12 10:49:19 2006
6 * Copyright 2015 Tim Niemueller, Nicolas Limpert
7 *
8 ****************************************************************************/
9
10/* This program is free software; you can redistribute it and/or modify
11 * it under the terms of the GNU General Public License as published by
12 * the Free Software Foundation; either version 2 of the License, or
13 * (at your option) any later version. A runtime exception applies to
14 * this software (see LICENSE.GPL_WRE file mentioned below for details).
15 *
16 * This program is distributed in the hope that it will be useful,
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19 * GNU Library General Public License for more details.
20 *
21 * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
22 */
23
24#include <interfaces/DynamixelServoInterface.h>
25
26#include <core/exceptions/software.h>
27
28#include <map>
29#include <string>
30#include <cstring>
31#include <cstdlib>
32
33namespace fawkes {
34
35/** @class DynamixelServoInterface <interfaces/DynamixelServoInterface.h>
36 * DynamixelServoInterface Fawkes BlackBoard Interface.
37 *
38 Interface to access Robotis Dynamixel Servos.
39
40 * @ingroup FawkesInterfaces
41 */
42
43
44
45/** Constructor */
46DynamixelServoInterface::DynamixelServoInterface() : Interface()
47{
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};
113 set_hash(tmp_hash);
114}
115
116/** Destructor */
117DynamixelServoInterface::~DynamixelServoInterface()
118{
119 free(data_ptr);
120}
121/** Convert ErrorCode constant to string.
122 * @param value value to convert to string
123 * @return constant value as string.
124 */
125const char *
126DynamixelServoInterface::tostring_ErrorCode(ErrorCode value) const
127{
128 switch (value) {
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";
134 }
135}
136/** Convert WorkingMode constant to string.
137 * @param value value to convert to string
138 * @return constant value as string.
139 */
140const char *
141DynamixelServoInterface::tostring_WorkingMode(WorkingMode value) const
142{
143 switch (value) {
144 case JOINT: return "JOINT";
145 case WHEEL: return "WHEEL";
146 default: return "UNKNOWN";
147 }
148}
149/* Methods */
150/** Get model value.
151 * Model if known
152 * @return model value
153 */
154char *
155DynamixelServoInterface::model() const
156{
157 return data->model;
158}
159
160/** Get maximum length of model value.
161 * @return length of model value, can be length of the array or number of
162 * maximum number of characters for a string
163 */
164size_t
165DynamixelServoInterface::maxlenof_model() const
166{
167 return 8;
168}
169
170/** Set model value.
171 * Model if known
172 * @param new_model new model value
173 */
174void
175DynamixelServoInterface::set_model(const char * new_model)
176{
177 set_field(data->model, new_model);
178}
179
180/** Get model_number value.
181 * Model number
182 * @return model_number value
183 */
184uint32_t
185DynamixelServoInterface::model_number() const
186{
187 return data->model_number;
188}
189
190/** Get maximum length of model_number value.
191 * @return length of model_number value, can be length of the array or number of
192 * maximum number of characters for a string
193 */
194size_t
195DynamixelServoInterface::maxlenof_model_number() const
196{
197 return 1;
198}
199
200/** Set model_number value.
201 * Model number
202 * @param new_model_number new model_number value
203 */
204void
205DynamixelServoInterface::set_model_number(const uint32_t new_model_number)
206{
207 set_field(data->model_number, new_model_number);
208}
209
210/** Get cw_angle_limit value.
211 * Clockwise angle limit
212 * @return cw_angle_limit value
213 */
214uint32_t
215DynamixelServoInterface::cw_angle_limit() const
216{
217 return data->cw_angle_limit;
218}
219
220/** Get maximum length of cw_angle_limit value.
221 * @return length of cw_angle_limit value, can be length of the array or number of
222 * maximum number of characters for a string
223 */
224size_t
225DynamixelServoInterface::maxlenof_cw_angle_limit() const
226{
227 return 1;
228}
229
230/** Set cw_angle_limit value.
231 * Clockwise angle limit
232 * @param new_cw_angle_limit new cw_angle_limit value
233 */
234void
235DynamixelServoInterface::set_cw_angle_limit(const uint32_t new_cw_angle_limit)
236{
237 set_field(data->cw_angle_limit, new_cw_angle_limit);
238}
239
240/** Get ccw_angle_limit value.
241 * Counter-clockwise angle limit
242 * @return ccw_angle_limit value
243 */
244uint32_t
245DynamixelServoInterface::ccw_angle_limit() const
246{
247 return data->ccw_angle_limit;
248}
249
250/** Get maximum length of ccw_angle_limit value.
251 * @return length of ccw_angle_limit value, can be length of the array or number of
252 * maximum number of characters for a string
253 */
254size_t
255DynamixelServoInterface::maxlenof_ccw_angle_limit() const
256{
257 return 1;
258}
259
260/** Set ccw_angle_limit value.
261 * Counter-clockwise angle limit
262 * @param new_ccw_angle_limit new ccw_angle_limit value
263 */
264void
265DynamixelServoInterface::set_ccw_angle_limit(const uint32_t new_ccw_angle_limit)
266{
267 set_field(data->ccw_angle_limit, new_ccw_angle_limit);
268}
269
270/** Get temperature_limit value.
271 * Temperature limit
272 * @return temperature_limit value
273 */
274uint8_t
275DynamixelServoInterface::temperature_limit() const
276{
277 return data->temperature_limit;
278}
279
280/** Get maximum length of temperature_limit value.
281 * @return length of temperature_limit value, can be length of the array or number of
282 * maximum number of characters for a string
283 */
284size_t
285DynamixelServoInterface::maxlenof_temperature_limit() const
286{
287 return 1;
288}
289
290/** Set temperature_limit value.
291 * Temperature limit
292 * @param new_temperature_limit new temperature_limit value
293 */
294void
295DynamixelServoInterface::set_temperature_limit(const uint8_t new_temperature_limit)
296{
297 set_field(data->temperature_limit, new_temperature_limit);
298}
299
300/** Get max_torque value.
301 * Max torque
302 * @return max_torque value
303 */
304uint32_t
305DynamixelServoInterface::max_torque() const
306{
307 return data->max_torque;
308}
309
310/** Get maximum length of max_torque value.
311 * @return length of max_torque value, can be length of the array or number of
312 * maximum number of characters for a string
313 */
314size_t
315DynamixelServoInterface::maxlenof_max_torque() const
316{
317 return 1;
318}
319
320/** Set max_torque value.
321 * Max torque
322 * @param new_max_torque new max_torque value
323 */
324void
325DynamixelServoInterface::set_max_torque(const uint32_t new_max_torque)
326{
327 set_field(data->max_torque, new_max_torque);
328}
329
330/** Get cw_margin value.
331 * CW Compliance Margin
332 * @return cw_margin value
333 */
334uint8_t
335DynamixelServoInterface::cw_margin() const
336{
337 return data->cw_margin;
338}
339
340/** Get maximum length of cw_margin value.
341 * @return length of cw_margin value, can be length of the array or number of
342 * maximum number of characters for a string
343 */
344size_t
345DynamixelServoInterface::maxlenof_cw_margin() const
346{
347 return 1;
348}
349
350/** Set cw_margin value.
351 * CW Compliance Margin
352 * @param new_cw_margin new cw_margin value
353 */
354void
355DynamixelServoInterface::set_cw_margin(const uint8_t new_cw_margin)
356{
357 set_field(data->cw_margin, new_cw_margin);
358}
359
360/** Get ccw_margin value.
361 * CCW Compliance Margin
362 * @return ccw_margin value
363 */
364uint8_t
365DynamixelServoInterface::ccw_margin() const
366{
367 return data->ccw_margin;
368}
369
370/** Get maximum length of ccw_margin value.
371 * @return length of ccw_margin value, can be length of the array or number of
372 * maximum number of characters for a string
373 */
374size_t
375DynamixelServoInterface::maxlenof_ccw_margin() const
376{
377 return 1;
378}
379
380/** Set ccw_margin value.
381 * CCW Compliance Margin
382 * @param new_ccw_margin new ccw_margin value
383 */
384void
385DynamixelServoInterface::set_ccw_margin(const uint8_t new_ccw_margin)
386{
387 set_field(data->ccw_margin, new_ccw_margin);
388}
389
390/** Get cw_slope value.
391 * CW Compliance Slope
392 * @return cw_slope value
393 */
394uint8_t
395DynamixelServoInterface::cw_slope() const
396{
397 return data->cw_slope;
398}
399
400/** Get maximum length of cw_slope value.
401 * @return length of cw_slope value, can be length of the array or number of
402 * maximum number of characters for a string
403 */
404size_t
405DynamixelServoInterface::maxlenof_cw_slope() const
406{
407 return 1;
408}
409
410/** Set cw_slope value.
411 * CW Compliance Slope
412 * @param new_cw_slope new cw_slope value
413 */
414void
415DynamixelServoInterface::set_cw_slope(const uint8_t new_cw_slope)
416{
417 set_field(data->cw_slope, new_cw_slope);
418}
419
420/** Get ccw_slope value.
421 * CCW Compliance Slope
422 * @return ccw_slope value
423 */
424uint8_t
425DynamixelServoInterface::ccw_slope() const
426{
427 return data->ccw_slope;
428}
429
430/** Get maximum length of ccw_slope value.
431 * @return length of ccw_slope value, can be length of the array or number of
432 * maximum number of characters for a string
433 */
434size_t
435DynamixelServoInterface::maxlenof_ccw_slope() const
436{
437 return 1;
438}
439
440/** Set ccw_slope value.
441 * CCW Compliance Slope
442 * @param new_ccw_slope new ccw_slope value
443 */
444void
445DynamixelServoInterface::set_ccw_slope(const uint8_t new_ccw_slope)
446{
447 set_field(data->ccw_slope, new_ccw_slope);
448}
449
450/** Get goal_position value.
451 * Goal position
452 * @return goal_position value
453 */
454uint32_t
455DynamixelServoInterface::goal_position() const
456{
457 return data->goal_position;
458}
459
460/** Get maximum length of goal_position value.
461 * @return length of goal_position value, can be length of the array or number of
462 * maximum number of characters for a string
463 */
464size_t
465DynamixelServoInterface::maxlenof_goal_position() const
466{
467 return 1;
468}
469
470/** Set goal_position value.
471 * Goal position
472 * @param new_goal_position new goal_position value
473 */
474void
475DynamixelServoInterface::set_goal_position(const uint32_t new_goal_position)
476{
477 set_field(data->goal_position, new_goal_position);
478}
479
480/** Get goal_speed value.
481 * Goal speed
482 * @return goal_speed value
483 */
484uint32_t
485DynamixelServoInterface::goal_speed() const
486{
487 return data->goal_speed;
488}
489
490/** Get maximum length of goal_speed value.
491 * @return length of goal_speed value, can be length of the array or number of
492 * maximum number of characters for a string
493 */
494size_t
495DynamixelServoInterface::maxlenof_goal_speed() const
496{
497 return 1;
498}
499
500/** Set goal_speed value.
501 * Goal speed
502 * @param new_goal_speed new goal_speed value
503 */
504void
505DynamixelServoInterface::set_goal_speed(const uint32_t new_goal_speed)
506{
507 set_field(data->goal_speed, new_goal_speed);
508}
509
510/** Get torque_limit value.
511 * Torque limit
512 * @return torque_limit value
513 */
514uint32_t
515DynamixelServoInterface::torque_limit() const
516{
517 return data->torque_limit;
518}
519
520/** Get maximum length of torque_limit value.
521 * @return length of torque_limit value, can be length of the array or number of
522 * maximum number of characters for a string
523 */
524size_t
525DynamixelServoInterface::maxlenof_torque_limit() const
526{
527 return 1;
528}
529
530/** Set torque_limit value.
531 * Torque limit
532 * @param new_torque_limit new torque_limit value
533 */
534void
535DynamixelServoInterface::set_torque_limit(const uint32_t new_torque_limit)
536{
537 set_field(data->torque_limit, new_torque_limit);
538}
539
540/** Get position value.
541 * Present position
542 * @return position value
543 */
544uint32_t
545DynamixelServoInterface::position() const
546{
547 return data->position;
548}
549
550/** Get maximum length of position value.
551 * @return length of position value, can be length of the array or number of
552 * maximum number of characters for a string
553 */
554size_t
555DynamixelServoInterface::maxlenof_position() const
556{
557 return 1;
558}
559
560/** Set position value.
561 * Present position
562 * @param new_position new position value
563 */
564void
565DynamixelServoInterface::set_position(const uint32_t new_position)
566{
567 set_field(data->position, new_position);
568}
569
570/** Get speed value.
571 * Present speed
572 * @return speed value
573 */
574uint32_t
575DynamixelServoInterface::speed() const
576{
577 return data->speed;
578}
579
580/** Get maximum length of speed value.
581 * @return length of speed value, can be length of the array or number of
582 * maximum number of characters for a string
583 */
584size_t
585DynamixelServoInterface::maxlenof_speed() const
586{
587 return 1;
588}
589
590/** Set speed value.
591 * Present speed
592 * @param new_speed new speed value
593 */
594void
595DynamixelServoInterface::set_speed(const uint32_t new_speed)
596{
597 set_field(data->speed, new_speed);
598}
599
600/** Get load value.
601 * Present load
602 * @return load value
603 */
604uint32_t
605DynamixelServoInterface::load() const
606{
607 return data->load;
608}
609
610/** Get maximum length of load value.
611 * @return length of load value, can be length of the array or number of
612 * maximum number of characters for a string
613 */
614size_t
615DynamixelServoInterface::maxlenof_load() const
616{
617 return 1;
618}
619
620/** Set load value.
621 * Present load
622 * @param new_load new load value
623 */
624void
625DynamixelServoInterface::set_load(const uint32_t new_load)
626{
627 set_field(data->load, new_load);
628}
629
630/** Get voltage value.
631 * Present voltage
632 * @return voltage value
633 */
634uint8_t
635DynamixelServoInterface::voltage() const
636{
637 return data->voltage;
638}
639
640/** Get maximum length of voltage value.
641 * @return length of voltage value, can be length of the array or number of
642 * maximum number of characters for a string
643 */
644size_t
645DynamixelServoInterface::maxlenof_voltage() const
646{
647 return 1;
648}
649
650/** Set voltage value.
651 * Present voltage
652 * @param new_voltage new voltage value
653 */
654void
655DynamixelServoInterface::set_voltage(const uint8_t new_voltage)
656{
657 set_field(data->voltage, new_voltage);
658}
659
660/** Get temperature value.
661 * Present temperature
662 * @return temperature value
663 */
664uint8_t
665DynamixelServoInterface::temperature() const
666{
667 return data->temperature;
668}
669
670/** Get maximum length of temperature value.
671 * @return length of temperature value, can be length of the array or number of
672 * maximum number of characters for a string
673 */
674size_t
675DynamixelServoInterface::maxlenof_temperature() const
676{
677 return 1;
678}
679
680/** Set temperature value.
681 * Present temperature
682 * @param new_temperature new temperature value
683 */
684void
685DynamixelServoInterface::set_temperature(const uint8_t new_temperature)
686{
687 set_field(data->temperature, new_temperature);
688}
689
690/** Get punch value.
691 * Punch
692 * @return punch value
693 */
694uint32_t
695DynamixelServoInterface::punch() const
696{
697 return data->punch;
698}
699
700/** Get maximum length of punch value.
701 * @return length of punch value, can be length of the array or number of
702 * maximum number of characters for a string
703 */
704size_t
705DynamixelServoInterface::maxlenof_punch() const
706{
707 return 1;
708}
709
710/** Set punch value.
711 * Punch
712 * @param new_punch new punch value
713 */
714void
715DynamixelServoInterface::set_punch(const uint32_t new_punch)
716{
717 set_field(data->punch, new_punch);
718}
719
720/** Get alarm_shutdown value.
721 * Alarm Shutdown.
722 The bitmask is set as follows (taken from Trossen Robotics PDF "AX-12(English).pdf"):
723 Bit 7: 0
724 Bit 6: If set to 1, torque off when an Instruction Error occurs
725 Bit 5: If set to 1, torque off when an Overload Error occurs
726 Bit 4: If set to 1, torque off when a Checksum Error occurs
727 Bit 3: If set to 1, torque off when a Range Error occurs
728 Bit 2: If set to 1, torque off when an Overheating Error occurs
729 Bit 1: If set to 1, torque off when an Angle Limit Error occurs
730 Bit 0: If set to 1, torque off when an Input Voltage Error occurs
731
732 * @return alarm_shutdown value
733 */
734uint8_t
735DynamixelServoInterface::alarm_shutdown() const
736{
737 return data->alarm_shutdown;
738}
739
740/** Get maximum length of alarm_shutdown value.
741 * @return length of alarm_shutdown value, can be length of the array or number of
742 * maximum number of characters for a string
743 */
744size_t
745DynamixelServoInterface::maxlenof_alarm_shutdown() const
746{
747 return 1;
748}
749
750/** Set alarm_shutdown value.
751 * Alarm Shutdown.
752 The bitmask is set as follows (taken from Trossen Robotics PDF "AX-12(English).pdf"):
753 Bit 7: 0
754 Bit 6: If set to 1, torque off when an Instruction Error occurs
755 Bit 5: If set to 1, torque off when an Overload Error occurs
756 Bit 4: If set to 1, torque off when a Checksum Error occurs
757 Bit 3: If set to 1, torque off when a Range Error occurs
758 Bit 2: If set to 1, torque off when an Overheating Error occurs
759 Bit 1: If set to 1, torque off when an Angle Limit Error occurs
760 Bit 0: If set to 1, torque off when an Input Voltage Error occurs
761
762 * @param new_alarm_shutdown new alarm_shutdown value
763 */
764void
765DynamixelServoInterface::set_alarm_shutdown(const uint8_t new_alarm_shutdown)
766{
767 set_field(data->alarm_shutdown, new_alarm_shutdown);
768}
769
770/** Get error value.
771 * Raw error code from servo.
772 The bitmask is set as follows (taken from Trossen Robotics PDF "AX-12(English).pdf"):
773 Bit 7: 0
774 Bit 6: Set to 1 if an undefined instruction is sent or an action instruction is sent without a Reg_Write instruction.
775 Bit 5: Set to 1 if the specified maximum torque can't control the applied load.
776 Bit 4: Set to 1 if the checksum of the instruction packet is incorrect.
777 Bit 3: Set to 1 if the instruction sent is out of the defined range.
778 Bit 2: Set to 1 if the internal temperature of the Dynamixel unit is above the operating temperature range as defined in the control table.
779 Bit 1: Set as 1 if the Goal Position is set outside of the range between CW Angle Limit and CCW Angle Limit.
780 Bit 0: Set to 1 if the voltage is out of the operating voltage range as defined in the control table.
781
782 * @return error value
783 */
784uint8_t
785DynamixelServoInterface::error() const
786{
787 return data->error;
788}
789
790/** Get maximum length of error value.
791 * @return length of error value, can be length of the array or number of
792 * maximum number of characters for a string
793 */
794size_t
795DynamixelServoInterface::maxlenof_error() const
796{
797 return 1;
798}
799
800/** Set error value.
801 * Raw error code from servo.
802 The bitmask is set as follows (taken from Trossen Robotics PDF "AX-12(English).pdf"):
803 Bit 7: 0
804 Bit 6: Set to 1 if an undefined instruction is sent or an action instruction is sent without a Reg_Write instruction.
805 Bit 5: Set to 1 if the specified maximum torque can't control the applied load.
806 Bit 4: Set to 1 if the checksum of the instruction packet is incorrect.
807 Bit 3: Set to 1 if the instruction sent is out of the defined range.
808 Bit 2: Set to 1 if the internal temperature of the Dynamixel unit is above the operating temperature range as defined in the control table.
809 Bit 1: Set as 1 if the Goal Position is set outside of the range between CW Angle Limit and CCW Angle Limit.
810 Bit 0: Set to 1 if the voltage is out of the operating voltage range as defined in the control table.
811
812 * @param new_error new error value
813 */
814void
815DynamixelServoInterface::set_error(const uint8_t new_error)
816{
817 set_field(data->error, new_error);
818}
819
820/** Get enable_prevent_alarm_shutdown value.
821 * Enable alarm shutdown
822 * @return enable_prevent_alarm_shutdown value
823 */
824bool
825DynamixelServoInterface::is_enable_prevent_alarm_shutdown() const
826{
827 return data->enable_prevent_alarm_shutdown;
828}
829
830/** Get maximum length of enable_prevent_alarm_shutdown value.
831 * @return length of enable_prevent_alarm_shutdown value, can be length of the array or number of
832 * maximum number of characters for a string
833 */
834size_t
835DynamixelServoInterface::maxlenof_enable_prevent_alarm_shutdown() const
836{
837 return 1;
838}
839
840/** Set enable_prevent_alarm_shutdown value.
841 * Enable alarm shutdown
842 * @param new_enable_prevent_alarm_shutdown new enable_prevent_alarm_shutdown value
843 */
844void
845DynamixelServoInterface::set_enable_prevent_alarm_shutdown(const bool new_enable_prevent_alarm_shutdown)
846{
847 set_field(data->enable_prevent_alarm_shutdown, new_enable_prevent_alarm_shutdown);
848}
849
850/** Get angle value.
851 * Current angle.
852 * @return angle value
853 */
854float
855DynamixelServoInterface::angle() const
856{
857 return data->angle;
858}
859
860/** Get maximum length of angle value.
861 * @return length of angle value, can be length of the array or number of
862 * maximum number of characters for a string
863 */
864size_t
865DynamixelServoInterface::maxlenof_angle() const
866{
867 return 1;
868}
869
870/** Set angle value.
871 * Current angle.
872 * @param new_angle new angle value
873 */
874void
875DynamixelServoInterface::set_angle(const float new_angle)
876{
877 set_field(data->angle, new_angle);
878}
879
880/** Get enabled value.
881 * Is the servo enabled?
882 * @return enabled value
883 */
884bool
885DynamixelServoInterface::is_enabled() const
886{
887 return data->enabled;
888}
889
890/** Get maximum length of enabled value.
891 * @return length of enabled value, can be length of the array or number of
892 * maximum number of characters for a string
893 */
894size_t
895DynamixelServoInterface::maxlenof_enabled() const
896{
897 return 1;
898}
899
900/** Set enabled value.
901 * Is the servo enabled?
902 * @param new_enabled new enabled value
903 */
904void
905DynamixelServoInterface::set_enabled(const bool new_enabled)
906{
907 set_field(data->enabled, new_enabled);
908}
909
910/** Get min_angle value.
911 * Minimum angle allowed.
912 * @return min_angle value
913 */
914float
915DynamixelServoInterface::min_angle() const
916{
917 return data->min_angle;
918}
919
920/** Get maximum length of min_angle value.
921 * @return length of min_angle value, can be length of the array or number of
922 * maximum number of characters for a string
923 */
924size_t
925DynamixelServoInterface::maxlenof_min_angle() const
926{
927 return 1;
928}
929
930/** Set min_angle value.
931 * Minimum angle allowed.
932 * @param new_min_angle new min_angle value
933 */
934void
935DynamixelServoInterface::set_min_angle(const float new_min_angle)
936{
937 set_field(data->min_angle, new_min_angle);
938}
939
940/** Get max_angle value.
941 * Maximum angle allowed.
942 * @return max_angle value
943 */
944float
945DynamixelServoInterface::max_angle() const
946{
947 return data->max_angle;
948}
949
950/** Get maximum length of max_angle value.
951 * @return length of max_angle value, can be length of the array or number of
952 * maximum number of characters for a string
953 */
954size_t
955DynamixelServoInterface::maxlenof_max_angle() const
956{
957 return 1;
958}
959
960/** Set max_angle value.
961 * Maximum angle allowed.
962 * @param new_max_angle new max_angle value
963 */
964void
965DynamixelServoInterface::set_max_angle(const float new_max_angle)
966{
967 set_field(data->max_angle, new_max_angle);
968}
969
970/** Get max_velocity value.
971 * Maximum supported velocity.
972 * @return max_velocity value
973 */
974float
975DynamixelServoInterface::max_velocity() const
976{
977 return data->max_velocity;
978}
979
980/** Get maximum length of max_velocity value.
981 * @return length of max_velocity value, can be length of the array or number of
982 * maximum number of characters for a string
983 */
984size_t
985DynamixelServoInterface::maxlenof_max_velocity() const
986{
987 return 1;
988}
989
990/** Set max_velocity value.
991 * Maximum supported velocity.
992 * @param new_max_velocity new max_velocity value
993 */
994void
995DynamixelServoInterface::set_max_velocity(const float new_max_velocity)
996{
997 set_field(data->max_velocity, new_max_velocity);
998}
999
1000/** Get velocity value.
1001 * Maximum servo velocity currently reached.
1002 * @return velocity value
1003 */
1004float
1005DynamixelServoInterface::velocity() const
1006{
1007 return data->velocity;
1008}
1009
1010/** Get maximum length of velocity value.
1011 * @return length of velocity value, can be length of the array or number of
1012 * maximum number of characters for a string
1013 */
1014size_t
1015DynamixelServoInterface::maxlenof_velocity() const
1016{
1017 return 1;
1018}
1019
1020/** Set velocity value.
1021 * Maximum servo velocity currently reached.
1022 * @param new_velocity new velocity value
1023 */
1024void
1025DynamixelServoInterface::set_velocity(const float new_velocity)
1026{
1027 set_field(data->velocity, new_velocity);
1028}
1029
1030/** Get mode value.
1031 * Working mode, can either be JOINT or WHEEL
1032 * @return mode value
1033 */
1034char *
1035DynamixelServoInterface::mode() const
1036{
1037 return data->mode;
1038}
1039
1040/** Get maximum length of mode value.
1041 * @return length of mode value, can be length of the array or number of
1042 * maximum number of characters for a string
1043 */
1044size_t
1045DynamixelServoInterface::maxlenof_mode() const
1046{
1047 return 5;
1048}
1049
1050/** Set mode value.
1051 * Working mode, can either be JOINT or WHEEL
1052 * @param new_mode new mode value
1053 */
1054void
1055DynamixelServoInterface::set_mode(const char * new_mode)
1056{
1057 set_field(data->mode, new_mode);
1058}
1059
1060/** Get angle_margin value.
1061 *
1062 Margin in radians around a target servo value to consider the
1063 motion as final.
1064
1065 * @return angle_margin value
1066 */
1067float
1068DynamixelServoInterface::angle_margin() const
1069{
1070 return data->angle_margin;
1071}
1072
1073/** Get maximum length of angle_margin value.
1074 * @return length of angle_margin value, can be length of the array or number of
1075 * maximum number of characters for a string
1076 */
1077size_t
1078DynamixelServoInterface::maxlenof_angle_margin() const
1079{
1080 return 1;
1081}
1082
1083/** Set angle_margin value.
1084 *
1085 Margin in radians around a target servo value to consider the
1086 motion as final.
1087
1088 * @param new_angle_margin new angle_margin value
1089 */
1090void
1091DynamixelServoInterface::set_angle_margin(const float new_angle_margin)
1092{
1093 set_field(data->angle_margin, new_angle_margin);
1094}
1095
1096/** Get autorecover_enabled value.
1097 * Automatically recover on alarm shutdown
1098 * @return autorecover_enabled value
1099 */
1100bool
1101DynamixelServoInterface::is_autorecover_enabled() const
1102{
1103 return data->autorecover_enabled;
1104}
1105
1106/** Get maximum length of autorecover_enabled value.
1107 * @return length of autorecover_enabled value, can be length of the array or number of
1108 * maximum number of characters for a string
1109 */
1110size_t
1111DynamixelServoInterface::maxlenof_autorecover_enabled() const
1112{
1113 return 1;
1114}
1115
1116/** Set autorecover_enabled value.
1117 * Automatically recover on alarm shutdown
1118 * @param new_autorecover_enabled new autorecover_enabled value
1119 */
1120void
1121DynamixelServoInterface::set_autorecover_enabled(const bool new_autorecover_enabled)
1122{
1123 set_field(data->autorecover_enabled, new_autorecover_enabled);
1124}
1125
1126/** Get msgid value.
1127 * The ID of the message that is currently being
1128 processed, or 0 if no message is being processed.
1129 * @return msgid value
1130 */
1131uint32_t
1132DynamixelServoInterface::msgid() const
1133{
1134 return data->msgid;
1135}
1136
1137/** Get maximum length of msgid value.
1138 * @return length of msgid value, can be length of the array or number of
1139 * maximum number of characters for a string
1140 */
1141size_t
1142DynamixelServoInterface::maxlenof_msgid() const
1143{
1144 return 1;
1145}
1146
1147/** Set msgid value.
1148 * The ID of the message that is currently being
1149 processed, or 0 if no message is being processed.
1150 * @param new_msgid new msgid value
1151 */
1152void
1153DynamixelServoInterface::set_msgid(const uint32_t new_msgid)
1154{
1155 set_field(data->msgid, new_msgid);
1156}
1157
1158/** Get final value.
1159 * True, if the last goto command has been finished,
1160 false if it is still running
1161 * @return final value
1162 */
1163bool
1164DynamixelServoInterface::is_final() const
1165{
1166 return data->final;
1167}
1168
1169/** Get maximum length of final value.
1170 * @return length of final value, can be length of the array or number of
1171 * maximum number of characters for a string
1172 */
1173size_t
1174DynamixelServoInterface::maxlenof_final() const
1175{
1176 return 1;
1177}
1178
1179/** Set final value.
1180 * True, if the last goto command has been finished,
1181 false if it is still running
1182 * @param new_final new final value
1183 */
1184void
1185DynamixelServoInterface::set_final(const bool new_final)
1186{
1187 set_field(data->final, new_final);
1188}
1189
1190/** Get error_code value.
1191 * Failure code set if
1192 final is true. ERROR_NONE if no error occured.
1193 * @return error_code value
1194 */
1196DynamixelServoInterface::error_code() const
1197{
1198 return (DynamixelServoInterface::ErrorCode)data->error_code;
1199}
1200
1201/** Get maximum length of error_code value.
1202 * @return length of error_code value, can be length of the array or number of
1203 * maximum number of characters for a string
1204 */
1205size_t
1206DynamixelServoInterface::maxlenof_error_code() const
1207{
1208 return 1;
1209}
1210
1211/** Set error_code value.
1212 * Failure code set if
1213 final is true. ERROR_NONE if no error occured.
1214 * @param new_error_code new error_code value
1215 */
1216void
1217DynamixelServoInterface::set_error_code(const ErrorCode new_error_code)
1218{
1219 set_field(data->error_code, new_error_code);
1220}
1221
1222/* =========== message create =========== */
1223Message *
1224DynamixelServoInterface::create_message(const char *type) const
1225{
1226 if ( strncmp("StopMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1227 return new StopMessage();
1228 } else if ( strncmp("FlushMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1229 return new FlushMessage();
1230 } else if ( strncmp("GotoMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1231 return new GotoMessage();
1232 } else if ( strncmp("TimedGotoMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1233 return new TimedGotoMessage();
1234 } else if ( strncmp("SetModeMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1235 return new SetModeMessage();
1236 } else if ( strncmp("SetSpeedMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1237 return new SetSpeedMessage();
1238 } else if ( strncmp("SetEnabledMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1239 return new SetEnabledMessage();
1240 } else if ( strncmp("SetVelocityMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1241 return new SetVelocityMessage();
1242 } else if ( strncmp("SetMarginMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1243 return new SetMarginMessage();
1244 } else if ( strncmp("SetComplianceValuesMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1245 return new SetComplianceValuesMessage();
1246 } else if ( strncmp("SetGoalSpeedMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1247 return new SetGoalSpeedMessage();
1248 } else if ( strncmp("SetTorqueLimitMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1249 return new SetTorqueLimitMessage();
1250 } else if ( strncmp("SetPunchMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1251 return new SetPunchMessage();
1252 } else if ( strncmp("GotoPositionMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1253 return new GotoPositionMessage();
1254 } else if ( strncmp("SetAngleLimitsMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1255 return new SetAngleLimitsMessage();
1256 } else if ( strncmp("ResetRawErrorMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1257 return new ResetRawErrorMessage();
1258 } else if ( strncmp("SetPreventAlarmShutdownMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1259 return new SetPreventAlarmShutdownMessage();
1260 } else if ( strncmp("SetAutorecoverEnabledMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1261 return new SetAutorecoverEnabledMessage();
1262 } else if ( strncmp("RecoverMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1263 return new RecoverMessage();
1264 } else {
1265 throw UnknownTypeException("The given type '%s' does not match any known "
1266 "message type for this interface type.", type);
1267 }
1268}
1269
1270
1271/** Copy values from other interface.
1272 * @param other other interface to copy values from
1273 */
1274void
1275DynamixelServoInterface::copy_values(const Interface *other)
1276{
1277 const DynamixelServoInterface *oi = dynamic_cast<const DynamixelServoInterface *>(other);
1278 if (oi == NULL) {
1279 throw TypeMismatchException("Can only copy values from interface of same type (%s vs. %s)",
1280 type(), other->type());
1281 }
1282 memcpy(data, oi->data, sizeof(DynamixelServoInterface_data_t));
1283}
1284
1285const char *
1286DynamixelServoInterface::enum_tostring(const char *enumtype, int val) const
1287{
1288 if (strcmp(enumtype, "ErrorCode") == 0) {
1289 return tostring_ErrorCode((ErrorCode)val);
1290 }
1291 if (strcmp(enumtype, "WorkingMode") == 0) {
1292 return tostring_WorkingMode((WorkingMode)val);
1293 }
1294 throw UnknownTypeException("Unknown enum type %s", enumtype);
1295}
1296
1297/* =========== messages =========== */
1298/** @class DynamixelServoInterface::StopMessage <interfaces/DynamixelServoInterface.h>
1299 * StopMessage Fawkes BlackBoard Interface Message.
1300 *
1301
1302 */
1303
1304
1305/** Constructor */
1306DynamixelServoInterface::StopMessage::StopMessage() : Message("StopMessage")
1307{
1308 data_size = sizeof(StopMessage_data_t);
1309 data_ptr = malloc(data_size);
1310 memset(data_ptr, 0, data_size);
1311 data = (StopMessage_data_t *)data_ptr;
1313 enum_map_ErrorCode[(int)ERROR_NONE] = "ERROR_NONE";
1314 enum_map_ErrorCode[(int)ERROR_UNSPECIFIC] = "ERROR_UNSPECIFIC";
1315 enum_map_ErrorCode[(int)ERROR_COMMUNICATION] = "ERROR_COMMUNICATION";
1316 enum_map_ErrorCode[(int)ERROR_ANGLE_OUTOFRANGE] = "ERROR_ANGLE_OUTOFRANGE";
1317 enum_map_WorkingMode[(int)JOINT] = "JOINT";
1318 enum_map_WorkingMode[(int)WHEEL] = "WHEEL";
1319}
1320
1321/** Destructor */
1323{
1324 free(data_ptr);
1325}
1326
1327/** Copy constructor.
1328 * @param m message to copy from
1329 */
1331{
1332 data_size = m->data_size;
1333 data_ptr = malloc(data_size);
1334 memcpy(data_ptr, m->data_ptr, data_size);
1335 data = (StopMessage_data_t *)data_ptr;
1337}
1338
1339/* Methods */
1340/** Clone this message.
1341 * Produces a message of the same type as this message and copies the
1342 * data to the new message.
1343 * @return clone of this message
1344 */
1345Message *
1347{
1348 return new DynamixelServoInterface::StopMessage(this);
1349}
1350/** @class DynamixelServoInterface::FlushMessage <interfaces/DynamixelServoInterface.h>
1351 * FlushMessage Fawkes BlackBoard Interface Message.
1352 *
1353
1354 */
1355
1356
1357/** Constructor */
1359{
1360 data_size = sizeof(FlushMessage_data_t);
1361 data_ptr = malloc(data_size);
1362 memset(data_ptr, 0, data_size);
1363 data = (FlushMessage_data_t *)data_ptr;
1365 enum_map_ErrorCode[(int)ERROR_NONE] = "ERROR_NONE";
1366 enum_map_ErrorCode[(int)ERROR_UNSPECIFIC] = "ERROR_UNSPECIFIC";
1367 enum_map_ErrorCode[(int)ERROR_COMMUNICATION] = "ERROR_COMMUNICATION";
1368 enum_map_ErrorCode[(int)ERROR_ANGLE_OUTOFRANGE] = "ERROR_ANGLE_OUTOFRANGE";
1369 enum_map_WorkingMode[(int)JOINT] = "JOINT";
1370 enum_map_WorkingMode[(int)WHEEL] = "WHEEL";
1371}
1372
1373/** Destructor */
1375{
1376 free(data_ptr);
1377}
1378
1379/** Copy constructor.
1380 * @param m message to copy from
1381 */
1383{
1384 data_size = m->data_size;
1385 data_ptr = malloc(data_size);
1386 memcpy(data_ptr, m->data_ptr, data_size);
1387 data = (FlushMessage_data_t *)data_ptr;
1389}
1390
1391/* Methods */
1392/** Clone this message.
1393 * Produces a message of the same type as this message and copies the
1394 * data to the new message.
1395 * @return clone of this message
1396 */
1397Message *
1399{
1401}
1402/** @class DynamixelServoInterface::GotoMessage <interfaces/DynamixelServoInterface.h>
1403 * GotoMessage Fawkes BlackBoard Interface Message.
1404 *
1405
1406 */
1407
1408
1409/** Constructor with initial values.
1410 * @param ini_angle initial value for angle
1411 */
1412DynamixelServoInterface::GotoMessage::GotoMessage(const float ini_angle) : Message("GotoMessage")
1413{
1414 data_size = sizeof(GotoMessage_data_t);
1415 data_ptr = malloc(data_size);
1416 memset(data_ptr, 0, data_size);
1417 data = (GotoMessage_data_t *)data_ptr;
1419 data->angle = ini_angle;
1420 enum_map_ErrorCode[(int)ERROR_NONE] = "ERROR_NONE";
1421 enum_map_ErrorCode[(int)ERROR_UNSPECIFIC] = "ERROR_UNSPECIFIC";
1422 enum_map_ErrorCode[(int)ERROR_COMMUNICATION] = "ERROR_COMMUNICATION";
1423 enum_map_ErrorCode[(int)ERROR_ANGLE_OUTOFRANGE] = "ERROR_ANGLE_OUTOFRANGE";
1424 enum_map_WorkingMode[(int)JOINT] = "JOINT";
1425 enum_map_WorkingMode[(int)WHEEL] = "WHEEL";
1426 add_fieldinfo(IFT_FLOAT, "angle", 1, &data->angle);
1427}
1428/** Constructor */
1430{
1431 data_size = sizeof(GotoMessage_data_t);
1432 data_ptr = malloc(data_size);
1433 memset(data_ptr, 0, data_size);
1434 data = (GotoMessage_data_t *)data_ptr;
1436 enum_map_ErrorCode[(int)ERROR_NONE] = "ERROR_NONE";
1437 enum_map_ErrorCode[(int)ERROR_UNSPECIFIC] = "ERROR_UNSPECIFIC";
1438 enum_map_ErrorCode[(int)ERROR_COMMUNICATION] = "ERROR_COMMUNICATION";
1439 enum_map_ErrorCode[(int)ERROR_ANGLE_OUTOFRANGE] = "ERROR_ANGLE_OUTOFRANGE";
1440 enum_map_WorkingMode[(int)JOINT] = "JOINT";
1441 enum_map_WorkingMode[(int)WHEEL] = "WHEEL";
1442 add_fieldinfo(IFT_FLOAT, "angle", 1, &data->angle);
1443}
1444
1445/** Destructor */
1447{
1448 free(data_ptr);
1449}
1450
1451/** Copy constructor.
1452 * @param m message to copy from
1453 */
1455{
1456 data_size = m->data_size;
1457 data_ptr = malloc(data_size);
1458 memcpy(data_ptr, m->data_ptr, data_size);
1459 data = (GotoMessage_data_t *)data_ptr;
1461}
1462
1463/* Methods */
1464/** Get angle value.
1465 * Current angle.
1466 * @return angle value
1467 */
1468float
1470{
1471 return data->angle;
1472}
1473
1474/** Get maximum length of angle value.
1475 * @return length of angle value, can be length of the array or number of
1476 * maximum number of characters for a string
1477 */
1478size_t
1480{
1481 return 1;
1482}
1483
1484/** Set angle value.
1485 * Current angle.
1486 * @param new_angle new angle value
1487 */
1488void
1490{
1491 set_field(data->angle, new_angle);
1492}
1493
1494/** Clone this message.
1495 * Produces a message of the same type as this message and copies the
1496 * data to the new message.
1497 * @return clone of this message
1498 */
1499Message *
1501{
1502 return new DynamixelServoInterface::GotoMessage(this);
1503}
1504/** @class DynamixelServoInterface::TimedGotoMessage <interfaces/DynamixelServoInterface.h>
1505 * TimedGotoMessage Fawkes BlackBoard Interface Message.
1506 *
1507
1508 */
1509
1510
1511/** Constructor with initial values.
1512 * @param ini_time_sec initial value for time_sec
1513 * @param ini_angle initial value for angle
1514 */
1515DynamixelServoInterface::TimedGotoMessage::TimedGotoMessage(const float ini_time_sec, const float ini_angle) : Message("TimedGotoMessage")
1516{
1517 data_size = sizeof(TimedGotoMessage_data_t);
1518 data_ptr = malloc(data_size);
1519 memset(data_ptr, 0, data_size);
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";
1525 enum_map_ErrorCode[(int)ERROR_UNSPECIFIC] = "ERROR_UNSPECIFIC";
1526 enum_map_ErrorCode[(int)ERROR_COMMUNICATION] = "ERROR_COMMUNICATION";
1527 enum_map_ErrorCode[(int)ERROR_ANGLE_OUTOFRANGE] = "ERROR_ANGLE_OUTOFRANGE";
1528 enum_map_WorkingMode[(int)JOINT] = "JOINT";
1529 enum_map_WorkingMode[(int)WHEEL] = "WHEEL";
1530 add_fieldinfo(IFT_FLOAT, "time_sec", 1, &data->time_sec);
1531 add_fieldinfo(IFT_FLOAT, "angle", 1, &data->angle);
1532}
1533/** Constructor */
1535{
1536 data_size = sizeof(TimedGotoMessage_data_t);
1537 data_ptr = malloc(data_size);
1538 memset(data_ptr, 0, data_size);
1539 data = (TimedGotoMessage_data_t *)data_ptr;
1541 enum_map_ErrorCode[(int)ERROR_NONE] = "ERROR_NONE";
1542 enum_map_ErrorCode[(int)ERROR_UNSPECIFIC] = "ERROR_UNSPECIFIC";
1543 enum_map_ErrorCode[(int)ERROR_COMMUNICATION] = "ERROR_COMMUNICATION";
1544 enum_map_ErrorCode[(int)ERROR_ANGLE_OUTOFRANGE] = "ERROR_ANGLE_OUTOFRANGE";
1545 enum_map_WorkingMode[(int)JOINT] = "JOINT";
1546 enum_map_WorkingMode[(int)WHEEL] = "WHEEL";
1547 add_fieldinfo(IFT_FLOAT, "time_sec", 1, &data->time_sec);
1548 add_fieldinfo(IFT_FLOAT, "angle", 1, &data->angle);
1549}
1550
1551/** Destructor */
1553{
1554 free(data_ptr);
1555}
1556
1557/** Copy constructor.
1558 * @param m message to copy from
1559 */
1561{
1562 data_size = m->data_size;
1563 data_ptr = malloc(data_size);
1564 memcpy(data_ptr, m->data_ptr, data_size);
1565 data = (TimedGotoMessage_data_t *)data_ptr;
1567}
1568
1569/* Methods */
1570/** Get time_sec value.
1571 * Time in seconds when to reach
1572 the final position.
1573 * @return time_sec value
1574 */
1575float
1577{
1578 return data->time_sec;
1579}
1580
1581/** Get maximum length of time_sec value.
1582 * @return length of time_sec value, can be length of the array or number of
1583 * maximum number of characters for a string
1584 */
1585size_t
1587{
1588 return 1;
1589}
1590
1591/** Set time_sec value.
1592 * Time in seconds when to reach
1593 the final position.
1594 * @param new_time_sec new time_sec value
1595 */
1596void
1598{
1599 set_field(data->time_sec, new_time_sec);
1600}
1601
1602/** Get angle value.
1603 * Current angle.
1604 * @return angle value
1605 */
1606float
1608{
1609 return data->angle;
1610}
1611
1612/** Get maximum length of angle value.
1613 * @return length of angle value, can be length of the array or number of
1614 * maximum number of characters for a string
1615 */
1616size_t
1618{
1619 return 1;
1620}
1621
1622/** Set angle value.
1623 * Current angle.
1624 * @param new_angle new angle value
1625 */
1626void
1628{
1629 set_field(data->angle, new_angle);
1630}
1631
1632/** Clone this message.
1633 * Produces a message of the same type as this message and copies the
1634 * data to the new message.
1635 * @return clone of this message
1636 */
1637Message *
1639{
1641}
1642/** @class DynamixelServoInterface::SetModeMessage <interfaces/DynamixelServoInterface.h>
1643 * SetModeMessage Fawkes BlackBoard Interface Message.
1644 *
1645
1646 */
1647
1648
1649/** Constructor with initial values.
1650 * @param ini_mode initial value for mode
1651 */
1652DynamixelServoInterface::SetModeMessage::SetModeMessage(const uint8_t ini_mode) : Message("SetModeMessage")
1653{
1654 data_size = sizeof(SetModeMessage_data_t);
1655 data_ptr = malloc(data_size);
1656 memset(data_ptr, 0, data_size);
1657 data = (SetModeMessage_data_t *)data_ptr;
1659 data->mode = ini_mode;
1660 enum_map_ErrorCode[(int)ERROR_NONE] = "ERROR_NONE";
1661 enum_map_ErrorCode[(int)ERROR_UNSPECIFIC] = "ERROR_UNSPECIFIC";
1662 enum_map_ErrorCode[(int)ERROR_COMMUNICATION] = "ERROR_COMMUNICATION";
1663 enum_map_ErrorCode[(int)ERROR_ANGLE_OUTOFRANGE] = "ERROR_ANGLE_OUTOFRANGE";
1664 enum_map_WorkingMode[(int)JOINT] = "JOINT";
1665 enum_map_WorkingMode[(int)WHEEL] = "WHEEL";
1666 add_fieldinfo(IFT_UINT8, "mode", 1, &data->mode);
1667}
1668/** Constructor */
1670{
1671 data_size = sizeof(SetModeMessage_data_t);
1672 data_ptr = malloc(data_size);
1673 memset(data_ptr, 0, data_size);
1674 data = (SetModeMessage_data_t *)data_ptr;
1676 enum_map_ErrorCode[(int)ERROR_NONE] = "ERROR_NONE";
1677 enum_map_ErrorCode[(int)ERROR_UNSPECIFIC] = "ERROR_UNSPECIFIC";
1678 enum_map_ErrorCode[(int)ERROR_COMMUNICATION] = "ERROR_COMMUNICATION";
1679 enum_map_ErrorCode[(int)ERROR_ANGLE_OUTOFRANGE] = "ERROR_ANGLE_OUTOFRANGE";
1680 enum_map_WorkingMode[(int)JOINT] = "JOINT";
1681 enum_map_WorkingMode[(int)WHEEL] = "WHEEL";
1682 add_fieldinfo(IFT_UINT8, "mode", 1, &data->mode);
1683}
1684
1685/** Destructor */
1687{
1688 free(data_ptr);
1689}
1690
1691/** Copy constructor.
1692 * @param m message to copy from
1693 */
1695{
1696 data_size = m->data_size;
1697 data_ptr = malloc(data_size);
1698 memcpy(data_ptr, m->data_ptr, data_size);
1699 data = (SetModeMessage_data_t *)data_ptr;
1701}
1702
1703/* Methods */
1704/** Get mode value.
1705 * New mode, see the enum WorkingMode in this interface
1706 * @return mode value
1707 */
1708uint8_t
1710{
1711 return data->mode;
1712}
1713
1714/** Get maximum length of mode value.
1715 * @return length of mode value, can be length of the array or number of
1716 * maximum number of characters for a string
1717 */
1718size_t
1720{
1721 return 1;
1722}
1723
1724/** Set mode value.
1725 * New mode, see the enum WorkingMode in this interface
1726 * @param new_mode new mode value
1727 */
1728void
1730{
1731 set_field(data->mode, new_mode);
1732}
1733
1734/** Clone this message.
1735 * Produces a message of the same type as this message and copies the
1736 * data to the new message.
1737 * @return clone of this message
1738 */
1739Message *
1741{
1743}
1744/** @class DynamixelServoInterface::SetSpeedMessage <interfaces/DynamixelServoInterface.h>
1745 * SetSpeedMessage Fawkes BlackBoard Interface Message.
1746 *
1747
1748 */
1749
1750
1751/** Constructor with initial values.
1752 * @param ini_speed initial value for speed
1753 */
1754DynamixelServoInterface::SetSpeedMessage::SetSpeedMessage(const uint16_t ini_speed) : Message("SetSpeedMessage")
1755{
1756 data_size = sizeof(SetSpeedMessage_data_t);
1757 data_ptr = malloc(data_size);
1758 memset(data_ptr, 0, data_size);
1759 data = (SetSpeedMessage_data_t *)data_ptr;
1761 data->speed = ini_speed;
1762 enum_map_ErrorCode[(int)ERROR_NONE] = "ERROR_NONE";
1763 enum_map_ErrorCode[(int)ERROR_UNSPECIFIC] = "ERROR_UNSPECIFIC";
1764 enum_map_ErrorCode[(int)ERROR_COMMUNICATION] = "ERROR_COMMUNICATION";
1765 enum_map_ErrorCode[(int)ERROR_ANGLE_OUTOFRANGE] = "ERROR_ANGLE_OUTOFRANGE";
1766 enum_map_WorkingMode[(int)JOINT] = "JOINT";
1767 enum_map_WorkingMode[(int)WHEEL] = "WHEEL";
1768 add_fieldinfo(IFT_UINT16, "speed", 1, &data->speed);
1769}
1770/** Constructor */
1772{
1773 data_size = sizeof(SetSpeedMessage_data_t);
1774 data_ptr = malloc(data_size);
1775 memset(data_ptr, 0, data_size);
1776 data = (SetSpeedMessage_data_t *)data_ptr;
1778 enum_map_ErrorCode[(int)ERROR_NONE] = "ERROR_NONE";
1779 enum_map_ErrorCode[(int)ERROR_UNSPECIFIC] = "ERROR_UNSPECIFIC";
1780 enum_map_ErrorCode[(int)ERROR_COMMUNICATION] = "ERROR_COMMUNICATION";
1781 enum_map_ErrorCode[(int)ERROR_ANGLE_OUTOFRANGE] = "ERROR_ANGLE_OUTOFRANGE";
1782 enum_map_WorkingMode[(int)JOINT] = "JOINT";
1783 enum_map_WorkingMode[(int)WHEEL] = "WHEEL";
1784 add_fieldinfo(IFT_UINT16, "speed", 1, &data->speed);
1785}
1786
1787/** Destructor */
1789{
1790 free(data_ptr);
1791}
1792
1793/** Copy constructor.
1794 * @param m message to copy from
1795 */
1797{
1798 data_size = m->data_size;
1799 data_ptr = malloc(data_size);
1800 memcpy(data_ptr, m->data_ptr, data_size);
1801 data = (SetSpeedMessage_data_t *)data_ptr;
1803}
1804
1805/* Methods */
1806/** Get speed value.
1807 * New speed. Used when the servo is in wheel mode. Bits 0-9 determine the rotation speed and bit 10 determines the rotation direction (0 for ccw and 1 for cw)
1808 * @return speed value
1809 */
1810uint16_t
1812{
1813 return data->speed;
1814}
1815
1816/** Get maximum length of speed value.
1817 * @return length of speed value, can be length of the array or number of
1818 * maximum number of characters for a string
1819 */
1820size_t
1822{
1823 return 1;
1824}
1825
1826/** Set speed value.
1827 * New speed. Used when the servo is in wheel mode. Bits 0-9 determine the rotation speed and bit 10 determines the rotation direction (0 for ccw and 1 for cw)
1828 * @param new_speed new speed value
1829 */
1830void
1832{
1833 set_field(data->speed, new_speed);
1834}
1835
1836/** Clone this message.
1837 * Produces a message of the same type as this message and copies the
1838 * data to the new message.
1839 * @return clone of this message
1840 */
1841Message *
1843{
1845}
1846/** @class DynamixelServoInterface::SetEnabledMessage <interfaces/DynamixelServoInterface.h>
1847 * SetEnabledMessage Fawkes BlackBoard Interface Message.
1848 *
1849
1850 */
1851
1852
1853/** Constructor with initial values.
1854 * @param ini_enabled initial value for enabled
1855 */
1857{
1858 data_size = sizeof(SetEnabledMessage_data_t);
1859 data_ptr = malloc(data_size);
1860 memset(data_ptr, 0, data_size);
1861 data = (SetEnabledMessage_data_t *)data_ptr;
1863 data->enabled = ini_enabled;
1864 enum_map_ErrorCode[(int)ERROR_NONE] = "ERROR_NONE";
1865 enum_map_ErrorCode[(int)ERROR_UNSPECIFIC] = "ERROR_UNSPECIFIC";
1866 enum_map_ErrorCode[(int)ERROR_COMMUNICATION] = "ERROR_COMMUNICATION";
1867 enum_map_ErrorCode[(int)ERROR_ANGLE_OUTOFRANGE] = "ERROR_ANGLE_OUTOFRANGE";
1868 enum_map_WorkingMode[(int)JOINT] = "JOINT";
1869 enum_map_WorkingMode[(int)WHEEL] = "WHEEL";
1870 add_fieldinfo(IFT_BOOL, "enabled", 1, &data->enabled);
1871}
1872/** Constructor */
1874{
1875 data_size = sizeof(SetEnabledMessage_data_t);
1876 data_ptr = malloc(data_size);
1877 memset(data_ptr, 0, data_size);
1878 data = (SetEnabledMessage_data_t *)data_ptr;
1880 enum_map_ErrorCode[(int)ERROR_NONE] = "ERROR_NONE";
1881 enum_map_ErrorCode[(int)ERROR_UNSPECIFIC] = "ERROR_UNSPECIFIC";
1882 enum_map_ErrorCode[(int)ERROR_COMMUNICATION] = "ERROR_COMMUNICATION";
1883 enum_map_ErrorCode[(int)ERROR_ANGLE_OUTOFRANGE] = "ERROR_ANGLE_OUTOFRANGE";
1884 enum_map_WorkingMode[(int)JOINT] = "JOINT";
1885 enum_map_WorkingMode[(int)WHEEL] = "WHEEL";
1886 add_fieldinfo(IFT_BOOL, "enabled", 1, &data->enabled);
1887}
1888
1889/** Destructor */
1891{
1892 free(data_ptr);
1893}
1894
1895/** Copy constructor.
1896 * @param m message to copy from
1897 */
1899{
1900 data_size = m->data_size;
1901 data_ptr = malloc(data_size);
1902 memcpy(data_ptr, m->data_ptr, data_size);
1903 data = (SetEnabledMessage_data_t *)data_ptr;
1905}
1906
1907/* Methods */
1908/** Get enabled value.
1909 * Is the servo enabled?
1910 * @return enabled value
1911 */
1912bool
1914{
1915 return data->enabled;
1916}
1917
1918/** Get maximum length of enabled value.
1919 * @return length of enabled value, can be length of the array or number of
1920 * maximum number of characters for a string
1921 */
1922size_t
1924{
1925 return 1;
1926}
1927
1928/** Set enabled value.
1929 * Is the servo enabled?
1930 * @param new_enabled new enabled value
1931 */
1932void
1934{
1935 set_field(data->enabled, new_enabled);
1936}
1937
1938/** Clone this message.
1939 * Produces a message of the same type as this message and copies the
1940 * data to the new message.
1941 * @return clone of this message
1942 */
1943Message *
1945{
1947}
1948/** @class DynamixelServoInterface::SetVelocityMessage <interfaces/DynamixelServoInterface.h>
1949 * SetVelocityMessage Fawkes BlackBoard Interface Message.
1950 *
1951
1952 */
1953
1954
1955/** Constructor with initial values.
1956 * @param ini_velocity initial value for velocity
1957 */
1958DynamixelServoInterface::SetVelocityMessage::SetVelocityMessage(const float ini_velocity) : Message("SetVelocityMessage")
1959{
1960 data_size = sizeof(SetVelocityMessage_data_t);
1961 data_ptr = malloc(data_size);
1962 memset(data_ptr, 0, data_size);
1963 data = (SetVelocityMessage_data_t *)data_ptr;
1965 data->velocity = ini_velocity;
1966 enum_map_ErrorCode[(int)ERROR_NONE] = "ERROR_NONE";
1967 enum_map_ErrorCode[(int)ERROR_UNSPECIFIC] = "ERROR_UNSPECIFIC";
1968 enum_map_ErrorCode[(int)ERROR_COMMUNICATION] = "ERROR_COMMUNICATION";
1969 enum_map_ErrorCode[(int)ERROR_ANGLE_OUTOFRANGE] = "ERROR_ANGLE_OUTOFRANGE";
1970 enum_map_WorkingMode[(int)JOINT] = "JOINT";
1971 enum_map_WorkingMode[(int)WHEEL] = "WHEEL";
1972 add_fieldinfo(IFT_FLOAT, "velocity", 1, &data->velocity);
1973}
1974/** Constructor */
1976{
1977 data_size = sizeof(SetVelocityMessage_data_t);
1978 data_ptr = malloc(data_size);
1979 memset(data_ptr, 0, data_size);
1980 data = (SetVelocityMessage_data_t *)data_ptr;
1982 enum_map_ErrorCode[(int)ERROR_NONE] = "ERROR_NONE";
1983 enum_map_ErrorCode[(int)ERROR_UNSPECIFIC] = "ERROR_UNSPECIFIC";
1984 enum_map_ErrorCode[(int)ERROR_COMMUNICATION] = "ERROR_COMMUNICATION";
1985 enum_map_ErrorCode[(int)ERROR_ANGLE_OUTOFRANGE] = "ERROR_ANGLE_OUTOFRANGE";
1986 enum_map_WorkingMode[(int)JOINT] = "JOINT";
1987 enum_map_WorkingMode[(int)WHEEL] = "WHEEL";
1988 add_fieldinfo(IFT_FLOAT, "velocity", 1, &data->velocity);
1989}
1990
1991/** Destructor */
1993{
1994 free(data_ptr);
1995}
1996
1997/** Copy constructor.
1998 * @param m message to copy from
1999 */
2001{
2002 data_size = m->data_size;
2003 data_ptr = malloc(data_size);
2004 memcpy(data_ptr, m->data_ptr, data_size);
2005 data = (SetVelocityMessage_data_t *)data_ptr;
2007}
2008
2009/* Methods */
2010/** Get velocity value.
2011 * Maximum servo velocity currently reached.
2012 * @return velocity value
2013 */
2014float
2016{
2017 return data->velocity;
2018}
2019
2020/** Get maximum length of velocity value.
2021 * @return length of velocity value, can be length of the array or number of
2022 * maximum number of characters for a string
2023 */
2024size_t
2026{
2027 return 1;
2028}
2029
2030/** Set velocity value.
2031 * Maximum servo velocity currently reached.
2032 * @param new_velocity new velocity value
2033 */
2034void
2036{
2037 set_field(data->velocity, new_velocity);
2038}
2039
2040/** Clone this message.
2041 * Produces a message of the same type as this message and copies the
2042 * data to the new message.
2043 * @return clone of this message
2044 */
2045Message *
2047{
2049}
2050/** @class DynamixelServoInterface::SetMarginMessage <interfaces/DynamixelServoInterface.h>
2051 * SetMarginMessage Fawkes BlackBoard Interface Message.
2052 *
2053
2054 */
2055
2056
2057/** Constructor with initial values.
2058 * @param ini_angle_margin initial value for angle_margin
2059 */
2060DynamixelServoInterface::SetMarginMessage::SetMarginMessage(const float ini_angle_margin) : Message("SetMarginMessage")
2061{
2062 data_size = sizeof(SetMarginMessage_data_t);
2063 data_ptr = malloc(data_size);
2064 memset(data_ptr, 0, data_size);
2065 data = (SetMarginMessage_data_t *)data_ptr;
2067 data->angle_margin = ini_angle_margin;
2068 enum_map_ErrorCode[(int)ERROR_NONE] = "ERROR_NONE";
2069 enum_map_ErrorCode[(int)ERROR_UNSPECIFIC] = "ERROR_UNSPECIFIC";
2070 enum_map_ErrorCode[(int)ERROR_COMMUNICATION] = "ERROR_COMMUNICATION";
2071 enum_map_ErrorCode[(int)ERROR_ANGLE_OUTOFRANGE] = "ERROR_ANGLE_OUTOFRANGE";
2072 enum_map_WorkingMode[(int)JOINT] = "JOINT";
2073 enum_map_WorkingMode[(int)WHEEL] = "WHEEL";
2074 add_fieldinfo(IFT_FLOAT, "angle_margin", 1, &data->angle_margin);
2075}
2076/** Constructor */
2078{
2079 data_size = sizeof(SetMarginMessage_data_t);
2080 data_ptr = malloc(data_size);
2081 memset(data_ptr, 0, data_size);
2082 data = (SetMarginMessage_data_t *)data_ptr;
2084 enum_map_ErrorCode[(int)ERROR_NONE] = "ERROR_NONE";
2085 enum_map_ErrorCode[(int)ERROR_UNSPECIFIC] = "ERROR_UNSPECIFIC";
2086 enum_map_ErrorCode[(int)ERROR_COMMUNICATION] = "ERROR_COMMUNICATION";
2087 enum_map_ErrorCode[(int)ERROR_ANGLE_OUTOFRANGE] = "ERROR_ANGLE_OUTOFRANGE";
2088 enum_map_WorkingMode[(int)JOINT] = "JOINT";
2089 enum_map_WorkingMode[(int)WHEEL] = "WHEEL";
2090 add_fieldinfo(IFT_FLOAT, "angle_margin", 1, &data->angle_margin);
2091}
2092
2093/** Destructor */
2095{
2096 free(data_ptr);
2097}
2098
2099/** Copy constructor.
2100 * @param m message to copy from
2101 */
2103{
2104 data_size = m->data_size;
2105 data_ptr = malloc(data_size);
2106 memcpy(data_ptr, m->data_ptr, data_size);
2107 data = (SetMarginMessage_data_t *)data_ptr;
2109}
2110
2111/* Methods */
2112/** Get angle_margin value.
2113 *
2114 Margin in radians around a target servo value to consider the
2115 motion as final.
2116
2117 * @return angle_margin value
2118 */
2119float
2121{
2122 return data->angle_margin;
2123}
2124
2125/** Get maximum length of angle_margin value.
2126 * @return length of angle_margin value, can be length of the array or number of
2127 * maximum number of characters for a string
2128 */
2129size_t
2131{
2132 return 1;
2133}
2134
2135/** Set angle_margin value.
2136 *
2137 Margin in radians around a target servo value to consider the
2138 motion as final.
2139
2140 * @param new_angle_margin new angle_margin value
2141 */
2142void
2144{
2145 set_field(data->angle_margin, new_angle_margin);
2146}
2147
2148/** Clone this message.
2149 * Produces a message of the same type as this message and copies the
2150 * data to the new message.
2151 * @return clone of this message
2152 */
2153Message *
2155{
2157}
2158/** @class DynamixelServoInterface::SetComplianceValuesMessage <interfaces/DynamixelServoInterface.h>
2159 * SetComplianceValuesMessage Fawkes BlackBoard Interface Message.
2160 *
2161
2162 */
2163
2164
2165/** Constructor with initial values.
2166 * @param ini_cw_margin initial value for cw_margin
2167 * @param ini_ccw_margin initial value for ccw_margin
2168 * @param ini_cw_slope initial value for cw_slope
2169 * @param ini_ccw_slope initial value for ccw_slope
2170 */
2171DynamixelServoInterface::SetComplianceValuesMessage::SetComplianceValuesMessage(const uint8_t ini_cw_margin, const uint8_t ini_ccw_margin, const uint8_t ini_cw_slope, const uint8_t ini_ccw_slope) : Message("SetComplianceValuesMessage")
2172{
2173 data_size = sizeof(SetComplianceValuesMessage_data_t);
2174 data_ptr = malloc(data_size);
2175 memset(data_ptr, 0, data_size);
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";
2183 enum_map_ErrorCode[(int)ERROR_UNSPECIFIC] = "ERROR_UNSPECIFIC";
2184 enum_map_ErrorCode[(int)ERROR_COMMUNICATION] = "ERROR_COMMUNICATION";
2185 enum_map_ErrorCode[(int)ERROR_ANGLE_OUTOFRANGE] = "ERROR_ANGLE_OUTOFRANGE";
2186 enum_map_WorkingMode[(int)JOINT] = "JOINT";
2187 enum_map_WorkingMode[(int)WHEEL] = "WHEEL";
2188 add_fieldinfo(IFT_UINT8, "cw_margin", 1, &data->cw_margin);
2189 add_fieldinfo(IFT_UINT8, "ccw_margin", 1, &data->ccw_margin);
2190 add_fieldinfo(IFT_UINT8, "cw_slope", 1, &data->cw_slope);
2191 add_fieldinfo(IFT_UINT8, "ccw_slope", 1, &data->ccw_slope);
2192}
2193/** Constructor */
2195{
2196 data_size = sizeof(SetComplianceValuesMessage_data_t);
2197 data_ptr = malloc(data_size);
2198 memset(data_ptr, 0, data_size);
2199 data = (SetComplianceValuesMessage_data_t *)data_ptr;
2201 enum_map_ErrorCode[(int)ERROR_NONE] = "ERROR_NONE";
2202 enum_map_ErrorCode[(int)ERROR_UNSPECIFIC] = "ERROR_UNSPECIFIC";
2203 enum_map_ErrorCode[(int)ERROR_COMMUNICATION] = "ERROR_COMMUNICATION";
2204 enum_map_ErrorCode[(int)ERROR_ANGLE_OUTOFRANGE] = "ERROR_ANGLE_OUTOFRANGE";
2205 enum_map_WorkingMode[(int)JOINT] = "JOINT";
2206 enum_map_WorkingMode[(int)WHEEL] = "WHEEL";
2207 add_fieldinfo(IFT_UINT8, "cw_margin", 1, &data->cw_margin);
2208 add_fieldinfo(IFT_UINT8, "ccw_margin", 1, &data->ccw_margin);
2209 add_fieldinfo(IFT_UINT8, "cw_slope", 1, &data->cw_slope);
2210 add_fieldinfo(IFT_UINT8, "ccw_slope", 1, &data->ccw_slope);
2211}
2212
2213/** Destructor */
2215{
2216 free(data_ptr);
2217}
2218
2219/** Copy constructor.
2220 * @param m message to copy from
2221 */
2223{
2224 data_size = m->data_size;
2225 data_ptr = malloc(data_size);
2226 memcpy(data_ptr, m->data_ptr, data_size);
2227 data = (SetComplianceValuesMessage_data_t *)data_ptr;
2229}
2230
2231/* Methods */
2232/** Get cw_margin value.
2233 * New CW compliance margin
2234 * @return cw_margin value
2235 */
2236uint8_t
2238{
2239 return data->cw_margin;
2240}
2241
2242/** Get maximum length of cw_margin value.
2243 * @return length of cw_margin value, can be length of the array or number of
2244 * maximum number of characters for a string
2245 */
2246size_t
2248{
2249 return 1;
2250}
2251
2252/** Set cw_margin value.
2253 * New CW compliance margin
2254 * @param new_cw_margin new cw_margin value
2255 */
2256void
2258{
2259 set_field(data->cw_margin, new_cw_margin);
2260}
2261
2262/** Get ccw_margin value.
2263 * New CCW compliance margin
2264 * @return ccw_margin value
2265 */
2266uint8_t
2268{
2269 return data->ccw_margin;
2270}
2271
2272/** Get maximum length of ccw_margin value.
2273 * @return length of ccw_margin value, can be length of the array or number of
2274 * maximum number of characters for a string
2275 */
2276size_t
2278{
2279 return 1;
2280}
2281
2282/** Set ccw_margin value.
2283 * New CCW compliance margin
2284 * @param new_ccw_margin new ccw_margin value
2285 */
2286void
2288{
2289 set_field(data->ccw_margin, new_ccw_margin);
2290}
2291
2292/** Get cw_slope value.
2293 * New CW compliance slope
2294 * @return cw_slope value
2295 */
2296uint8_t
2298{
2299 return data->cw_slope;
2300}
2301
2302/** Get maximum length of cw_slope value.
2303 * @return length of cw_slope value, can be length of the array or number of
2304 * maximum number of characters for a string
2305 */
2306size_t
2308{
2309 return 1;
2310}
2311
2312/** Set cw_slope value.
2313 * New CW compliance slope
2314 * @param new_cw_slope new cw_slope value
2315 */
2316void
2318{
2319 set_field(data->cw_slope, new_cw_slope);
2320}
2321
2322/** Get ccw_slope value.
2323 * New LED enabled value
2324 * @return ccw_slope value
2325 */
2326uint8_t
2328{
2329 return data->ccw_slope;
2330}
2331
2332/** Get maximum length of ccw_slope value.
2333 * @return length of ccw_slope value, can be length of the array or number of
2334 * maximum number of characters for a string
2335 */
2336size_t
2338{
2339 return 1;
2340}
2341
2342/** Set ccw_slope value.
2343 * New LED enabled value
2344 * @param new_ccw_slope new ccw_slope value
2345 */
2346void
2348{
2349 set_field(data->ccw_slope, new_ccw_slope);
2350}
2351
2352/** Clone this message.
2353 * Produces a message of the same type as this message and copies the
2354 * data to the new message.
2355 * @return clone of this message
2356 */
2357Message *
2359{
2361}
2362/** @class DynamixelServoInterface::SetGoalSpeedMessage <interfaces/DynamixelServoInterface.h>
2363 * SetGoalSpeedMessage Fawkes BlackBoard Interface Message.
2364 *
2365
2366 */
2367
2368
2369/** Constructor with initial values.
2370 * @param ini_goal_speed initial value for goal_speed
2371 */
2372DynamixelServoInterface::SetGoalSpeedMessage::SetGoalSpeedMessage(const uint32_t ini_goal_speed) : Message("SetGoalSpeedMessage")
2373{
2374 data_size = sizeof(SetGoalSpeedMessage_data_t);
2375 data_ptr = malloc(data_size);
2376 memset(data_ptr, 0, data_size);
2377 data = (SetGoalSpeedMessage_data_t *)data_ptr;
2379 data->goal_speed = ini_goal_speed;
2380 enum_map_ErrorCode[(int)ERROR_NONE] = "ERROR_NONE";
2381 enum_map_ErrorCode[(int)ERROR_UNSPECIFIC] = "ERROR_UNSPECIFIC";
2382 enum_map_ErrorCode[(int)ERROR_COMMUNICATION] = "ERROR_COMMUNICATION";
2383 enum_map_ErrorCode[(int)ERROR_ANGLE_OUTOFRANGE] = "ERROR_ANGLE_OUTOFRANGE";
2384 enum_map_WorkingMode[(int)JOINT] = "JOINT";
2385 enum_map_WorkingMode[(int)WHEEL] = "WHEEL";
2386 add_fieldinfo(IFT_UINT32, "goal_speed", 1, &data->goal_speed);
2387}
2388/** Constructor */
2390{
2391 data_size = sizeof(SetGoalSpeedMessage_data_t);
2392 data_ptr = malloc(data_size);
2393 memset(data_ptr, 0, data_size);
2394 data = (SetGoalSpeedMessage_data_t *)data_ptr;
2396 enum_map_ErrorCode[(int)ERROR_NONE] = "ERROR_NONE";
2397 enum_map_ErrorCode[(int)ERROR_UNSPECIFIC] = "ERROR_UNSPECIFIC";
2398 enum_map_ErrorCode[(int)ERROR_COMMUNICATION] = "ERROR_COMMUNICATION";
2399 enum_map_ErrorCode[(int)ERROR_ANGLE_OUTOFRANGE] = "ERROR_ANGLE_OUTOFRANGE";
2400 enum_map_WorkingMode[(int)JOINT] = "JOINT";
2401 enum_map_WorkingMode[(int)WHEEL] = "WHEEL";
2402 add_fieldinfo(IFT_UINT32, "goal_speed", 1, &data->goal_speed);
2403}
2404
2405/** Destructor */
2407{
2408 free(data_ptr);
2409}
2410
2411/** Copy constructor.
2412 * @param m message to copy from
2413 */
2415{
2416 data_size = m->data_size;
2417 data_ptr = malloc(data_size);
2418 memcpy(data_ptr, m->data_ptr, data_size);
2419 data = (SetGoalSpeedMessage_data_t *)data_ptr;
2421}
2422
2423/* Methods */
2424/** Get goal_speed value.
2425 * New goal speed
2426 * @return goal_speed value
2427 */
2428uint32_t
2430{
2431 return data->goal_speed;
2432}
2433
2434/** Get maximum length of goal_speed value.
2435 * @return length of goal_speed value, can be length of the array or number of
2436 * maximum number of characters for a string
2437 */
2438size_t
2440{
2441 return 1;
2442}
2443
2444/** Set goal_speed value.
2445 * New goal speed
2446 * @param new_goal_speed new goal_speed value
2447 */
2448void
2450{
2451 set_field(data->goal_speed, new_goal_speed);
2452}
2453
2454/** Clone this message.
2455 * Produces a message of the same type as this message and copies the
2456 * data to the new message.
2457 * @return clone of this message
2458 */
2459Message *
2461{
2463}
2464/** @class DynamixelServoInterface::SetTorqueLimitMessage <interfaces/DynamixelServoInterface.h>
2465 * SetTorqueLimitMessage Fawkes BlackBoard Interface Message.
2466 *
2467
2468 */
2469
2470
2471/** Constructor with initial values.
2472 * @param ini_torque_limit initial value for torque_limit
2473 */
2474DynamixelServoInterface::SetTorqueLimitMessage::SetTorqueLimitMessage(const uint32_t ini_torque_limit) : Message("SetTorqueLimitMessage")
2475{
2476 data_size = sizeof(SetTorqueLimitMessage_data_t);
2477 data_ptr = malloc(data_size);
2478 memset(data_ptr, 0, data_size);
2479 data = (SetTorqueLimitMessage_data_t *)data_ptr;
2481 data->torque_limit = ini_torque_limit;
2482 enum_map_ErrorCode[(int)ERROR_NONE] = "ERROR_NONE";
2483 enum_map_ErrorCode[(int)ERROR_UNSPECIFIC] = "ERROR_UNSPECIFIC";
2484 enum_map_ErrorCode[(int)ERROR_COMMUNICATION] = "ERROR_COMMUNICATION";
2485 enum_map_ErrorCode[(int)ERROR_ANGLE_OUTOFRANGE] = "ERROR_ANGLE_OUTOFRANGE";
2486 enum_map_WorkingMode[(int)JOINT] = "JOINT";
2487 enum_map_WorkingMode[(int)WHEEL] = "WHEEL";
2488 add_fieldinfo(IFT_UINT32, "torque_limit", 1, &data->torque_limit);
2489}
2490/** Constructor */
2492{
2493 data_size = sizeof(SetTorqueLimitMessage_data_t);
2494 data_ptr = malloc(data_size);
2495 memset(data_ptr, 0, data_size);
2496 data = (SetTorqueLimitMessage_data_t *)data_ptr;
2498 enum_map_ErrorCode[(int)ERROR_NONE] = "ERROR_NONE";
2499 enum_map_ErrorCode[(int)ERROR_UNSPECIFIC] = "ERROR_UNSPECIFIC";
2500 enum_map_ErrorCode[(int)ERROR_COMMUNICATION] = "ERROR_COMMUNICATION";
2501 enum_map_ErrorCode[(int)ERROR_ANGLE_OUTOFRANGE] = "ERROR_ANGLE_OUTOFRANGE";
2502 enum_map_WorkingMode[(int)JOINT] = "JOINT";
2503 enum_map_WorkingMode[(int)WHEEL] = "WHEEL";
2504 add_fieldinfo(IFT_UINT32, "torque_limit", 1, &data->torque_limit);
2505}
2506
2507/** Destructor */
2509{
2510 free(data_ptr);
2511}
2512
2513/** Copy constructor.
2514 * @param m message to copy from
2515 */
2517{
2518 data_size = m->data_size;
2519 data_ptr = malloc(data_size);
2520 memcpy(data_ptr, m->data_ptr, data_size);
2521 data = (SetTorqueLimitMessage_data_t *)data_ptr;
2523}
2524
2525/* Methods */
2526/** Get torque_limit value.
2527 * New torque limit
2528 * @return torque_limit value
2529 */
2530uint32_t
2532{
2533 return data->torque_limit;
2534}
2535
2536/** Get maximum length of torque_limit value.
2537 * @return length of torque_limit value, can be length of the array or number of
2538 * maximum number of characters for a string
2539 */
2540size_t
2542{
2543 return 1;
2544}
2545
2546/** Set torque_limit value.
2547 * New torque limit
2548 * @param new_torque_limit new torque_limit value
2549 */
2550void
2552{
2553 set_field(data->torque_limit, new_torque_limit);
2554}
2555
2556/** Clone this message.
2557 * Produces a message of the same type as this message and copies the
2558 * data to the new message.
2559 * @return clone of this message
2560 */
2561Message *
2563{
2565}
2566/** @class DynamixelServoInterface::SetPunchMessage <interfaces/DynamixelServoInterface.h>
2567 * SetPunchMessage Fawkes BlackBoard Interface Message.
2568 *
2569
2570 */
2571
2572
2573/** Constructor with initial values.
2574 * @param ini_punch initial value for punch
2575 */
2576DynamixelServoInterface::SetPunchMessage::SetPunchMessage(const uint32_t ini_punch) : Message("SetPunchMessage")
2577{
2578 data_size = sizeof(SetPunchMessage_data_t);
2579 data_ptr = malloc(data_size);
2580 memset(data_ptr, 0, data_size);
2581 data = (SetPunchMessage_data_t *)data_ptr;
2583 data->punch = ini_punch;
2584 enum_map_ErrorCode[(int)ERROR_NONE] = "ERROR_NONE";
2585 enum_map_ErrorCode[(int)ERROR_UNSPECIFIC] = "ERROR_UNSPECIFIC";
2586 enum_map_ErrorCode[(int)ERROR_COMMUNICATION] = "ERROR_COMMUNICATION";
2587 enum_map_ErrorCode[(int)ERROR_ANGLE_OUTOFRANGE] = "ERROR_ANGLE_OUTOFRANGE";
2588 enum_map_WorkingMode[(int)JOINT] = "JOINT";
2589 enum_map_WorkingMode[(int)WHEEL] = "WHEEL";
2590 add_fieldinfo(IFT_UINT32, "punch", 1, &data->punch);
2591}
2592/** Constructor */
2594{
2595 data_size = sizeof(SetPunchMessage_data_t);
2596 data_ptr = malloc(data_size);
2597 memset(data_ptr, 0, data_size);
2598 data = (SetPunchMessage_data_t *)data_ptr;
2600 enum_map_ErrorCode[(int)ERROR_NONE] = "ERROR_NONE";
2601 enum_map_ErrorCode[(int)ERROR_UNSPECIFIC] = "ERROR_UNSPECIFIC";
2602 enum_map_ErrorCode[(int)ERROR_COMMUNICATION] = "ERROR_COMMUNICATION";
2603 enum_map_ErrorCode[(int)ERROR_ANGLE_OUTOFRANGE] = "ERROR_ANGLE_OUTOFRANGE";
2604 enum_map_WorkingMode[(int)JOINT] = "JOINT";
2605 enum_map_WorkingMode[(int)WHEEL] = "WHEEL";
2606 add_fieldinfo(IFT_UINT32, "punch", 1, &data->punch);
2607}
2608
2609/** Destructor */
2611{
2612 free(data_ptr);
2613}
2614
2615/** Copy constructor.
2616 * @param m message to copy from
2617 */
2619{
2620 data_size = m->data_size;
2621 data_ptr = malloc(data_size);
2622 memcpy(data_ptr, m->data_ptr, data_size);
2623 data = (SetPunchMessage_data_t *)data_ptr;
2625}
2626
2627/* Methods */
2628/** Get punch value.
2629 * New punch
2630 * @return punch value
2631 */
2632uint32_t
2634{
2635 return data->punch;
2636}
2637
2638/** Get maximum length of punch value.
2639 * @return length of punch value, can be length of the array or number of
2640 * maximum number of characters for a string
2641 */
2642size_t
2644{
2645 return 1;
2646}
2647
2648/** Set punch value.
2649 * New punch
2650 * @param new_punch new punch value
2651 */
2652void
2654{
2655 set_field(data->punch, new_punch);
2656}
2657
2658/** Clone this message.
2659 * Produces a message of the same type as this message and copies the
2660 * data to the new message.
2661 * @return clone of this message
2662 */
2663Message *
2665{
2667}
2668/** @class DynamixelServoInterface::GotoPositionMessage <interfaces/DynamixelServoInterface.h>
2669 * GotoPositionMessage Fawkes BlackBoard Interface Message.
2670 *
2671
2672 */
2673
2674
2675/** Constructor with initial values.
2676 * @param ini_position initial value for position
2677 */
2678DynamixelServoInterface::GotoPositionMessage::GotoPositionMessage(const uint32_t ini_position) : Message("GotoPositionMessage")
2679{
2680 data_size = sizeof(GotoPositionMessage_data_t);
2681 data_ptr = malloc(data_size);
2682 memset(data_ptr, 0, data_size);
2683 data = (GotoPositionMessage_data_t *)data_ptr;
2685 data->position = ini_position;
2686 enum_map_ErrorCode[(int)ERROR_NONE] = "ERROR_NONE";
2687 enum_map_ErrorCode[(int)ERROR_UNSPECIFIC] = "ERROR_UNSPECIFIC";
2688 enum_map_ErrorCode[(int)ERROR_COMMUNICATION] = "ERROR_COMMUNICATION";
2689 enum_map_ErrorCode[(int)ERROR_ANGLE_OUTOFRANGE] = "ERROR_ANGLE_OUTOFRANGE";
2690 enum_map_WorkingMode[(int)JOINT] = "JOINT";
2691 enum_map_WorkingMode[(int)WHEEL] = "WHEEL";
2692 add_fieldinfo(IFT_UINT32, "position", 1, &data->position);
2693}
2694/** Constructor */
2696{
2697 data_size = sizeof(GotoPositionMessage_data_t);
2698 data_ptr = malloc(data_size);
2699 memset(data_ptr, 0, data_size);
2700 data = (GotoPositionMessage_data_t *)data_ptr;
2702 enum_map_ErrorCode[(int)ERROR_NONE] = "ERROR_NONE";
2703 enum_map_ErrorCode[(int)ERROR_UNSPECIFIC] = "ERROR_UNSPECIFIC";
2704 enum_map_ErrorCode[(int)ERROR_COMMUNICATION] = "ERROR_COMMUNICATION";
2705 enum_map_ErrorCode[(int)ERROR_ANGLE_OUTOFRANGE] = "ERROR_ANGLE_OUTOFRANGE";
2706 enum_map_WorkingMode[(int)JOINT] = "JOINT";
2707 enum_map_WorkingMode[(int)WHEEL] = "WHEEL";
2708 add_fieldinfo(IFT_UINT32, "position", 1, &data->position);
2709}
2710
2711/** Destructor */
2713{
2714 free(data_ptr);
2715}
2716
2717/** Copy constructor.
2718 * @param m message to copy from
2719 */
2721{
2722 data_size = m->data_size;
2723 data_ptr = malloc(data_size);
2724 memcpy(data_ptr, m->data_ptr, data_size);
2725 data = (GotoPositionMessage_data_t *)data_ptr;
2727}
2728
2729/* Methods */
2730/** Get position value.
2731 * New position
2732 * @return position value
2733 */
2734uint32_t
2736{
2737 return data->position;
2738}
2739
2740/** Get maximum length of position value.
2741 * @return length of position value, can be length of the array or number of
2742 * maximum number of characters for a string
2743 */
2744size_t
2746{
2747 return 1;
2748}
2749
2750/** Set position value.
2751 * New position
2752 * @param new_position new position value
2753 */
2754void
2756{
2757 set_field(data->position, new_position);
2758}
2759
2760/** Clone this message.
2761 * Produces a message of the same type as this message and copies the
2762 * data to the new message.
2763 * @return clone of this message
2764 */
2765Message *
2767{
2769}
2770/** @class DynamixelServoInterface::SetAngleLimitsMessage <interfaces/DynamixelServoInterface.h>
2771 * SetAngleLimitsMessage Fawkes BlackBoard Interface Message.
2772 *
2773
2774 */
2775
2776
2777/** Constructor with initial values.
2778 * @param ini_angle_limit_cw initial value for angle_limit_cw
2779 * @param ini_angle_limit_ccw initial value for angle_limit_ccw
2780 */
2781DynamixelServoInterface::SetAngleLimitsMessage::SetAngleLimitsMessage(const uint32_t ini_angle_limit_cw, const uint32_t ini_angle_limit_ccw) : Message("SetAngleLimitsMessage")
2782{
2783 data_size = sizeof(SetAngleLimitsMessage_data_t);
2784 data_ptr = malloc(data_size);
2785 memset(data_ptr, 0, data_size);
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";
2791 enum_map_ErrorCode[(int)ERROR_UNSPECIFIC] = "ERROR_UNSPECIFIC";
2792 enum_map_ErrorCode[(int)ERROR_COMMUNICATION] = "ERROR_COMMUNICATION";
2793 enum_map_ErrorCode[(int)ERROR_ANGLE_OUTOFRANGE] = "ERROR_ANGLE_OUTOFRANGE";
2794 enum_map_WorkingMode[(int)JOINT] = "JOINT";
2795 enum_map_WorkingMode[(int)WHEEL] = "WHEEL";
2796 add_fieldinfo(IFT_UINT32, "angle_limit_cw", 1, &data->angle_limit_cw);
2797 add_fieldinfo(IFT_UINT32, "angle_limit_ccw", 1, &data->angle_limit_ccw);
2798}
2799/** Constructor */
2801{
2802 data_size = sizeof(SetAngleLimitsMessage_data_t);
2803 data_ptr = malloc(data_size);
2804 memset(data_ptr, 0, data_size);
2805 data = (SetAngleLimitsMessage_data_t *)data_ptr;
2807 enum_map_ErrorCode[(int)ERROR_NONE] = "ERROR_NONE";
2808 enum_map_ErrorCode[(int)ERROR_UNSPECIFIC] = "ERROR_UNSPECIFIC";
2809 enum_map_ErrorCode[(int)ERROR_COMMUNICATION] = "ERROR_COMMUNICATION";
2810 enum_map_ErrorCode[(int)ERROR_ANGLE_OUTOFRANGE] = "ERROR_ANGLE_OUTOFRANGE";
2811 enum_map_WorkingMode[(int)JOINT] = "JOINT";
2812 enum_map_WorkingMode[(int)WHEEL] = "WHEEL";
2813 add_fieldinfo(IFT_UINT32, "angle_limit_cw", 1, &data->angle_limit_cw);
2814 add_fieldinfo(IFT_UINT32, "angle_limit_ccw", 1, &data->angle_limit_ccw);
2815}
2816
2817/** Destructor */
2819{
2820 free(data_ptr);
2821}
2822
2823/** Copy constructor.
2824 * @param m message to copy from
2825 */
2827{
2828 data_size = m->data_size;
2829 data_ptr = malloc(data_size);
2830 memcpy(data_ptr, m->data_ptr, data_size);
2831 data = (SetAngleLimitsMessage_data_t *)data_ptr;
2833}
2834
2835/* Methods */
2836/** Get angle_limit_cw value.
2837 * New cw angle limit
2838 * @return angle_limit_cw value
2839 */
2840uint32_t
2842{
2843 return data->angle_limit_cw;
2844}
2845
2846/** Get maximum length of angle_limit_cw value.
2847 * @return length of angle_limit_cw value, can be length of the array or number of
2848 * maximum number of characters for a string
2849 */
2850size_t
2852{
2853 return 1;
2854}
2855
2856/** Set angle_limit_cw value.
2857 * New cw angle limit
2858 * @param new_angle_limit_cw new angle_limit_cw value
2859 */
2860void
2862{
2863 set_field(data->angle_limit_cw, new_angle_limit_cw);
2864}
2865
2866/** Get angle_limit_ccw value.
2867 * New ccw angle limit
2868 * @return angle_limit_ccw value
2869 */
2870uint32_t
2872{
2873 return data->angle_limit_ccw;
2874}
2875
2876/** Get maximum length of angle_limit_ccw value.
2877 * @return length of angle_limit_ccw value, can be length of the array or number of
2878 * maximum number of characters for a string
2879 */
2880size_t
2882{
2883 return 1;
2884}
2885
2886/** Set angle_limit_ccw value.
2887 * New ccw angle limit
2888 * @param new_angle_limit_ccw new angle_limit_ccw value
2889 */
2890void
2892{
2893 set_field(data->angle_limit_ccw, new_angle_limit_ccw);
2894}
2895
2896/** Clone this message.
2897 * Produces a message of the same type as this message and copies the
2898 * data to the new message.
2899 * @return clone of this message
2900 */
2901Message *
2903{
2905}
2906/** @class DynamixelServoInterface::ResetRawErrorMessage <interfaces/DynamixelServoInterface.h>
2907 * ResetRawErrorMessage Fawkes BlackBoard Interface Message.
2908 *
2909
2910 */
2911
2912
2913/** Constructor */
2915{
2916 data_size = sizeof(ResetRawErrorMessage_data_t);
2917 data_ptr = malloc(data_size);
2918 memset(data_ptr, 0, data_size);
2919 data = (ResetRawErrorMessage_data_t *)data_ptr;
2921 enum_map_ErrorCode[(int)ERROR_NONE] = "ERROR_NONE";
2922 enum_map_ErrorCode[(int)ERROR_UNSPECIFIC] = "ERROR_UNSPECIFIC";
2923 enum_map_ErrorCode[(int)ERROR_COMMUNICATION] = "ERROR_COMMUNICATION";
2924 enum_map_ErrorCode[(int)ERROR_ANGLE_OUTOFRANGE] = "ERROR_ANGLE_OUTOFRANGE";
2925 enum_map_WorkingMode[(int)JOINT] = "JOINT";
2926 enum_map_WorkingMode[(int)WHEEL] = "WHEEL";
2927}
2928
2929/** Destructor */
2931{
2932 free(data_ptr);
2933}
2934
2935/** Copy constructor.
2936 * @param m message to copy from
2937 */
2939{
2940 data_size = m->data_size;
2941 data_ptr = malloc(data_size);
2942 memcpy(data_ptr, m->data_ptr, data_size);
2943 data = (ResetRawErrorMessage_data_t *)data_ptr;
2945}
2946
2947/* Methods */
2948/** Clone this message.
2949 * Produces a message of the same type as this message and copies the
2950 * data to the new message.
2951 * @return clone of this message
2952 */
2953Message *
2955{
2957}
2958/** @class DynamixelServoInterface::SetPreventAlarmShutdownMessage <interfaces/DynamixelServoInterface.h>
2959 * SetPreventAlarmShutdownMessage Fawkes BlackBoard Interface Message.
2960 *
2961
2962 */
2963
2964
2965/** Constructor with initial values.
2966 * @param ini_enable_prevent_alarm_shutdown initial value for enable_prevent_alarm_shutdown
2967 */
2968DynamixelServoInterface::SetPreventAlarmShutdownMessage::SetPreventAlarmShutdownMessage(const bool ini_enable_prevent_alarm_shutdown) : Message("SetPreventAlarmShutdownMessage")
2969{
2970 data_size = sizeof(SetPreventAlarmShutdownMessage_data_t);
2971 data_ptr = malloc(data_size);
2972 memset(data_ptr, 0, data_size);
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";
2977 enum_map_ErrorCode[(int)ERROR_UNSPECIFIC] = "ERROR_UNSPECIFIC";
2978 enum_map_ErrorCode[(int)ERROR_COMMUNICATION] = "ERROR_COMMUNICATION";
2979 enum_map_ErrorCode[(int)ERROR_ANGLE_OUTOFRANGE] = "ERROR_ANGLE_OUTOFRANGE";
2980 enum_map_WorkingMode[(int)JOINT] = "JOINT";
2981 enum_map_WorkingMode[(int)WHEEL] = "WHEEL";
2982 add_fieldinfo(IFT_BOOL, "enable_prevent_alarm_shutdown", 1, &data->enable_prevent_alarm_shutdown);
2983}
2984/** Constructor */
2986{
2987 data_size = sizeof(SetPreventAlarmShutdownMessage_data_t);
2988 data_ptr = malloc(data_size);
2989 memset(data_ptr, 0, data_size);
2990 data = (SetPreventAlarmShutdownMessage_data_t *)data_ptr;
2992 enum_map_ErrorCode[(int)ERROR_NONE] = "ERROR_NONE";
2993 enum_map_ErrorCode[(int)ERROR_UNSPECIFIC] = "ERROR_UNSPECIFIC";
2994 enum_map_ErrorCode[(int)ERROR_COMMUNICATION] = "ERROR_COMMUNICATION";
2995 enum_map_ErrorCode[(int)ERROR_ANGLE_OUTOFRANGE] = "ERROR_ANGLE_OUTOFRANGE";
2996 enum_map_WorkingMode[(int)JOINT] = "JOINT";
2997 enum_map_WorkingMode[(int)WHEEL] = "WHEEL";
2998 add_fieldinfo(IFT_BOOL, "enable_prevent_alarm_shutdown", 1, &data->enable_prevent_alarm_shutdown);
2999}
3000
3001/** Destructor */
3003{
3004 free(data_ptr);
3005}
3006
3007/** Copy constructor.
3008 * @param m message to copy from
3009 */
3011{
3012 data_size = m->data_size;
3013 data_ptr = malloc(data_size);
3014 memcpy(data_ptr, m->data_ptr, data_size);
3015 data = (SetPreventAlarmShutdownMessage_data_t *)data_ptr;
3017}
3018
3019/* Methods */
3020/** Get enable_prevent_alarm_shutdown value.
3021 * Enable alarm shutdown
3022 * @return enable_prevent_alarm_shutdown value
3023 */
3024bool
3026{
3027 return data->enable_prevent_alarm_shutdown;
3028}
3029
3030/** Get maximum length of enable_prevent_alarm_shutdown value.
3031 * @return length of enable_prevent_alarm_shutdown value, can be length of the array or number of
3032 * maximum number of characters for a string
3033 */
3034size_t
3036{
3037 return 1;
3038}
3039
3040/** Set enable_prevent_alarm_shutdown value.
3041 * Enable alarm shutdown
3042 * @param new_enable_prevent_alarm_shutdown new enable_prevent_alarm_shutdown value
3043 */
3044void
3046{
3047 set_field(data->enable_prevent_alarm_shutdown, new_enable_prevent_alarm_shutdown);
3048}
3049
3050/** Clone this message.
3051 * Produces a message of the same type as this message and copies the
3052 * data to the new message.
3053 * @return clone of this message
3054 */
3055Message *
3057{
3059}
3060/** @class DynamixelServoInterface::SetAutorecoverEnabledMessage <interfaces/DynamixelServoInterface.h>
3061 * SetAutorecoverEnabledMessage Fawkes BlackBoard Interface Message.
3062 *
3063
3064 */
3065
3066
3067/** Constructor with initial values.
3068 * @param ini_autorecover_enabled initial value for autorecover_enabled
3069 */
3070DynamixelServoInterface::SetAutorecoverEnabledMessage::SetAutorecoverEnabledMessage(const bool ini_autorecover_enabled) : Message("SetAutorecoverEnabledMessage")
3071{
3072 data_size = sizeof(SetAutorecoverEnabledMessage_data_t);
3073 data_ptr = malloc(data_size);
3074 memset(data_ptr, 0, data_size);
3075 data = (SetAutorecoverEnabledMessage_data_t *)data_ptr;
3077 data->autorecover_enabled = ini_autorecover_enabled;
3078 enum_map_ErrorCode[(int)ERROR_NONE] = "ERROR_NONE";
3079 enum_map_ErrorCode[(int)ERROR_UNSPECIFIC] = "ERROR_UNSPECIFIC";
3080 enum_map_ErrorCode[(int)ERROR_COMMUNICATION] = "ERROR_COMMUNICATION";
3081 enum_map_ErrorCode[(int)ERROR_ANGLE_OUTOFRANGE] = "ERROR_ANGLE_OUTOFRANGE";
3082 enum_map_WorkingMode[(int)JOINT] = "JOINT";
3083 enum_map_WorkingMode[(int)WHEEL] = "WHEEL";
3084 add_fieldinfo(IFT_BOOL, "autorecover_enabled", 1, &data->autorecover_enabled);
3085}
3086/** Constructor */
3088{
3089 data_size = sizeof(SetAutorecoverEnabledMessage_data_t);
3090 data_ptr = malloc(data_size);
3091 memset(data_ptr, 0, data_size);
3092 data = (SetAutorecoverEnabledMessage_data_t *)data_ptr;
3094 enum_map_ErrorCode[(int)ERROR_NONE] = "ERROR_NONE";
3095 enum_map_ErrorCode[(int)ERROR_UNSPECIFIC] = "ERROR_UNSPECIFIC";
3096 enum_map_ErrorCode[(int)ERROR_COMMUNICATION] = "ERROR_COMMUNICATION";
3097 enum_map_ErrorCode[(int)ERROR_ANGLE_OUTOFRANGE] = "ERROR_ANGLE_OUTOFRANGE";
3098 enum_map_WorkingMode[(int)JOINT] = "JOINT";
3099 enum_map_WorkingMode[(int)WHEEL] = "WHEEL";
3100 add_fieldinfo(IFT_BOOL, "autorecover_enabled", 1, &data->autorecover_enabled);
3101}
3102
3103/** Destructor */
3105{
3106 free(data_ptr);
3107}
3108
3109/** Copy constructor.
3110 * @param m message to copy from
3111 */
3113{
3114 data_size = m->data_size;
3115 data_ptr = malloc(data_size);
3116 memcpy(data_ptr, m->data_ptr, data_size);
3117 data = (SetAutorecoverEnabledMessage_data_t *)data_ptr;
3119}
3120
3121/* Methods */
3122/** Get autorecover_enabled value.
3123 * Automatically recover on alarm shutdown
3124 * @return autorecover_enabled value
3125 */
3126bool
3128{
3129 return data->autorecover_enabled;
3130}
3131
3132/** Get maximum length of autorecover_enabled value.
3133 * @return length of autorecover_enabled value, can be length of the array or number of
3134 * maximum number of characters for a string
3135 */
3136size_t
3138{
3139 return 1;
3140}
3141
3142/** Set autorecover_enabled value.
3143 * Automatically recover on alarm shutdown
3144 * @param new_autorecover_enabled new autorecover_enabled value
3145 */
3146void
3148{
3149 set_field(data->autorecover_enabled, new_autorecover_enabled);
3150}
3151
3152/** Clone this message.
3153 * Produces a message of the same type as this message and copies the
3154 * data to the new message.
3155 * @return clone of this message
3156 */
3157Message *
3159{
3161}
3162/** @class DynamixelServoInterface::RecoverMessage <interfaces/DynamixelServoInterface.h>
3163 * RecoverMessage Fawkes BlackBoard Interface Message.
3164 *
3165
3166 */
3167
3168
3169/** Constructor */
3171{
3172 data_size = sizeof(RecoverMessage_data_t);
3173 data_ptr = malloc(data_size);
3174 memset(data_ptr, 0, data_size);
3175 data = (RecoverMessage_data_t *)data_ptr;
3177 enum_map_ErrorCode[(int)ERROR_NONE] = "ERROR_NONE";
3178 enum_map_ErrorCode[(int)ERROR_UNSPECIFIC] = "ERROR_UNSPECIFIC";
3179 enum_map_ErrorCode[(int)ERROR_COMMUNICATION] = "ERROR_COMMUNICATION";
3180 enum_map_ErrorCode[(int)ERROR_ANGLE_OUTOFRANGE] = "ERROR_ANGLE_OUTOFRANGE";
3181 enum_map_WorkingMode[(int)JOINT] = "JOINT";
3182 enum_map_WorkingMode[(int)WHEEL] = "WHEEL";
3183}
3184
3185/** Destructor */
3187{
3188 free(data_ptr);
3189}
3190
3191/** Copy constructor.
3192 * @param m message to copy from
3193 */
3195{
3196 data_size = m->data_size;
3197 data_ptr = malloc(data_size);
3198 memcpy(data_ptr, m->data_ptr, data_size);
3199 data = (RecoverMessage_data_t *)data_ptr;
3201}
3202
3203/* Methods */
3204/** Clone this message.
3205 * Produces a message of the same type as this message and copies the
3206 * data to the new message.
3207 * @return clone of this message
3208 */
3209Message *
3211{
3213}
3214/** Check if message is valid and can be enqueued.
3215 * @param message Message to check
3216 * @return true if the message is valid, false otherwise.
3217 */
3218bool
3220{
3221 const StopMessage *m0 = dynamic_cast<const StopMessage *>(message);
3222 if ( m0 != NULL ) {
3223 return true;
3224 }
3225 const FlushMessage *m1 = dynamic_cast<const FlushMessage *>(message);
3226 if ( m1 != NULL ) {
3227 return true;
3228 }
3229 const GotoMessage *m2 = dynamic_cast<const GotoMessage *>(message);
3230 if ( m2 != NULL ) {
3231 return true;
3232 }
3233 const TimedGotoMessage *m3 = dynamic_cast<const TimedGotoMessage *>(message);
3234 if ( m3 != NULL ) {
3235 return true;
3236 }
3237 const SetModeMessage *m4 = dynamic_cast<const SetModeMessage *>(message);
3238 if ( m4 != NULL ) {
3239 return true;
3240 }
3241 const SetSpeedMessage *m5 = dynamic_cast<const SetSpeedMessage *>(message);
3242 if ( m5 != NULL ) {
3243 return true;
3244 }
3245 const SetEnabledMessage *m6 = dynamic_cast<const SetEnabledMessage *>(message);
3246 if ( m6 != NULL ) {
3247 return true;
3248 }
3249 const SetVelocityMessage *m7 = dynamic_cast<const SetVelocityMessage *>(message);
3250 if ( m7 != NULL ) {
3251 return true;
3252 }
3253 const SetMarginMessage *m8 = dynamic_cast<const SetMarginMessage *>(message);
3254 if ( m8 != NULL ) {
3255 return true;
3256 }
3257 const SetComplianceValuesMessage *m9 = dynamic_cast<const SetComplianceValuesMessage *>(message);
3258 if ( m9 != NULL ) {
3259 return true;
3260 }
3261 const SetGoalSpeedMessage *m10 = dynamic_cast<const SetGoalSpeedMessage *>(message);
3262 if ( m10 != NULL ) {
3263 return true;
3264 }
3265 const SetTorqueLimitMessage *m11 = dynamic_cast<const SetTorqueLimitMessage *>(message);
3266 if ( m11 != NULL ) {
3267 return true;
3268 }
3269 const SetPunchMessage *m12 = dynamic_cast<const SetPunchMessage *>(message);
3270 if ( m12 != NULL ) {
3271 return true;
3272 }
3273 const GotoPositionMessage *m13 = dynamic_cast<const GotoPositionMessage *>(message);
3274 if ( m13 != NULL ) {
3275 return true;
3276 }
3277 const SetAngleLimitsMessage *m14 = dynamic_cast<const SetAngleLimitsMessage *>(message);
3278 if ( m14 != NULL ) {
3279 return true;
3280 }
3281 const ResetRawErrorMessage *m15 = dynamic_cast<const ResetRawErrorMessage *>(message);
3282 if ( m15 != NULL ) {
3283 return true;
3284 }
3285 const SetPreventAlarmShutdownMessage *m16 = dynamic_cast<const SetPreventAlarmShutdownMessage *>(message);
3286 if ( m16 != NULL ) {
3287 return true;
3288 }
3289 const SetAutorecoverEnabledMessage *m17 = dynamic_cast<const SetAutorecoverEnabledMessage *>(message);
3290 if ( m17 != NULL ) {
3291 return true;
3292 }
3293 const RecoverMessage *m18 = dynamic_cast<const RecoverMessage *>(message);
3294 if ( m18 != NULL ) {
3295 return true;
3296 }
3297 return false;
3298}
3299
3300/// @cond INTERNALS
3301EXPORT_INTERFACE(DynamixelServoInterface)
3302/// @endcond
3303
3304
3305} // end namespace fawkes
FlushMessage Fawkes BlackBoard Interface Message.
virtual Message * clone() const
Clone this message.
GotoMessage Fawkes BlackBoard Interface Message.
virtual Message * clone() const
Clone this message.
void set_angle(const float new_angle)
Set 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.
size_t maxlenof_position() const
Get maximum length of position value.
virtual Message * clone() const
Clone this message.
RecoverMessage Fawkes BlackBoard Interface Message.
virtual Message * clone() const
Clone this message.
ResetRawErrorMessage Fawkes BlackBoard Interface Message.
virtual Message * clone() const
Clone this message.
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.
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.
SetAutorecoverEnabledMessage Fawkes BlackBoard Interface Message.
void set_autorecover_enabled(const bool new_autorecover_enabled)
Set autorecover_enabled value.
size_t maxlenof_autorecover_enabled() const
Get maximum length of autorecover_enabled value.
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.
size_t maxlenof_ccw_margin() const
Get maximum length of ccw_margin value.
size_t maxlenof_cw_slope() const
Get maximum length of cw_slope 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.
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.
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.
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.
virtual Message * clone() const
Clone this message.
SetMarginMessage Fawkes BlackBoard Interface Message.
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.
SetModeMessage Fawkes BlackBoard Interface Message.
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.
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.
void set_enable_prevent_alarm_shutdown(const bool new_enable_prevent_alarm_shutdown)
Set enable_prevent_alarm_shutdown value.
SetPunchMessage Fawkes BlackBoard Interface Message.
size_t maxlenof_punch() const
Get maximum length of punch value.
virtual Message * clone() const
Clone this message.
void set_punch(const uint32_t new_punch)
Set punch value.
SetSpeedMessage Fawkes BlackBoard Interface Message.
virtual Message * clone() const
Clone this message.
size_t maxlenof_speed() const
Get maximum length of speed value.
void set_speed(const uint16_t new_speed)
Set 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.
size_t maxlenof_torque_limit() const
Get maximum length of torque_limit value.
SetVelocityMessage Fawkes BlackBoard Interface Message.
size_t maxlenof_velocity() const
Get maximum length of velocity value.
virtual Message * clone() const
Clone this message.
void set_velocity(const float new_velocity)
Set velocity value.
StopMessage Fawkes BlackBoard Interface Message.
virtual Message * clone() const
Clone this message.
TimedGotoMessage Fawkes BlackBoard Interface Message.
void set_time_sec(const float new_time_sec)
Set time_sec value.
void set_angle(const float new_angle)
Set 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_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.
Definition: interface.h:80
const char * type() const
Get type of interface.
Definition: interface.cpp:652
void * data_ptr
Pointer to local memory storage.
Definition: interface.h:244
void set_field(FieldT &field, DataT &data)
Set a field, set data_changed to true and update data_changed accordingly.
Definition: interface.h:304
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:44
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.
Definition: message.cpp:435
void * data_ptr
Pointer to memory that contains local data.
Definition: message.h:146
message_data_ts_t * data_ts
data timestamp aliasing pointer
Definition: message.h:156
unsigned int data_size
Size of memory needed to hold all data.
Definition: message.h:147
Fawkes library namespace.
@ IFT_UINT32
32 bit unsigned integer field
Definition: types.h:43
@ IFT_FLOAT
float field
Definition: types.h:46
@ IFT_UINT16
16 bit unsigned integer field
Definition: types.h:41
@ IFT_BOOL
boolean field
Definition: types.h:37
@ IFT_UINT8
8 bit unsigned integer field
Definition: types.h:39
Timestamp data, must be present and first entries for each interface data structs!...
Definition: message.h:152