Fawkes API  Fawkes Development Version
roomba_500.cpp
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, &param) == -1) {
00298       Exception e(errno, "Getting the port parameters failed");
00299       ::close(__fd);
00300       __fd = -1;
00301       throw e;
00302     }
00303 
00304     cfsetospeed(&param, B115200);
00305     cfsetispeed(&param, 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, &param) != 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, &param, 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 }