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