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