Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * roomba_500.cpp - Roomba Open Interface implementation for 500 series 00004 * 00005 * Created: Sat Jan 01 19:37:13 2011 00006 * Copyright 2006-2010 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. 00014 * 00015 * This program is distributed in the hope that it will be useful, 00016 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00017 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00018 * GNU Library General Public License for more details. 00019 * 00020 * Read the full text in the LICENSE.GPL file in the doc directory. 00021 */ 00022 00023 #include "roomba_500.h" 00024 00025 #include <core/exceptions/system.h> 00026 #include <core/threading/mutex.h> 00027 #include <core/threading/mutex_locker.h> 00028 00029 #include <cstring> 00030 #include <cstdlib> 00031 #include <cmath> 00032 #include <termios.h> 00033 #include <fcntl.h> 00034 #include <errno.h> 00035 #include <netinet/in.h> 00036 #include <unistd.h> 00037 00038 #ifdef HAVE_BLUEZ 00039 # include <bluetooth/bluetooth.h> 00040 # include <bluetooth/hci.h> 00041 # include <bluetooth/hci_lib.h> 00042 # include <bluetooth/rfcomm.h> 00043 # include <sys/socket.h> 00044 # include <fnmatch.h> 00045 00046 static const bdaddr_t _BDADDR_ANY = {{0, 0, 0, 0, 0, 0}}; 00047 #endif 00048 00049 using namespace fawkes; 00050 00051 /// @cond SELFEXPLAINING 00052 const unsigned char Roomba500::BUTTON_CLEAN = 1; 00053 const unsigned char Roomba500::BUTTON_SPOT = 2; 00054 const unsigned char Roomba500::BUTTON_DOCK = 4; 00055 const unsigned char Roomba500::BUTTON_MINUTE = 8; 00056 const unsigned char Roomba500::BUTTON_HOUR = 16; 00057 const unsigned char Roomba500::BUTTON_DAY = 32; 00058 const unsigned char Roomba500::BUTTON_SCHEDULE = 64; 00059 const unsigned char Roomba500::BUTTON_CLOCK = 128; 00060 00061 const unsigned char Roomba500::WHEEL_DROP_LEFT = 8; 00062 const unsigned char Roomba500::WHEEL_DROP_RIGHT = 4; 00063 const unsigned char Roomba500::BUMP_LEFT = 2; 00064 const unsigned char Roomba500::BUMP_RIGHT = 1; 00065 00066 const unsigned char Roomba500::OVERCURRENT_WHEEL_LEFT = 16; 00067 const unsigned char Roomba500::OVERCURRENT_WHEEL_RIGHT = 8; 00068 const unsigned char Roomba500::OVERCURRENT_MAIN_BRUSH = 4; 00069 const unsigned char Roomba500::OVERCURRENT_SIDE_BRUSH = 1; 00070 00071 const unsigned char Roomba500::CHARGING_SOURCE_HOME_BASE = 2; 00072 const unsigned char Roomba500::CHARGING_SOURCE_INTERNAL = 1; 00073 00074 const unsigned char Roomba500::BUMPER_LEFT = 1; 00075 const unsigned char Roomba500::BUMPER_FRONT_LEFT = 2; 00076 const unsigned char Roomba500::BUMPER_CENTER_LEFT = 4; 00077 const unsigned char Roomba500::BUMPER_CENTER_RIGHT = 8; 00078 const unsigned char Roomba500::BUMPER_FRONT_RIGHT = 16; 00079 const unsigned char Roomba500::BUMPER_RIGHT = 32; 00080 00081 const unsigned char Roomba500::LED_DEBRIS = 1; 00082 const unsigned char Roomba500::LED_SPOT = 2; 00083 const unsigned char Roomba500::LED_DOCK = 4; 00084 const unsigned char Roomba500::LED_CHECK_ROBOT = 8; 00085 00086 const unsigned char Roomba500::WEEKDAY_LED_SUN = 1; 00087 const unsigned char Roomba500::WEEKDAY_LED_MON = 2; 00088 const unsigned char Roomba500::WEEKDAY_LED_TUE = 4; 00089 const unsigned char Roomba500::WEEKDAY_LED_WED = 8; 00090 const unsigned char Roomba500::WEEKDAY_LED_THU = 16; 00091 const unsigned char Roomba500::WEEKDAY_LED_FRI = 32; 00092 const unsigned char Roomba500::WEEKDAY_LED_SAT = 64; 00093 00094 const unsigned char Roomba500::SCHEDULING_LED_COLON = 1; 00095 const unsigned char Roomba500::SCHEDULING_LED_PM = 2; 00096 const unsigned char Roomba500::SCHEDULING_LED_AM = 4; 00097 const unsigned char Roomba500::SCHEDULING_LED_CLOCK = 8; 00098 const unsigned char Roomba500::SCHEDULING_LED_SCHEDULE = 16; 00099 00100 const unsigned char Roomba500::DIGIT_LED_NORTH = 1; 00101 const unsigned char Roomba500::DIGIT_LED_NORTH_WEST = 32; 00102 const unsigned char Roomba500::DIGIT_LED_NORTH_EAST = 2; 00103 const unsigned char Roomba500::DIGIT_LED_CENTER = 64; 00104 const unsigned char Roomba500::DIGIT_LED_SOUTH_WEST = 16; 00105 const unsigned char Roomba500::DIGIT_LED_SOUTH_EAST = 4; 00106 const unsigned char Roomba500::DIGIT_LED_SOUTH = 8; 00107 00108 const unsigned char Roomba500::MOTOR_SIDE_BRUSH = 1; 00109 const unsigned char Roomba500::MOTOR_VACUUM = 2; 00110 const unsigned char Roomba500::MOTOR_MAIN_BRUSHES = 4; 00111 const unsigned char Roomba500::MOTOR_SIDE_BRUSH_BACKWARD = 8; 00112 const unsigned char Roomba500::MOTOR_MAIN_BRUSHES_BACKWARD = 16; 00113 00114 const unsigned char Roomba500::CHARGER_HOME_BASE = 2; 00115 const unsigned char Roomba500::CHARGER_INTERNAL = 1; 00116 00117 const unsigned short int Roomba500::SENSPACK_SIZE_GROUP_0 = 26; 00118 const unsigned short int Roomba500::SENSPACK_SIZE_GROUP_1 = 10; 00119 const unsigned short int Roomba500::SENSPACK_SIZE_GROUP_2 = 6; 00120 const unsigned short int Roomba500::SENSPACK_SIZE_GROUP_3 = 10; 00121 const unsigned short int Roomba500::SENSPACK_SIZE_GROUP_4 = 14; 00122 const unsigned short int Roomba500::SENSPACK_SIZE_GROUP_5 = 12; 00123 const unsigned short int Roomba500::SENSPACK_SIZE_GROUP_6 = 52; 00124 const unsigned short int Roomba500::SENSPACK_SIZE_GROUP_ALL = 80; 00125 const unsigned short int Roomba500::SENSPACK_SIZE_GROUP_101 = 28; 00126 const unsigned short int Roomba500::SENSPACK_SIZE_GROUP_106 = 12; 00127 const unsigned short int Roomba500::SENSPACK_SIZE_GROUP_107 = 9; 00128 const unsigned short int Roomba500::SENSPACK_SIZE_BUMPS_DROPS = 1; 00129 const unsigned short int Roomba500::SENSPACK_SIZE_WALL = 1; 00130 const unsigned short int Roomba500::SENSPACK_SIZE_CLIFF_LEFT = 1; 00131 const unsigned short int Roomba500::SENSPACK_SIZE_CLIFF_FRONT_LEFT = 1; 00132 const unsigned short int Roomba500::SENSPACK_SIZE_CLIFF_FRONT_RIGHT = 1; 00133 const unsigned short int Roomba500::SENSPACK_SIZE_CLIFF_RIGHT = 1; 00134 const unsigned short int Roomba500::SENSPACK_SIZE_VIRTUAL_WALL = 1; 00135 const unsigned short int Roomba500::SENSPACK_SIZE_WHEEL_OVERCURRENTS = 1; 00136 const unsigned short int Roomba500::SENSPACK_SIZE_DIRT_DETECT = 1; 00137 const unsigned short int Roomba500::SENSPACK_SIZE_IR_CHAR_OMNI = 1; 00138 const unsigned short int Roomba500::SENSPACK_SIZE_IR_CHAR_LEFT = 1; 00139 const unsigned short int Roomba500::SENSPACK_SIZE_IR_CHAR_RIGHT = 1; 00140 const unsigned short int Roomba500::SENSPACK_SIZE_BUTTONS = 1; 00141 const unsigned short int Roomba500::SENSPACK_SIZE_DISTANCE = 2; 00142 const unsigned short int Roomba500::SENSPACK_SIZE_ANGLE = 2; 00143 const unsigned short int Roomba500::SENSPACK_SIZE_CHARGING_STATE = 1; 00144 const unsigned short int Roomba500::SENSPACK_SIZE_VOLTAGE = 2; 00145 const unsigned short int Roomba500::SENSPACK_SIZE_CURRENT = 2; 00146 const unsigned short int Roomba500::SENSPACK_SIZE_TEMPERATURE = 1; 00147 const unsigned short int Roomba500::SENSPACK_SIZE_BATTERY_CHARGE = 2; 00148 const unsigned short int Roomba500::SENSPACK_SIZE_BATTERY_CAPACITY = 2; 00149 const unsigned short int Roomba500::SENSPACK_SIZE_WALL_SIGNAL = 2; 00150 const unsigned short int Roomba500::SENSPACK_SIZE_CLIFF_LEFT_SIGNAL = 2; 00151 const unsigned short int Roomba500::SENSPACK_SIZE_CLIFF_FRONT_LEFT_SIGNAL = 2; 00152 const unsigned short int Roomba500::SENSPACK_SIZE_CLIFF_FRONT_RIGHT_SIGNAL= 2; 00153 const unsigned short int Roomba500::SENSPACK_SIZE_CLIFF_RIGHT_SIGNAL = 2; 00154 const unsigned short int Roomba500::SENSPACK_SIZE_CHARGE_SOURCES = 1; 00155 const unsigned short int Roomba500::SENSPACK_SIZE_OI_MODE = 1; 00156 const unsigned short int Roomba500::SENSPACK_SIZE_SONG_NUMBER = 1; 00157 const unsigned short int Roomba500::SENSPACK_SIZE_SONG_PLAYING = 1; 00158 const unsigned short int Roomba500::SENSPACK_SIZE_STREAM_PACKETS = 1; 00159 const unsigned short int Roomba500::SENSPACK_SIZE_REQ_VELOCITY = 2; 00160 const unsigned short int Roomba500::SENSPACK_SIZE_REQ_RADIUS = 2; 00161 const unsigned short int Roomba500::SENSPACK_SIZE_REQ_RIGHT_VELOCITY = 2; 00162 const unsigned short int Roomba500::SENSPACK_SIZE_REQ_LEFT_VELOCITY = 2; 00163 const unsigned short int Roomba500::SENSPACK_SIZE_RIGHT_ENCODER = 2; 00164 const unsigned short int Roomba500::SENSPACK_SIZE_LEFT_ENCODER = 2; 00165 const unsigned short int Roomba500::SENSPACK_SIZE_LIGHT_BUMPER = 1; 00166 const unsigned short int Roomba500::SENSPACK_SIZE_LIGHT_BUMPER_LEFT = 2; 00167 const unsigned short int Roomba500::SENSPACK_SIZE_LIGHT_BUMPER_FRONT_LEFT = 2; 00168 const unsigned short int Roomba500::SENSPACK_SIZE_LIGHT_BUMPER_CENTER_LEFT = 2; 00169 const unsigned short int Roomba500::SENSPACK_SIZE_LIGHT_BUMPER_CENTER_RIGHT = 2; 00170 const unsigned short int Roomba500::SENSPACK_SIZE_LIGHT_BUMPER_FRONT_RIGHT = 2; 00171 const unsigned short int Roomba500::SENSPACK_SIZE_LIGHT_BUMPER_RIGHT = 2; 00172 const unsigned short int Roomba500::SENSPACK_SIZE_LEFT_MOTOR_CURRENT = 2; 00173 const unsigned short int Roomba500::SENSPACK_SIZE_RIGHT_MOTOR_CURRENT = 2; 00174 const unsigned short int Roomba500::SENSPACK_SIZE_BRUSH_MOTOR_CURRENT = 2; 00175 const unsigned short int Roomba500::SENSPACK_SIZE_SIDE_BRUSH_MOTOR_CURRENT = 2; 00176 const unsigned short int Roomba500::SENSPACK_SIZE_STASIS = 1; 00177 00178 const float Roomba500::DIAMETER = 0.33; 00179 const float Roomba500::BUMPER_X_OFFSET = 0.05; 00180 const float Roomba500::AXLE_LENGTH = 0.235; 00181 00182 const short int Roomba500::MAX_LIN_VEL_MM_S = 500; 00183 const short int Roomba500::MAX_RADIUS_MM = 2000; 00184 00185 const unsigned short int Roomba500::MAX_ENCODER_COUNT = 65535; 00186 const short int Roomba500::MAX_PWM = 255; 00187 00188 const unsigned short int Roomba500::STREAM_INTERVAL_MS = 15; 00189 const unsigned short int Roomba500::MODE_CHANGE_WAIT_MS = 20; 00190 00191 const unsigned char Roomba500::CHECKSUM_SIZE = 1; 00192 00193 /// @endcond 00194 00195 /// @cond INTERNALS 00196 #pragma pack(push,1) 00197 00198 typedef struct { 00199 int16_t velocity; 00200 int16_t radius; 00201 } DriveParams; 00202 00203 typedef struct { 00204 int16_t left_velocity; 00205 int16_t right_velocity; 00206 } DriveDirectParams; 00207 00208 typedef struct { 00209 int16_t left_pwm; 00210 int16_t right_pwm; 00211 } DrivePWMParams; 00212 00213 typedef struct { 00214 uint8_t num_packets; 00215 uint8_t packet_id; 00216 } StreamOnePacketParams; 00217 00218 #pragma pack(pop) 00219 /// @endcond 00220 00221 00222 /** @class Roomba500 "roomba_500.h" 00223 * Roomba 500 series communication class. 00224 * This class implements the serial communication with Roomba robots of the 00225 * 500 series. 00226 * 00227 * RFCOMM by reading http://people.csail.mit.edu/albert/bluez-intro/. 00228 * 00229 * @author Tim Niemueller 00230 */ 00231 00232 /** Constructor. 00233 * @param conntype connection type 00234 * @param device for CONN_SERIAL connection type this is the device file for 00235 * the serial connection. For CONN_ROOTOOTH this is either a name pattern 00236 * of a bluetooth device to query or a bluetooth address. The name can be the 00237 * full name, or a pattern using shell globs (e.g. FireFly-*). The bluetooth 00238 * address must be given in hexadecimal manner (e.g. 00:11:22:33:44:55). 00239 * @param flags ConnectionFlags constants, joined with bit-wise "or" (|). 00240 */ 00241 Roomba500::Roomba500(Roomba500::ConnectionType conntype, const char *device, 00242 unsigned int flags) 00243 { 00244 __conntype = conntype; 00245 __conn_flags = flags; 00246 #ifndef HAVE_BLUEZ 00247 if (__conntype == CONNTYPE_ROOTOOTH) { 00248 throw Exception("Native RooTooth not available at compile time."); 00249 } 00250 #endif 00251 __mode = MODE_OFF; 00252 __fd = -1; 00253 __packet_id = SENSPACK_GROUP_ALL; 00254 __sensors_enabled = false; 00255 00256 __device = strdup(device); 00257 00258 __sensor_mutex = new Mutex(); 00259 __read_mutex = new Mutex(); 00260 __write_mutex = new Mutex(); 00261 00262 try { 00263 open(); 00264 } catch (Exception &e) { 00265 free(__device); 00266 delete __write_mutex; 00267 delete __read_mutex; 00268 delete __sensor_mutex; 00269 throw; 00270 } 00271 } 00272 00273 00274 /** Destructor. */ 00275 Roomba500::~Roomba500() 00276 { 00277 close(); 00278 free(__device); 00279 delete __write_mutex; 00280 delete __read_mutex; 00281 delete __sensor_mutex; 00282 } 00283 00284 00285 /** Open serial port. */ 00286 void 00287 Roomba500::open() 00288 { 00289 if (__conntype == CONNTYPE_SERIAL) { 00290 struct termios param; 00291 00292 __fd = ::open(__device, O_NOCTTY | O_RDWR); 00293 if (__fd == -1) { 00294 throw CouldNotOpenFileException(__device, errno, "Cannot open device file"); 00295 } 00296 00297 if (tcgetattr(__fd, ¶m) == -1) { 00298 Exception e(errno, "Getting the port parameters failed"); 00299 ::close(__fd); 00300 __fd = -1; 00301 throw e; 00302 } 00303 00304 cfsetospeed(¶m, B115200); 00305 cfsetispeed(¶m, B115200); 00306 00307 param.c_lflag &= ~(ICANON | ECHO | ECHOE | ECHOK | ECHONL | ISIG | IEXTEN); 00308 param.c_iflag &= ~(INLCR | IGNCR | ICRNL | IGNBRK | PARMRK); 00309 00310 // turn off hardware flow control 00311 param.c_iflag &= ~(IXON | IXOFF | IXANY); 00312 param.c_cflag &= ~CRTSCTS; 00313 00314 param.c_cflag |= (CREAD | CLOCAL); 00315 00316 // number of data bits: 8 00317 param.c_cflag &= ~CS5 & ~CS6 & ~CS7 & ~CS8; 00318 param.c_cflag |= CS8; 00319 00320 // parity: none 00321 param.c_cflag &= ~(PARENB | PARODD); 00322 param.c_iflag &= ~(INPCK | ISTRIP); 00323 00324 // stop bits: 1 00325 param.c_cflag &= ~CSTOPB; 00326 00327 //enable raw output 00328 param.c_oflag &= ~OPOST; 00329 00330 param.c_cc[VMIN] = 1; 00331 param.c_cc[VTIME] = 0; 00332 00333 tcflush(__fd, TCIOFLUSH); 00334 00335 if (tcsetattr(__fd, TCSANOW, ¶m) != 0) { 00336 Exception e(errno, "Setting the port parameters failed"); 00337 ::close(__fd); 00338 __fd = -1; 00339 throw e; 00340 } 00341 } else { 00342 #ifndef HAVE_BLUEZ 00343 throw Exception("Native RooTooth support unavailable at compile time."); 00344 #else 00345 struct hci_dev_info di; 00346 inquiry_info *ii = NULL; 00347 int max_rsp, num_rsp; 00348 int dev_id, sock, len, flags; 00349 char addrstr[19] = { 0 }; 00350 char name[248] = { 0 }; 00351 bdaddr_t baddr = _BDADDR_ANY; 00352 00353 if ((dev_id = hci_get_route(NULL)) < 0) { 00354 throw Exception("RooTooth: local bluetooth device is not available"); 00355 } 00356 00357 if (hci_devinfo(dev_id, &di) < 0) { 00358 throw Exception("RooTooth: cannot get local device info."); 00359 } 00360 00361 if ((sock = hci_open_dev(dev_id)) < 0) { 00362 throw Exception("RooTooth: failed to open socket."); 00363 } 00364 00365 len = 8; 00366 max_rsp = 255; 00367 flags = IREQ_CACHE_FLUSH; 00368 ii = (inquiry_info*)malloc(max_rsp * sizeof(inquiry_info)); 00369 00370 if (strcmp(__device, "") == 0) { 00371 // we simply guess from the device class 00372 00373 num_rsp = hci_inquiry(dev_id, len, max_rsp, NULL, &ii, flags); 00374 if (num_rsp < 0) { 00375 throw Exception(errno, "HCI inquiry failed"); 00376 } 00377 00378 for (int i = 0; i < num_rsp; i++) { 00379 00380 uint8_t b[6]; 00381 baswap((bdaddr_t *) b, &(ii+i)->bdaddr); 00382 00383 ba2str(&(ii+i)->bdaddr, addrstr); 00384 /* 00385 printf("Comparing (0x%2.2x%2.2x%2.2x) (0x%2.2x%2.2x%2.2x) %s %s\n", 00386 b[0], b[1], b[2], 00387 ii[i].dev_class[0], ii[i].dev_class[1], ii[i].dev_class[2], 00388 name, addrstr); 00389 */ 00390 00391 // This checks for the RooTooth I have which identifies itself as 00392 // FireFly-XXXX, where XXXX are the last four digits of the BT addr 00393 if (// check OUI for Roving Networks 00394 (b[0] == 0x00) && (b[1] == 0x06) && (b[2] == 0x66) && 00395 00396 // check device class 00397 (ii[i].dev_class[0] == 0x00) && 00398 (ii[i].dev_class[1] == 0x1f) && 00399 (ii[i].dev_class[2] == 0x00) ) 00400 { 00401 // verify the name 00402 memset(name, 0, sizeof(name)); 00403 hci_read_remote_name(sock, &(ii+i)->bdaddr, sizeof(name), name, 0); 00404 00405 if ( // Now check the advertised name 00406 (fnmatch("FireFly-*", name, FNM_NOESCAPE) == 0) || 00407 (strcmp("RooTooth", name) == 0) ) 00408 { 00409 // found a device which is likely a 00410 ba2str(&(ii+i)->bdaddr, addrstr); 00411 //printf("found A: %s %s\n", addrstr, name); 00412 free(__device); 00413 __device = strdup(addrstr); 00414 bacpy(&baddr, &(ii+i)->bdaddr); 00415 break; 00416 } 00417 } 00418 } 00419 } else { 00420 bool is_bdaddr = (bachk(__device) == 0); 00421 00422 if (is_bdaddr) { 00423 //printf("Match by bdaddr\n"); 00424 00425 str2ba(__device, &baddr); 00426 ba2str(&baddr, addrstr); 00427 00428 //printf("found B: %s %s\n", addrstr, name); 00429 } else { 00430 //printf("Match by btname\n"); 00431 00432 num_rsp = hci_inquiry(dev_id, len, max_rsp, NULL, &ii, flags); 00433 if (num_rsp < 0) { 00434 throw Exception(errno, "HCI inquiry failed"); 00435 } 00436 00437 // we have a name pattern to check 00438 for (int i = 0; i < num_rsp; i++) { 00439 ba2str(&(ii+i)->bdaddr, addrstr); 00440 memset(name, 0, sizeof(name)); 00441 if (hci_read_remote_name(sock, &(ii+i)->bdaddr, sizeof(name), 00442 name, 0) < 0) 00443 { 00444 strcpy(name, "[unknown]"); 00445 } 00446 if (fnmatch(__device, name, FNM_NOESCAPE) == 0) { 00447 // found the device 00448 //printf("found C: %s %s\n", addrstr, name); 00449 free(__device); 00450 __device = strdup(addrstr); 00451 bacpy(&baddr, &(ii+i)->bdaddr); 00452 break; 00453 } 00454 } 00455 } 00456 } 00457 00458 free(ii); 00459 ::close(sock); 00460 00461 if (bacmp(&baddr, &_BDADDR_ANY) == 0) { 00462 throw Exception("Could not find RooTooth device."); 00463 } 00464 ba2str(&baddr, addrstr); 00465 00466 // connect via RFCOMM 00467 struct sockaddr_rc rcaddr = { 0 }; 00468 00469 // allocate a socket 00470 __fd = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM); 00471 00472 // set the connection parameters (who to connect to) 00473 rcaddr.rc_family = AF_BLUETOOTH; 00474 rcaddr.rc_channel = (uint8_t) 1; 00475 bacpy(&rcaddr.rc_bdaddr, &baddr); 00476 00477 // connect to server 00478 if (connect(__fd, (struct sockaddr *)&rcaddr, sizeof(rcaddr)) < 0) { 00479 throw Exception(errno, "Failed to connect to %s", addrstr); 00480 } 00481 00482 try { 00483 // Set to passive mode to ensure that auto-detection does no harm 00484 send(OPCODE_START); 00485 usleep(MODE_CHANGE_WAIT_MS * 1000); 00486 __mode = MODE_PASSIVE; 00487 // disable sensors just in case 00488 disable_sensors(); 00489 } catch (Exception &e) { 00490 ::close(__fd); 00491 __mode = MODE_OFF; 00492 throw; 00493 } 00494 00495 if (flags & FLAG_FIREFLY_FASTMODE) { 00496 const char *cmd_seq = "$$$"; 00497 if (write(__fd, cmd_seq, 3) != 3) { 00498 ::close(__fd); 00499 throw Exception(errno, "Roomba500 (RooTooth): Failed to send command " 00500 "sequence to enable fast mode"); 00501 } 00502 00503 timeval timeout = {1, 500000}; 00504 fd_set read_fds; 00505 FD_ZERO(&read_fds); 00506 FD_SET(__fd, &read_fds); 00507 00508 int rv = 0; 00509 rv = select(__fd + 1, &read_fds, NULL, NULL, &timeout); 00510 00511 if (rv > 0) { 00512 char cmd_reply[4]; 00513 if (read(__fd, cmd_reply, 4) == 4) { 00514 if (strncmp(cmd_reply, "CMD", 3) == 0) { 00515 // We entered command mode, enable fast mode 00516 const char *cmd_fastmode = "F,1\r"; 00517 if (write(__fd, cmd_fastmode, 4) != 4) { 00518 ::close(__fd); 00519 throw Exception(errno, "Roomba500 (RooTooth): Failed to send fast " 00520 "mode command sequence."); 00521 } // else fast mode enabled 00522 } // else invalid reply, again assume fast mode 00523 } // assume fast mode 00524 } // else assume already enabled fast mode 00525 } // else do not enable fast mode, assume user knows what he is doing 00526 00527 #endif 00528 } 00529 00530 try { 00531 send(OPCODE_START); 00532 usleep(MODE_CHANGE_WAIT_MS * 1000); 00533 __mode = MODE_PASSIVE; 00534 } catch (Exception &e) { 00535 ::close(__fd); 00536 __mode = MODE_OFF; 00537 throw; 00538 } 00539 00540 } 00541 00542 00543 /** Close serial connection. */ 00544 void 00545 Roomba500::close() 00546 { 00547 if (__fd >= 0) { 00548 ::close(__fd); 00549 __fd = -1; 00550 } 00551 __mode = MODE_OFF; 00552 } 00553 00554 /** Send instruction packet. 00555 * @param opcode Opcode of command 00556 * @param params parameter bytes of the message 00557 * @param plength length of the params array 00558 */ 00559 void 00560 Roomba500::send(Roomba500::OpCode opcode, 00561 const void *params, const size_t plength) 00562 { 00563 MutexLocker write_lock(__write_mutex); 00564 00565 // Byte 0 and 1 must be 0xFF 00566 __obuffer[0] = opcode; 00567 __obuffer_length = 1; 00568 00569 if (params && (plength > 0)) { 00570 if (plength > (sizeof(__obuffer) - __obuffer_length)) { 00571 throw Exception("Parameters for command %i too long, maximum length is %zu", 00572 opcode, (sizeof(__obuffer) - __obuffer_length)); 00573 } 00574 unsigned char *pbytes = (unsigned char *)params; 00575 for (size_t i = 0; i < plength; ++i) { 00576 __obuffer[1+i] = pbytes[i]; 00577 } 00578 __obuffer_length += plength; 00579 } 00580 00581 int written = write(__fd, __obuffer, __obuffer_length); 00582 00583 /* 00584 printf("Wrote %i of %i bytes:\n", written, __obuffer_length); 00585 for (int i = 0; i < __obuffer_length; ++i) { 00586 printf("%2u %s", __obuffer[i], i == written ? "| " : ""); 00587 } 00588 printf("\n"); 00589 */ 00590 00591 if ( written < 0 ) { 00592 throw Exception(errno, "Failed to write Roomba 500 packet %i", opcode); 00593 } else if (written < __obuffer_length) { 00594 throw Exception("Failed to write Roomba 500 packet %i, " 00595 "only %d of %d bytes sent", 00596 opcode, written, __obuffer_length); 00597 } 00598 } 00599 00600 00601 /** Receive a packet. 00602 * @param index index in ibuffer to fill 00603 * @param num_bytes number of bytes to read 00604 * @param timeout_ms maximum wait time in miliseconds, 0 to wait indefinitely. 00605 */ 00606 void 00607 Roomba500::recv(size_t index, size_t num_bytes, unsigned int timeout_ms) 00608 { 00609 timeval timeout = {0, timeout_ms * 1000}; 00610 00611 fd_set read_fds; 00612 FD_ZERO(&read_fds); 00613 FD_SET(__fd, &read_fds); 00614 00615 int rv = 0; 00616 rv = select(__fd + 1, &read_fds, NULL, NULL, (timeout_ms > 0) ? &timeout : NULL); 00617 00618 if ( rv == -1 ) { 00619 throw Exception(errno, "Roomba500::recv(): select on file descriptor failed"); 00620 } else if ( rv == 0 ) { 00621 throw TimeoutException("Timeout while waiting for incoming Roomba data"); 00622 } 00623 00624 __ibuffer_length = 0; 00625 00626 // get octets one by one 00627 int bytes_read = 0; 00628 while (bytes_read < (int)num_bytes) { 00629 int rv = read(__fd, &__ibuffer[index] +bytes_read, num_bytes -bytes_read); 00630 if (rv == -1) { 00631 throw Exception(errno, "Roomba500::recv(): read failed"); 00632 } 00633 bytes_read += rv; 00634 } 00635 00636 if (bytes_read < (int)num_bytes) { 00637 throw Exception("Roomba500::recv(): failed to read packet data"); 00638 } 00639 00640 __ibuffer_length = index + num_bytes; 00641 } 00642 00643 00644 /** Check if data is available. 00645 * @return true if data is available and can be read, false otherwise 00646 */ 00647 bool 00648 Roomba500::is_data_available() 00649 { 00650 if (!__sensors_enabled) { 00651 throw Exception("Roomba 500 sensors have not been enabled."); 00652 } 00653 00654 timeval timeout = {0, 0}; 00655 00656 fd_set read_fds; 00657 FD_ZERO(&read_fds); 00658 FD_SET(__fd, &read_fds); 00659 00660 int rv = 0; 00661 rv = select(__fd + 1, &read_fds, NULL, NULL, &timeout); 00662 00663 return (rv > 0); 00664 } 00665 00666 00667 /** Read sensor values. 00668 * Enable sensors before using enable_sensors(). This method will block until new 00669 * data has been read. You can call is_data_available() to check if this method 00670 * will block or not. 00671 */ 00672 void 00673 Roomba500::read_sensors() 00674 { 00675 MutexLocker read_lock(__read_mutex); 00676 00677 if (!__sensors_enabled) { 00678 throw Exception("Roomba 500 sensors have not been enabled."); 00679 } 00680 00681 bool done = false; 00682 unsigned int skipped = 0; 00683 while (!done) { 00684 __ibuffer_length = 0; 00685 00686 recv(__ibuffer_length, 1); 00687 if (__ibuffer[0] != 19) { 00688 ++skipped; 00689 continue; 00690 } 00691 00692 recv(__ibuffer_length, 1); 00693 if (__ibuffer[1] != __packet_length + 1) { 00694 ++skipped; 00695 continue; 00696 } 00697 00698 recv(__ibuffer_length, 1); 00699 if (__ibuffer[2] != __packet_id) { 00700 ++skipped; 00701 continue; 00702 } 00703 00704 recv(__ibuffer_length, __packet_length); 00705 00706 recv(__ibuffer_length++, 1); 00707 00708 unsigned int sum = 0; 00709 for (int i = 0; i < __ibuffer_length; ++i) { 00710 sum += __ibuffer[i]; 00711 } 00712 00713 if ((sum & 0xFF) != 0) { 00714 __sensor_packet_received = false; 00715 } else { 00716 __sensor_mutex->lock(); 00717 memcpy(&__sensor_packet, &__ibuffer[3], sizeof(SensorPacketGroupAll)); 00718 __sensor_packet_received = true; 00719 __sensor_mutex->unlock(); 00720 } 00721 00722 done = true; 00723 } 00724 00725 /* 00726 printf("Read %u bytes: ", __ibuffer_length); 00727 for (int i = 0; i < __ibuffer_length; ++i) { 00728 printf("%2u ", __ibuffer[i]); 00729 } 00730 printf(" (skipped %u)\n", skipped); 00731 */ 00732 } 00733 00734 /** Enable sensor data stream. 00735 * For simplicity and efficiency only the single SENSPACK_GROUP_ALL packet can 00736 * be streamed at this time. Make sure that the used connection is fast enough. 00737 * 56700 bit/sec should suffice, but 115200 is strongly recommended. If using 00738 * RooTooth make sure to use it in fast mode. 00739 */ 00740 void 00741 Roomba500::enable_sensors() 00742 { 00743 assert_connected(); 00744 00745 StreamOnePacketParams sp; 00746 sp.num_packets = 1; 00747 sp.packet_id = SENSPACK_GROUP_ALL; 00748 00749 send(OPCODE_STREAM, &sp, sizeof(StreamOnePacketParams)); 00750 00751 __packet_id = SENSPACK_GROUP_ALL; 00752 __packet_reply_id = 19; 00753 __packet_length = get_packet_size(SENSPACK_GROUP_ALL); 00754 __sensors_enabled = true; 00755 __sensor_packet_received = false; 00756 } 00757 00758 00759 /** Disable sensor data stream. */ 00760 void 00761 Roomba500::disable_sensors() 00762 { 00763 assert_connected(); 00764 00765 unsigned char streamstate = STREAM_DISABLE; 00766 00767 send(OPCODE_PAUSE_RESUME_STREAM, &streamstate, 1); 00768 00769 __sensors_enabled = false; 00770 __sensor_packet_received = false; 00771 } 00772 00773 00774 /** Query sensor once. 00775 * For simplicity and efficiency only the single SENSPACK_GROUP_ALL packet can 00776 * be streamed at this time. 00777 */ 00778 void 00779 Roomba500::query_sensors() 00780 { 00781 assert_connected(); 00782 00783 unsigned char p = SENSPACK_GROUP_ALL; 00784 00785 send(OPCODE_QUERY, &p, 1); 00786 00787 __packet_id = SENSPACK_GROUP_ALL; 00788 __packet_reply_id = 0; 00789 __packet_length = get_packet_size(SENSPACK_GROUP_ALL); 00790 __sensor_packet_received = true; 00791 00792 00793 __read_mutex->lock(); 00794 recv(0, __packet_length); 00795 __read_mutex->unlock(); 00796 00797 __sensor_mutex->lock(); 00798 memcpy(&__sensor_packet, __ibuffer, sizeof(SensorPacketGroupAll)); 00799 __sensor_mutex->unlock(); 00800 } 00801 00802 00803 /** Get latest sensor packet. 00804 * @return sensor packet 00805 */ 00806 const Roomba500::SensorPacketGroupAll 00807 Roomba500::get_sensor_packet() const 00808 { 00809 MutexLocker lock(__sensor_mutex); 00810 if (! __sensor_packet_received) { 00811 throw Exception("No valid data received, yet."); 00812 } 00813 00814 return __sensor_packet; 00815 } 00816 00817 /** Set control mode. 00818 * @param mode the mode can be either MODE_FULL or MODE_SAFE (recommended). 00819 * MODE_OFF and MODE_PASSIVE cannot be set explicitly. To enter MODE_PASSIVE 00820 * issue a command which transitions mode, like clean() or seek_dock(). 00821 * @exception Exception thrown if an invalid mode is passed 00822 */ 00823 void 00824 Roomba500::set_mode(Roomba500::Mode mode) 00825 { 00826 if (mode == MODE_PASSIVE) { 00827 send(OPCODE_START); 00828 } if (mode == MODE_SAFE) { 00829 send(OPCODE_SAFE); 00830 } else if (mode == MODE_FULL) { 00831 send(OPCODE_FULL); 00832 } else if (mode == MODE_OFF) { 00833 throw Exception("Mode OFF cannot be set, use power_down() instead"); 00834 } 00835 00836 usleep(MODE_CHANGE_WAIT_MS * 1000); 00837 __mode = mode; 00838 } 00839 00840 00841 /** Start normal cleaning operation. 00842 * Transitions to passive mode. 00843 */ 00844 void 00845 Roomba500::clean() 00846 { 00847 send(OPCODE_CLEAN); 00848 00849 __mode = MODE_PASSIVE; 00850 } 00851 00852 00853 /** Start spot cleaning operation. 00854 * Transitions to passive mode. 00855 */ 00856 void 00857 Roomba500::clean_spot() 00858 { 00859 send(OPCODE_SPOT); 00860 00861 __mode = MODE_PASSIVE; 00862 } 00863 00864 00865 /** Seek for the home base and dock. 00866 * Transitions to passive mode. 00867 */ 00868 void 00869 Roomba500::seek_dock() 00870 { 00871 assert_connected(); 00872 00873 send(OPCODE_SEEK_DOCK); 00874 __mode = MODE_PASSIVE; 00875 } 00876 00877 00878 /** Powers down the Roomba. 00879 * Transitions to passive mode. 00880 */ 00881 void 00882 Roomba500::power_down() 00883 { 00884 assert_connected(); 00885 00886 send(OPCODE_POWER); 00887 __mode = MODE_PASSIVE; 00888 } 00889 00890 00891 /** Stop moption of the Roomba. 00892 * Available only in safe or full mode. 00893 */ 00894 void 00895 Roomba500::stop() 00896 { 00897 assert_control(); 00898 drive_pwm(0, 0); 00899 } 00900 00901 00902 00903 /** Drive Roomba straight. 00904 * Available only in safe or full mode. 00905 * @param velo_mm_per_sec desired velocity in m/sec 00906 */ 00907 void 00908 Roomba500::drive_straight(short int velo_mm_per_sec) 00909 { 00910 assert_control(); 00911 00912 if (velo_mm_per_sec < -MAX_LIN_VEL_MM_S) velo_mm_per_sec = -MAX_LIN_VEL_MM_S; 00913 if (velo_mm_per_sec > MAX_LIN_VEL_MM_S) velo_mm_per_sec = MAX_LIN_VEL_MM_S; 00914 00915 DriveParams dp; 00916 dp.velocity = htons(velo_mm_per_sec); 00917 dp.radius = htons(0x8000); 00918 00919 send(OPCODE_DRIVE, &dp, sizeof(DriveParams)); 00920 } 00921 00922 /** Turn robot on the spot. 00923 * Available only in safe or full mode. 00924 * @param direction turning direction 00925 */ 00926 void 00927 Roomba500::drive_turn(Roomba500::TurnDirection direction) 00928 { 00929 assert_control(); 00930 00931 DriveParams dp; 00932 dp.velocity = htons(0); 00933 dp.radius = (direction == TURN_CLOCKWISE) ? -1 : 1; 00934 00935 send(OPCODE_DRIVE, &dp, sizeof(DriveParams)); 00936 } 00937 00938 00939 /** Drive Roomba on an arc. 00940 * Available only in safe or full mode. 00941 * @param velo_mm_per_sec desired velocity in m/sec 00942 * @param radius_mm desired radius of arc in m 00943 */ 00944 void 00945 Roomba500::drive_arc(short int velo_mm_per_sec, short int radius_mm) 00946 { 00947 assert_control(); 00948 00949 if (velo_mm_per_sec < -MAX_LIN_VEL_MM_S) velo_mm_per_sec = -MAX_LIN_VEL_MM_S; 00950 if (velo_mm_per_sec > MAX_LIN_VEL_MM_S) velo_mm_per_sec = MAX_LIN_VEL_MM_S; 00951 00952 if (radius_mm < -MAX_RADIUS_MM) radius_mm = -MAX_RADIUS_MM; 00953 if (radius_mm > MAX_RADIUS_MM) radius_mm = MAX_RADIUS_MM; 00954 00955 DriveParams dp; 00956 dp.velocity = htons(velo_mm_per_sec); 00957 dp.radius = htons(radius_mm); 00958 00959 send(OPCODE_DRIVE, &dp, sizeof(DriveParams)); 00960 } 00961 00962 /** Drive Roomba. 00963 * Available only in safe or full mode. 00964 * @param velo_mm_per_sec desired velocity in m/sec 00965 * @param radius_mm desired radius of arc in m 00966 */ 00967 void 00968 Roomba500::drive(short int velo_mm_per_sec, short int radius_mm) 00969 { 00970 assert_control(); 00971 00972 if (velo_mm_per_sec < -MAX_LIN_VEL_MM_S) velo_mm_per_sec = -MAX_LIN_VEL_MM_S; 00973 if (velo_mm_per_sec > MAX_LIN_VEL_MM_S) velo_mm_per_sec = MAX_LIN_VEL_MM_S; 00974 00975 if (radius_mm < -MAX_RADIUS_MM) radius_mm = -MAX_RADIUS_MM; 00976 if (radius_mm > MAX_RADIUS_MM) radius_mm = 0x8000; // drive straight 00977 00978 DriveParams dp; 00979 dp.velocity = htons(velo_mm_per_sec); 00980 dp.radius = htons(radius_mm); 00981 00982 send(OPCODE_DRIVE, &dp, sizeof(DriveParams)); 00983 } 00984 00985 00986 /** Directly control wheel velocities. 00987 * Available only in safe or full mode. 00988 * @param left_mm_per_sec velocity of left wheel in m/sec 00989 * @param right_mm_per_sec velocity of right wheel in m/sec 00990 */ 00991 void 00992 Roomba500::drive_direct(short int left_mm_per_sec, 00993 short int right_mm_per_sec) 00994 { 00995 assert_control(); 00996 00997 if (left_mm_per_sec < -MAX_LIN_VEL_MM_S) left_mm_per_sec = -MAX_LIN_VEL_MM_S; 00998 if (left_mm_per_sec > MAX_LIN_VEL_MM_S) left_mm_per_sec = MAX_LIN_VEL_MM_S; 00999 01000 if (right_mm_per_sec < -MAX_LIN_VEL_MM_S) right_mm_per_sec = -MAX_LIN_VEL_MM_S; 01001 if (right_mm_per_sec > MAX_LIN_VEL_MM_S) right_mm_per_sec = MAX_LIN_VEL_MM_S; 01002 01003 DriveDirectParams dp; 01004 dp.left_velocity = htons(left_mm_per_sec); 01005 dp.right_velocity = htons(right_mm_per_sec); 01006 01007 send(OPCODE_DRIVE, &dp, sizeof(DriveDirectParams)); 01008 } 01009 01010 01011 /** Directly control wheel velocities via PWM. 01012 * Available only in safe or full mode. 01013 * @param left_wheel_pwm PWM parameter for left wheel 01014 * @param right_wheel_pwm PWM parameter for right wheel 01015 */ 01016 void 01017 Roomba500::drive_pwm(short int left_wheel_pwm, short int right_wheel_pwm) 01018 { 01019 assert_control(); 01020 01021 if (left_wheel_pwm < -MAX_PWM) left_wheel_pwm = -MAX_PWM; 01022 if (left_wheel_pwm > MAX_PWM) left_wheel_pwm = MAX_PWM; 01023 01024 if (right_wheel_pwm < -MAX_PWM) right_wheel_pwm = -MAX_PWM; 01025 if (right_wheel_pwm > MAX_PWM) right_wheel_pwm = MAX_PWM; 01026 01027 DrivePWMParams dp; 01028 dp.left_pwm = htons(left_wheel_pwm); 01029 dp.right_pwm = htons(right_wheel_pwm); 01030 01031 send(OPCODE_DRIVE, &dp, sizeof(DrivePWMParams)); 01032 } 01033 01034 01035 /** Set motor states (brushes and vacuum). 01036 * Available only in safe or full mode. 01037 * @param main true to enable main brushes 01038 * @param side true to enable side brush 01039 * @param vacuum true to enable vacuuming 01040 * @param main_backward true to enable backward operation of main brushes 01041 * @param side_backward true to enable backward operation of side brush 01042 */ 01043 void 01044 Roomba500::set_motors(bool main, bool side, bool vacuum, 01045 bool main_backward, bool side_backward) 01046 { 01047 assert_control(); 01048 01049 unsigned char param = 0; 01050 if (main) param |= MOTOR_MAIN_BRUSHES; 01051 if (side) param |= MOTOR_SIDE_BRUSH; 01052 if (vacuum) param |= MOTOR_VACUUM; 01053 if (main_backward) param |= MOTOR_MAIN_BRUSHES_BACKWARD; 01054 if (side_backward) param |= MOTOR_SIDE_BRUSH_BACKWARD; 01055 01056 send(OPCODE_MOTORS, ¶m, 1); 01057 } 01058 01059 01060 /** Set LED status of main LEDs. 01061 * Available only in safe or full mode. 01062 * @param debris true to enable debris LED, false to disable 01063 * @param spot true to enable spot LED, false to disable 01064 * @param dock true to enable dock LED, false to disable 01065 * @param check_robot true to enable check_robot LED, false to disable 01066 * @param clean_color color of clean button LED from green (0) to red (255) 01067 * @param clean_intensity intensity of clean button LED from off (0) to 01068 * full intensity (255) 01069 */ 01070 void 01071 Roomba500::set_leds(bool debris, bool spot, bool dock, bool check_robot, 01072 unsigned char clean_color, unsigned char clean_intensity) 01073 { 01074 assert_control(); 01075 01076 unsigned char param[3] = {0, clean_color, clean_intensity}; 01077 if (debris) param[0] |= LED_DEBRIS; 01078 if (spot) param[0] |= LED_SPOT; 01079 if (dock) param[0] |= LED_DOCK; 01080 if (check_robot) param[0] |= LED_CHECK_ROBOT; 01081 01082 send(OPCODE_LEDS, param, 3); 01083 } 01084 01085 01086 /** Set digit LEDs. 01087 * Available only in safe or full mode. 01088 * Note, that not all characters are availabe. You can use ASCII table entries 01089 * 32-39, 44-63, 65-96, and 123-126. 01090 * @param digits array of digit values 01091 */ 01092 void 01093 Roomba500::set_digit_leds(const char digits[4]) 01094 { 01095 assert_control(); 01096 01097 send(OPCODE_DIGIT_LEDS_ASCII, digits, 4); 01098 } 01099 01100 01101 /** Play a simple fanfare. 01102 * You can play this for example upon connection to inform the user. 01103 */ 01104 void 01105 Roomba500::play_fanfare() 01106 { 01107 unsigned char p[14]; 01108 p[0] = 0; 01109 p[1] = 6; 01110 01111 // C,E,G,G,E,G 01112 p[2] = 72; 01113 p[3] = 6; 01114 01115 p[4] = 76; 01116 p[5] = 6; 01117 01118 p[6] = 79; 01119 p[7] = 8; 01120 01121 p[8] = 79; 01122 p[9] = 10; 01123 01124 p[10] = 76; 01125 p[11] = 8; 01126 01127 p[12] = 79; 01128 p[13] = 8; 01129 01130 unsigned char play; 01131 play = 0; 01132 01133 send(OPCODE_SONG, p, sizeof(p)); 01134 send(OPCODE_PLAY, &play, 1); 01135 } 01136 01137 01138 /** Get size of packet. 01139 * @param packet ID of packet to query size for 01140 * @return size of packet 01141 * @exception Exception thrown for unknown packet IDs. 01142 */ 01143 unsigned short int 01144 Roomba500::get_packet_size(Roomba500::SensorPacketID packet) 01145 { 01146 switch (packet) { 01147 case SENSPACK_BUMPS_DROPS: return SENSPACK_SIZE_BUMPS_DROPS; 01148 case SENSPACK_WALL: return SENSPACK_SIZE_WALL; 01149 case SENSPACK_CLIFF_LEFT: return SENSPACK_SIZE_CLIFF_LEFT; 01150 case SENSPACK_CLIFF_FRONT_LEFT: return SENSPACK_SIZE_CLIFF_FRONT_LEFT; 01151 case SENSPACK_CLIFF_FRONT_RIGHT: return SENSPACK_SIZE_CLIFF_FRONT_RIGHT; 01152 case SENSPACK_CLIFF_RIGHT: return SENSPACK_SIZE_CLIFF_RIGHT; 01153 case SENSPACK_VIRTUAL_WALL: return SENSPACK_SIZE_VIRTUAL_WALL; 01154 case SENSPACK_WHEEL_OVERCURRENTS: return SENSPACK_SIZE_WHEEL_OVERCURRENTS; 01155 case SENSPACK_DIRT_DETECT: return SENSPACK_SIZE_DIRT_DETECT; 01156 case SENSPACK_IR_CHAR_OMNI: return SENSPACK_SIZE_IR_CHAR_OMNI; 01157 case SENSPACK_BUTTONS: return SENSPACK_SIZE_BUTTONS; 01158 case SENSPACK_DISTANCE: return SENSPACK_SIZE_DISTANCE; 01159 case SENSPACK_ANGLE: return SENSPACK_SIZE_ANGLE; 01160 case SENSPACK_CHARGING_STATE: return SENSPACK_SIZE_CHARGING_STATE; 01161 case SENSPACK_VOLTAGE: return SENSPACK_SIZE_VOLTAGE; 01162 case SENSPACK_CURRENT: return SENSPACK_SIZE_CURRENT; 01163 case SENSPACK_TEMPERATURE: return SENSPACK_SIZE_TEMPERATURE; 01164 case SENSPACK_BATTERY_CHARGE: return SENSPACK_SIZE_BATTERY_CHARGE; 01165 case SENSPACK_BATTERY_CAPACITY: return SENSPACK_SIZE_BATTERY_CAPACITY; 01166 case SENSPACK_WALL_SIGNAL: return SENSPACK_SIZE_WALL_SIGNAL; 01167 case SENSPACK_CLIFF_LEFT_SIGNAL: return SENSPACK_SIZE_CLIFF_LEFT_SIGNAL; 01168 case SENSPACK_CLIFF_FRONT_LEFT_SIGNAL: 01169 return SENSPACK_SIZE_CLIFF_FRONT_LEFT_SIGNAL; 01170 case SENSPACK_CLIFF_FRONT_RIGHT_SIGNAL: 01171 return SENSPACK_SIZE_CLIFF_FRONT_RIGHT_SIGNAL; 01172 case SENSPACK_CLIFF_RIGHT_SIGNAL: return SENSPACK_SIZE_CLIFF_RIGHT_SIGNAL; 01173 case SENSPACK_CHARGE_SOURCES: return SENSPACK_SIZE_CHARGE_SOURCES; 01174 case SENSPACK_OI_MODE: return SENSPACK_SIZE_OI_MODE; 01175 case SENSPACK_SONG_NUMBER: return SENSPACK_SIZE_SONG_NUMBER; 01176 case SENSPACK_SONG_PLAYING: return SENSPACK_SIZE_SONG_PLAYING; 01177 case SENSPACK_STREAM_PACKETS: return SENSPACK_SIZE_STREAM_PACKETS; 01178 case SENSPACK_REQ_VELOCITY: return SENSPACK_SIZE_REQ_VELOCITY; 01179 case SENSPACK_REQ_RADIUS: return SENSPACK_SIZE_REQ_RADIUS; 01180 case SENSPACK_REQ_RIGHT_VELOCITY: return SENSPACK_SIZE_REQ_RIGHT_VELOCITY; 01181 case SENSPACK_REQ_LEFT_VELOCITY: return SENSPACK_SIZE_REQ_LEFT_VELOCITY; 01182 case SENSPACK_RIGHT_ENCODER: return SENSPACK_SIZE_RIGHT_ENCODER; 01183 case SENSPACK_LEFT_ENCODER: return SENSPACK_SIZE_LEFT_ENCODER; 01184 case SENSPACK_LIGHT_BUMPER: return SENSPACK_SIZE_LIGHT_BUMPER; 01185 case SENSPACK_LIGHT_BUMPER_LEFT: return SENSPACK_SIZE_LIGHT_BUMPER_LEFT; 01186 case SENSPACK_LIGHT_BUMPER_FRONT_LEFT: 01187 return SENSPACK_SIZE_LIGHT_BUMPER_FRONT_LEFT; 01188 case SENSPACK_LIGHT_BUMPER_CENTER_LEFT: 01189 return SENSPACK_SIZE_LIGHT_BUMPER_CENTER_LEFT; 01190 case SENSPACK_LIGHT_BUMPER_CENTER_RIGHT: 01191 return SENSPACK_SIZE_LIGHT_BUMPER_CENTER_RIGHT; 01192 case SENSPACK_LIGHT_BUMPER_FRONT_RIGHT: 01193 return SENSPACK_SIZE_LIGHT_BUMPER_FRONT_RIGHT; 01194 case SENSPACK_LIGHT_BUMPER_RIGHT: 01195 return SENSPACK_SIZE_LIGHT_BUMPER_RIGHT; 01196 case SENSPACK_IR_CHAR_LEFT: return SENSPACK_SIZE_IR_CHAR_LEFT; 01197 case SENSPACK_IR_CHAR_RIGHT: return SENSPACK_SIZE_IR_CHAR_RIGHT; 01198 case SENSPACK_LEFT_MOTOR_CURRENT: return SENSPACK_SIZE_LEFT_MOTOR_CURRENT; 01199 case SENSPACK_RIGHT_MOTOR_CURRENT: return SENSPACK_SIZE_RIGHT_MOTOR_CURRENT; 01200 case SENSPACK_BRUSH_MOTOR_CURRENT: return SENSPACK_SIZE_BRUSH_MOTOR_CURRENT; 01201 case SENSPACK_SIDE_BRUSH_MOTOR_CURRENT: 01202 return SENSPACK_SIZE_SIDE_BRUSH_MOTOR_CURRENT; 01203 case SENSPACK_STASIS: return SENSPACK_SIZE_STASIS; 01204 case SENSPACK_GROUP_0: return SENSPACK_SIZE_GROUP_0; 01205 case SENSPACK_GROUP_1: return SENSPACK_SIZE_GROUP_1; 01206 case SENSPACK_GROUP_2: return SENSPACK_SIZE_GROUP_2; 01207 case SENSPACK_GROUP_3: return SENSPACK_SIZE_GROUP_3; 01208 case SENSPACK_GROUP_4: return SENSPACK_SIZE_GROUP_4; 01209 case SENSPACK_GROUP_5: return SENSPACK_SIZE_GROUP_5; 01210 case SENSPACK_GROUP_6: return SENSPACK_SIZE_GROUP_6; 01211 case SENSPACK_GROUP_ALL: return SENSPACK_SIZE_GROUP_ALL; 01212 case SENSPACK_GROUP_101: return SENSPACK_SIZE_GROUP_101; 01213 case SENSPACK_GROUP_106: return SENSPACK_SIZE_GROUP_106; 01214 case SENSPACK_GROUP_107: return SENSPACK_SIZE_GROUP_107; 01215 default: 01216 throw Exception("Roomba500:get_packet_size(): unknown packet ID %i", packet); 01217 } 01218 }