Fawkes API  Fawkes Development Version
rx28.cpp
1 
2 /***************************************************************************
3  * rx28.cpp - Controller for Visca cams
4  *
5  * Created: Tue Jun 16 11:09:32 2009 (based on visca.cpp)
6  * Copyright 2005-2009 Tim Niemueller [www.niemueller.de]
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 "rx28.h"
25 
26 #include <core/exceptions/software.h>
27 #include <core/exceptions/system.h>
28 #include <sys/ioctl.h>
29 #include <sys/time.h>
30 #include <utils/math/angle.h>
31 
32 #include <cstdarg>
33 #include <cstdlib>
34 #include <cstring>
35 #include <errno.h>
36 #include <fcntl.h>
37 #include <stdio.h>
38 #include <termios.h>
39 #include <unistd.h>
40 
41 const unsigned char RobotisRX28::BROADCAST_ID = 0xfe; /**< BROADCAST_ID */
42 const unsigned int RobotisRX28::MAX_POSITION = 0x3ff; /**< MAX_POSITION */
43 const unsigned int RobotisRX28::CENTER_POSITION = 0x1ff; /**< CENTER_POSITION */
44 const unsigned int RobotisRX28::MAX_SPEED = 0x3ff; /**< MAX_SPEED */
45 const float RobotisRX28::MAX_ANGLE_DEG = 300; /**< MAX_ANGLE_DEG */
46 const float RobotisRX28::MAX_ANGLE_RAD =
47  fawkes::deg2rad(RobotisRX28::MAX_ANGLE_DEG); /**< MAX_ANGLE_RAD */
49  RobotisRX28::MAX_ANGLE_RAD / (float)(RobotisRX28::MAX_POSITION); /**< RAD_PER_POS_TICK */
51  (float)(RobotisRX28::MAX_POSITION) / RobotisRX28::MAX_ANGLE_RAD; /**< POS_TICKS_PER_RAD */
52 const float RobotisRX28::SEC_PER_60DEG_12V = 0.167; /**< SEC_PER_60DEG_12V */
53 const float RobotisRX28::SEC_PER_60DEG_16V = 0.126; /**< SEC_PER_60DEG_16V */
54 
55 const unsigned char RobotisRX28::SRL_RESPOND_NONE = 0; /**< SRL_RESPOND_NONE */
56 const unsigned char RobotisRX28::SRL_RESPOND_READ = 1; /**< SRL_RESPOND_READ */
57 const unsigned char RobotisRX28::SRL_RESPOND_ALL = 2; /**< SRL_RESPOND_ALL */
58 
59 const unsigned char RobotisRX28::P_MODEL_NUMBER_L = 0; /**< P_MODEL_NUMBER_L */
60 const unsigned char RobotisRX28::P_MODEL_NUMBER_H = 1; /**< P_MODEL_NUMBER_H */
61 const unsigned char RobotisRX28::P_VERSION = 2; /**< P_VERSION */
62 const unsigned char RobotisRX28::P_ID = 3; /**< P_ID */
63 const unsigned char RobotisRX28::P_BAUD_RATE = 4; /**< P_BAUD_RATE */
64 const unsigned char RobotisRX28::P_RETURN_DELAY_TIME = 5; /**< P_RETURN_DELAY_TIME */
65 const unsigned char RobotisRX28::P_CW_ANGLE_LIMIT_L = 6; /**< P_CW_ANGLE_LIMIT_L */
66 const unsigned char RobotisRX28::P_CW_ANGLE_LIMIT_H = 7; /**< P_CW_ANGLE_LIMIT_H */
67 const unsigned char RobotisRX28::P_CCW_ANGLE_LIMIT_L = 8; /**< P_CCW_ANGLE_LIMIT_L */
68 const unsigned char RobotisRX28::P_CCW_ANGLE_LIMIT_H = 9; /**< P_CCW_ANGLE_LIMIT_H */
69 const unsigned char RobotisRX28::P_SYSTEM_DATA2 = 10; /**< P_SYSTEM_DATA2 */
70 const unsigned char RobotisRX28::P_LIMIT_TEMPERATURE = 11; /**< P_LIMIT_TEMPERATURE */
71 const unsigned char RobotisRX28::P_DOWN_LIMIT_VOLTAGE = 12; /**< P_DOWN_LIMIT_VOLTAGE */
72 const unsigned char RobotisRX28::P_UP_LIMIT_VOLTAGE = 13; /**< P_UP_LIMIT_VOLTAGE */
73 const unsigned char RobotisRX28::P_MAX_TORQUE_L = 14; /**< P_MAX_TORQUE_L */
74 const unsigned char RobotisRX28::P_MAX_TORQUE_H = 15; /**< P_MAX_TORQUE_H */
75 const unsigned char RobotisRX28::P_RETURN_LEVEL = 16; /**< P_RETURN_LEVEL */
76 const unsigned char RobotisRX28::P_ALARM_LED = 17; /**< P_ALARM_LED */
77 const unsigned char RobotisRX28::P_ALARM_SHUTDOWN = 18; /**< P_ALARM_SHUTDOWN */
78 const unsigned char RobotisRX28::P_OPERATING_MODE = 19; /**< P_OPERATING_MODE */
79 const unsigned char RobotisRX28::P_DOWN_CALIBRATION_L = 20; /**< P_DOWN_CALIBRATION_L */
80 const unsigned char RobotisRX28::P_DOWN_CALIBRATION_H = 21; /**< P_DOWN_CALIBRATION_H */
81 const unsigned char RobotisRX28::P_UP_CALIBRATION_L = 22; /**< P_UP_CALIBRATION_L */
82 const unsigned char RobotisRX28::P_UP_CALIBRATION_H = 23; /**< P_UP_CALIBRATION_H */
83 
84 const unsigned char RobotisRX28::P_TORQUE_ENABLE = 24; /**< P_TORQUE_ENABLE */
85 const unsigned char RobotisRX28::P_LED = 25; /**< P_LED */
86 const unsigned char RobotisRX28::P_CW_COMPLIANCE_MARGIN = 26; /**< P_CW_COMPLIANCE_MARGIN */
87 const unsigned char RobotisRX28::P_CCW_COMPLIANCE_MARGIN = 27; /**< P_CCW_COMPLIANCE_MARGIN */
88 const unsigned char RobotisRX28::P_CW_COMPLIANCE_SLOPE = 28; /**< P_CW_COMPLIANCE_SLOPE */
89 const unsigned char RobotisRX28::P_CCW_COMPLIANCE_SLOPE = 29; /**< P_CCW_COMPLIANCE_SLOPE */
90 const unsigned char RobotisRX28::P_GOAL_POSITION_L = 30; /**< P_GOAL_POSITION_L */
91 const unsigned char RobotisRX28::P_GOAL_POSITION_H = 31; /**< P_GOAL_POSITION_H */
92 const unsigned char RobotisRX28::P_GOAL_SPEED_L = 32; /**< P_GOAL_SPEED_L */
93 const unsigned char RobotisRX28::P_GOAL_SPEED_H = 33; /**< P_GOAL_SPEED_H */
94 const unsigned char RobotisRX28::P_TORQUE_LIMIT_L = 34; /**< P_TORQUE_LIMIT_L */
95 const unsigned char RobotisRX28::P_TORQUE_LIMIT_H = 35; /**< P_TORQUE_LIMIT_H */
96 const unsigned char RobotisRX28::P_PRESENT_POSITION_L = 36; /**< P_PRESENT_POSITION_L */
97 const unsigned char RobotisRX28::P_PRESENT_POSITION_H = 37; /**< P_PRESENT_POSITION_H */
98 const unsigned char RobotisRX28::P_PRESENT_SPEED_L = 38; /**< P_PRESENT_SPEED_L */
99 const unsigned char RobotisRX28::P_PRESENT_SPEED_H = 39; /**< P_PRESENT_SPEED_H */
100 const unsigned char RobotisRX28::P_PRESENT_LOAD_L = 40; /**< P_PRESENT_LOAD_L */
101 const unsigned char RobotisRX28::P_PRESENT_LOAD_H = 41; /**< P_PRESENT_LOAD_H */
102 const unsigned char RobotisRX28::P_PRESENT_VOLTAGE = 42; /**< P_PRESENT_VOLTAGE */
103 const unsigned char RobotisRX28::P_PRESENT_TEMPERATURE = 43; /**< P_PRESENT_TEMPERATURE */
104 const unsigned char RobotisRX28::P_REGISTERED_INSTRUCTION = 44; /**< P_REGISTERED_INSTRUCTION */
105 const unsigned char RobotisRX28::P_PAUSE_TIME = 45; /**< P_PAUSE_TIME */
106 const unsigned char RobotisRX28::P_MOVING = 46; /**< P_MOVING */
107 const unsigned char RobotisRX28::P_LOCK = 47; /**< P_LOCK */
108 const unsigned char RobotisRX28::P_PUNCH_L = 48; /**< P_PUNCH_L */
109 const unsigned char RobotisRX28::P_PUNCH_H = 49; /**< P_PUNCH_H */
110 
111 //--- Instructions ---
112 const unsigned char RobotisRX28::INST_PING = 0x01; /**< INST_PING */
113 const unsigned char RobotisRX28::INST_READ = 0x02; /**< INST_READ */
114 const unsigned char RobotisRX28::INST_WRITE = 0x03; /**< INST_WRITE */
115 const unsigned char RobotisRX28::INST_REG_WRITE = 0x04; /**< INST_REG_WRITE */
116 const unsigned char RobotisRX28::INST_ACTION = 0x05; /**< INST_ACTION */
117 const unsigned char RobotisRX28::INST_RESET = 0x06; /**< INST_RESET */
118 const unsigned char RobotisRX28::INST_DIGITAL_RESET = 0x07; /**< INST_DIGITAL_RESET */
119 const unsigned char RobotisRX28::INST_SYSTEM_READ = 0x0C; /**< INST_SYSTEM_READ */
120 const unsigned char RobotisRX28::INST_SYSTEM_WRITE = 0x0D; /**< INST_SYSTEM_WRITE */
121 const unsigned char RobotisRX28::INST_SYNC_WRITE = 0x83; /**< INST_SYNC_WRITE */
122 const unsigned char RobotisRX28::INST_SYNC_REG_WRITE = 0x84; /**< INST_SYNC_REG_WRITE */
123 
124 const unsigned char RobotisRX28::PACKET_OFFSET_ID = 2; /**< PACKET_OFFSET_ID */
125 const unsigned char RobotisRX28::PACKET_OFFSET_LENGTH = 3; /**< PACKET_OFFSET_LENGTH */
126 const unsigned char RobotisRX28::PACKET_OFFSET_INST = 4; /**< PACKET_OFFSET_INST */
127 const unsigned char RobotisRX28::PACKET_OFFSET_PARAM = 5; /**< PACKET_OFFSET_PARAM */
128 const unsigned char RobotisRX28::PACKET_OFFSET_ERROR = 4; /**< PACKET_OFFSET_ERROR */
129 
130 using namespace std;
131 using namespace fawkes;
132 
133 /** @class RobotisRX28 "rx28.h"
134  * Class to access a chain of Robotis RX28 servos.
135  * One instance of this class communicates with a chain of up to 254 Robotis
136  * RX28 servos, which are uniquely identified with an ID. Before making use of
137  * the chain, connect each servo individually and set its ID. See the
138  * discover() method for more information about numbering of the servos.
139  * To achieve a higher speed, it is recommended to set the status return level
140  * to reply only on READ instructions. You can do this for the whole chain with
141  * @code
142  * rx28->set_status_return_level(RobotisRX28::BROADCAST_ID, RobotisRX28::SRL_RESPOND_READ);
143  * @endcode
144  *
145  * @author Tim Niemueller
146  */
147 
148 /** Constructor.
149  * @param device_file device file of the serial port
150  * @param default_timeout_ms the timeout to apply by default to reading operations
151  */
152 RobotisRX28::RobotisRX28(const char *device_file, unsigned int default_timeout_ms)
153 {
154  default_timeout_ms_ = default_timeout_ms;
155  device_file_ = strdup(device_file);
156  fd_ = -1;
157  obuffer_length_ = 0;
158  ibuffer_length_ = 0;
159  memset(control_table_, 0, RX28_MAX_NUM_SERVOS * RX28_CONTROL_TABLE_LENGTH);
160  try {
161  open();
162  } catch (Exception &e) {
163  free(device_file_);
164  throw;
165  }
166  for (size_t i = 0; i < sizeof(obuffer_) / sizeof(obuffer_[0]); ++i) {
167  obuffer_[i] = 0;
168  }
169  for (size_t i = 0; i < sizeof(ibuffer_) / sizeof(ibuffer_[0]); ++i) {
170  ibuffer_[i] = 0;
171  }
172 }
173 
174 /** Destructor. */
176 {
177  free(device_file_);
178 }
179 
180 /** Open serial port. */
181 void
183 {
184  struct termios param;
185 
186  fd_ = ::open(device_file_, O_NOCTTY | O_RDWR);
187  if (fd_ == -1) {
188  throw CouldNotOpenFileException(device_file_, errno, "Cannot open device file");
189  }
190  tcflush(fd_, TCIOFLUSH);
191 
192  if (tcgetattr(fd_, &param) == -1) {
193  Exception e(errno, "Getting the port parameters failed");
194  ::close(fd_);
195  fd_ = -1;
196  throw e;
197  }
198 
199  cfsetospeed(&param, B57600);
200  cfsetispeed(&param, B57600);
201 
202  param.c_lflag &= ~(ICANON | ECHO | ECHOE | ECHOK | ECHONL | ISIG | IEXTEN);
203  param.c_iflag &= ~(INLCR | IGNCR | ICRNL | IGNBRK | PARMRK);
204 
205  // turn off hardware flow control
206  param.c_iflag &= ~(IXON | IXOFF | IXANY);
207  param.c_cflag &= ~CRTSCTS;
208 
209  param.c_cflag |= (CREAD | CLOCAL);
210 
211  // number of data bits: 8
212  param.c_cflag &= ~CS5 & ~CS6 & ~CS7 & ~CS8;
213  param.c_cflag |= CS8;
214 
215  // parity: none
216  param.c_cflag &= ~(PARENB | PARODD);
217  param.c_iflag &= ~(INPCK | ISTRIP);
218 
219  // stop bits: 1
220  param.c_cflag &= ~CSTOPB;
221 
222  //enable raw output
223  param.c_oflag &= ~OPOST;
224 
225  param.c_cc[VMIN] = 1;
226  param.c_cc[VTIME] = 0;
227 
228  tcflush(fd_, TCIOFLUSH);
229 
230  if (tcsetattr(fd_, TCSANOW, &param) != 0) {
231  Exception e(errno, "Setting the port parameters failed");
232  ::close(fd_);
233  fd_ = -1;
234  throw e;
235  }
236 
237  //char junk[1];
238  //read(fd_, junk, 1);
239 
240 #ifdef TIMETRACKER_VISCA
241  tracker = new TimeTracker();
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");
247 #endif
248 
249  // success
250 }
251 
252 /** Close port. */
253 void
255 {
256  if (fd_ >= 0) {
257  ::close(fd_);
258  fd_ = -1;
259  }
260 }
261 
262 /** Calculate the checksum for the given packet.
263  * @param id servo ID
264  * @param instruction instruction to send
265  * @param params parameters in the message
266  * @param plength length of the params array
267  * @return checksum as defined in the RX28 manual
268  */
269 unsigned char
270 RobotisRX28::calc_checksum(const unsigned char id,
271  const unsigned char instruction,
272  const unsigned char *params,
273  const unsigned char plength)
274 {
275  unsigned int checksum = id + instruction + plength + 2;
276  for (unsigned char i = 0; i < plength; ++i) {
277  checksum += params[i];
278  }
279 
280  return ~(checksum & 0xFF);
281 }
282 
283 /** Send instruction packet.
284  * @param id servo ID
285  * @param instruction instruction to send
286  * @param params parameters in the message
287  * @param plength length of the params array
288  */
289 void
290 RobotisRX28::send(const unsigned char id,
291  const unsigned char instruction,
292  const unsigned char *params,
293  const unsigned char plength)
294 {
295  // Byte 0 and 1 must be 0xFF
296  obuffer_[0] = 0xFF;
297  obuffer_[1] = 0xFF;
298  obuffer_[PACKET_OFFSET_ID] = id;
299  obuffer_[PACKET_OFFSET_LENGTH] = plength + 2;
300  obuffer_[PACKET_OFFSET_INST] = instruction;
301 
302  unsigned int checksum = id + plength + 2;
303 
304  for (unsigned char i = 0; i < plength; ++i) {
305  obuffer_[PACKET_OFFSET_PARAM + i] = params[i];
306  checksum += params[i];
307  }
308 
309  // actually +5, but zero-based array, therefore index 4, but fifth value
310  obuffer_[3 + plength + 2] = calc_checksum(id, instruction, params, plength);
311  obuffer_length_ = plength + 2 + 4; // 4 for 0xFF 0xFF ID LENGTH
312 
313 #ifdef DEBUG_RX28_COMM
314  printf("Sending: ");
315  for (int i = 0; i < obuffer_length_; ++i) {
316  printf("%X ", obuffer_[i]);
317  }
318  printf("\n");
319 #endif
320 
321  int written = write(fd_, obuffer_, obuffer_length_);
322  //printf("Wrote %d bytes\n", written);
323 
324  // For some reason we have to read the shit immediately, although ECHO is off
325  int readd = 0;
326  while (readd < obuffer_length_) {
327  readd += read(fd_, ibuffer_ + readd, obuffer_length_ - readd);
328  }
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]);
333  }
334  printf("\n");
335 #endif
336 
337  if (written < 0) {
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",
341  instruction,
342  id,
343  written,
344  obuffer_length_);
345  }
346 }
347 
348 /** Receive a packet.
349  * @param timeout_ms maximum wait time in miliseconds
350  * @param exp_length expected number of bytes
351  */
352 void
353 RobotisRX28::recv(const unsigned char exp_length, unsigned int timeout_ms)
354 {
355  timeval timeout = {0,
356  (suseconds_t)(timeout_ms == 0xFFFFFFFF ? default_timeout_ms_ : timeout_ms)
357  * 1000};
358 
359  fd_set read_fds;
360  FD_ZERO(&read_fds);
361  FD_SET(fd_, &read_fds);
362 
363  int rv = 0;
364  rv = select(fd_ + 1, &read_fds, NULL, NULL, &timeout);
365 
366  if (rv == -1) {
367  throw Exception(errno, "Select on FD failed");
368  } else if (rv == 0) {
369  //printf("Timeout, no data :-/\n");
370  throw TimeoutException("Timeout reached while waiting for incoming RX28 data");
371  }
372 
373  ibuffer_length_ = 0;
374 
375  // get octets one by one
376  int bytes_read = 0;
377  while (bytes_read < 6) {
378 #ifdef DEBUG_RX28_COMM
379  printf("Trying to read %d bytes\n", 6 - bytes_read);
380 #endif
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]);
386  }
387  printf("\n");
388 #endif
389  }
390  if ((ibuffer_[0] != 0xFF) || (ibuffer_[1] != 0xFF)) {
391  throw Exception("Packet does not start with 0xFFFF.");
392  }
393  if (exp_length != ibuffer_[PACKET_OFFSET_LENGTH] - 2) {
394  tcflush(fd_, TCIFLUSH);
395  throw Exception("Wrong packet length, expected %u, got %u",
396  exp_length,
397  ibuffer_[PACKET_OFFSET_LENGTH] - 2);
398  }
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);
402 #endif
403  if (plength > 0) {
404  bytes_read = 0;
405  while (bytes_read < plength) {
406  bytes_read += read(fd_, &ibuffer_[6] + bytes_read, plength - bytes_read);
407  }
408  if (bytes_read < plength) {
409  throw Exception("Failed to read packet data");
410  }
411  }
412 
413  ibuffer_length_ = plength + 2 + 4;
414 #ifdef DEBUG_RX28_COMM
415  printf("Read: ");
416  for (int i = 0; i < ibuffer_length_; ++i) {
417  printf("%X ", ibuffer_[i]);
418  }
419  printf("\n");
420 #endif
421 
422  // verify checksum
423  unsigned char checksum = calc_checksum(ibuffer_[PACKET_OFFSET_ID],
424  ibuffer_[PACKET_OFFSET_INST],
425  &ibuffer_[PACKET_OFFSET_PARAM],
426  plength);
427  if (checksum != ibuffer_[plength + 5]) {
428  throw Exception("Checksum error while receiving packet, expected %d, got %d",
429  checksum,
430  ibuffer_[plength + 5]);
431  }
432 
433  ibuffer_length_ = plength + 2 + 4;
434 }
435 
436 /** Check data availability.
437  * @return true if data is available, false otherwise
438  */
439 bool
441 {
442  int num_bytes = 0;
443  ioctl(fd_, FIONREAD, &num_bytes);
444  return (num_bytes > 0);
445 }
446 
447 /** Discover devices on the bus.
448  * This method will send a PING instruction to the broadcast ID and collect
449  * responses. This assumes that the return delay time is set appropriately that
450  * all responses can be received without collisions, and that the difference
451  * between the time of two consecutive servos is smaller than the given timeout
452  * (note that this might be void if you have one servo with ID 1 and one with
453  * ID 253). After sending the packet this method will do up to
454  * RX28_MAX_NUM_SERVOS receive operations, each with the given timeout. After the
455  * first timeout the discovery is aborted assuming that all replies have been
456  * received. You can set the timeout really high (several seconds) to be sure
457  * that all connected servos are recognized.
458  * For this to work best it is recommended to set consecutive servo IDs starting
459  * from 1 on the servos.
460  * After the servos are found, the control tables of all recognized servos are
461  * received to ensure that all other methods return valid data.
462  * @param timeout_ms maximum timeout to wait for replies.
463  * @return list of detected servo IDs
464  */
466 RobotisRX28::discover(unsigned int timeout_ms)
467 {
468  DeviceList rv;
469 
470  // simply re-throw exception upwards
471  send(BROADCAST_ID, INST_PING, NULL, 0);
472 
473  for (unsigned int i = 0; i < RX28_MAX_NUM_SERVOS; ++i) {
474  try {
475  recv(0, timeout_ms);
476  rv.push_back(ibuffer_[PACKET_OFFSET_ID]);
477  } catch (TimeoutException &e) {
478  // the first timeout, no more devices can be expected to respond
479  break;
480  }
481  }
482 
483  // now get data about all servos
484  for (DeviceList::iterator i = rv.begin(); i != rv.end(); ++i) {
485  try {
486  read_table_values(*i);
487  } catch (Exception &e) {
488  e.append("Failed to receive control table for servo %u", *i);
489  throw;
490  }
491  }
492 
493  return rv;
494 }
495 
496 /** Ping servo.
497  * This pings the given servo by sending a PING instruction and
498  * reading the reply.
499  * @param id servo ID, not the broadcast ID
500  * @param timeout_ms maximum wait time in miliseconds
501  * @return true if the ping was successful, false otherwise
502  */
503 bool
504 RobotisRX28::ping(unsigned char id, unsigned int timeout_ms)
505 {
506  assert_valid_id(id);
507  try {
508  send(id, INST_PING, NULL, 0);
509  recv(0, timeout_ms);
510  return true;
511  } catch (Exception &e) {
512  e.print_trace();
513  return false;
514  }
515 }
516 
517 /** Read all table values for given servo.
518  * This issues a READ comment for the whole control table and waits for the
519  * response.
520  * @param id servo ID
521  */
522 void
524 {
525  start_read_table_values(id);
526  finish_read_table_values();
527 }
528 
529 /** Start to receive table values.
530  * This method sends a READ instruction packet for the whole table, but it does
531  * not wait for the reply. This can be used to overlap the receiving with other
532  * operations. You have to ensure to call finish_read_table_values() before
533  * sending any other data.
534  * @param id servo ID, not the broadcast ID
535  */
536 void
538 {
539  assert_valid_id(id);
540  unsigned char param[2];
541  param[0] = 0x00;
542  param[1] = RX28_CONTROL_TABLE_LENGTH;
543 
544  send(id, INST_READ, param, 2);
545 }
546 
547 /** Finish control table receive operations.
548  * This executes the receive operation initiated by start_read_table_values().
549  * This will read the values and write the output to the control table
550  * (in memory, not in the servo), such that the appropriate get methods will
551  * return the new data.
552  */
553 void
555 {
556  recv(RX28_CONTROL_TABLE_LENGTH);
557 
558  if (ibuffer_length_ != 5 + RX28_CONTROL_TABLE_LENGTH + 1) {
559  throw Exception("Input buffer of invalid size: %u vs. %u",
560  ibuffer_length_,
561  5 + RX28_CONTROL_TABLE_LENGTH + 1);
562  }
563  memcpy(control_table_[ibuffer_[PACKET_OFFSET_ID]],
564  &ibuffer_[PACKET_OFFSET_PARAM],
565  RX28_CONTROL_TABLE_LENGTH);
566 }
567 
568 /** Read a table value.
569  * This will read the given value(s) and write the output to the control table
570  * (in memory, not in the servo), such that the appropriate get method will return
571  * the new value.
572  * @param id servo ID, not the broadcast ID
573  * @param addr start addr, one of the P_* constants.
574  * @param read_length number of bytes to read
575  */
576 void
577 RobotisRX28::read_table_value(unsigned char id, unsigned char addr, unsigned char read_length)
578 {
579  assert_valid_id(id);
580 
581  unsigned char param[2];
582  param[0] = addr;
583  param[1] = read_length;
584 
585  send(id, INST_READ, param, 2);
586  recv(read_length);
587 
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),
591  ibuffer_length_);
592  }
593 
594  for (unsigned int i = 0; i < read_length; ++i) {
595  control_table_[id][addr + i] = ibuffer_[PACKET_OFFSET_PARAM + i];
596  }
597 }
598 
599 /** Write a table value.
600  * @param id servo ID, may be the broadcast ID
601  * @param addr start addr, one of the P_* constants.
602  * @param value value to write
603  * @param double_byte if true, will assume value to be a two-byte value, otherwise
604  * it is considered as a one-byte value.
605  */
606 void
608  unsigned char addr,
609  unsigned int value,
610  bool double_byte)
611 {
612  unsigned char param[3];
613  param[0] = addr;
614  param[1] = value & 0xFF;
615  param[2] = (value >> 8) & 0xFF;
616 
617  try {
618  send(id, INST_WRITE, param, double_byte ? 3 : 2);
619 
620  if (id == BROADCAST_ID) {
621  for (unsigned int i = 0; i < RX28_MAX_NUM_SERVOS; ++i) {
622  control_table_[i][addr] = param[1];
623  if (double_byte)
624  control_table_[i][addr + 1] = param[2];
625  }
626  } else {
627  control_table_[id][addr] = param[1];
628  if (double_byte)
629  control_table_[id][addr + 1] = param[2];
630  }
631 
632  if ((id != BROADCAST_ID) && responds_all(id))
633  recv(0);
634  } catch (Exception &e) {
635  e.print_trace();
636  throw;
637  }
638 }
639 
640 /** Write multiple table values.
641  * @param id servo ID, may be the broadcast ID
642  * @param start_addr start addr, one of the P_* constants.
643  * @param values values to write
644  * @param num_values length in bytes of the values array
645  */
646 void
648  unsigned char start_addr,
649  unsigned char *values,
650  unsigned int num_values)
651 {
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];
656  }
657 
658  try {
659  send(id, INST_WRITE, param, num_values + 1);
660 
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];
665  }
666  }
667  } else {
668  for (unsigned int j = 0; j < num_values; ++j) {
669  control_table_[id][start_addr + j] = values[j];
670  }
671  }
672 
673  if ((id != BROADCAST_ID) && responds_all(id))
674  recv(0);
675  } catch (Exception &e) {
676  e.print_trace();
677  throw;
678  }
679 }
680 
681 /** Assert that the ID is valid.
682  * @exception Exception thrown if \p id is the broadcast ID
683  * @exception OutOfBoundsException thrown if the number is greater than the
684  * maximum number of servos.
685  */
686 void
687 RobotisRX28::assert_valid_id(unsigned char id)
688 {
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) {
692  throw OutOfBoundsException("Servo ID out of bounds", id, 0, RX28_MAX_NUM_SERVOS);
693  }
694 }
695 
696 /** Merge two values to a two-byte value.
697  * @param id servo id, not the broadcast ID
698  * @param ind_l low index in control table
699  * @param ind_h high index in control table
700  */
701 unsigned int
702 RobotisRX28::merge_twobyte_value(unsigned int id, unsigned char ind_l, unsigned char ind_h)
703 {
704  unsigned int rv = (control_table_[id][ind_h] & 0xFF) << 8;
705  rv |= control_table_[id][ind_l] & 0xFF;
706  return rv;
707 }
708 
709 /** Get a value from the control table, possibly from servo.
710  * @param id servo ID, not the broadcast ID
711  * @param refresh if true, will issue a read command for the value
712  * @param ind_l low index in control table
713  * @param ind_h high index in control table, only set if value is a
714  * two-byte value.
715  * @return value
716  */
717 unsigned int
718 RobotisRX28::get_value(unsigned int id, bool refresh, unsigned int ind_l, unsigned int ind_h)
719 {
720  assert_valid_id(id);
721 
722  if (refresh)
723  read_table_value(id, ind_l, (ind_h == 0xFFFFFFFF ? 1 : 2));
724 
725  if (ind_h == 0xFFFFFFFF) {
726  return control_table_[id][ind_l];
727  } else {
728  return merge_twobyte_value(id, ind_l, ind_h);
729  }
730 }
731 
732 /** Get model.
733  * @param id servo ID, not the broadcast ID
734  * @param refresh if true, will issue a read command for the value
735  * @return model
736  */
737 unsigned int
738 RobotisRX28::get_model(unsigned char id, bool refresh)
739 {
740  return get_value(id, refresh, P_MODEL_NUMBER_L, P_MODEL_NUMBER_H);
741 }
742 
743 /** Get current position.
744  * @param id servo ID, not the broadcast ID
745  * @param refresh if true, will issue a read command for the value
746  * @return current position
747  */
748 unsigned int
749 RobotisRX28::get_position(unsigned char id, bool refresh)
750 {
751  return get_value(id, refresh, P_PRESENT_POSITION_L, P_PRESENT_POSITION_H);
752 }
753 
754 /** Get firmware version.
755  * @param id servo ID, not the broadcast ID
756  * @param refresh if true, will issue a read command for the value
757  * @return firmware version
758  */
759 unsigned char
760 RobotisRX28::get_firmware_version(unsigned char id, bool refresh)
761 {
762  return get_value(id, refresh, P_VERSION);
763 }
764 
765 /** Get baud rate.
766  * @param id servo ID, not the broadcast ID
767  * @param refresh if true, will issue a read command for the value
768  * @return baud rate
769  */
770 unsigned char
771 RobotisRX28::get_baudrate(unsigned char id, bool refresh)
772 {
773  return get_value(id, refresh, P_BAUD_RATE);
774 }
775 
776 /** Get time of the delay before replies are sent.
777  * @param id servo ID, not the broadcast ID
778  * @param refresh if true, will issue a read command for the value
779  * @return delay time
780  */
781 unsigned char
782 RobotisRX28::get_delay_time(unsigned char id, bool refresh)
783 {
784  return get_value(id, refresh, P_RETURN_DELAY_TIME);
785 }
786 
787 /** Get angle limits.
788  * @param id servo ID, not the broadcast ID
789  * @param refresh if true, will issue a read command for the value
790  * @param cw_limit upon return contains the clockwise angle limit
791  * @param ccw_limit upon return contains the counter-clockwise angle limit
792  */
793 void
795  unsigned int &cw_limit,
796  unsigned int &ccw_limit,
797  bool refresh)
798 {
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);
801 }
802 
803 /** Get temperature limit.
804  * @param id servo ID, not the broadcast ID
805  * @param refresh if true, will issue a read command for the value
806  * @return temperature limit.
807  */
808 unsigned char
809 RobotisRX28::get_temperature_limit(unsigned char id, bool refresh)
810 {
811  return get_value(id, refresh, P_LIMIT_TEMPERATURE);
812 }
813 
814 /** Get voltage limits.
815  * @param id servo ID, not the broadcast ID
816  * @param refresh if true, will issue a read command for the value
817  * @param low upon return contains low voltage limit
818  * @param high upon return contans high voltage limit
819  */
820 void
822  unsigned char &low,
823  unsigned char &high,
824  bool refresh)
825 {
826  low = get_value(id, refresh, P_DOWN_LIMIT_VOLTAGE);
827  high = get_value(id, refresh, P_UP_LIMIT_VOLTAGE);
828 }
829 
830 /** Get maximum torque.
831  * @param id servo ID, not the broadcast ID
832  * @param refresh if true, will issue a read command for the value
833  * @return maximum torque
834  */
835 unsigned int
836 RobotisRX28::get_max_torque(unsigned char id, bool refresh)
837 {
838  return get_value(id, refresh, P_MAX_TORQUE_L, P_MAX_TORQUE_H);
839 }
840 
841 /** Get status return level.
842  * @param id servo ID, not the broadcast ID
843  * @param refresh if true, will issue a read command for the value
844  * @return status return level
845  */
846 unsigned char
847 RobotisRX28::get_status_return_level(unsigned char id, bool refresh)
848 {
849  return get_value(id, refresh, P_RETURN_LEVEL);
850 }
851 
852 /** Get alarm LED status.
853  * @param id servo ID, not the broadcast ID
854  * @param refresh if true, will issue a read command for the value
855  * @return alarm LED status.
856  */
857 unsigned char
858 RobotisRX28::get_alarm_led(unsigned char id, bool refresh)
859 {
860  return get_value(id, refresh, P_ALARM_LED);
861 }
862 
863 /** Get shutdown on alarm state.
864  * @param id servo ID, not the broadcast ID
865  * @param refresh if true, will issue a read command for the value
866  * @return shutdown on alarm state
867  */
868 unsigned char
869 RobotisRX28::get_alarm_shutdown(unsigned char id, bool refresh)
870 {
871  return get_value(id, refresh, P_ALARM_SHUTDOWN);
872 }
873 
874 /** Get calibration data.
875  * @param id servo ID, not the broadcast ID
876  * @param refresh if true, will issue a read command for the value
877  * @param down_calib downward calibration
878  * @param up_calib upward calibration
879  */
880 void
882  unsigned int &down_calib,
883  unsigned int &up_calib,
884  bool refresh)
885 {
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);
888 }
889 
890 /** Check if torque is enabled
891  * @param id servo ID, not the broadcast ID
892  * @param refresh if true, will issue a read command for the value
893  * @return true if torque is enabled, false otherwise
894  */
895 bool
896 RobotisRX28::is_torque_enabled(unsigned char id, bool refresh)
897 {
898  return (get_value(id, refresh, P_TORQUE_ENABLE) == 1);
899 }
900 
901 /** Check if LED is enabled
902  * @param id servo ID, not the broadcast ID
903  * @param refresh if true, will issue a read command for the value
904  * @return true if led is enabled, false otherwise.
905  */
906 bool
907 RobotisRX28::is_led_enabled(unsigned char id, bool refresh)
908 {
909  return (get_value(id, refresh, P_LED) == 1);
910 }
911 
912 /** Get compliance values.
913  * @param id servo ID, not the broadcast ID
914  * @param refresh if true, will issue a read command for the value
915  * @param cw_margin upon return contains clockwise margin
916  * @param cw_slope upon return contains clockwise slope
917  * @param ccw_margin upon return contains counter-clockwise margin
918  * @param ccw_slope upon return contains counter-clockwise slope
919  */
920 void
922  unsigned char &cw_margin,
923  unsigned char &cw_slope,
924  unsigned char &ccw_margin,
925  unsigned char &ccw_slope,
926  bool refresh)
927 {
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);
932 }
933 
934 /** Get goal position.
935  * @param id servo ID, not the broadcast ID
936  * @param refresh if true, will issue a read command for the value
937  * @return goal position
938  */
939 unsigned int
940 RobotisRX28::get_goal_position(unsigned char id, bool refresh)
941 {
942  return get_value(id, refresh, P_GOAL_POSITION_L, P_GOAL_POSITION_H);
943 }
944 
945 /** Get goal speed.
946  * @param id servo ID, not the broadcast ID
947  * @param refresh if true, will issue a read command for the value
948  * @return goal speed
949  */
950 unsigned int
951 RobotisRX28::get_goal_speed(unsigned char id, bool refresh)
952 {
953  return get_value(id, refresh, P_GOAL_SPEED_L, P_GOAL_SPEED_H);
954 }
955 
956 /** Get maximum supported speed.
957  * @param id servo ID, not the broadcast ID
958  * @param refresh if true, will issue a read command for the value
959  * @return maximum supported speed in rad/s
960  */
961 float
962 RobotisRX28::get_max_supported_speed(unsigned char id, bool refresh)
963 {
964  float voltage = get_voltage(id, refresh) / 10.0;
965 
966  if ((voltage < 12.0) || (voltage > 16.0)) {
967  throw OutOfBoundsException("Voltage is outside of specs", voltage, 12.0, 16.0);
968  }
969 
970  float sec_per_deg_12V = SEC_PER_60DEG_12V / 60.0;
971  float sec_per_deg_16V = SEC_PER_60DEG_16V / 60.0;
972 
973  float range_sec_per_deg = sec_per_deg_12V - sec_per_deg_16V;
974  float pos = voltage - 12.0;
975 
976  float sec_per_deg = sec_per_deg_16V + pos * range_sec_per_deg;
977  float deg_per_sec = 1.0 / sec_per_deg;
978 
979  return deg2rad(deg_per_sec);
980 }
981 
982 /** Get torque limit.
983  * @param id servo ID, not the broadcast ID
984  * @param refresh if true, will issue a read command for the value
985  * @return torque limit
986  */
987 unsigned int
988 RobotisRX28::get_torque_limit(unsigned char id, bool refresh)
989 {
990  return get_value(id, refresh, P_TORQUE_LIMIT_L, P_TORQUE_LIMIT_H);
991 }
992 
993 /** Get current speed.
994  * @param id servo ID, not the broadcast ID
995  * @param refresh if true, will issue a read command for the value
996  * @return current speed
997  */
998 unsigned int
999 RobotisRX28::get_speed(unsigned char id, bool refresh)
1000 {
1001  return get_value(id, refresh, P_PRESENT_SPEED_L, P_PRESENT_SPEED_H);
1002 }
1003 
1004 /** Get current load.
1005  * @param id servo ID, not the broadcast ID
1006  * @param refresh if true, will issue a read command for the value
1007  * @return current load
1008  */
1009 unsigned int
1010 RobotisRX28::get_load(unsigned char id, bool refresh)
1011 {
1012  return get_value(id, refresh, P_PRESENT_LOAD_L, P_PRESENT_LOAD_H);
1013 }
1014 
1015 /** Get current voltage.
1016  * @param id servo ID, not the broadcast ID
1017  * @param refresh if true, will issue a read command for the value
1018  * @return voltage, divide by 10 to get V
1019  */
1020 unsigned char
1021 RobotisRX28::get_voltage(unsigned char id, bool refresh)
1022 {
1023  return get_value(id, refresh, P_PRESENT_VOLTAGE);
1024 }
1025 
1026 /** Get temperature.
1027  * @param id servo ID, not the broadcast ID
1028  * @param refresh if true, will issue a read command for the value
1029  * @return temperature in degrees Celsius
1030  */
1031 unsigned char
1032 RobotisRX28::get_temperature(unsigned char id, bool refresh)
1033 {
1034  return get_value(id, refresh, P_PRESENT_TEMPERATURE);
1035 }
1036 
1037 /** Check if servo is moving.
1038  * @param id servo ID, not the broadcast ID
1039  * @param refresh if true, will issue a read command for the value
1040  * @return true if servo is moving, false otherwise
1041  */
1042 bool
1043 RobotisRX28::is_moving(unsigned char id, bool refresh)
1044 {
1045  return (get_value(id, refresh, P_MOVING) == 1);
1046 }
1047 
1048 /** Check is servo is locked.
1049  * @param id servo ID, not the broadcast ID
1050  * @param refresh if true, will issue a read command for the value
1051  * @return true if servo config is locked, false otherwise
1052  */
1053 bool
1054 RobotisRX28::is_locked(unsigned char id, bool refresh)
1055 {
1056  return (get_value(id, refresh, P_LOCK) == 1);
1057 }
1058 
1059 /** Get punch.
1060  * @param id servo ID, not the broadcast ID
1061  * @param refresh if true, will issue a read command for the value
1062  * @return punch
1063  */
1064 unsigned int
1065 RobotisRX28::get_punch(unsigned char id, bool refresh)
1066 {
1067  return get_value(id, refresh, P_PUNCH_L, P_PUNCH_H);
1068 }
1069 
1070 /** Set ID.
1071  * @param id servo ID
1072  * @param new_id new ID to set
1073  */
1074 void
1075 RobotisRX28::set_id(unsigned char id, unsigned char new_id)
1076 {
1077  write_table_value(id, P_ID, new_id);
1078 }
1079 
1080 /** Set baud rate.
1081  * @param id servo ID
1082  * @param baudrate new baudrate
1083  */
1084 void
1085 RobotisRX28::set_baudrate(unsigned char id, unsigned char baudrate)
1086 {
1087  write_table_value(id, P_BAUD_RATE, baudrate);
1088 }
1089 
1090 /** Set return delay time.
1091  * @param id servo ID
1092  * @param return_delay_time new return delay time
1093  */
1094 void
1095 RobotisRX28::set_return_delay_time(unsigned char id, unsigned char return_delay_time)
1096 {
1097  write_table_value(id, P_RETURN_DELAY_TIME, return_delay_time);
1098 }
1099 
1100 /** Set angle limits.
1101  * @param id servo ID
1102  * @param cw_limit new clockwise limit
1103  * @param ccw_limit new counter-clockwise limit
1104  */
1105 void
1106 RobotisRX28::set_angle_limits(unsigned char id, unsigned int cw_limit, unsigned int ccw_limit)
1107 {
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);
1110 }
1111 
1112 /** Set temperature limit.
1113  * @param id servo ID
1114  * @param temp_limit new temperature limit (in degrees Celsius)
1115  */
1116 void
1117 RobotisRX28::set_temperature_limit(unsigned char id, unsigned char temp_limit)
1118 {
1119  write_table_value(id, P_LIMIT_TEMPERATURE, temp_limit);
1120 }
1121 
1122 /** Set voltage limits.
1123  * @param id servo ID
1124  * @param low lower bound (give Volts * 10)
1125  * @param high higher bound (give Volts * 10)
1126  */
1127 void
1128 RobotisRX28::set_voltage_limits(unsigned char id, unsigned char low, unsigned char high)
1129 {
1130  unsigned char param[2];
1131  param[0] = low;
1132  param[1] = high;
1133  write_table_values(id, P_DOWN_LIMIT_VOLTAGE, param, 2);
1134 }
1135 
1136 /** Set maximum torque.
1137  * @param id servo ID
1138  * @param max_torque new maximum torque
1139  */
1140 void
1141 RobotisRX28::set_max_torque(unsigned char id, unsigned int max_torque)
1142 {
1143  write_table_value(id, P_MAX_TORQUE_L, max_torque, true);
1144 }
1145 
1146 /** Set status return level
1147  * @param id servo ID
1148  * @param status_return_level status return level, one of SRL_RESPOND_NONE,
1149  * SRL_RESPOND_READ or SRL_RESPOND_ALL.
1150  */
1151 void
1152 RobotisRX28::set_status_return_level(unsigned char id, unsigned char status_return_level)
1153 {
1154  write_table_value(id, P_RETURN_LEVEL, status_return_level);
1155 }
1156 
1157 /** Set alarm LED settings.
1158  * @param id servo ID
1159  * @param alarm_led new LED alarm value.
1160  */
1161 void
1162 RobotisRX28::set_alarm_led(unsigned char id, unsigned char alarm_led)
1163 {
1164  write_table_value(id, P_ALARM_LED, alarm_led);
1165 }
1166 
1167 /** Set shutdown on alarm.
1168  * @param id servo ID
1169  * @param alarm_shutdown alarm shutdown settings
1170  */
1171 void
1172 RobotisRX28::set_alarm_shutdown(unsigned char id, unsigned char alarm_shutdown)
1173 {
1174  write_table_value(id, P_ALARM_SHUTDOWN, alarm_shutdown);
1175 }
1176 
1177 /** Enable or disable torque.
1178  * @param id servo ID
1179  * @param enabled true to enable (servo is powered) false to disable
1180  * (servo power disabled, servo can be freely moved manually)
1181  */
1182 void
1183 RobotisRX28::set_torque_enabled(unsigned char id, bool enabled)
1184 {
1185  write_table_value(id, P_TORQUE_ENABLE, enabled ? 1 : 0);
1186 }
1187 
1188 /** Enable or disable torque for multiple (selected) servos at once.
1189  * Given the number of servos the same number of variadic arguments must be
1190  * passed, one for each servo ID that should be enabled/disabled.
1191  * @param enabled true to enable (servo is powered) false to disable
1192  * (servo power disabled, servo can be freely moved manually)
1193  * @param num_servos number of servos to set, maximum is 120
1194  */
1195 void
1196 RobotisRX28::set_torques_enabled(bool enabled, unsigned int num_servos, ...)
1197 {
1198  if (num_servos > 120) {
1199  // not enough space for everything in the parameters..
1200  throw Exception("You cannot set more than 120 servos at once");
1201  }
1202 
1203  va_list arg;
1204  va_start(arg, num_servos);
1205 
1206  unsigned int plength = 2 * num_servos + 2;
1207  unsigned char param[plength];
1208  param[0] = P_TORQUE_ENABLE;
1209  param[1] = 1;
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;
1214  }
1215  va_end(arg);
1216 
1217  send(BROADCAST_ID, INST_SYNC_WRITE, param, plength);
1218 }
1219 
1220 /** Turn LED on or off.
1221  * @param id servo ID
1222  * @param led_enabled true to turn LED on, false to turn off
1223  */
1224 void
1225 RobotisRX28::set_led_enabled(unsigned char id, bool led_enabled)
1226 {
1227  write_table_value(id, P_LED, led_enabled ? 1 : 0);
1228 }
1229 
1230 /** Set compliance values.
1231  * @param id servo ID
1232  * @param cw_margin clockwise margin
1233  * @param cw_slope clockwise slope
1234  * @param ccw_margin counter-clockwise margin
1235  * @param ccw_slope counter-clockwise slope
1236  */
1237 void
1239  unsigned char cw_margin,
1240  unsigned char cw_slope,
1241  unsigned char ccw_margin,
1242  unsigned char ccw_slope)
1243 {
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);
1250 }
1251 
1252 /** Set goal speed.
1253  * @param id servo ID
1254  * @param goal_speed desired goal speed, 1024 is maximum, 0 means "no velicity
1255  * control", i.e. move as fast as possible depending on the voltage
1256  */
1257 void
1258 RobotisRX28::set_goal_speed(unsigned char id, unsigned int goal_speed)
1259 {
1260  write_table_value(id, P_GOAL_SPEED_L, goal_speed, true);
1261 }
1262 
1263 /** Set goal speeds for multiple servos.
1264  * Given the number of servos the variadic arguments must contain two values
1265  * for each servo, first is the ID, second the value.
1266  * @param num_servos number of servos, maximum is 83
1267  */
1268 void
1269 RobotisRX28::set_goal_speeds(unsigned int num_servos, ...)
1270 {
1271  if (num_servos > 83) {
1272  // not enough space for everything in the parameters..
1273  throw Exception("You cannot set more than 83 speeds at once");
1274  }
1275 
1276  va_list arg;
1277  va_start(arg, num_servos);
1278 
1279  unsigned int plength = 3 * num_servos + 2;
1280  unsigned char param[plength];
1281  param[0] = P_GOAL_SPEED_L;
1282  param[1] = 2;
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);
1286  //printf("Servo speed %u to %u\n", id, value);
1287  param[2 + i * 3] = id;
1288  param[2 + i * 3 + 1] = (value & 0xFF);
1289  param[2 + i * 3 + 2] = (value >> 8) & 0xFF;
1290  }
1291  va_end(arg);
1292 
1293  send(BROADCAST_ID, INST_SYNC_WRITE, param, plength);
1294 }
1295 
1296 /** Set torque limit.
1297  * @param id servo ID
1298  * @param torque_limit new torque limit
1299  */
1300 void
1301 RobotisRX28::set_torque_limit(unsigned char id, unsigned int torque_limit)
1302 {
1303  write_table_value(id, P_TORQUE_LIMIT_L, torque_limit, true);
1304 }
1305 
1306 /** Set punch.
1307  * @param id servo ID
1308  * @param punch new punch value
1309  */
1310 void
1311 RobotisRX28::set_punch(unsigned char id, unsigned int punch)
1312 {
1313  write_table_value(id, P_PUNCH_L, punch, true);
1314 }
1315 
1316 /** Lock config.
1317  * Locks the config, configuration values can no longer be modified until the
1318  * next power cycle.
1319  * @param id servo ID
1320  */
1321 void
1322 RobotisRX28::lock_config(unsigned char id)
1323 {
1324  write_table_value(id, P_LOCK, 1);
1325 }
1326 
1327 /** Move servo to specified position.
1328  * @param id servo ID
1329  * @param value position, value between 0 and 1023 (inclusive), covering
1330  * an angle range from 0 to 300 degrees.
1331  */
1332 void
1333 RobotisRX28::goto_position(unsigned char id, unsigned int value)
1334 {
1335  write_table_value(id, P_GOAL_POSITION_L, value, true);
1336 }
1337 
1338 /** Move several servos to specified positions.
1339  * Given the number of servos the variadic arguments must contain two values
1340  * for each servo, first is the ID, second the position (see goto_position() for
1341  * information on the valid values).
1342  * @param num_servos number of servos, maximum is 83
1343  */
1344 void
1345 RobotisRX28::goto_positions(unsigned int num_servos, ...)
1346 {
1347  if (num_servos > 83) {
1348  // not enough space for everything in the parameters..
1349  throw Exception("You cannot set more than 83 servos at once");
1350  }
1351 
1352  va_list arg;
1353  va_start(arg, num_servos);
1354 
1355  unsigned int plength = 3 * num_servos + 2;
1356  unsigned char param[plength];
1357  param[0] = P_GOAL_POSITION_L;
1358  param[1] = 2;
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;
1365  }
1366  va_end(arg);
1367 
1368  send(BROADCAST_ID, INST_SYNC_WRITE, param, plength);
1369 }
static const unsigned char P_PUNCH_L
P_PUNCH_L.
Definition: rx28.h:208
static const unsigned char P_CCW_COMPLIANCE_SLOPE
P_CCW_COMPLIANCE_SLOPE.
Definition: rx28.h:189
void read_table_value(unsigned char id, unsigned char addr, unsigned char read_length)
Read a table value.
Definition: rx28.cpp:577
std::list< unsigned char > DeviceList
List of servo IDs.
Definition: rx28.h:47
File could not be opened.
Definition: system.h:52
static const float MAX_ANGLE_RAD
MAX_ANGLE_RAD.
Definition: rx28.h:151
void set_torque_enabled(unsigned char id, bool enabled)
Enable or disable torque.
Definition: rx28.cpp:1183
void start_read_table_values(unsigned char id)
Start to receive table values.
Definition: rx28.cpp:537
static const unsigned char P_UP_LIMIT_VOLTAGE
P_UP_LIMIT_VOLTAGE.
Definition: rx28.h:172
static const unsigned char P_VERSION
P_VERSION.
Definition: rx28.h:161
unsigned int get_goal_speed(unsigned char id, bool refresh=false)
Get goal speed.
Definition: rx28.cpp:951
void write_table_values(unsigned char id, unsigned char start_addr, unsigned char *values, unsigned int num_values)
Write multiple table values.
Definition: rx28.cpp:647
void close()
Close port.
Definition: rx28.cpp:254
static const float SEC_PER_60DEG_12V
SEC_PER_60DEG_12V.
Definition: rx28.h:154
void set_goal_speed(unsigned char id, unsigned int goal_speed)
Set goal speed.
Definition: rx28.cpp:1258
static const unsigned char P_PRESENT_POSITION_H
P_PRESENT_POSITION_H.
Definition: rx28.h:197
static const unsigned char P_UP_CALIBRATION_L
P_UP_CALIBRATION_L.
Definition: rx28.h:181
unsigned char get_delay_time(unsigned char id, bool refresh=false)
Get time of the delay before replies are sent.
Definition: rx28.cpp:782
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.
Definition: rx28.cpp:921
unsigned char get_status_return_level(unsigned char id, bool refresh=false)
Get status return level.
Definition: rx28.cpp:847
static const unsigned char P_DOWN_LIMIT_VOLTAGE
P_DOWN_LIMIT_VOLTAGE.
Definition: rx28.h:171
static const unsigned char P_SYSTEM_DATA2
P_SYSTEM_DATA2.
Definition: rx28.h:169
void set_punch(unsigned char id, unsigned int punch)
Set punch.
Definition: rx28.cpp:1311
void set_alarm_led(unsigned char id, unsigned char alarm_led)
Set alarm LED settings.
Definition: rx28.cpp:1162
static const unsigned char P_PAUSE_TIME
P_PAUSE_TIME.
Definition: rx28.h:205
static const unsigned char P_LIMIT_TEMPERATURE
P_LIMIT_TEMPERATURE.
Definition: rx28.h:170
Fawkes library namespace.
float get_max_supported_speed(unsigned char id, bool refresh=false)
Get maximum supported speed.
Definition: rx28.cpp:962
void set_led_enabled(unsigned char id, bool enabled)
Turn LED on or off.
Definition: rx28.cpp:1225
static const unsigned char P_PRESENT_TEMPERATURE
P_PRESENT_TEMPERATURE.
Definition: rx28.h:203
unsigned char get_temperature_limit(unsigned char id, bool refresh=false)
Get temperature limit.
Definition: rx28.cpp:809
static const unsigned char P_PRESENT_SPEED_L
P_PRESENT_SPEED_L.
Definition: rx28.h:198
unsigned int get_goal_position(unsigned char id, bool refresh=false)
Get goal position.
Definition: rx28.cpp:940
bool is_moving(unsigned char id, bool refresh=false)
Check if servo is moving.
Definition: rx28.cpp:1043
static const unsigned char P_PRESENT_LOAD_L
P_PRESENT_LOAD_L.
Definition: rx28.h:200
~RobotisRX28()
Destructor.
Definition: rx28.cpp:175
unsigned int get_punch(unsigned char id, bool refresh=false)
Get punch.
Definition: rx28.cpp:1065
The current system call has timed out before completion.
Definition: system.h:45
void finish_read_table_values()
Finish control table receive operations.
Definition: rx28.cpp:554
void set_id(unsigned char id, unsigned char new_id)
Set ID.
Definition: rx28.cpp:1075
void set_torque_limit(unsigned char id, unsigned int torque_limit)
Set torque limit.
Definition: rx28.cpp:1301
bool is_led_enabled(unsigned char id, bool refresh=false)
Check if LED is enabled.
Definition: rx28.cpp:907
void set_max_torque(unsigned char id, unsigned int max_torque)
Set maximum torque.
Definition: rx28.cpp:1141
static const unsigned char P_REGISTERED_INSTRUCTION
P_REGISTERED_INSTRUCTION.
Definition: rx28.h:204
static const unsigned char SRL_RESPOND_NONE
SRL_RESPOND_NONE.
Definition: rx28.h:143
static const unsigned char P_PRESENT_SPEED_H
P_PRESENT_SPEED_H.
Definition: rx28.h:199
static const unsigned char P_MAX_TORQUE_H
P_MAX_TORQUE_H.
Definition: rx28.h:174
unsigned int get_load(unsigned char id, bool refresh=false)
Get current load.
Definition: rx28.cpp:1010
static const unsigned char P_GOAL_SPEED_L
P_GOAL_SPEED_L.
Definition: rx28.h:192
static const unsigned char P_CCW_COMPLIANCE_MARGIN
P_CCW_COMPLIANCE_MARGIN.
Definition: rx28.h:187
void write_table_value(unsigned char id, unsigned char addr, unsigned int value, bool double_byte=false)
Write a table value.
Definition: rx28.cpp:607
static const unsigned char P_TORQUE_ENABLE
P_TORQUE_ENABLE.
Definition: rx28.h:184
static const unsigned char P_MOVING
P_MOVING.
Definition: rx28.h:206
unsigned char get_alarm_shutdown(unsigned char id, bool refresh=false)
Get shutdown on alarm state.
Definition: rx28.cpp:869
unsigned int get_model(unsigned char id, bool refresh=false)
Get model.
Definition: rx28.cpp:738
unsigned char get_voltage(unsigned char id, bool refresh=false)
Get current voltage.
Definition: rx28.cpp:1021
bool is_torque_enabled(unsigned char id, bool refresh=false)
Check if torque is enabled.
Definition: rx28.cpp:896
void set_goal_speeds(unsigned int num_servos,...)
Set goal speeds for multiple servos.
Definition: rx28.cpp:1269
static const float SEC_PER_60DEG_16V
SEC_PER_60DEG_16V.
Definition: rx28.h:155
void get_angle_limits(unsigned char id, unsigned int &cw_limit, unsigned int &ccw_limit, bool refresh=false)
Get angle limits.
Definition: rx28.cpp:794
unsigned char get_baudrate(unsigned char id, bool refresh=false)
Get baud rate.
Definition: rx28.cpp:771
static const unsigned char P_GOAL_SPEED_H
P_GOAL_SPEED_H.
Definition: rx28.h:193
static const unsigned char P_RETURN_DELAY_TIME
P_RETURN_DELAY_TIME.
Definition: rx28.h:164
DeviceList discover(unsigned int total_timeout_ms=50)
Discover devices on the bus.
Definition: rx28.cpp:466
static const unsigned char P_DOWN_CALIBRATION_H
P_DOWN_CALIBRATION_H.
Definition: rx28.h:180
Base class for exceptions in Fawkes.
Definition: exception.h:35
static const unsigned char SRL_RESPOND_ALL
SRL_RESPOND_ALL.
Definition: rx28.h:145
static const unsigned char P_LED
P_LED.
Definition: rx28.h:185
static const float POS_TICKS_PER_RAD
POS_TICKS_PER_RAD.
Definition: rx28.h:153
static const float MAX_ANGLE_DEG
MAX_ANGLE_DEG.
Definition: rx28.h:150
static const unsigned char P_GOAL_POSITION_L
P_GOAL_POSITION_L.
Definition: rx28.h:190
unsigned int get_speed(unsigned char id, bool refresh=false)
Get current speed.
Definition: rx28.cpp:999
unsigned char get_temperature(unsigned char id, bool refresh=false)
Get temperature.
Definition: rx28.cpp:1032
static const unsigned int MAX_SPEED
MAX_SPEED.
Definition: rx28.h:156
static const unsigned char P_RETURN_LEVEL
P_RETURN_LEVEL.
Definition: rx28.h:175
unsigned int get_position(unsigned char id, bool refresh=false)
Get current position.
Definition: rx28.cpp:749
static const unsigned char P_MODEL_NUMBER_L
P_MODEL_NUMBER_L.
Definition: rx28.h:159
static const unsigned char P_CW_ANGLE_LIMIT_L
P_CW_ANGLE_LIMIT_L.
Definition: rx28.h:165
void set_baudrate(unsigned char id, unsigned char baudrate)
Set baud rate.
Definition: rx28.cpp:1085
static const unsigned char P_CW_COMPLIANCE_SLOPE
P_CW_COMPLIANCE_SLOPE.
Definition: rx28.h:188
bool data_available()
Check data availability.
Definition: rx28.cpp:440
Time tracking utility.
Definition: tracker.h:36
static const unsigned int MAX_POSITION
MAX_POSITION.
Definition: rx28.h:148
static const unsigned char P_GOAL_POSITION_H
P_GOAL_POSITION_H.
Definition: rx28.h:191
void open()
Open serial port.
Definition: rx28.cpp:182
void goto_position(unsigned char id, unsigned int value)
Move servo to specified position.
Definition: rx28.cpp:1333
void get_voltage_limits(unsigned char id, unsigned char &low, unsigned char &high, bool refresh=false)
Get voltage limits.
Definition: rx28.cpp:821
void set_voltage_limits(unsigned char id, unsigned char low, unsigned char high)
Set voltage limits.
Definition: rx28.cpp:1128
static const unsigned char P_BAUD_RATE
P_BAUD_RATE.
Definition: rx28.h:163
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.
Definition: rx28.cpp:1238
static const unsigned char P_ALARM_LED
P_ALARM_LED.
Definition: rx28.h:176
unsigned char get_firmware_version(unsigned char id, bool refresh=false)
Get firmware version.
Definition: rx28.cpp:760
static const unsigned char P_CCW_ANGLE_LIMIT_L
P_CCW_ANGLE_LIMIT_L.
Definition: rx28.h:167
static const unsigned char P_MODEL_NUMBER_H
P_MODEL_NUMBER_H.
Definition: rx28.h:160
void set_temperature_limit(unsigned char id, unsigned char temp_limit)
Set temperature limit.
Definition: rx28.cpp:1117
static const unsigned char P_PRESENT_LOAD_H
P_PRESENT_LOAD_H.
Definition: rx28.h:201
void print_trace()
Prints trace to stderr.
Definition: exception.cpp:601
static const unsigned char P_PRESENT_POSITION_L
P_PRESENT_POSITION_L.
Definition: rx28.h:196
static const unsigned char P_ALARM_SHUTDOWN
P_ALARM_SHUTDOWN.
Definition: rx28.h:177
static const unsigned char P_PUNCH_H
P_PUNCH_H.
Definition: rx28.h:209
void set_status_return_level(unsigned char id, unsigned char status_return_level)
Set status return level.
Definition: rx28.cpp:1152
static const unsigned char P_CW_COMPLIANCE_MARGIN
P_CW_COMPLIANCE_MARGIN.
Definition: rx28.h:186
RobotisRX28(const char *device_file, unsigned int default_timeout_ms=30)
Constructor.
Definition: rx28.cpp:152
static const unsigned int CENTER_POSITION
CENTER_POSITION.
Definition: rx28.h:149
static const unsigned char P_PRESENT_VOLTAGE
P_PRESENT_VOLTAGE.
Definition: rx28.h:202
static const unsigned char P_TORQUE_LIMIT_L
P_TORQUE_LIMIT_L.
Definition: rx28.h:194
void set_return_delay_time(unsigned char id, unsigned char return_delay_time)
Set return delay time.
Definition: rx28.cpp:1095
bool is_locked(unsigned char id, bool refresh=false)
Check is servo is locked.
Definition: rx28.cpp:1054
static const unsigned char P_CCW_ANGLE_LIMIT_H
P_CCW_ANGLE_LIMIT_H.
Definition: rx28.h:168
static const float RAD_PER_POS_TICK
RAD_PER_POS_TICK.
Definition: rx28.h:152
unsigned int get_torque_limit(unsigned char id, bool refresh=false)
Get torque limit.
Definition: rx28.cpp:988
static const unsigned char SRL_RESPOND_READ
SRL_RESPOND_READ.
Definition: rx28.h:144
static const unsigned char P_MAX_TORQUE_L
P_MAX_TORQUE_L.
Definition: rx28.h:173
static const unsigned char BROADCAST_ID
BROADCAST_ID.
Definition: rx28.h:147
static const unsigned char P_UP_CALIBRATION_H
P_UP_CALIBRATION_H.
Definition: rx28.h:182
static const unsigned char P_CW_ANGLE_LIMIT_H
P_CW_ANGLE_LIMIT_H.
Definition: rx28.h:166
void lock_config(unsigned char id)
Lock config.
Definition: rx28.cpp:1322
void set_angle_limits(unsigned char id, unsigned int cw_limit, unsigned int ccw_limit)
Set angle limits.
Definition: rx28.cpp:1106
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:36
Index out of bounds.
Definition: software.h:85
static const unsigned char P_TORQUE_LIMIT_H
P_TORQUE_LIMIT_H.
Definition: rx28.h:195
void set_alarm_shutdown(unsigned char id, unsigned char alarm_shutdown)
Set shutdown on alarm.
Definition: rx28.cpp:1172
unsigned int get_max_torque(unsigned char id, bool refresh=false)
Get maximum torque.
Definition: rx28.cpp:836
static const unsigned char P_ID
P_ID.
Definition: rx28.h:162
void goto_positions(unsigned int num_positions,...)
Move several servos to specified positions.
Definition: rx28.cpp:1345
static const unsigned char P_OPERATING_MODE
P_OPERATING_MODE.
Definition: rx28.h:178
static const unsigned char P_LOCK
P_LOCK.
Definition: rx28.h:207
void set_torques_enabled(bool enabled, unsigned int num_servos,...)
Enable or disable torque for multiple (selected) servos at once.
Definition: rx28.cpp:1196
void append(const char *format,...)
Append messages to the message list.
Definition: exception.cpp:333
unsigned char get_alarm_led(unsigned char id, bool refresh=false)
Get alarm LED status.
Definition: rx28.cpp:858
static const unsigned char P_DOWN_CALIBRATION_L
P_DOWN_CALIBRATION_L.
Definition: rx28.h:179
void read_table_values(unsigned char id)
Read all table values for given servo.
Definition: rx28.cpp:523
void get_calibration(unsigned char id, unsigned int &down_calib, unsigned int &up_calib, bool refresh=false)
Get calibration data.
Definition: rx28.cpp:881
bool ping(unsigned char id, unsigned int timeout_ms=100)
Ping servo.
Definition: rx28.cpp:504