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