Fawkes API
Fawkes Development Version
|
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, ¶m) == -1) { 00188 Exception e(errno, "Getting the port parameters failed"); 00189 ::close(__fd); 00190 __fd = -1; 00191 throw e; 00192 } 00193 00194 cfsetospeed(¶m, B57600); 00195 cfsetispeed(¶m, 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, ¶m) != 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