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

Generated on Tue Feb 22 13:31:29 2011 for Fawkes API by  doxygen 1.4.7