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