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