23 #include "servo_chain.h" 25 #include <core/exceptions/system.h> 26 #include <core/exceptions/software.h> 28 #include <utils/math/angle.h> 30 #include <sys/ioctl.h> 109 const unsigned char DynamixelChain::INST_PING = 0x01;
110 const unsigned char DynamixelChain::INST_READ = 0x02;
111 const unsigned char DynamixelChain::INST_WRITE = 0x03;
112 const unsigned char DynamixelChain::INST_REG_WRITE = 0x04;
113 const unsigned char DynamixelChain::INST_ACTION = 0x05;
114 const unsigned char DynamixelChain::INST_RESET = 0x06;
115 const unsigned char DynamixelChain::INST_DIGITAL_RESET = 0x07;
116 const unsigned char DynamixelChain::INST_SYSTEM_READ = 0x0C;
117 const unsigned char DynamixelChain::INST_SYSTEM_WRITE = 0x0D;
118 const unsigned char DynamixelChain::INST_SYNC_WRITE = 0x83;
119 const unsigned char DynamixelChain::INST_SYNC_REG_WRITE = 0x84;
121 const unsigned char DynamixelChain::PACKET_OFFSET_ID = 2;
122 const unsigned char DynamixelChain::PACKET_OFFSET_LENGTH = 3;
123 const unsigned char DynamixelChain::PACKET_OFFSET_INST = 4;
124 const unsigned char DynamixelChain::PACKET_OFFSET_PARAM = 5;
125 const unsigned char DynamixelChain::PACKET_OFFSET_ERROR = 4;
155 DynamixelChain::DynamixelChain(
const char *device_file,
unsigned int default_timeout_ms,
bool enable_echo_fix,
bool enable_connection_stability,
float min_voltage,
float max_voltage)
157 __default_timeout_ms = default_timeout_ms;
158 __device_file = strdup(device_file);
160 __obuffer_length = 0;
161 __ibuffer_length = 0;
162 __enable_echo_fix = enable_echo_fix;
163 __enable_connection_stability = enable_connection_stability;
164 __min_voltage = min_voltage;
165 __max_voltage = max_voltage;
166 memset(__control_table, 0, DYNAMIXEL_MAX_NUM_SERVOS * DYNAMIXEL_CONTROL_TABLE_LENGTH);
186 struct termios param;
188 __fd = ::open(__device_file, O_NOCTTY | O_RDWR);
192 tcflush(__fd, TCIOFLUSH);
194 if (tcgetattr(__fd, ¶m) == -1) {
195 Exception e(errno,
"Getting the port parameters failed");
201 cfsetospeed(¶m, B57600);
202 cfsetispeed(¶m, B57600);
204 param.c_lflag &= ~(ICANON | ECHO | ECHOE | ECHOK | ECHONL | ISIG | IEXTEN);
205 param.c_iflag &= ~(INLCR | IGNCR | ICRNL | IGNBRK | PARMRK);
208 param.c_iflag &= ~(IXON | IXOFF | IXANY);
209 param.c_cflag &= ~CRTSCTS;
211 param.c_cflag |= (CREAD | CLOCAL);
214 param.c_cflag &= ~CS5 & ~CS6 & ~CS7 & ~CS8;
215 param.c_cflag |= CS8;
218 param.c_cflag &= ~(PARENB | PARODD);
219 param.c_iflag &= ~(INPCK | ISTRIP);
222 param.c_cflag &= ~CSTOPB;
225 param.c_oflag &= ~OPOST;
227 param.c_cc[VMIN] = 1;
228 param.c_cc[VTIME] = 0;
230 tcflush(__fd, TCIOFLUSH);
232 if (tcsetattr(__fd, TCSANOW, ¶m) != 0) {
233 Exception e(errno,
"Setting the port parameters failed");
242 #ifdef TIMETRACKER_VISCA 244 track_file.open(
"tracker_visca.txt");
245 ttcls_pantilt_get_send = tracker->addClass(
"getPanTilt: send");
246 ttcls_pantilt_get_read = tracker->addClass(
"getPanTilt: read");
247 ttcls_pantilt_get_handle = tracker->addClass(
"getPanTilt: handling responses");
248 ttcls_pantilt_get_interpret = tracker->addClass(
"getPanTilt: interpreting");
274 DynamixelChain::calc_checksum(
const unsigned char id,
const unsigned char instruction,
275 const unsigned char *params,
const unsigned char plength)
277 unsigned int checksum =
id + instruction + plength+2;
278 for (
unsigned char i = 0; i < plength; ++i) {
279 checksum += params[i];
282 return ~(checksum & 0xFF);
293 DynamixelChain::send(
const unsigned char id,
const unsigned char instruction,
294 const unsigned char *params,
const unsigned char plength)
299 __obuffer[PACKET_OFFSET_ID] = id;
300 __obuffer[PACKET_OFFSET_LENGTH] = plength+2;
301 __obuffer[PACKET_OFFSET_INST] = instruction;
303 unsigned int checksum =
id + plength+2;
305 for (
unsigned char i = 0; i < plength; ++i) {
306 __obuffer[PACKET_OFFSET_PARAM + i] = params[i];
307 checksum += params[i];
311 __obuffer[3 + plength+2] = calc_checksum(
id, instruction, params, plength);
312 __obuffer_length = plength+2 + 4 ;
314 #ifdef DEBUG_ServoChain_COMM 316 for (
int i = 0; i < __obuffer_length; ++i) {
317 printf(
"%X ", __obuffer[i]);
322 int written = write(__fd, __obuffer, __obuffer_length);
325 if (__enable_echo_fix) {
328 while (readd < __obuffer_length) {
329 readd += read(__fd, __ibuffer + readd, __obuffer_length - readd);
331 #ifdef DEBUG_ServoChain_COMM 332 printf(
"Read %d junk bytes: ", readd);
333 for (
int i = 0; i < readd; ++i) {
334 printf(
"%X ", __ibuffer[i]);
340 throw Exception(errno,
"Failed to write ServoChain packet %x for %x", instruction,
id);
341 }
else if (written < __obuffer_length) {
342 throw Exception(
"Failed to write ServoChain packet %x for %x, only %d of %d bytes sent",
343 instruction,
id, written, __obuffer_length);
353 DynamixelChain::recv(
const unsigned char exp_length,
unsigned int timeout_ms)
355 timeval timeout = {0, (suseconds_t)(timeout_ms == 0xFFFFFFFF ? __default_timeout_ms : timeout_ms) * 1000};
359 FD_SET(__fd, &read_fds);
362 rv = select(__fd + 1, &read_fds, NULL, NULL, &timeout);
365 throw Exception(errno,
"Select on FD failed");
366 }
else if ( rv == 0 ) {
368 throw TimeoutException(
"Timeout reached while waiting for incoming ServoChain data");
371 __ibuffer_length = 0;
375 while (bytes_read < 6) {
376 #ifdef DEBUG_ServoChain_COMM 377 printf(
"Trying to read %d bytes\n", 6 - bytes_read);
379 if (__enable_connection_stability) {
381 rv = select(__fd + 1, &read_fds, NULL, NULL, &timeout);
384 throw Exception(errno,
"Select on FD failed");
385 }
else if ( rv == 0 ) {
387 throw TimeoutException(
"Timeout reached while waiting for incoming ServoChain data");
390 bytes_read += read(__fd, __ibuffer + bytes_read, 6 - bytes_read);
391 #ifdef DEBUG_ServoChain_COMM 392 printf(
"%d bytes read ", bytes_read);
393 for (
int i = 0; i < bytes_read; ++i) {
394 printf(
"%X ", __ibuffer[i]);
399 if (bytes_read < 6) {
400 throw Exception(
"Failed to read packet header");
402 if ( (__ibuffer[0] != 0xFF) || (__ibuffer[1] != 0xFF) ) {
403 throw Exception(
"Packet does not start with 0xFFFF.");
405 if (exp_length != __ibuffer[PACKET_OFFSET_LENGTH] - 2) {
406 tcflush(__fd, TCIFLUSH);
407 throw Exception(
"Wrong packet length, expected %u, got %u",
408 exp_length, __ibuffer[PACKET_OFFSET_LENGTH] - 2);
410 const unsigned char plength = __ibuffer[PACKET_OFFSET_LENGTH] - 2;
411 #ifdef DEBUG_ServoChain_COMM 412 printf(
"header read, params have length %d\n", plength);
416 while (bytes_read < plength) {
417 if (__enable_connection_stability) {
419 rv = select(__fd + 1, &read_fds, NULL, NULL, &timeout);
422 throw Exception(errno,
"Select on FD failed");
423 }
else if ( rv == 0 ) {
425 throw TimeoutException(
"Timeout reached while waiting for incoming ServoChain data");
428 bytes_read += read(__fd, &__ibuffer[6] + bytes_read, plength - bytes_read);
430 if (bytes_read < plength) {
431 throw Exception(
"Failed to read packet data");
435 __ibuffer_length = plength+2 + 4;
436 #ifdef DEBUG_ServoChain_COMM 438 for (
int i = 0; i < __ibuffer_length; ++i) {
439 printf(
"%X ", __ibuffer[i]);
445 unsigned char checksum = calc_checksum(__ibuffer[PACKET_OFFSET_ID],
446 __ibuffer[PACKET_OFFSET_INST],
447 &__ibuffer[PACKET_OFFSET_PARAM], plength);
448 if (checksum != __ibuffer[plength + 5]) {
449 throw Exception(
"Checksum error while receiving packet, expected %d, got %d",
450 checksum, __ibuffer[plength + 5]);
453 __ibuffer_length = plength+2 + 4;
464 ioctl(__fd, FIONREAD, &num_bytes);
465 return (num_bytes > 0);
494 if (servos.size() == 0) {
498 send(BROADCAST_ID, INST_PING, NULL, 0);
499 for (
unsigned int i = 0; i < DYNAMIXEL_MAX_NUM_SERVOS ; ++i) {
502 rv.push_back(__ibuffer[PACKET_OFFSET_ID]);
511 for (std::vector<unsigned int>::const_iterator iS = servos.begin() ; iS != servos.end(); ++iS)
513 send(*iS, INST_PING, NULL, 0);
515 rv.push_back(__ibuffer[PACKET_OFFSET_ID]);
517 throw Exception(
"Desired servo with ID %u not found!", *iS);
522 for (DeviceList::iterator i = rv.begin(); i != rv.end(); ++i) {
524 read_table_values(*i);
526 e.
append(
"Failed to receive control table for servo %u", *i);
547 send(
id, INST_PING, NULL, 0);
565 start_read_table_values(
id);
566 finish_read_table_values();
581 unsigned char param[2];
583 param[1] = DYNAMIXEL_CONTROL_TABLE_LENGTH;
585 send(
id, INST_READ, param, 2);
597 recv(DYNAMIXEL_CONTROL_TABLE_LENGTH);
599 if (__ibuffer_length != 5 + DYNAMIXEL_CONTROL_TABLE_LENGTH + 1) {
600 throw Exception(
"Input buffer of invalid size: %u vs. %u", __ibuffer_length, 5 + DYNAMIXEL_CONTROL_TABLE_LENGTH + 1);
602 memcpy(__control_table[__ibuffer[PACKET_OFFSET_ID]],
603 &__ibuffer[PACKET_OFFSET_PARAM], DYNAMIXEL_CONTROL_TABLE_LENGTH);
617 unsigned char addr,
unsigned char read_length)
621 unsigned char param[2];
623 param[1] = read_length;
625 send(
id, INST_READ, param, 2);
628 if (__ibuffer_length != (5 + read_length + 1)) {
629 throw Exception(
"Input buffer not of expected size, expected %u, got %u",
630 (5 + read_length + 1), __ibuffer_length);
633 for (
unsigned int i = 0; i < read_length; ++i) {
634 __control_table[id][addr + i] = __ibuffer[PACKET_OFFSET_PARAM + i];
648 unsigned int value,
bool double_byte)
650 unsigned char param[3];
652 param[1] = value & 0xFF;
653 param[2] = (value >> 8) & 0xFF;
656 send(
id, INST_WRITE, param, double_byte ? 3 : 2);
658 if (
id == BROADCAST_ID) {
659 for (
unsigned int i = 0; i < DYNAMIXEL_MAX_NUM_SERVOS; ++i) {
660 __control_table[i][addr] = param[1];
661 if (double_byte) __control_table[i][addr + 1] = param[2];
664 __control_table[id][addr] = param[1];
665 if (double_byte) __control_table[id][addr + 1] = param[2];
668 if ((
id != BROADCAST_ID) && responds_all(
id)) recv(0);
684 unsigned char *values,
unsigned int num_values)
686 unsigned char param[num_values + 1];
687 param[0] = start_addr;
688 for (
unsigned int i = 0; i < num_values; ++i) {
689 param[i + 1] = values[i];
693 send(
id, INST_WRITE, param, num_values + 1);
695 if (
id == BROADCAST_ID) {
696 for (
unsigned int i = 0; i < DYNAMIXEL_MAX_NUM_SERVOS; ++i) {
697 for (
unsigned int j = 0; j < num_values; ++j) {
698 __control_table[i][start_addr + j] = values[j];
702 for (
unsigned int j = 0; j < num_values; ++j) {
703 __control_table[id][start_addr + j] = values[j];
707 if ((
id != BROADCAST_ID) && responds_all(
id)) recv(0);
721 DynamixelChain::assert_valid_id(
unsigned char id)
723 if (
id == BROADCAST_ID) {
724 throw Exception(
"Data can only be queried for a specific servo");
725 }
else if (
id >= DYNAMIXEL_MAX_NUM_SERVOS) {
737 DynamixelChain::merge_twobyte_value(
unsigned int id,
738 unsigned char ind_l,
unsigned char ind_h)
740 unsigned int rv = (__control_table[id][ind_h] & 0xFF) << 8;
741 rv |= __control_table[id][ind_l] & 0xFF;
755 DynamixelChain::get_value(
unsigned int id,
bool refresh,
756 unsigned int ind_l,
unsigned int ind_h)
760 if (refresh) read_table_value(
id, ind_l, (ind_h == 0xFFFFFFFF ? 1 : 2));
762 if (ind_h == 0xFFFFFFFF) {
763 return __control_table[id][ind_l];
765 return merge_twobyte_value(
id, ind_l, ind_h);
777 const unsigned int model = get_model_number(
id, refresh);
779 case 12:
return "AX-12";
780 case 18:
return "AX-18";
781 case 24:
return "RX-24F";
782 case 28:
return "RX-28";
783 case 29:
return "MX-28";
784 case 54:
return "MX-64";
785 case 64:
return "RX-64";
786 case 104:
return "MAX-12W";
787 case 107:
return "EX-106+";
788 case 310:
return "MX-64AT";
789 case 320:
return "MX-106";
790 default:
return "UNKNOWN";
802 return get_value(
id, refresh, P_MODEL_NUMBER_L, P_MODEL_NUMBER_H);
814 return get_value(
id, refresh, P_PRESENT_POSITION_L, P_PRESENT_POSITION_H);
826 return get_value(
id, refresh, P_VERSION);
838 return get_value(
id, refresh, P_BAUD_RATE);
850 return get_value(
id, refresh, P_RETURN_DELAY_TIME);
861 return __ibuffer[PACKET_OFFSET_ERROR];
873 unsigned int &cw_limit,
unsigned int &ccw_limit,
876 cw_limit = get_value(
id, refresh, P_CW_ANGLE_LIMIT_L, P_CW_ANGLE_LIMIT_H);
877 ccw_limit = get_value(
id, refresh, P_CCW_ANGLE_LIMIT_L, P_CCW_ANGLE_LIMIT_H);
889 return get_value(
id, refresh, P_LIMIT_TEMPERATURE);
901 unsigned char &low,
unsigned char &high,
904 low = get_value(
id, refresh, P_DOWN_LIMIT_VOLTAGE);
905 high = get_value(
id, refresh, P_UP_LIMIT_VOLTAGE);
917 return get_value(
id, refresh, P_MAX_TORQUE_L, P_MAX_TORQUE_H);
929 return get_value(
id, refresh, P_RETURN_LEVEL);
941 return get_value(
id, refresh, P_ALARM_LED);
953 return get_value(
id, refresh, P_ALARM_SHUTDOWN);
965 unsigned int &down_calib,
unsigned int &up_calib,
968 down_calib = get_value(
id, refresh, P_DOWN_CALIBRATION_L, P_DOWN_CALIBRATION_H);
969 up_calib = get_value(
id, refresh, P_UP_CALIBRATION_L, P_UP_CALIBRATION_H);
981 return (get_value(
id, refresh, P_TORQUE_ENABLE) == 1);
993 return (get_value(
id, refresh, P_LED) == 1);
1007 unsigned char &cw_margin,
unsigned char &cw_slope,
1008 unsigned char &ccw_margin,
unsigned char &ccw_slope,
1011 cw_margin = get_value(
id, refresh, P_CW_COMPLIANCE_MARGIN);
1012 cw_slope = get_value(
id, refresh, P_CW_COMPLIANCE_SLOPE);
1013 ccw_margin = get_value(
id, refresh, P_CCW_COMPLIANCE_MARGIN);
1014 ccw_slope = get_value(
id, refresh, P_CCW_COMPLIANCE_SLOPE);
1026 return get_value(
id, refresh, P_GOAL_POSITION_L, P_GOAL_POSITION_H);
1038 return get_value(
id, refresh, P_GOAL_SPEED_L, P_GOAL_SPEED_H);
1050 float voltage = get_voltage(
id, refresh) / 10.0;
1052 if ((voltage < __min_voltage) || (voltage > __max_voltage)) {
1053 throw OutOfBoundsException(
"Voltage is outside of specs", voltage, __min_voltage, __max_voltage);
1056 float sec_per_deg_12V = SEC_PER_60DEG_12V / 60.0;
1057 float sec_per_deg_16V = SEC_PER_60DEG_16V / 60.0;
1059 float range_sec_per_deg = sec_per_deg_12V - sec_per_deg_16V;
1060 float pos = voltage - 12.0;
1062 float sec_per_deg = sec_per_deg_16V + pos * range_sec_per_deg;
1063 float deg_per_sec = 1.0 / sec_per_deg;
1077 return get_value(
id, refresh, P_TORQUE_LIMIT_L, P_TORQUE_LIMIT_H);
1089 return get_value(
id, refresh, P_PRESENT_SPEED_L, P_PRESENT_SPEED_H);
1101 return get_value(
id, refresh, P_PRESENT_LOAD_L, P_PRESENT_LOAD_H);
1113 return get_value(
id, refresh, P_PRESENT_VOLTAGE);
1125 return get_value(
id, refresh, P_PRESENT_TEMPERATURE);
1137 return (get_value(
id, refresh, P_MOVING) == 1);
1149 return (get_value(
id, refresh, P_LOCK) == 1);
1161 return get_value(
id, refresh, P_PUNCH_L, P_PUNCH_H);
1172 write_table_value(
id, P_ID, new_id);
1183 write_table_value(
id, P_BAUD_RATE, baudrate);
1194 write_table_value(
id, P_RETURN_DELAY_TIME, return_delay_time);
1205 unsigned int cw_limit,
unsigned int ccw_limit)
1207 write_table_value(
id, P_CW_ANGLE_LIMIT_L, cw_limit,
true);
1208 write_table_value(
id, P_CCW_ANGLE_LIMIT_L, ccw_limit,
true);
1219 write_table_value(
id, P_LIMIT_TEMPERATURE, temp_limit);
1231 unsigned char param[2];
1234 write_table_values(
id, P_DOWN_LIMIT_VOLTAGE, param, 2);
1245 write_table_value(
id, P_MAX_TORQUE_L, max_torque,
true);
1257 write_table_value(
id, P_RETURN_LEVEL, status_return_level);
1268 write_table_value(
id, P_ALARM_LED, alarm_led);
1279 write_table_value(
id, P_ALARM_SHUTDOWN, alarm_shutdown);
1291 write_table_value(
id, P_TORQUE_ENABLE, enabled ? 1 : 0);
1305 if (num_servos > 120) {
1307 throw Exception(
"You cannot set more than 120 servos at once");
1311 va_start(arg, num_servos);
1313 unsigned int plength = 2 * num_servos + 2;
1314 unsigned char param[plength];
1315 param[0] = P_TORQUE_ENABLE;
1317 for (
unsigned int i = 0; i < num_servos; ++i) {
1318 unsigned char id = va_arg(arg,
unsigned int);
1319 param[2 + i * 2] = id;
1320 param[2 + i * 2 + 1] = enabled ? 1 : 0;
1324 send(BROADCAST_ID, INST_SYNC_WRITE, param, plength);
1335 write_table_value(
id, P_LED, led_enabled ? 1 : 0);
1348 unsigned char cw_margin,
unsigned char cw_slope,
1349 unsigned char ccw_margin,
unsigned char ccw_slope)
1351 unsigned char param[4];
1352 param[0] = cw_margin;
1353 param[1] = ccw_margin;
1354 param[2] = cw_slope;
1355 param[3] = ccw_slope;
1356 write_table_values(
id, P_CW_COMPLIANCE_MARGIN, param, 4);
1368 write_table_value(
id, P_GOAL_SPEED_L, goal_speed,
true);
1380 if (num_servos > 83) {
1382 throw Exception(
"You cannot set more than 83 speeds at once");
1386 va_start(arg, num_servos);
1388 unsigned int plength = 3 * num_servos + 2;
1389 unsigned char param[plength];
1390 param[0] = P_GOAL_SPEED_L;
1392 for (
unsigned int i = 0; i < num_servos; ++i) {
1393 unsigned char id = va_arg(arg,
unsigned int);
1394 unsigned int value = va_arg(arg,
unsigned int);
1396 param[2 + i * 3] = id;
1397 param[2 + i * 3 + 1] = (value & 0xFF);
1398 param[2 + i * 3 + 2] = (value >> 8) & 0xFF;
1402 send(BROADCAST_ID, INST_SYNC_WRITE, param, plength);
1413 write_table_value(
id, P_TORQUE_LIMIT_L, torque_limit,
true);
1424 write_table_value(
id, P_PUNCH_L, punch,
true);
1436 write_table_value(
id, P_LOCK, 1);
1448 write_table_value(
id, P_GOAL_POSITION_L, value,
true);
1461 if (num_servos > 83) {
1463 throw Exception(
"You cannot set more than 83 servos at once");
1467 va_start(arg, num_servos);
1469 unsigned int plength = 3 * num_servos + 2;
1470 unsigned char param[plength];
1471 param[0] = P_GOAL_POSITION_L;
1473 for (
unsigned int i = 0; i < num_servos; ++i) {
1474 unsigned char id = va_arg(arg,
unsigned int);
1475 unsigned int value = va_arg(arg,
unsigned int);
1476 param[2 + i * 3] = id;
1477 param[2 + i * 3 + 1] = (value & 0xFF);
1478 param[2 + i * 3 + 2] = (value >> 8) & 0xFF;
1482 send(BROADCAST_ID, INST_SYNC_WRITE, param, plength);
static const unsigned char P_ID
P_ID.
static const unsigned char P_RETURN_LEVEL
P_RETURN_LEVEL.
static const unsigned char SRL_RESPOND_NONE
SRL_RESPOND_NONE.
void get_compliance_values(unsigned char id, unsigned char &cw_margin, unsigned char &cw_slope, unsigned char &ccw_margin, unsigned char &ccw_slope, bool refresh=false)
Get compliance values.
std::list< unsigned char > DeviceList
List of servo IDs.
unsigned int get_speed(unsigned char id, bool refresh=false)
Get current speed.
static const unsigned char P_CCW_COMPLIANCE_SLOPE
P_CCW_COMPLIANCE_SLOPE.
static const unsigned char P_UP_CALIBRATION_L
P_UP_CALIBRATION_L.
File could not be opened.
unsigned char get_alarm_led(unsigned char id, bool refresh=false)
Get alarm LED status.
static const unsigned char P_MODEL_NUMBER_L
P_MODEL_NUMBER_L.
static const float SEC_PER_60DEG_12V
SEC_PER_60DEG_12V.
unsigned char get_alarm_shutdown(unsigned char id, bool refresh=false)
Get shutdown on alarm state.
bool is_locked(unsigned char id, bool refresh=false)
Check is servo is locked.
void start_read_table_values(unsigned char id)
Start to receive table values.
static const unsigned char P_REGISTERED_INSTRUCTION
P_REGISTERED_INSTRUCTION.
bool is_torque_enabled(unsigned char id, bool refresh=false)
Check if torque is enabled.
static const unsigned char P_MOVING
P_MOVING.
DynamixelChain(const char *device_file, unsigned int default_timeout_ms=30, bool enable_echo_fix=false, bool enable_connection_stability=false, float min_voltage=12.0, float max_voltage=16.0)
Constructor.
static const unsigned char SRL_RESPOND_ALL
SRL_RESPOND_ALL.
static const unsigned char P_PRESENT_POSITION_L
P_PRESENT_POSITION_L.
static const unsigned char BROADCAST_ID
BROADCAST_ID.
float get_max_supported_speed(unsigned char id, bool refresh=false)
Get maximum supported speed.
unsigned char get_firmware_version(unsigned char id, bool refresh=false)
Get firmware version.
void set_alarm_led(unsigned char id, unsigned char alarm_led)
Set alarm LED settings.
void set_temperature_limit(unsigned char id, unsigned char temp_limit)
Set temperature limit.
static const unsigned char P_TORQUE_ENABLE
P_TORQUE_ENABLE.
void set_torques_enabled(bool enabled, unsigned char num_servos,...)
Enable or disable torque for multiple (selected) servos at once.
Fawkes library namespace.
const char * get_model(unsigned char id, bool refresh=false)
Get model string.
unsigned int get_position(unsigned char id, bool refresh=false)
Get current position.
static const unsigned char P_DOWN_CALIBRATION_H
P_DOWN_CALIBRATION_H.
static const unsigned char P_GOAL_POSITION_L
P_GOAL_POSITION_L.
static const unsigned char P_MODEL_NUMBER_H
P_MODEL_NUMBER_H.
static const unsigned char P_TORQUE_LIMIT_H
P_TORQUE_LIMIT_H.
static const unsigned char P_VERSION
P_VERSION.
unsigned int get_max_torque(unsigned char id, bool refresh=false)
Get maximum torque.
unsigned int get_model_number(unsigned char id, bool refresh=false)
Get model.
unsigned int get_goal_speed(unsigned char id, bool refresh=false)
Get goal speed.
The current system call has timed out before completion.
static const unsigned char P_CCW_ANGLE_LIMIT_H
P_CCW_ANGLE_LIMIT_H.
static const unsigned char P_PRESENT_LOAD_H
P_PRESENT_LOAD_H.
unsigned char get_error(unsigned char id)
Get error flags set by the servo.
unsigned char get_voltage(unsigned char id, bool refresh=false)
Get current voltage.
void set_angle_limits(unsigned char id, unsigned int cw_limit, unsigned int ccw_limit)
Set angle limits.
static const unsigned char P_PAUSE_TIME
P_PAUSE_TIME.
static const float RAD_PER_POS_TICK
RAD_PER_POS_TICK.
void goto_positions(unsigned int num_positions,...)
Move several servos to specified positions.
void get_voltage_limits(unsigned char id, unsigned char &low, unsigned char &high, bool refresh=false)
Get voltage limits.
static const unsigned char P_LOCK
P_LOCK.
void get_calibration(unsigned char id, unsigned int &down_calib, unsigned int &up_calib, bool refresh=false)
Get calibration data.
static const float SEC_PER_60DEG_16V
SEC_PER_60DEG_16V.
void get_angle_limits(unsigned char id, unsigned int &cw_limit, unsigned int &ccw_limit, bool refresh=false)
Get angle limits.
unsigned int get_goal_position(unsigned char id, bool refresh=false)
Get goal position.
static const unsigned char P_ALARM_LED
P_ALARM_LED.
void read_table_values(unsigned char id)
Read all table values for given servo.
bool is_moving(unsigned char id, bool refresh=false)
Check if servo is moving.
void goto_position(unsigned char id, unsigned int value)
Move servo to specified position.
static const unsigned char P_CW_COMPLIANCE_MARGIN
P_CW_COMPLIANCE_MARGIN.
static const unsigned char P_DOWN_CALIBRATION_L
P_DOWN_CALIBRATION_L.
static const unsigned char P_GOAL_SPEED_L
P_GOAL_SPEED_L.
Base class for exceptions in Fawkes.
static const unsigned char SRL_RESPOND_READ
SRL_RESPOND_READ.
static const unsigned char P_PRESENT_POSITION_H
P_PRESENT_POSITION_H.
void set_id(unsigned char id, unsigned char new_id)
Set ID.
unsigned char get_delay_time(unsigned char id, bool refresh=false)
Get time of the delay before replies are sent.
void open()
Open serial port.
void set_max_torque(unsigned char id, unsigned int max_torque)
Set maximum torque.
static const float POS_TICKS_PER_RAD
POS_TICKS_PER_RAD.
static const unsigned char P_SYSTEM_DATA2
P_SYSTEM_DATA2.
static const unsigned char P_CW_ANGLE_LIMIT_L
P_CW_ANGLE_LIMIT_L.
void set_alarm_shutdown(unsigned char id, unsigned char alarm_shutdown)
Set shutdown on alarm.
static const unsigned char P_GOAL_SPEED_H
P_GOAL_SPEED_H.
static const unsigned char P_PUNCH_H
P_PUNCH_H.
void set_led_enabled(unsigned char id, bool enabled)
Turn LED on or off.
~DynamixelChain()
Destructor.
static const unsigned char P_BAUD_RATE
P_BAUD_RATE.
static const unsigned char P_PRESENT_SPEED_L
P_PRESENT_SPEED_L.
bool ping(unsigned char id, unsigned int timeout_ms=100)
Ping servo.
static const unsigned char P_LED
P_LED.
unsigned int get_load(unsigned char id, bool refresh=false)
Get current load.
static const unsigned char P_CCW_COMPLIANCE_MARGIN
P_CCW_COMPLIANCE_MARGIN.
void set_compliance_values(unsigned char id, unsigned char cw_margin, unsigned char cw_slope, unsigned char ccw_margin, unsigned char ccw_slope)
Set compliance values.
void read_table_value(unsigned char id, unsigned char addr, unsigned char read_length)
Read a table value.
static const unsigned char P_OPERATING_MODE
P_OPERATING_MODE.
static const unsigned char P_UP_LIMIT_VOLTAGE
P_UP_LIMIT_VOLTAGE.
unsigned int get_punch(unsigned char id, bool refresh=false)
Get punch.
static const unsigned char P_DOWN_LIMIT_VOLTAGE
P_DOWN_LIMIT_VOLTAGE.
static const unsigned char P_UP_CALIBRATION_H
P_UP_CALIBRATION_H.
void set_goal_speeds(unsigned int num_servos,...)
Set goal speeds for multiple servos.
void print_trace()
Prints trace to stderr.
static const unsigned char P_MAX_TORQUE_L
P_MAX_TORQUE_L.
void set_status_return_level(unsigned char id, unsigned char status_return_level)
Set status return level.
static const unsigned char P_ALARM_SHUTDOWN
P_ALARM_SHUTDOWN.
static const unsigned char P_CW_ANGLE_LIMIT_H
P_CW_ANGLE_LIMIT_H.
static const unsigned int CENTER_POSITION
CENTER_POSITION.
DeviceList discover(unsigned int total_timeout_ms=50, const std::vector< unsigned int > servos=std::vector< unsigned int >())
Discover devices on the bus.
bool data_available()
Check data availability.
void lock_config(unsigned char id)
Lock config.
static const unsigned char P_CW_COMPLIANCE_SLOPE
P_CW_COMPLIANCE_SLOPE.
static const unsigned char P_LIMIT_TEMPERATURE
P_LIMIT_TEMPERATURE.
void set_baudrate(unsigned char id, unsigned char baudrate)
Set baud rate.
static const unsigned char P_TORQUE_LIMIT_L
P_TORQUE_LIMIT_L.
bool is_led_enabled(unsigned char id, bool refresh=false)
Check if LED is enabled.
unsigned char get_status_return_level(unsigned char id, bool refresh=false)
Get status return level.
void set_return_delay_time(unsigned char id, unsigned char return_delay_time)
Set return delay time.
void set_torque_enabled(unsigned char id, bool enabled)
Enable or disable torque.
static const unsigned char P_CCW_ANGLE_LIMIT_L
P_CCW_ANGLE_LIMIT_L.
static const unsigned char P_PRESENT_VOLTAGE
P_PRESENT_VOLTAGE.
void finish_read_table_values()
Finish control table receive operations.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
void set_goal_speed(unsigned char id, unsigned int goal_speed)
Set goal speed.
unsigned char get_temperature_limit(unsigned char id, bool refresh=false)
Get temperature limit.
unsigned int get_torque_limit(unsigned char id, bool refresh=false)
Get torque limit.
static const unsigned char P_GOAL_POSITION_H
P_GOAL_POSITION_H.
void set_voltage_limits(unsigned char id, unsigned char low, unsigned char high)
Set voltage limits.
void set_torque_limit(unsigned char id, unsigned int torque_limit)
Set torque limit.
static const float MAX_ANGLE_DEG
MAX_ANGLE_DEG.
static const unsigned char P_MAX_TORQUE_H
P_MAX_TORQUE_H.
void write_table_values(unsigned char id, unsigned char start_addr, unsigned char *values, unsigned int num_values)
Write multiple table values.
static const unsigned int MAX_SPEED
MAX_SPEED.
static const unsigned char P_PRESENT_TEMPERATURE
P_PRESENT_TEMPERATURE.
static const unsigned int MAX_POSITION
MAX_POSITION.
static const unsigned char P_PRESENT_LOAD_L
P_PRESENT_LOAD_L.
void append(const char *format,...)
Append messages to the message list.
static const unsigned char P_RETURN_DELAY_TIME
P_RETURN_DELAY_TIME.
unsigned char get_baudrate(unsigned char id, bool refresh=false)
Get baud rate.
unsigned char get_temperature(unsigned char id, bool refresh=false)
Get temperature.
static const unsigned char P_PRESENT_SPEED_H
P_PRESENT_SPEED_H.
static const unsigned char P_PUNCH_L
P_PUNCH_L.
void write_table_value(unsigned char id, unsigned char addr, unsigned int value, bool double_byte=false)
Write a table value.
void set_punch(unsigned char id, unsigned int punch)
Set punch.
static const float MAX_ANGLE_RAD
MAX_ANGLE_RAD.