26 #include <core/exceptions/software.h>
27 #include <core/exceptions/system.h>
28 #include <sys/ioctl.h>
30 #include <utils/math/angle.h>
112 const unsigned char RobotisRX28::INST_PING = 0x01;
113 const unsigned char RobotisRX28::INST_READ = 0x02;
114 const unsigned char RobotisRX28::INST_WRITE = 0x03;
115 const unsigned char RobotisRX28::INST_REG_WRITE = 0x04;
116 const unsigned char RobotisRX28::INST_ACTION = 0x05;
117 const unsigned char RobotisRX28::INST_RESET = 0x06;
118 const unsigned char RobotisRX28::INST_DIGITAL_RESET = 0x07;
119 const unsigned char RobotisRX28::INST_SYSTEM_READ = 0x0C;
120 const unsigned char RobotisRX28::INST_SYSTEM_WRITE = 0x0D;
121 const unsigned char RobotisRX28::INST_SYNC_WRITE = 0x83;
122 const unsigned char RobotisRX28::INST_SYNC_REG_WRITE = 0x84;
124 const unsigned char RobotisRX28::PACKET_OFFSET_ID = 2;
125 const unsigned char RobotisRX28::PACKET_OFFSET_LENGTH = 3;
126 const unsigned char RobotisRX28::PACKET_OFFSET_INST = 4;
127 const unsigned char RobotisRX28::PACKET_OFFSET_PARAM = 5;
128 const unsigned char RobotisRX28::PACKET_OFFSET_ERROR = 4;
154 default_timeout_ms_ = default_timeout_ms;
155 device_file_ = strdup(device_file);
159 memset(control_table_, 0, RX28_MAX_NUM_SERVOS * RX28_CONTROL_TABLE_LENGTH);
166 for (
size_t i = 0; i <
sizeof(obuffer_) /
sizeof(obuffer_[0]); ++i) {
169 for (
size_t i = 0; i <
sizeof(ibuffer_) /
sizeof(ibuffer_[0]); ++i) {
184 struct termios param;
186 fd_ = ::open(device_file_, O_NOCTTY | O_RDWR);
190 tcflush(fd_, TCIOFLUSH);
192 if (tcgetattr(fd_, ¶m) == -1) {
193 Exception e(errno,
"Getting the port parameters failed");
199 cfsetospeed(¶m, B57600);
200 cfsetispeed(¶m, B57600);
202 param.c_lflag &= ~(ICANON | ECHO | ECHOE | ECHOK | ECHONL | ISIG | IEXTEN);
203 param.c_iflag &= ~(INLCR | IGNCR | ICRNL | IGNBRK | PARMRK);
206 param.c_iflag &= ~(IXON | IXOFF | IXANY);
207 param.c_cflag &= ~CRTSCTS;
209 param.c_cflag |= (CREAD | CLOCAL);
212 param.c_cflag &= ~CS5 & ~CS6 & ~CS7 & ~CS8;
213 param.c_cflag |= CS8;
216 param.c_cflag &= ~(PARENB | PARODD);
217 param.c_iflag &= ~(INPCK | ISTRIP);
220 param.c_cflag &= ~CSTOPB;
223 param.c_oflag &= ~OPOST;
225 param.c_cc[VMIN] = 1;
226 param.c_cc[VTIME] = 0;
228 tcflush(fd_, TCIOFLUSH);
230 if (tcsetattr(fd_, TCSANOW, ¶m) != 0) {
231 Exception e(errno,
"Setting the port parameters failed");
240 #ifdef TIMETRACKER_VISCA
242 track_file.open(
"tracker_visca.txt");
243 ttcls_pantilt_get_send = tracker->addClass(
"getPanTilt: send");
244 ttcls_pantilt_get_read = tracker->addClass(
"getPanTilt: read");
245 ttcls_pantilt_get_handle = tracker->addClass(
"getPanTilt: handling responses");
246 ttcls_pantilt_get_interpret = tracker->addClass(
"getPanTilt: interpreting");
270 RobotisRX28::calc_checksum(
const unsigned char id,
271 const unsigned char instruction,
272 const unsigned char *params,
273 const unsigned char plength)
275 unsigned int checksum =
id + instruction + plength + 2;
276 for (
unsigned char i = 0; i < plength; ++i) {
277 checksum += params[i];
280 return ~(checksum & 0xFF);
290 RobotisRX28::send(
const unsigned char id,
291 const unsigned char instruction,
292 const unsigned char *params,
293 const unsigned char plength)
298 obuffer_[PACKET_OFFSET_ID] = id;
299 obuffer_[PACKET_OFFSET_LENGTH] = plength + 2;
300 obuffer_[PACKET_OFFSET_INST] = instruction;
302 unsigned int checksum =
id + plength + 2;
304 for (
unsigned char i = 0; i < plength; ++i) {
305 obuffer_[PACKET_OFFSET_PARAM + i] = params[i];
306 checksum += params[i];
310 obuffer_[3 + plength + 2] = calc_checksum(
id, instruction, params, plength);
311 obuffer_length_ = plength + 2 + 4;
313 #ifdef DEBUG_RX28_COMM
315 for (
int i = 0; i < obuffer_length_; ++i) {
316 printf(
"%X ", obuffer_[i]);
321 int written = write(fd_, obuffer_, obuffer_length_);
326 while (readd < obuffer_length_) {
327 readd += read(fd_, ibuffer_ + readd, obuffer_length_ - readd);
329 #ifdef DEBUG_RX28_COMM
330 printf(
"Read %d junk bytes: ", readd);
331 for (
int i = 0; i < readd; ++i) {
332 printf(
"%X ", ibuffer_[i]);
338 throw Exception(errno,
"Failed to write RX28 packet %x for %x", instruction,
id);
339 }
else if (written < obuffer_length_) {
340 throw Exception(
"Failed to write RX28 packet %x for %x, only %d of %d bytes sent",
353 RobotisRX28::recv(
const unsigned char exp_length,
unsigned int timeout_ms)
355 timeval timeout = {0,
356 (suseconds_t)(timeout_ms == 0xFFFFFFFF ? default_timeout_ms_ : timeout_ms)
361 FD_SET(fd_, &read_fds);
364 rv = select(fd_ + 1, &read_fds, NULL, NULL, &timeout);
367 throw Exception(errno,
"Select on FD failed");
368 }
else if (rv == 0) {
370 throw TimeoutException(
"Timeout reached while waiting for incoming RX28 data");
377 while (bytes_read < 6) {
378 #ifdef DEBUG_RX28_COMM
379 printf(
"Trying to read %d bytes\n", 6 - bytes_read);
381 bytes_read += read(fd_, ibuffer_ + bytes_read, 6 - bytes_read);
382 #ifdef DEBUG_RX28_COMM
383 printf(
"%d bytes read ", bytes_read);
384 for (
int i = 0; i < bytes_read; ++i) {
385 printf(
"%X ", ibuffer_[i]);
390 if ((ibuffer_[0] != 0xFF) || (ibuffer_[1] != 0xFF)) {
391 throw Exception(
"Packet does not start with 0xFFFF.");
393 if (exp_length != ibuffer_[PACKET_OFFSET_LENGTH] - 2) {
394 tcflush(fd_, TCIFLUSH);
395 throw Exception(
"Wrong packet length, expected %u, got %u",
397 ibuffer_[PACKET_OFFSET_LENGTH] - 2);
399 const unsigned char plength = ibuffer_[PACKET_OFFSET_LENGTH] - 2;
400 #ifdef DEBUG_RX28_COMM
401 printf(
"header read, params have length %d\n", plength);
405 while (bytes_read < plength) {
406 bytes_read += read(fd_, &ibuffer_[6] + bytes_read, plength - bytes_read);
408 if (bytes_read < plength) {
409 throw Exception(
"Failed to read packet data");
413 ibuffer_length_ = plength + 2 + 4;
414 #ifdef DEBUG_RX28_COMM
416 for (
int i = 0; i < ibuffer_length_; ++i) {
417 printf(
"%X ", ibuffer_[i]);
423 unsigned char checksum = calc_checksum(ibuffer_[PACKET_OFFSET_ID],
424 ibuffer_[PACKET_OFFSET_INST],
425 &ibuffer_[PACKET_OFFSET_PARAM],
427 if (checksum != ibuffer_[plength + 5]) {
428 throw Exception(
"Checksum error while receiving packet, expected %d, got %d",
430 ibuffer_[plength + 5]);
433 ibuffer_length_ = plength + 2 + 4;
443 ioctl(fd_, FIONREAD, &num_bytes);
444 return (num_bytes > 0);
471 send(BROADCAST_ID, INST_PING, NULL, 0);
473 for (
unsigned int i = 0; i < RX28_MAX_NUM_SERVOS; ++i) {
476 rv.push_back(ibuffer_[PACKET_OFFSET_ID]);
484 for (DeviceList::iterator i = rv.begin(); i != rv.end(); ++i) {
486 read_table_values(*i);
488 e.
append(
"Failed to receive control table for servo %u", *i);
508 send(
id, INST_PING, NULL, 0);
525 start_read_table_values(
id);
526 finish_read_table_values();
540 unsigned char param[2];
542 param[1] = RX28_CONTROL_TABLE_LENGTH;
544 send(
id, INST_READ, param, 2);
556 recv(RX28_CONTROL_TABLE_LENGTH);
558 if (ibuffer_length_ != 5 + RX28_CONTROL_TABLE_LENGTH + 1) {
559 throw Exception(
"Input buffer of invalid size: %u vs. %u",
561 5 + RX28_CONTROL_TABLE_LENGTH + 1);
563 memcpy(control_table_[ibuffer_[PACKET_OFFSET_ID]],
564 &ibuffer_[PACKET_OFFSET_PARAM],
565 RX28_CONTROL_TABLE_LENGTH);
581 unsigned char param[2];
583 param[1] = read_length;
585 send(
id, INST_READ, param, 2);
588 if (ibuffer_length_ != (5 + read_length + 1)) {
589 throw Exception(
"Input buffer not of expected size, expected %u, got %u",
590 (5 + read_length + 1),
594 for (
unsigned int i = 0; i < read_length; ++i) {
595 control_table_[id][addr + i] = ibuffer_[PACKET_OFFSET_PARAM + i];
612 unsigned char param[3];
614 param[1] = value & 0xFF;
615 param[2] = (value >> 8) & 0xFF;
618 send(
id, INST_WRITE, param, double_byte ? 3 : 2);
620 if (
id == BROADCAST_ID) {
621 for (
unsigned int i = 0; i < RX28_MAX_NUM_SERVOS; ++i) {
622 control_table_[i][addr] = param[1];
624 control_table_[i][addr + 1] = param[2];
627 control_table_[id][addr] = param[1];
629 control_table_[id][addr + 1] = param[2];
632 if ((
id != BROADCAST_ID) && responds_all(
id))
648 unsigned char start_addr,
649 unsigned char *values,
650 unsigned int num_values)
652 unsigned char param[num_values + 1];
653 param[0] = start_addr;
654 for (
unsigned int i = 0; i < num_values; ++i) {
655 param[i + 1] = values[i];
659 send(
id, INST_WRITE, param, num_values + 1);
661 if (
id == BROADCAST_ID) {
662 for (
unsigned int i = 0; i < RX28_MAX_NUM_SERVOS; ++i) {
663 for (
unsigned int j = 0; j < num_values; ++j) {
664 control_table_[i][start_addr + j] = values[j];
668 for (
unsigned int j = 0; j < num_values; ++j) {
669 control_table_[id][start_addr + j] = values[j];
673 if ((
id != BROADCAST_ID) && responds_all(
id))
687 RobotisRX28::assert_valid_id(
unsigned char id)
689 if (
id == BROADCAST_ID) {
690 throw Exception(
"Data can only be queried for a specific servo");
691 }
else if (
id >= RX28_MAX_NUM_SERVOS) {
702 RobotisRX28::merge_twobyte_value(
unsigned int id,
unsigned char ind_l,
unsigned char ind_h)
704 unsigned int rv = (control_table_[id][ind_h] & 0xFF) << 8;
705 rv |= control_table_[id][ind_l] & 0xFF;
718 RobotisRX28::get_value(
unsigned int id,
bool refresh,
unsigned int ind_l,
unsigned int ind_h)
723 read_table_value(
id, ind_l, (ind_h == 0xFFFFFFFF ? 1 : 2));
725 if (ind_h == 0xFFFFFFFF) {
726 return control_table_[id][ind_l];
728 return merge_twobyte_value(
id, ind_l, ind_h);
740 return get_value(
id, refresh, P_MODEL_NUMBER_L, P_MODEL_NUMBER_H);
751 return get_value(
id, refresh, P_PRESENT_POSITION_L, P_PRESENT_POSITION_H);
762 return get_value(
id, refresh, P_VERSION);
773 return get_value(
id, refresh, P_BAUD_RATE);
784 return get_value(
id, refresh, P_RETURN_DELAY_TIME);
795 unsigned int &cw_limit,
796 unsigned int &ccw_limit,
799 cw_limit = get_value(
id, refresh, P_CW_ANGLE_LIMIT_L, P_CW_ANGLE_LIMIT_H);
800 ccw_limit = get_value(
id, refresh, P_CCW_ANGLE_LIMIT_L, P_CCW_ANGLE_LIMIT_H);
811 return get_value(
id, refresh, P_LIMIT_TEMPERATURE);
826 low = get_value(
id, refresh, P_DOWN_LIMIT_VOLTAGE);
827 high = get_value(
id, refresh, P_UP_LIMIT_VOLTAGE);
838 return get_value(
id, refresh, P_MAX_TORQUE_L, P_MAX_TORQUE_H);
849 return get_value(
id, refresh, P_RETURN_LEVEL);
860 return get_value(
id, refresh, P_ALARM_LED);
871 return get_value(
id, refresh, P_ALARM_SHUTDOWN);
882 unsigned int &down_calib,
883 unsigned int &up_calib,
886 down_calib = get_value(
id, refresh, P_DOWN_CALIBRATION_L, P_DOWN_CALIBRATION_H);
887 up_calib = get_value(
id, refresh, P_UP_CALIBRATION_L, P_UP_CALIBRATION_H);
898 return (get_value(
id, refresh, P_TORQUE_ENABLE) == 1);
909 return (get_value(
id, refresh, P_LED) == 1);
922 unsigned char &cw_margin,
923 unsigned char &cw_slope,
924 unsigned char &ccw_margin,
925 unsigned char &ccw_slope,
928 cw_margin = get_value(
id, refresh, P_CW_COMPLIANCE_MARGIN);
929 cw_slope = get_value(
id, refresh, P_CW_COMPLIANCE_SLOPE);
930 ccw_margin = get_value(
id, refresh, P_CCW_COMPLIANCE_MARGIN);
931 ccw_slope = get_value(
id, refresh, P_CCW_COMPLIANCE_SLOPE);
942 return get_value(
id, refresh, P_GOAL_POSITION_L, P_GOAL_POSITION_H);
953 return get_value(
id, refresh, P_GOAL_SPEED_L, P_GOAL_SPEED_H);
964 float voltage = get_voltage(
id, refresh) / 10.0;
966 if ((voltage < 12.0) || (voltage > 16.0)) {
970 float sec_per_deg_12V = SEC_PER_60DEG_12V / 60.0;
971 float sec_per_deg_16V = SEC_PER_60DEG_16V / 60.0;
973 float range_sec_per_deg = sec_per_deg_12V - sec_per_deg_16V;
974 float pos = voltage - 12.0;
976 float sec_per_deg = sec_per_deg_16V + pos * range_sec_per_deg;
977 float deg_per_sec = 1.0 / sec_per_deg;
990 return get_value(
id, refresh, P_TORQUE_LIMIT_L, P_TORQUE_LIMIT_H);
1001 return get_value(
id, refresh, P_PRESENT_SPEED_L, P_PRESENT_SPEED_H);
1012 return get_value(
id, refresh, P_PRESENT_LOAD_L, P_PRESENT_LOAD_H);
1023 return get_value(
id, refresh, P_PRESENT_VOLTAGE);
1034 return get_value(
id, refresh, P_PRESENT_TEMPERATURE);
1045 return (get_value(
id, refresh, P_MOVING) == 1);
1056 return (get_value(
id, refresh, P_LOCK) == 1);
1067 return get_value(
id, refresh, P_PUNCH_L, P_PUNCH_H);
1077 write_table_value(
id, P_ID, new_id);
1087 write_table_value(
id, P_BAUD_RATE, baudrate);
1097 write_table_value(
id, P_RETURN_DELAY_TIME, return_delay_time);
1108 write_table_value(
id, P_CW_ANGLE_LIMIT_L, cw_limit,
true);
1109 write_table_value(
id, P_CCW_ANGLE_LIMIT_L, ccw_limit,
true);
1119 write_table_value(
id, P_LIMIT_TEMPERATURE, temp_limit);
1130 unsigned char param[2];
1133 write_table_values(
id, P_DOWN_LIMIT_VOLTAGE, param, 2);
1143 write_table_value(
id, P_MAX_TORQUE_L, max_torque,
true);
1154 write_table_value(
id, P_RETURN_LEVEL, status_return_level);
1164 write_table_value(
id, P_ALARM_LED, alarm_led);
1174 write_table_value(
id, P_ALARM_SHUTDOWN, alarm_shutdown);
1185 write_table_value(
id, P_TORQUE_ENABLE, enabled ? 1 : 0);
1198 if (num_servos > 120) {
1200 throw Exception(
"You cannot set more than 120 servos at once");
1204 va_start(arg, num_servos);
1206 unsigned int plength = 2 * num_servos + 2;
1207 unsigned char param[plength];
1208 param[0] = P_TORQUE_ENABLE;
1210 for (
unsigned int i = 0; i < num_servos; ++i) {
1211 unsigned char id = va_arg(arg,
unsigned int);
1212 param[2 + i * 2] = id;
1213 param[2 + i * 2 + 1] = enabled ? 1 : 0;
1217 send(BROADCAST_ID, INST_SYNC_WRITE, param, plength);
1227 write_table_value(
id, P_LED, led_enabled ? 1 : 0);
1239 unsigned char cw_margin,
1240 unsigned char cw_slope,
1241 unsigned char ccw_margin,
1242 unsigned char ccw_slope)
1244 unsigned char param[4];
1245 param[0] = cw_margin;
1246 param[1] = ccw_margin;
1247 param[2] = cw_slope;
1248 param[3] = ccw_slope;
1249 write_table_values(
id, P_CW_COMPLIANCE_MARGIN, param, 4);
1260 write_table_value(
id, P_GOAL_SPEED_L, goal_speed,
true);
1271 if (num_servos > 83) {
1273 throw Exception(
"You cannot set more than 83 speeds at once");
1277 va_start(arg, num_servos);
1279 unsigned int plength = 3 * num_servos + 2;
1280 unsigned char param[plength];
1281 param[0] = P_GOAL_SPEED_L;
1283 for (
unsigned int i = 0; i < num_servos; ++i) {
1284 unsigned char id = va_arg(arg,
unsigned int);
1285 unsigned int value = va_arg(arg,
unsigned int);
1287 param[2 + i * 3] = id;
1288 param[2 + i * 3 + 1] = (value & 0xFF);
1289 param[2 + i * 3 + 2] = (value >> 8) & 0xFF;
1293 send(BROADCAST_ID, INST_SYNC_WRITE, param, plength);
1303 write_table_value(
id, P_TORQUE_LIMIT_L, torque_limit,
true);
1313 write_table_value(
id, P_PUNCH_L, punch,
true);
1324 write_table_value(
id, P_LOCK, 1);
1335 write_table_value(
id, P_GOAL_POSITION_L, value,
true);
1347 if (num_servos > 83) {
1349 throw Exception(
"You cannot set more than 83 servos at once");
1353 va_start(arg, num_servos);
1355 unsigned int plength = 3 * num_servos + 2;
1356 unsigned char param[plength];
1357 param[0] = P_GOAL_POSITION_L;
1359 for (
unsigned int i = 0; i < num_servos; ++i) {
1360 unsigned char id = va_arg(arg,
unsigned int);
1361 unsigned int value = va_arg(arg,
unsigned int);
1362 param[2 + i * 3] = id;
1363 param[2 + i * 3 + 1] = (value & 0xFF);
1364 param[2 + i * 3 + 2] = (value >> 8) & 0xFF;
1368 send(BROADCAST_ID, INST_SYNC_WRITE, param, plength);