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