Fawkes API  Fawkes Development Version
rx28.cpp
00001 
00002 /***************************************************************************
00003  *  rx28.cpp - Controller for Visca cams
00004  *
00005  *  Created: Tue Jun 16 11:09:32 2009 (based on visca.cpp)
00006  *  Copyright  2005-2009  Tim Niemueller [www.niemueller.de]
00007  *
00008  ****************************************************************************/
00009 
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version. A runtime exception applies to
00014  *  this software (see LICENSE.GPL_WRE file mentioned below for details).
00015  *
00016  *  This program is distributed in the hope that it will be useful,
00017  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00018  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019  *  GNU Library General Public License for more details.
00020  *
00021  *  Read the full text in the LICENSE.GPL_WRE file in the doc directory.
00022  */
00023 
00024 #include "rx28.h"
00025 
00026 #include <core/exceptions/system.h>
00027 #include <core/exceptions/software.h>
00028 
00029 #include <utils/math/angle.h>
00030 
00031 #include <sys/ioctl.h>
00032 #include <stdio.h>
00033 #include <sys/time.h>
00034 #include <termios.h>
00035 #include <fcntl.h>
00036 #include <errno.h>
00037 #include <unistd.h>
00038 #include <cstring>
00039 #include <cstdlib>
00040 #include <cstdarg>
00041 
00042 const unsigned char RobotisRX28::BROADCAST_ID             = 0xfe; /**< BROADCAST_ID */
00043 const unsigned int  RobotisRX28::MAX_POSITION             = 0x3ff; /**< MAX_POSITION */
00044 const unsigned int  RobotisRX28::CENTER_POSITION          = 0x1ff; /**< CENTER_POSITION */
00045 const unsigned int  RobotisRX28::MAX_SPEED                = 0x3ff; /**< MAX_SPEED */
00046 const float         RobotisRX28::MAX_ANGLE_DEG            = 300; /**< MAX_ANGLE_DEG */
00047 const float         RobotisRX28::MAX_ANGLE_RAD            = fawkes::deg2rad(RobotisRX28::MAX_ANGLE_DEG); /**< MAX_ANGLE_RAD */
00048 const float         RobotisRX28::RAD_PER_POS_TICK         = RobotisRX28::MAX_ANGLE_RAD / (float)(RobotisRX28::MAX_POSITION); /**< RAD_PER_POS_TICK */
00049 const float         RobotisRX28::POS_TICKS_PER_RAD        = (float)(RobotisRX28::MAX_POSITION) / RobotisRX28::MAX_ANGLE_RAD; /**< POS_TICKS_PER_RAD */
00050 const float         RobotisRX28::SEC_PER_60DEG_12V        = 0.167; /**< SEC_PER_60DEG_12V */
00051 const float         RobotisRX28::SEC_PER_60DEG_16V        = 0.126; /**< SEC_PER_60DEG_16V */
00052 
00053 const unsigned char RobotisRX28::SRL_RESPOND_NONE         =  0; /**< SRL_RESPOND_NONE */
00054 const unsigned char RobotisRX28::SRL_RESPOND_READ         =  1; /**< SRL_RESPOND_READ */
00055 const unsigned char RobotisRX28::SRL_RESPOND_ALL          =  2; /**< SRL_RESPOND_ALL */
00056 
00057 const unsigned char RobotisRX28::P_MODEL_NUMBER_L         =  0; /**< P_MODEL_NUMBER_L */
00058 const unsigned char RobotisRX28::P_MODEL_NUMBER_H         =  1; /**< P_MODEL_NUMBER_H */
00059 const unsigned char RobotisRX28::P_VERSION                =  2; /**< P_VERSION */
00060 const unsigned char RobotisRX28::P_ID                     =  3; /**< P_ID */
00061 const unsigned char RobotisRX28::P_BAUD_RATE              =  4; /**< P_BAUD_RATE */
00062 const unsigned char RobotisRX28::P_RETURN_DELAY_TIME      =  5; /**< P_RETURN_DELAY_TIME */
00063 const unsigned char RobotisRX28::P_CW_ANGLE_LIMIT_L       =  6; /**< P_CW_ANGLE_LIMIT_L */
00064 const unsigned char RobotisRX28::P_CW_ANGLE_LIMIT_H       =  7; /**< P_CW_ANGLE_LIMIT_H */
00065 const unsigned char RobotisRX28::P_CCW_ANGLE_LIMIT_L      =  8; /**< P_CCW_ANGLE_LIMIT_L */
00066 const unsigned char RobotisRX28::P_CCW_ANGLE_LIMIT_H      =  9; /**< P_CCW_ANGLE_LIMIT_H */
00067 const unsigned char RobotisRX28::P_SYSTEM_DATA2           = 10; /**< P_SYSTEM_DATA2 */
00068 const unsigned char RobotisRX28::P_LIMIT_TEMPERATURE      = 11; /**< P_LIMIT_TEMPERATURE */
00069 const unsigned char RobotisRX28::P_DOWN_LIMIT_VOLTAGE     = 12; /**< P_DOWN_LIMIT_VOLTAGE */
00070 const unsigned char RobotisRX28::P_UP_LIMIT_VOLTAGE       = 13; /**< P_UP_LIMIT_VOLTAGE */
00071 const unsigned char RobotisRX28::P_MAX_TORQUE_L           = 14; /**< P_MAX_TORQUE_L */
00072 const unsigned char RobotisRX28::P_MAX_TORQUE_H           = 15; /**< P_MAX_TORQUE_H */
00073 const unsigned char RobotisRX28::P_RETURN_LEVEL           = 16; /**< P_RETURN_LEVEL */
00074 const unsigned char RobotisRX28::P_ALARM_LED              = 17; /**< P_ALARM_LED */
00075 const unsigned char RobotisRX28::P_ALARM_SHUTDOWN         = 18; /**< P_ALARM_SHUTDOWN */
00076 const unsigned char RobotisRX28::P_OPERATING_MODE         = 19; /**< P_OPERATING_MODE */
00077 const unsigned char RobotisRX28::P_DOWN_CALIBRATION_L     = 20; /**< P_DOWN_CALIBRATION_L */
00078 const unsigned char RobotisRX28::P_DOWN_CALIBRATION_H     = 21; /**< P_DOWN_CALIBRATION_H */
00079 const unsigned char RobotisRX28::P_UP_CALIBRATION_L       = 22; /**< P_UP_CALIBRATION_L */
00080 const unsigned char RobotisRX28::P_UP_CALIBRATION_H       = 23; /**< P_UP_CALIBRATION_H */
00081 
00082 const unsigned char RobotisRX28::P_TORQUE_ENABLE          = 24; /**< P_TORQUE_ENABLE */
00083 const unsigned char RobotisRX28::P_LED                    = 25; /**< P_LED */
00084 const unsigned char RobotisRX28::P_CW_COMPLIANCE_MARGIN   = 26; /**< P_CW_COMPLIANCE_MARGIN */
00085 const unsigned char RobotisRX28::P_CCW_COMPLIANCE_MARGIN  = 27; /**< P_CCW_COMPLIANCE_MARGIN */
00086 const unsigned char RobotisRX28::P_CW_COMPLIANCE_SLOPE    = 28; /**< P_CW_COMPLIANCE_SLOPE */
00087 const unsigned char RobotisRX28::P_CCW_COMPLIANCE_SLOPE   = 29; /**< P_CCW_COMPLIANCE_SLOPE */
00088 const unsigned char RobotisRX28::P_GOAL_POSITION_L        = 30; /**< P_GOAL_POSITION_L */
00089 const unsigned char RobotisRX28::P_GOAL_POSITION_H        = 31; /**< P_GOAL_POSITION_H */
00090 const unsigned char RobotisRX28::P_GOAL_SPEED_L           = 32; /**< P_GOAL_SPEED_L */
00091 const unsigned char RobotisRX28::P_GOAL_SPEED_H           = 33; /**< P_GOAL_SPEED_H */
00092 const unsigned char RobotisRX28::P_TORQUE_LIMIT_L         = 34; /**< P_TORQUE_LIMIT_L */
00093 const unsigned char RobotisRX28::P_TORQUE_LIMIT_H         = 35; /**< P_TORQUE_LIMIT_H */
00094 const unsigned char RobotisRX28::P_PRESENT_POSITION_L     = 36; /**< P_PRESENT_POSITION_L */
00095 const unsigned char RobotisRX28::P_PRESENT_POSITION_H     = 37; /**< P_PRESENT_POSITION_H */
00096 const unsigned char RobotisRX28::P_PRESENT_SPEED_L        = 38; /**< P_PRESENT_SPEED_L */
00097 const unsigned char RobotisRX28::P_PRESENT_SPEED_H        = 39; /**< P_PRESENT_SPEED_H */
00098 const unsigned char RobotisRX28::P_PRESENT_LOAD_L         = 40; /**< P_PRESENT_LOAD_L */
00099 const unsigned char RobotisRX28::P_PRESENT_LOAD_H         = 41; /**< P_PRESENT_LOAD_H */
00100 const unsigned char RobotisRX28::P_PRESENT_VOLTAGE        = 42; /**< P_PRESENT_VOLTAGE */
00101 const unsigned char RobotisRX28::P_PRESENT_TEMPERATURE    = 43; /**< P_PRESENT_TEMPERATURE */
00102 const unsigned char RobotisRX28::P_REGISTERED_INSTRUCTION = 44; /**< P_REGISTERED_INSTRUCTION */
00103 const unsigned char RobotisRX28::P_PAUSE_TIME             = 45; /**< P_PAUSE_TIME */
00104 const unsigned char RobotisRX28::P_MOVING                 = 46; /**< P_MOVING */
00105 const unsigned char RobotisRX28::P_LOCK                   = 47; /**< P_LOCK */
00106 const unsigned char RobotisRX28::P_PUNCH_L                = 48; /**< P_PUNCH_L */
00107 const unsigned char RobotisRX28::P_PUNCH_H                = 49; /**< P_PUNCH_H */
00108 
00109 //--- Instructions ---
00110 const unsigned char RobotisRX28::INST_PING                = 0x01; /**< INST_PING */
00111 const unsigned char RobotisRX28::INST_READ                = 0x02; /**< INST_READ */
00112 const unsigned char RobotisRX28::INST_WRITE               = 0x03; /**< INST_WRITE */
00113 const unsigned char RobotisRX28::INST_REG_WRITE           = 0x04; /**< INST_REG_WRITE */
00114 const unsigned char RobotisRX28::INST_ACTION              = 0x05; /**< INST_ACTION */
00115 const unsigned char RobotisRX28::INST_RESET               = 0x06; /**< INST_RESET */
00116 const unsigned char RobotisRX28::INST_DIGITAL_RESET       = 0x07; /**< INST_DIGITAL_RESET */
00117 const unsigned char RobotisRX28::INST_SYSTEM_READ         = 0x0C; /**< INST_SYSTEM_READ */
00118 const unsigned char RobotisRX28::INST_SYSTEM_WRITE        = 0x0D; /**< INST_SYSTEM_WRITE */
00119 const unsigned char RobotisRX28::INST_SYNC_WRITE          = 0x83; /**< INST_SYNC_WRITE */
00120 const unsigned char RobotisRX28::INST_SYNC_REG_WRITE      = 0x84; /**< INST_SYNC_REG_WRITE */
00121 
00122 const unsigned char RobotisRX28::PACKET_OFFSET_ID         = 2; /**< PACKET_OFFSET_ID */
00123 const unsigned char RobotisRX28::PACKET_OFFSET_LENGTH     = 3; /**< PACKET_OFFSET_LENGTH */
00124 const unsigned char RobotisRX28::PACKET_OFFSET_INST       = 4; /**< PACKET_OFFSET_INST */
00125 const unsigned char RobotisRX28::PACKET_OFFSET_PARAM      = 5; /**< PACKET_OFFSET_PARAM */
00126 const unsigned char RobotisRX28::PACKET_OFFSET_ERROR      = 4; /**< PACKET_OFFSET_ERROR */
00127 
00128 using namespace std;
00129 using namespace fawkes;
00130 
00131 
00132 /** @class RobotisRX28 "rx28.h"
00133  * Class to access a chain of Robotis RX28 servos.
00134  * One instance of this class communicates with a chain of up to 254 Robotis
00135  * RX28 servos, which are uniquely identified with an ID. Before making use of
00136  * the chain, connect each servo individually and set its ID. See the
00137  * discover() method for more information about numbering of the servos.
00138  * To achieve a higher speed, it is recommended to set the status return level
00139  * to reply only on READ instructions. You can do this for the whole chain with
00140  * @code
00141  * rx28->set_status_return_level(RobotisRX28::BROADCAST_ID, RobotisRX28::SRL_RESPOND_READ);
00142  * @endcode
00143  *
00144  * @author Tim Niemueller
00145  */
00146 
00147 
00148 /** Constructor.
00149  * @param device_file device file of the serial port
00150  * @param default_timeout_ms the timeout to apply by default to reading operations
00151  */
00152 RobotisRX28::RobotisRX28(const char *device_file, unsigned int default_timeout_ms)
00153 {
00154   __default_timeout_ms = default_timeout_ms;
00155   __device_file        = strdup(device_file);
00156   __fd                 = -1;
00157   __obuffer_length     = 0;
00158   __ibuffer_length     = 0;
00159   memset(__control_table, 0, RX28_MAX_NUM_SERVOS * RX28_CONTROL_TABLE_LENGTH);
00160   try {
00161     open();
00162   } catch (Exception &e) {
00163     free(__device_file);
00164     throw;
00165   }
00166 }
00167 
00168 
00169 /** Destructor. */
00170 RobotisRX28::~RobotisRX28()
00171 {
00172   free(__device_file);
00173 }
00174 
00175 /** Open serial port. */
00176 void
00177 RobotisRX28::open() {
00178 
00179   struct termios param;
00180 
00181   __fd = ::open(__device_file, O_NOCTTY | O_RDWR);
00182   if (__fd == -1) {
00183     throw CouldNotOpenFileException(__device_file, errno, "Cannot open device file");
00184   }
00185   tcflush(__fd, TCIOFLUSH);
00186 
00187   if (tcgetattr(__fd, &param) == -1) {
00188     Exception e(errno, "Getting the port parameters failed");
00189     ::close(__fd);
00190     __fd = -1;
00191     throw e;
00192   }
00193 
00194   cfsetospeed(&param, B57600);
00195   cfsetispeed(&param, B57600);
00196 
00197   param.c_lflag &= ~(ICANON | ECHO | ECHOE | ECHOK | ECHONL | ISIG | IEXTEN);
00198   param.c_iflag &= ~(INLCR | IGNCR | ICRNL | IGNBRK | PARMRK);
00199 
00200   // turn off hardware flow control
00201   param.c_iflag &= ~(IXON | IXOFF | IXANY);
00202   param.c_cflag &= ~CRTSCTS;
00203 
00204   param.c_cflag |= (CREAD | CLOCAL);
00205     
00206   // number of data bits: 8
00207   param.c_cflag &= ~CS5 & ~CS6 & ~CS7 & ~CS8;
00208   param.c_cflag |= CS8;
00209     
00210   // parity: none
00211   param.c_cflag &= ~(PARENB | PARODD);
00212   param.c_iflag &= ~(INPCK | ISTRIP);
00213     
00214   // stop bits: 1
00215   param.c_cflag &= ~CSTOPB;
00216 
00217   //enable raw output
00218   param.c_oflag &= ~OPOST;
00219 
00220   param.c_cc[VMIN]  = 1;
00221   param.c_cc[VTIME] = 0;
00222     
00223   tcflush(__fd, TCIOFLUSH);
00224 
00225   if (tcsetattr(__fd, TCSANOW, &param) != 0) {
00226     Exception e(errno, "Setting the port parameters failed");
00227     ::close(__fd);
00228     __fd = -1;
00229     throw e;
00230   }
00231 
00232   //char junk[1];
00233   //read(__fd, junk, 1);
00234 
00235 #ifdef TIMETRACKER_VISCA
00236   tracker = new TimeTracker();
00237   track_file.open("tracker_visca.txt");
00238   ttcls_pantilt_get_send = tracker->addClass("getPanTilt: send");
00239   ttcls_pantilt_get_read = tracker->addClass("getPanTilt: read");
00240   ttcls_pantilt_get_handle = tracker->addClass("getPanTilt: handling responses");
00241   ttcls_pantilt_get_interpret = tracker->addClass("getPanTilt: interpreting");
00242 #endif
00243 
00244   // success
00245 }
00246 
00247 
00248 /** Close port. */
00249 void
00250 RobotisRX28::close()
00251 {
00252   if (__fd >= 0) {
00253     ::close(__fd);
00254     __fd = -1;
00255   }
00256 }
00257 
00258 
00259 /** Calculate the checksum for the given packet.
00260  * @param id servo ID
00261  * @param instruction instruction to send
00262  * @param params parameters in the message
00263  * @param plength length of the params array
00264  * @return checksum as defined in the RX28 manual
00265  */
00266 unsigned char
00267 RobotisRX28::calc_checksum(const unsigned char id, const unsigned char instruction,
00268                            const unsigned char *params, const unsigned char plength)
00269 {
00270   unsigned int checksum = id + instruction + plength+2;
00271   for (unsigned char i = 0; i < plength; ++i) {
00272     checksum += params[i];
00273   }
00274 
00275   return ~(checksum & 0xFF);
00276 }
00277 
00278 
00279 /** Send instruction packet.
00280  * @param id servo ID
00281  * @param instruction instruction to send
00282  * @param params parameters in the message
00283  * @param plength length of the params array
00284  */
00285 void
00286 RobotisRX28::send(const unsigned char id, const unsigned char instruction,
00287                   const unsigned char *params, const unsigned char plength)
00288 {
00289   // Byte 0 and 1 must be 0xFF
00290   __obuffer[0] = 0xFF;
00291   __obuffer[1] = 0xFF;
00292   __obuffer[PACKET_OFFSET_ID]     = id;
00293   __obuffer[PACKET_OFFSET_LENGTH] = plength+2;
00294   __obuffer[PACKET_OFFSET_INST]   = instruction;
00295 
00296   unsigned int checksum = id + plength+2;
00297 
00298   for (unsigned char i = 0; i < plength; ++i) {
00299     __obuffer[PACKET_OFFSET_PARAM + i] = params[i];
00300     checksum += params[i];
00301   }
00302 
00303   // actually +5, but zero-based array, therefore index 4, but fifth value
00304   __obuffer[3 + plength+2] = calc_checksum(id, instruction, params, plength);
00305   __obuffer_length = plength+2 + 4 ; // 4 for 0xFF 0xFF ID LENGTH
00306 
00307 #ifdef DEBUG_RX28_COMM
00308   printf("Sending: ");
00309   for (int i = 0; i < __obuffer_length; ++i) {
00310     printf("%X ", __obuffer[i]);
00311   }
00312   printf("\n");
00313 #endif
00314 
00315   int written = write(__fd, __obuffer, __obuffer_length);
00316   //printf("Wrote %d bytes\n", written);
00317 
00318   // For some reason we have to read the shit immediately, although ECHO is off
00319   int readd = 0;
00320   while (readd < __obuffer_length) {
00321     readd += read(__fd, __ibuffer + readd, __obuffer_length - readd);
00322   }
00323 #ifdef DEBUG_RX28_COMM
00324   printf("Read %d junk bytes: ", readd);
00325   for (int i = 0; i < readd; ++i) {
00326     printf("%X ", __ibuffer[i]);
00327   }
00328   printf("\n");
00329 #endif
00330 
00331   if ( written < 0 ) {
00332     throw Exception(errno, "Failed to write RX28 packet %x for %x", instruction, id);
00333   } else if (written < __obuffer_length) {
00334     throw Exception("Failed to write RX28 packet %x for %x, only %d of %d bytes sent",
00335                     instruction, id, written, __obuffer_length);
00336   }
00337 }
00338 
00339 
00340 /** Receive a packet.
00341  * @param timeout_ms maximum wait time in miliseconds
00342  * @param exp_length expected number of bytes
00343  */
00344 void
00345 RobotisRX28::recv(const unsigned char exp_length, unsigned int timeout_ms)
00346 {
00347   timeval timeout = {0, (timeout_ms == 0xFFFFFFFF ? __default_timeout_ms : timeout_ms)  * 1000};
00348 
00349   fd_set read_fds;
00350   FD_ZERO(&read_fds);
00351   FD_SET(__fd, &read_fds);
00352 
00353   int rv = 0;
00354   rv = select(__fd + 1, &read_fds, NULL, NULL, &timeout);
00355 
00356   if ( rv == -1 ) {
00357    throw Exception(errno, "Select on FD failed");
00358   } else if ( rv == 0 ) {
00359     //printf("Timeout, no data :-/\n");
00360     throw TimeoutException("Timeout reached while waiting for incoming RX28 data");
00361   }
00362 
00363   __ibuffer_length = 0;
00364 
00365   // get octets one by one
00366   int bytes_read = 0;
00367   while (bytes_read < 6) {
00368 #ifdef DEBUG_RX28_COMM
00369     printf("Trying to read %d bytes\n", 6 - bytes_read);
00370 #endif
00371     bytes_read += read(__fd, __ibuffer + bytes_read, 6 - bytes_read);
00372 #ifdef DEBUG_RX28_COMM
00373     printf("%d bytes read  ", bytes_read);
00374     for (int i = 0; i < bytes_read; ++i) {
00375       printf("%X ", __ibuffer[i]);
00376     }
00377     printf("\n");
00378 #endif
00379   }
00380   if (bytes_read < 6) {
00381     throw Exception("Failed to read packet header");
00382   }
00383   if ( (__ibuffer[0] != 0xFF) || (__ibuffer[1] != 0xFF) ) {
00384     throw Exception("Packet does not start with 0xFFFF.");
00385   }
00386   if (exp_length != __ibuffer[PACKET_OFFSET_LENGTH] - 2) {
00387     tcflush(__fd, TCIFLUSH);
00388     throw Exception("Wrong packet length, expected %u, got %u",
00389                     exp_length,  __ibuffer[PACKET_OFFSET_LENGTH] - 2);
00390   }
00391   const unsigned char plength = __ibuffer[PACKET_OFFSET_LENGTH] - 2;
00392 #ifdef DEBUG_RX28_COMM
00393   printf("header read, params have length %d\n", plength);
00394 #endif
00395   if (plength > 0) {
00396     bytes_read = 0;
00397     while (bytes_read < plength) {
00398       bytes_read += read(__fd, &__ibuffer[6] + bytes_read, plength - bytes_read);
00399     }
00400     if (bytes_read < plength) {
00401       throw Exception("Failed to read packet data");
00402     }
00403   }
00404 
00405   __ibuffer_length = plength+2 + 4;
00406 #ifdef DEBUG_RX28_COMM
00407   printf("Read: ");
00408   for (int i = 0; i < __ibuffer_length; ++i) {
00409     printf("%X ", __ibuffer[i]);
00410   }
00411   printf("\n");
00412 #endif
00413 
00414   // verify checksum
00415   unsigned char checksum = calc_checksum(__ibuffer[PACKET_OFFSET_ID],
00416                                          __ibuffer[PACKET_OFFSET_INST],
00417                                          &__ibuffer[PACKET_OFFSET_PARAM], plength);
00418   if (checksum != __ibuffer[plength + 5]) {
00419     throw Exception("Checksum error while receiving packet, expected %d, got %d",
00420                     checksum, __ibuffer[plength + 5]);
00421   }
00422 
00423   __ibuffer_length = plength+2 + 4;
00424 }
00425 
00426 
00427 /** Check data availability.
00428  * @return true if data is available, false otherwise
00429  */
00430 bool
00431 RobotisRX28::data_available()
00432 {
00433   int num_bytes = 0;
00434   ioctl(__fd, FIONREAD, &num_bytes);
00435   return (num_bytes > 0);
00436 }
00437 
00438 
00439 /** Discover devices on the bus.
00440  * This method will send a PING instruction to the broadcast ID and collect
00441  * responses. This assumes that the return delay time is set appropriately that
00442  * all responses can be received without collisions, and that the difference
00443  * between the time of two consecutive servos is smaller than the given timeout
00444  * (note that this might be void if you have one servo with ID 1 and one with
00445  * ID 253). After sending the packet this method will do up to
00446  * RX28_MAX_NUM_SERVOS receive operations, each with the given timeout. After the
00447  * first timeout the discovery is aborted assuming that all replies have been
00448  * received. You can set the timeout really high (several seconds) to be sure
00449  * that all connected servos are recognized.
00450  * For this to work best it is recommended to set consecutive servo IDs starting
00451  * from 1 on the servos.
00452  * After the servos are found, the control tables of all recognized servos are
00453  * received to ensure that all other methods return valid data.
00454  * @param timeout_ms maximum timeout to wait for replies.
00455  * @return list of detected servo IDs
00456  */
00457 RobotisRX28::DeviceList
00458 RobotisRX28::discover(unsigned int timeout_ms)
00459 {
00460   DeviceList rv;
00461 
00462   // simply re-throw exception upwards
00463   send(BROADCAST_ID, INST_PING, NULL, 0);
00464 
00465   for (unsigned int i = 0; i < RX28_MAX_NUM_SERVOS; ++i) {
00466     try {
00467       recv(0, timeout_ms);
00468       rv.push_back(__ibuffer[PACKET_OFFSET_ID]);
00469     } catch (TimeoutException &e) {
00470       // the first timeout, no more devices can be expected to respond
00471       break;
00472     }
00473   }
00474 
00475   // now get data about all servos
00476   for (DeviceList::iterator i = rv.begin(); i != rv.end(); ++i) {
00477     try {
00478       read_table_values(*i);
00479     } catch (Exception &e) {
00480       e.append("Failed to receive control table for servo %u", *i);
00481       throw;
00482     }
00483   }
00484  
00485   return rv;
00486 }
00487 
00488 
00489 /** Ping servo.
00490  * This pings the given servo by sending a PING instruction and
00491  * reading the reply.
00492  * @param id servo ID, not the broadcast ID
00493  * @param timeout_ms maximum wait time in miliseconds
00494  * @return true if the ping was successful, false otherwise
00495  */
00496 bool
00497 RobotisRX28::ping(unsigned char id, unsigned int timeout_ms)
00498 {
00499   assert_valid_id(id);
00500   try {
00501     send(id, INST_PING, NULL, 0);
00502     recv(0, timeout_ms);
00503     return true;
00504   } catch (Exception &e) {
00505     e.print_trace();
00506     return false;
00507   }
00508 }
00509 
00510 
00511 /** Read all table values for given servo.
00512  * This issues a READ comment for the whole control table and waits for the
00513  * response.
00514  * @param id servo ID
00515  */
00516 void
00517 RobotisRX28::read_table_values(unsigned char id)
00518 {
00519   start_read_table_values(id);
00520   finish_read_table_values();
00521 }
00522 
00523 
00524 /** Start to receive table values.
00525  * This method sends a READ instruction packet for the whole table, but it does
00526  * not wait for the reply. This can be used to overlap the receiving with other
00527  * operations. You have to ensure to call finish_read_table_values() before
00528  * sending any other data.
00529  * @param id servo ID, not the broadcast ID
00530  */
00531 void
00532 RobotisRX28::start_read_table_values(unsigned char id)
00533 {
00534   assert_valid_id(id);
00535   unsigned char param[2];
00536   param[0] = 0x00;
00537   param[1] = RX28_CONTROL_TABLE_LENGTH;
00538 
00539   send(id, INST_READ, param, 2);
00540 }
00541 
00542 /** Finish control table receive operations.
00543  * This executes the receive operation initiated by start_read_table_values().
00544  * This will read the values and write the output to the control table
00545  * (in memory, not in the servo), such that the appropriate get methods will
00546  * return the new data.
00547  */
00548 void
00549 RobotisRX28::finish_read_table_values()
00550 {
00551   recv(RX28_CONTROL_TABLE_LENGTH);
00552 
00553   if (__ibuffer_length != 5 + RX28_CONTROL_TABLE_LENGTH + 1) {
00554     throw Exception("Input buffer of invalid size: %u vs. %u", __ibuffer_length, 5 + RX28_CONTROL_TABLE_LENGTH + 1);
00555   }
00556   memcpy(__control_table[__ibuffer[PACKET_OFFSET_ID]],
00557          &__ibuffer[PACKET_OFFSET_PARAM], RX28_CONTROL_TABLE_LENGTH);
00558 }
00559 
00560 
00561 /** Read a table value.
00562  * This will read the given value(s) and write the output to the control table
00563  * (in memory, not in the servo), such that the appropriate get method will return
00564  * the new value.
00565  * @param id servo ID, not the broadcast ID
00566  * @param addr start addr, one of the P_* constants.
00567  * @param read_length number of bytes to read
00568  */
00569 void
00570 RobotisRX28::read_table_value(unsigned char id,
00571                               unsigned char addr, unsigned char read_length)
00572 {
00573   assert_valid_id(id);
00574 
00575   unsigned char param[2];
00576   param[0] = addr;
00577   param[1] = read_length;
00578 
00579   send(id, INST_READ, param, 2);
00580   recv(read_length);
00581 
00582   if (__ibuffer_length != (5 + read_length + 1)) {
00583     throw Exception("Input buffer not of expected size, expected %u, got %u",
00584                     (5 + read_length + 1), __ibuffer_length);
00585   }
00586 
00587   for (unsigned int i = 0; i < read_length; ++i) {
00588     __control_table[id][addr + i] = __ibuffer[PACKET_OFFSET_PARAM + i];
00589   }
00590 }
00591 
00592 
00593 /** Write a table value.
00594  * @param id servo ID, may be the broadcast ID
00595  * @param addr start addr, one of the P_* constants.
00596  * @param value value to write
00597  * @param double_byte if true, will assume value to be a two-byte value, otherwise
00598  * it is considered as a one-byte value.
00599  */
00600 void
00601 RobotisRX28::write_table_value(unsigned char id, unsigned char addr,
00602                                unsigned int value, bool double_byte)
00603 {
00604   unsigned char param[3];
00605   param[0] = addr;
00606   param[1] = value & 0xFF;
00607   param[2] = (value >> 8) & 0xFF;
00608 
00609   try {
00610     send(id, INST_WRITE, param, double_byte ? 3 : 2);
00611 
00612     if (id == BROADCAST_ID) {
00613       for (unsigned int i = 0; i < RX28_MAX_NUM_SERVOS; ++i) {
00614         __control_table[i][addr] = param[1];
00615         if (double_byte) __control_table[i][addr + 1] = param[2];
00616       }
00617     } else {
00618       __control_table[id][addr] = param[1];
00619       if (double_byte) __control_table[id][addr + 1] = param[2];
00620     }
00621 
00622     if ((id != BROADCAST_ID) && responds_all(id))  recv(0);
00623   } catch (Exception &e) {
00624     e.print_trace();
00625     throw;
00626   }
00627 }
00628 
00629 
00630 /** Write multiple table values.
00631  * @param id servo ID, may be the broadcast ID
00632  * @param start_addr start addr, one of the P_* constants.
00633  * @param values values to write
00634  * @param num_values length in bytes of the values array
00635  */
00636 void
00637 RobotisRX28::write_table_values(unsigned char id, unsigned char start_addr,
00638                                 unsigned char *values, unsigned int num_values)
00639 {
00640   unsigned char param[num_values + 1];
00641   param[0] = start_addr;
00642   for (unsigned int i = 0; i < num_values; ++i) {
00643     param[i + 1] = values[i];
00644   }
00645 
00646   try {
00647     send(id, INST_WRITE, param, num_values + 1);
00648 
00649     if (id == BROADCAST_ID) {
00650       for (unsigned int i = 0; i < RX28_MAX_NUM_SERVOS; ++i) {
00651         for (unsigned int j = 0; j < num_values; ++j) {
00652           __control_table[i][start_addr + j] = values[j];
00653         }
00654       }
00655     } else {
00656       for (unsigned int j = 0; j < num_values; ++j) {
00657         __control_table[id][start_addr + j] = values[j];
00658       }
00659     }
00660 
00661     if ((id != BROADCAST_ID) && responds_all(id))  recv(0);
00662   } catch (Exception &e) {
00663     e.print_trace();
00664     throw;
00665   }
00666 }
00667 
00668 
00669 /** Assert that the ID is valid.
00670  * @exception Exception thrown if \p id is the broadcast ID
00671  * @exception OutOfBoundsException thrown if the number is greater than the
00672  * maximum number of servos.
00673  */
00674 void
00675 RobotisRX28::assert_valid_id(unsigned char id)
00676 {
00677   if (id == BROADCAST_ID) {
00678     throw Exception("Data can only be queried for a specific servo");
00679   } else if (id >= RX28_MAX_NUM_SERVOS) {
00680     throw OutOfBoundsException("Servo ID out of bounds", id, 0, RX28_MAX_NUM_SERVOS);
00681   }
00682 }
00683 
00684 
00685 /** Merge two values to a two-byte value.
00686  * @param id servo id, not the broadcast ID
00687  * @param ind_l low index in control table
00688  * @param ind_h high index in control table
00689  */
00690 unsigned int
00691 RobotisRX28::merge_twobyte_value(unsigned int id,
00692                                  unsigned char ind_l, unsigned char ind_h)
00693 {
00694   unsigned int rv = (__control_table[id][ind_h] & 0xFF) << 8;
00695   rv |= __control_table[id][ind_l] & 0xFF;
00696   return rv;
00697 }
00698 
00699 
00700 /** Get a value from the control table, possibly from servo.
00701  * @param id servo ID, not the broadcast ID
00702  * @param refresh if true, will issue a read command for the value
00703  * @param ind_l low index in control table
00704  * @param ind_h high index in control table, only set if value is a
00705  * two-byte value.
00706  * @return value
00707  */
00708 unsigned int
00709 RobotisRX28::get_value(unsigned int id, bool refresh,
00710                        unsigned int ind_l, unsigned int ind_h)
00711 {
00712   assert_valid_id(id);
00713 
00714   if (refresh)  read_table_value(id, ind_l, (ind_h == 0xFFFFFFFF ? 1 : 2));
00715 
00716   if (ind_h == 0xFFFFFFFF) {
00717     return __control_table[id][ind_l];
00718   } else {
00719     return merge_twobyte_value(id, ind_l, ind_h);
00720   }
00721 }
00722 
00723 /** Get model.
00724  * @param id servo ID, not the broadcast ID
00725  * @param refresh if true, will issue a read command for the value
00726  * @return model
00727  */
00728 unsigned int
00729 RobotisRX28::get_model(unsigned char id, bool refresh)
00730 {
00731   return get_value(id, refresh, P_MODEL_NUMBER_L, P_MODEL_NUMBER_H);
00732 }
00733 
00734 
00735 /** Get current position.
00736  * @param id servo ID, not the broadcast ID
00737  * @param refresh if true, will issue a read command for the value
00738  * @return current position
00739  */
00740 unsigned int
00741 RobotisRX28::get_position(unsigned char id, bool refresh)
00742 {
00743   return get_value(id, refresh, P_PRESENT_POSITION_L, P_PRESENT_POSITION_H);
00744 }
00745 
00746 
00747 /** Get firmware version.
00748  * @param id servo ID, not the broadcast ID
00749  * @param refresh if true, will issue a read command for the value
00750  * @return firmware version
00751  */
00752 unsigned char
00753 RobotisRX28::get_firmware_version(unsigned char id, bool refresh)
00754 {
00755   return get_value(id, refresh, P_VERSION);
00756 }
00757 
00758 
00759 /** Get baud rate.
00760  * @param id servo ID, not the broadcast ID
00761  * @param refresh if true, will issue a read command for the value
00762  * @return baud rate
00763  */
00764 unsigned char
00765 RobotisRX28::get_baudrate(unsigned char id, bool refresh)
00766 {
00767   return get_value(id, refresh, P_BAUD_RATE);
00768 }
00769 
00770 
00771 /** Get time of the delay before replies are sent.
00772  * @param id servo ID, not the broadcast ID
00773  * @param refresh if true, will issue a read command for the value
00774  * @return delay time
00775  */
00776 unsigned char
00777 RobotisRX28::get_delay_time(unsigned char id, bool refresh)
00778 {
00779   return get_value(id, refresh, P_RETURN_DELAY_TIME);
00780 }
00781 
00782 
00783 /** Get angle limits.
00784  * @param id servo ID, not the broadcast ID
00785  * @param refresh if true, will issue a read command for the value
00786  * @param cw_limit upon return contains the clockwise angle limit
00787  * @param ccw_limit upon return contains the counter-clockwise angle limit
00788  */
00789 void
00790 RobotisRX28::get_angle_limits(unsigned char id,
00791                               unsigned int &cw_limit, unsigned int &ccw_limit,
00792                               bool refresh)
00793 {
00794   cw_limit  = get_value(id, refresh, P_CW_ANGLE_LIMIT_L, P_CW_ANGLE_LIMIT_H);
00795   ccw_limit = get_value(id, refresh, P_CCW_ANGLE_LIMIT_L, P_CCW_ANGLE_LIMIT_H);
00796 }
00797 
00798 
00799 /** Get temperature limit.
00800  * @param id servo ID, not the broadcast ID
00801  * @param refresh if true, will issue a read command for the value
00802  * @return temperature limit.
00803  */
00804 unsigned char
00805 RobotisRX28::get_temperature_limit(unsigned char id, bool refresh)
00806 {
00807   return get_value(id, refresh, P_LIMIT_TEMPERATURE);
00808 }
00809 
00810 
00811 /** Get voltage limits.
00812  * @param id servo ID, not the broadcast ID
00813  * @param refresh if true, will issue a read command for the value
00814  * @param low upon return contains low voltage limit
00815  * @param high upon return contans high voltage limit
00816  */
00817 void
00818 RobotisRX28::get_voltage_limits(unsigned char id,
00819                               unsigned char &low, unsigned char &high,
00820                               bool refresh)
00821 {
00822   low  = get_value(id, refresh, P_DOWN_LIMIT_VOLTAGE);
00823   high = get_value(id, refresh, P_UP_LIMIT_VOLTAGE);
00824 }
00825 
00826 
00827 /** Get maximum torque.
00828  * @param id servo ID, not the broadcast ID
00829  * @param refresh if true, will issue a read command for the value
00830  * @return maximum torque
00831  */
00832 unsigned int
00833 RobotisRX28::get_max_torque(unsigned char id, bool refresh)
00834 {
00835   return get_value(id, refresh, P_MAX_TORQUE_L, P_MAX_TORQUE_H);
00836 }
00837 
00838 
00839 /** Get status return level.
00840  * @param id servo ID, not the broadcast ID
00841  * @param refresh if true, will issue a read command for the value
00842  * @return status return level
00843  */
00844 unsigned char
00845 RobotisRX28::get_status_return_level(unsigned char id, bool refresh)
00846 {
00847   return get_value(id, refresh, P_RETURN_LEVEL);
00848 }
00849 
00850 
00851 /** Get alarm LED status.
00852  * @param id servo ID, not the broadcast ID
00853  * @param refresh if true, will issue a read command for the value
00854  * @return alarm LED status.
00855  */
00856 unsigned char
00857 RobotisRX28::get_alarm_led(unsigned char id, bool refresh)
00858 {
00859   return get_value(id, refresh, P_ALARM_LED);
00860 }
00861 
00862 
00863 /** Get shutdown on alarm state.
00864  * @param id servo ID, not the broadcast ID
00865  * @param refresh if true, will issue a read command for the value
00866  * @return shutdown on alarm state
00867  */
00868 unsigned char
00869 RobotisRX28::get_alarm_shutdown(unsigned char id, bool refresh)
00870 {
00871   return get_value(id, refresh, P_ALARM_SHUTDOWN);
00872 }
00873 
00874 
00875 /** Get calibration data.
00876  * @param id servo ID, not the broadcast ID
00877  * @param refresh if true, will issue a read command for the value
00878  * @param down_calib downward calibration
00879  * @param up_calib upward calibration
00880  */
00881 void
00882 RobotisRX28::get_calibration(unsigned char id,
00883                              unsigned int &down_calib, unsigned int &up_calib,
00884                              bool refresh)
00885 {
00886   down_calib = get_value(id, refresh, P_DOWN_CALIBRATION_L, P_DOWN_CALIBRATION_H);
00887   up_calib   = get_value(id, refresh, P_UP_CALIBRATION_L, P_UP_CALIBRATION_H);
00888 }
00889 
00890 
00891 /** Check if torque is enabled
00892  * @param id servo ID, not the broadcast ID
00893  * @param refresh if true, will issue a read command for the value
00894  * @return true if torque is enabled, false otherwise
00895  */
00896 bool
00897 RobotisRX28::is_torque_enabled(unsigned char id, bool refresh)
00898 {
00899   return (get_value(id, refresh, P_TORQUE_ENABLE) == 1);
00900 }
00901 
00902 
00903 /** Check if LED is enabled
00904  * @param id servo ID, not the broadcast ID
00905  * @param refresh if true, will issue a read command for the value
00906  * @return true if led is enabled, false otherwise.
00907  */
00908 bool
00909 RobotisRX28::is_led_enabled(unsigned char id, bool refresh)
00910 {
00911   return (get_value(id, refresh, P_LED) == 1);
00912 }
00913 
00914 
00915 /** Get compliance values.
00916  * @param id servo ID, not the broadcast ID
00917  * @param refresh if true, will issue a read command for the value
00918  * @param cw_margin upon return contains clockwise margin
00919  * @param cw_slope upon return contains clockwise slope
00920  * @param ccw_margin upon return contains counter-clockwise margin
00921  * @param ccw_slope upon return contains counter-clockwise slope
00922  */
00923 void
00924 RobotisRX28::get_compliance_values(unsigned char id,
00925                                    unsigned char &cw_margin, unsigned char &cw_slope,
00926                                    unsigned char &ccw_margin, unsigned char &ccw_slope,
00927                                    bool refresh)
00928 {
00929   cw_margin  = get_value(id, refresh, P_CW_COMPLIANCE_MARGIN);
00930   cw_slope   = get_value(id, refresh, P_CW_COMPLIANCE_SLOPE);
00931   ccw_margin = get_value(id, refresh, P_CCW_COMPLIANCE_MARGIN);
00932   ccw_slope  = get_value(id, refresh, P_CCW_COMPLIANCE_SLOPE);
00933 }
00934 
00935 
00936 /** Get goal position.
00937  * @param id servo ID, not the broadcast ID
00938  * @param refresh if true, will issue a read command for the value
00939  * @return goal position
00940  */
00941 unsigned int
00942 RobotisRX28::get_goal_position(unsigned char id, bool refresh)
00943 {
00944   return get_value(id, refresh, P_GOAL_POSITION_L, P_GOAL_POSITION_H);
00945 }
00946 
00947 
00948 /** Get goal speed.
00949  * @param id servo ID, not the broadcast ID
00950  * @param refresh if true, will issue a read command for the value
00951  * @return goal speed
00952  */
00953 unsigned int
00954 RobotisRX28::get_goal_speed(unsigned char id, bool refresh)
00955 {
00956   return get_value(id, refresh, P_GOAL_SPEED_L, P_GOAL_SPEED_H);
00957 }
00958 
00959 
00960 /** Get maximum supported speed.
00961  * @param id servo ID, not the broadcast ID
00962  * @param refresh if true, will issue a read command for the value
00963  * @return maximum supported speed in rad/s
00964  */
00965 float
00966 RobotisRX28::get_max_supported_speed(unsigned char id, bool refresh)
00967 {
00968   float voltage = get_voltage(id, refresh) / 10.0;
00969 
00970   if ((voltage < 12.0) || (voltage > 16.0)) {
00971     throw OutOfBoundsException("Voltage is outside of specs", voltage, 12.0, 16.0);
00972   }
00973 
00974   float sec_per_deg_12V = SEC_PER_60DEG_12V / 60.0;
00975   float sec_per_deg_16V = SEC_PER_60DEG_16V / 60.0;
00976 
00977   float range_sec_per_deg = sec_per_deg_12V - sec_per_deg_16V;
00978   float pos = voltage - 12.0;
00979 
00980   float sec_per_deg = sec_per_deg_16V + pos * range_sec_per_deg;
00981   float deg_per_sec = 1.0 / sec_per_deg;
00982 
00983   return deg2rad(deg_per_sec);
00984 }
00985 
00986 
00987 /** Get torque limit.
00988  * @param id servo ID, not the broadcast ID
00989  * @param refresh if true, will issue a read command for the value
00990  * @return torque limit
00991  */
00992 unsigned int
00993 RobotisRX28::get_torque_limit(unsigned char id, bool refresh)
00994 {
00995   return get_value(id, refresh, P_TORQUE_LIMIT_L, P_TORQUE_LIMIT_H);
00996 }
00997 
00998 
00999 /** Get current speed.
01000  * @param id servo ID, not the broadcast ID
01001  * @param refresh if true, will issue a read command for the value
01002  * @return current speed
01003  */
01004 unsigned int
01005 RobotisRX28::get_speed(unsigned char id, bool refresh)
01006 {
01007   return get_value(id, refresh, P_PRESENT_SPEED_L, P_PRESENT_SPEED_H);
01008 }
01009 
01010 
01011 /** Get current load.
01012  * @param id servo ID, not the broadcast ID
01013  * @param refresh if true, will issue a read command for the value
01014  * @return current load
01015  */
01016 unsigned int
01017 RobotisRX28::get_load(unsigned char id, bool refresh)
01018 {
01019   return get_value(id, refresh, P_PRESENT_LOAD_L, P_PRESENT_LOAD_H);
01020 }
01021 
01022 
01023 /** Get current voltage.
01024  * @param id servo ID, not the broadcast ID
01025  * @param refresh if true, will issue a read command for the value
01026  * @return voltage, divide by 10 to get V
01027  */
01028 unsigned char
01029 RobotisRX28::get_voltage(unsigned char id, bool refresh)
01030 {
01031   return get_value(id, refresh, P_PRESENT_VOLTAGE);
01032 }
01033 
01034 
01035 /** Get temperature.
01036  * @param id servo ID, not the broadcast ID
01037  * @param refresh if true, will issue a read command for the value
01038  * @return temperature in degrees Celsius
01039  */
01040 unsigned char
01041 RobotisRX28::get_temperature(unsigned char id, bool refresh)
01042 {
01043   return get_value(id, refresh, P_PRESENT_TEMPERATURE);
01044 }
01045 
01046 
01047 /** Check if servo is moving.
01048  * @param id servo ID, not the broadcast ID
01049  * @param refresh if true, will issue a read command for the value
01050  * @return true if servo is moving, false otherwise
01051  */
01052 bool
01053 RobotisRX28::is_moving(unsigned char id, bool refresh)
01054 {
01055   return (get_value(id, refresh, P_MOVING) == 1);
01056 }
01057 
01058 
01059 /** Check is servo is locked.
01060  * @param id servo ID, not the broadcast ID
01061  * @param refresh if true, will issue a read command for the value
01062  * @return true if servo config is locked, false otherwise
01063  */
01064 bool
01065 RobotisRX28::is_locked(unsigned char id, bool refresh)
01066 {
01067   return (get_value(id, refresh, P_LOCK) == 1);
01068 }
01069 
01070 
01071 /** Get punch.
01072  * @param id servo ID, not the broadcast ID
01073  * @param refresh if true, will issue a read command for the value
01074  * @return punch
01075  */
01076 unsigned int
01077 RobotisRX28::get_punch(unsigned char id, bool refresh)
01078 {
01079   return get_value(id, refresh, P_PUNCH_L, P_PUNCH_H);
01080 }
01081 
01082 
01083 /** Set ID.
01084  * @param id servo ID
01085  * @param new_id new ID to set
01086  */
01087 void
01088 RobotisRX28::set_id(unsigned char id, unsigned char new_id)
01089 {
01090   write_table_value(id, P_ID, new_id);
01091 }
01092 
01093 
01094 /** Set baud rate.
01095  * @param id servo ID
01096  * @param baudrate new baudrate
01097  */
01098 void
01099 RobotisRX28::set_baudrate(unsigned char id, unsigned char baudrate)
01100 {
01101   write_table_value(id, P_BAUD_RATE, baudrate);
01102 }
01103 
01104 
01105 /** Set return delay time.
01106  * @param id servo ID
01107  * @param return_delay_time new return delay time
01108  */
01109 void
01110 RobotisRX28::set_return_delay_time(unsigned char id, unsigned char return_delay_time)
01111 {
01112   write_table_value(id, P_RETURN_DELAY_TIME, return_delay_time);
01113 }
01114 
01115 
01116 /** Set angle limits.
01117  * @param id servo ID
01118  * @param cw_limit new clockwise limit
01119  * @param ccw_limit new counter-clockwise limit
01120  */
01121 void
01122 RobotisRX28::set_angle_limits(unsigned char id,
01123                               unsigned int cw_limit, unsigned int ccw_limit)
01124 {
01125   write_table_value(id, P_CW_ANGLE_LIMIT_L, cw_limit, true);
01126   write_table_value(id, P_CCW_ANGLE_LIMIT_L, ccw_limit, true);
01127 }
01128 
01129 
01130 /** Set temperature limit.
01131  * @param id servo ID
01132  * @param temp_limit new temperature limit (in degrees Celsius)
01133  */
01134 void
01135 RobotisRX28::set_temperature_limit(unsigned char id, unsigned char temp_limit)
01136 {
01137   write_table_value(id, P_LIMIT_TEMPERATURE, temp_limit);
01138 }
01139 
01140 
01141 /** Set voltage limits.
01142  * @param id servo ID
01143  * @param low lower bound (give Volts * 10)
01144  * @param high higher bound (give Volts * 10)
01145  */
01146 void
01147 RobotisRX28::set_voltage_limits(unsigned char id, unsigned char low, unsigned char high)
01148 {
01149   unsigned char param[2];
01150   param[0] = low;
01151   param[1] = high;
01152   write_table_values(id, P_DOWN_LIMIT_VOLTAGE, param, 2);
01153 }
01154 
01155 
01156 /** Set maximum torque.
01157  * @param id servo ID
01158  * @param max_torque new maximum torque
01159  */
01160 void
01161 RobotisRX28::set_max_torque(unsigned char id, unsigned int max_torque)
01162 {
01163   write_table_value(id, P_MAX_TORQUE_L, max_torque, true);
01164 }
01165 
01166 
01167 /** Set status return level
01168  * @param id servo ID
01169  * @param status_return_level status return level, one of SRL_RESPOND_NONE,
01170  * SRL_RESPOND_READ or SRL_RESPOND_ALL.
01171  */
01172 void
01173 RobotisRX28::set_status_return_level(unsigned char id, unsigned char status_return_level)
01174 {
01175   write_table_value(id, P_RETURN_LEVEL, status_return_level);
01176 }
01177 
01178 
01179 /** Set alarm LED settings.
01180  * @param id servo ID
01181  * @param alarm_led new LED alarm value.
01182  */
01183 void
01184 RobotisRX28::set_alarm_led(unsigned char id, unsigned char alarm_led)
01185 {
01186   write_table_value(id, P_ALARM_LED, alarm_led);
01187 }
01188 
01189 
01190 /** Set shutdown on alarm.
01191  * @param id servo ID
01192  * @param alarm_shutdown alarm shutdown settings
01193  */
01194 void
01195 RobotisRX28::set_alarm_shutdown(unsigned char id, unsigned char alarm_shutdown)
01196 {
01197   write_table_value(id, P_ALARM_SHUTDOWN, alarm_shutdown);
01198 }
01199 
01200 
01201 /** Enable or disable torque.
01202  * @param id servo ID
01203  * @param enabled true to enable (servo is powered) false to disable
01204  * (servo power disabled, servo can be freely moved manually)
01205  */
01206 void
01207 RobotisRX28::set_torque_enabled(unsigned char id, bool enabled )
01208 {
01209   write_table_value(id, P_TORQUE_ENABLE, enabled ? 1 : 0);
01210 }
01211 
01212 
01213 /** Enable or disable torque for multiple (selected) servos at once.
01214  * Given the number of servos the same number of variadic arguments must be
01215  * passed, one for each servo ID that should be enabled/disabled.
01216  * @param enabled true to enable (servo is powered) false to disable
01217  * (servo power disabled, servo can be freely moved manually)
01218  * @param num_servos number of servos to set, maximum is 120
01219  */
01220 void
01221 RobotisRX28::set_torques_enabled(bool enabled, unsigned char num_servos, ...)
01222 {
01223   if (num_servos > 120) {
01224     // not enough space for everything in the parameters..
01225     throw Exception("You cannot set more than 120 servos at once");
01226   }
01227 
01228   va_list arg;
01229   va_start(arg, num_servos);
01230 
01231   unsigned int  plength = 2 * num_servos + 2;
01232   unsigned char param[plength];
01233   param[0] = P_TORQUE_ENABLE;
01234   param[1] = 1;
01235   for (unsigned int i = 0; i < num_servos; ++i) {
01236     unsigned char id    = va_arg(arg, unsigned int);
01237     param[2 + i * 2] = id;
01238     param[2 + i * 2 + 1] = enabled ? 1 : 0;
01239   }
01240   va_end(arg);
01241 
01242   send(BROADCAST_ID, INST_SYNC_WRITE, param, plength);
01243 }
01244 
01245 
01246 /** Turn LED on or off.
01247  * @param id servo ID
01248  * @param led_enabled true to turn LED on, false to turn off
01249  */
01250 void
01251 RobotisRX28::set_led_enabled(unsigned char id, bool led_enabled )
01252 {
01253   write_table_value(id, P_LED, led_enabled ? 1 : 0);
01254 }
01255 
01256 
01257 /** Set compliance values.
01258  * @param id servo ID
01259  * @param cw_margin clockwise margin
01260  * @param cw_slope clockwise slope
01261  * @param ccw_margin counter-clockwise margin
01262  * @param ccw_slope counter-clockwise slope
01263  */
01264 void
01265 RobotisRX28::set_compliance_values(unsigned char id,
01266                                    unsigned char cw_margin, unsigned char cw_slope,
01267                                    unsigned char ccw_margin, unsigned char ccw_slope)
01268 {
01269   unsigned char param[4];
01270   param[0] = cw_margin;
01271   param[1] = ccw_margin;
01272   param[2] = cw_slope;
01273   param[3] = ccw_slope;
01274   write_table_values(id, P_CW_COMPLIANCE_MARGIN, param, 4);
01275 }
01276 
01277 
01278 /** Set goal speed.
01279  * @param id servo ID
01280  * @param goal_speed desired goal speed, 1024 is maximum, 0 means "no velicity
01281  * control", i.e. move as fast as possible depending on the voltage
01282  */
01283 void
01284 RobotisRX28::set_goal_speed(unsigned char id, unsigned int goal_speed)
01285 {
01286   write_table_value(id, P_GOAL_SPEED_L, goal_speed, true);
01287 }
01288 
01289 
01290 /** Set goal speeds for multiple servos.
01291  * Given the number of servos the variadic arguments must contain two values
01292  * for each servo, first is the ID, second the value.
01293  * @param num_servos number of servos, maximum is 83
01294  */
01295 void
01296 RobotisRX28::set_goal_speeds(unsigned int num_servos, ...)
01297 {
01298   if (num_servos > 83) {
01299     // not enough space for everything in the parameters..
01300     throw Exception("You cannot set more than 83 speeds at once");
01301   }
01302 
01303   va_list arg;
01304   va_start(arg, num_servos);
01305 
01306   unsigned int  plength = 3 * num_servos + 2;
01307   unsigned char param[plength];
01308   param[0] = P_GOAL_SPEED_L;
01309   param[1] = 2;
01310   for (unsigned int i = 0; i < num_servos; ++i) {
01311     unsigned char id    = va_arg(arg, unsigned int);
01312     unsigned int  value = va_arg(arg, unsigned int);
01313     //printf("Servo speed %u to %u\n", id, value);
01314     param[2 + i * 3] = id;
01315     param[2 + i * 3 + 1] = (value & 0xFF);
01316     param[2 + i * 3 + 2] = (value >> 8) & 0xFF;
01317   }
01318   va_end(arg);
01319 
01320   send(BROADCAST_ID, INST_SYNC_WRITE, param, plength);
01321 }
01322 
01323 
01324 /** Set torque limit.
01325  * @param id servo ID
01326  * @param torque_limit new torque limit
01327  */
01328 void
01329 RobotisRX28::set_torque_limit(unsigned char id, unsigned int torque_limit)
01330 {
01331   write_table_value(id, P_TORQUE_LIMIT_L, torque_limit, true);
01332 }
01333 
01334 
01335 /** Set punch.
01336  * @param id servo ID
01337  * @param punch new punch value
01338  */
01339 void
01340 RobotisRX28::set_punch(unsigned char id, unsigned int punch)
01341 {
01342   write_table_value(id, P_PUNCH_L, punch, true);
01343 }
01344 
01345 
01346 /** Lock config.
01347  * Locks the config, configuration values can no longer be modified until the
01348  * next power cycle.
01349  * @param id servo ID
01350  */
01351 void
01352 RobotisRX28::lock_config(unsigned char id)
01353 {
01354   write_table_value(id, P_LOCK, 1);
01355 }
01356 
01357 
01358 /** Move servo to specified position.
01359  * @param id servo ID
01360  * @param value position, value between 0 and 1023 (inclusive), covering
01361  * an angle range from 0 to 300 degrees.
01362  */
01363 void
01364 RobotisRX28::goto_position(unsigned char id, unsigned int value)
01365 {
01366   write_table_value(id, P_GOAL_POSITION_L, value, true);
01367 }
01368 
01369 
01370 /** Move several servos to specified positions.
01371  * Given the number of servos the variadic arguments must contain two values
01372  * for each servo, first is the ID, second the position (see goto_position() for
01373  * information on the valid values).
01374  * @param num_servos number of servos, maximum is 83
01375  */
01376 void
01377 RobotisRX28::goto_positions(unsigned int num_servos, ...)
01378 {
01379   if (num_servos > 83) {
01380     // not enough space for everything in the parameters..
01381     throw Exception("You cannot set more than 83 servos at once");
01382   }
01383 
01384   va_list arg;
01385   va_start(arg, num_servos);
01386 
01387   unsigned int  plength = 3 * num_servos + 2;
01388   unsigned char param[plength];
01389   param[0] = P_GOAL_POSITION_L;
01390   param[1] = 2;
01391   for (unsigned int i = 0; i < num_servos; ++i) {
01392     unsigned char id    = va_arg(arg, unsigned int);
01393     unsigned int  value = va_arg(arg, unsigned int);
01394     param[2 + i * 3] = id;
01395     param[2 + i * 3 + 1] = (value & 0xFF);
01396     param[2 + i * 3 + 2] = (value >> 8) & 0xFF;
01397   }
01398   va_end(arg);
01399 
01400   send(BROADCAST_ID, INST_SYNC_WRITE, param, plength);
01401 }
01402