Fawkes API  Fawkes Development Version
roomba_500.cpp
1 
2 /***************************************************************************
3  * roomba_500.cpp - Roomba Open Interface implementation for 500 series
4  *
5  * Created: Sat Jan 01 19:37:13 2011
6  * Copyright 2006-2010 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "roomba_500.h"
24 
25 #include <core/exceptions/system.h>
26 #include <core/threading/mutex.h>
27 #include <core/threading/mutex_locker.h>
28 
29 #include <cstring>
30 #include <cstdlib>
31 #include <cmath>
32 #include <termios.h>
33 #include <fcntl.h>
34 #include <errno.h>
35 #include <netinet/in.h>
36 #include <unistd.h>
37 
38 #ifdef HAVE_BLUEZ
39 # include <bluetooth/bluetooth.h>
40 # include <bluetooth/hci.h>
41 # include <bluetooth/hci_lib.h>
42 # include <bluetooth/rfcomm.h>
43 # include <sys/socket.h>
44 # include <fnmatch.h>
45 
46 static const bdaddr_t _BDADDR_ANY = {{0, 0, 0, 0, 0, 0}};
47 #endif
48 
49 using namespace fawkes;
50 
51 /// @cond SELFEXPLAINING
52 const unsigned char Roomba500::BUTTON_CLEAN = 1;
53 const unsigned char Roomba500::BUTTON_SPOT = 2;
54 const unsigned char Roomba500::BUTTON_DOCK = 4;
55 const unsigned char Roomba500::BUTTON_MINUTE = 8;
56 const unsigned char Roomba500::BUTTON_HOUR = 16;
57 const unsigned char Roomba500::BUTTON_DAY = 32;
58 const unsigned char Roomba500::BUTTON_SCHEDULE = 64;
59 const unsigned char Roomba500::BUTTON_CLOCK = 128;
60 
61 const unsigned char Roomba500::WHEEL_DROP_LEFT = 8;
62 const unsigned char Roomba500::WHEEL_DROP_RIGHT = 4;
63 const unsigned char Roomba500::BUMP_LEFT = 2;
64 const unsigned char Roomba500::BUMP_RIGHT = 1;
65 
66 const unsigned char Roomba500::OVERCURRENT_WHEEL_LEFT = 16;
67 const unsigned char Roomba500::OVERCURRENT_WHEEL_RIGHT = 8;
68 const unsigned char Roomba500::OVERCURRENT_MAIN_BRUSH = 4;
69 const unsigned char Roomba500::OVERCURRENT_SIDE_BRUSH = 1;
70 
71 const unsigned char Roomba500::CHARGING_SOURCE_HOME_BASE = 2;
72 const unsigned char Roomba500::CHARGING_SOURCE_INTERNAL = 1;
73 
74 const unsigned char Roomba500::BUMPER_LEFT = 1;
75 const unsigned char Roomba500::BUMPER_FRONT_LEFT = 2;
76 const unsigned char Roomba500::BUMPER_CENTER_LEFT = 4;
77 const unsigned char Roomba500::BUMPER_CENTER_RIGHT = 8;
78 const unsigned char Roomba500::BUMPER_FRONT_RIGHT = 16;
79 const unsigned char Roomba500::BUMPER_RIGHT = 32;
80 
81 const unsigned char Roomba500::LED_DEBRIS = 1;
82 const unsigned char Roomba500::LED_SPOT = 2;
83 const unsigned char Roomba500::LED_DOCK = 4;
84 const unsigned char Roomba500::LED_CHECK_ROBOT = 8;
85 
86 const unsigned char Roomba500::WEEKDAY_LED_SUN = 1;
87 const unsigned char Roomba500::WEEKDAY_LED_MON = 2;
88 const unsigned char Roomba500::WEEKDAY_LED_TUE = 4;
89 const unsigned char Roomba500::WEEKDAY_LED_WED = 8;
90 const unsigned char Roomba500::WEEKDAY_LED_THU = 16;
91 const unsigned char Roomba500::WEEKDAY_LED_FRI = 32;
92 const unsigned char Roomba500::WEEKDAY_LED_SAT = 64;
93 
94 const unsigned char Roomba500::SCHEDULING_LED_COLON = 1;
95 const unsigned char Roomba500::SCHEDULING_LED_PM = 2;
96 const unsigned char Roomba500::SCHEDULING_LED_AM = 4;
97 const unsigned char Roomba500::SCHEDULING_LED_CLOCK = 8;
98 const unsigned char Roomba500::SCHEDULING_LED_SCHEDULE = 16;
99 
100 const unsigned char Roomba500::DIGIT_LED_NORTH = 1;
101 const unsigned char Roomba500::DIGIT_LED_NORTH_WEST = 32;
102 const unsigned char Roomba500::DIGIT_LED_NORTH_EAST = 2;
103 const unsigned char Roomba500::DIGIT_LED_CENTER = 64;
104 const unsigned char Roomba500::DIGIT_LED_SOUTH_WEST = 16;
105 const unsigned char Roomba500::DIGIT_LED_SOUTH_EAST = 4;
106 const unsigned char Roomba500::DIGIT_LED_SOUTH = 8;
107 
108 const unsigned char Roomba500::MOTOR_SIDE_BRUSH = 1;
109 const unsigned char Roomba500::MOTOR_VACUUM = 2;
110 const unsigned char Roomba500::MOTOR_MAIN_BRUSHES = 4;
111 const unsigned char Roomba500::MOTOR_SIDE_BRUSH_BACKWARD = 8;
112 const unsigned char Roomba500::MOTOR_MAIN_BRUSHES_BACKWARD = 16;
113 
114 const unsigned char Roomba500::CHARGER_HOME_BASE = 2;
115 const unsigned char Roomba500::CHARGER_INTERNAL = 1;
116 
117 const unsigned short int Roomba500::SENSPACK_SIZE_GROUP_0 = 26;
118 const unsigned short int Roomba500::SENSPACK_SIZE_GROUP_1 = 10;
119 const unsigned short int Roomba500::SENSPACK_SIZE_GROUP_2 = 6;
120 const unsigned short int Roomba500::SENSPACK_SIZE_GROUP_3 = 10;
121 const unsigned short int Roomba500::SENSPACK_SIZE_GROUP_4 = 14;
122 const unsigned short int Roomba500::SENSPACK_SIZE_GROUP_5 = 12;
123 const unsigned short int Roomba500::SENSPACK_SIZE_GROUP_6 = 52;
124 const unsigned short int Roomba500::SENSPACK_SIZE_GROUP_ALL = 80;
125 const unsigned short int Roomba500::SENSPACK_SIZE_GROUP_101 = 28;
126 const unsigned short int Roomba500::SENSPACK_SIZE_GROUP_106 = 12;
127 const unsigned short int Roomba500::SENSPACK_SIZE_GROUP_107 = 9;
128 const unsigned short int Roomba500::SENSPACK_SIZE_BUMPS_DROPS = 1;
129 const unsigned short int Roomba500::SENSPACK_SIZE_WALL = 1;
130 const unsigned short int Roomba500::SENSPACK_SIZE_CLIFF_LEFT = 1;
131 const unsigned short int Roomba500::SENSPACK_SIZE_CLIFF_FRONT_LEFT = 1;
132 const unsigned short int Roomba500::SENSPACK_SIZE_CLIFF_FRONT_RIGHT = 1;
133 const unsigned short int Roomba500::SENSPACK_SIZE_CLIFF_RIGHT = 1;
134 const unsigned short int Roomba500::SENSPACK_SIZE_VIRTUAL_WALL = 1;
135 const unsigned short int Roomba500::SENSPACK_SIZE_WHEEL_OVERCURRENTS = 1;
136 const unsigned short int Roomba500::SENSPACK_SIZE_DIRT_DETECT = 1;
137 const unsigned short int Roomba500::SENSPACK_SIZE_IR_CHAR_OMNI = 1;
138 const unsigned short int Roomba500::SENSPACK_SIZE_IR_CHAR_LEFT = 1;
139 const unsigned short int Roomba500::SENSPACK_SIZE_IR_CHAR_RIGHT = 1;
140 const unsigned short int Roomba500::SENSPACK_SIZE_BUTTONS = 1;
141 const unsigned short int Roomba500::SENSPACK_SIZE_DISTANCE = 2;
142 const unsigned short int Roomba500::SENSPACK_SIZE_ANGLE = 2;
143 const unsigned short int Roomba500::SENSPACK_SIZE_CHARGING_STATE = 1;
144 const unsigned short int Roomba500::SENSPACK_SIZE_VOLTAGE = 2;
145 const unsigned short int Roomba500::SENSPACK_SIZE_CURRENT = 2;
146 const unsigned short int Roomba500::SENSPACK_SIZE_TEMPERATURE = 1;
147 const unsigned short int Roomba500::SENSPACK_SIZE_BATTERY_CHARGE = 2;
148 const unsigned short int Roomba500::SENSPACK_SIZE_BATTERY_CAPACITY = 2;
149 const unsigned short int Roomba500::SENSPACK_SIZE_WALL_SIGNAL = 2;
150 const unsigned short int Roomba500::SENSPACK_SIZE_CLIFF_LEFT_SIGNAL = 2;
151 const unsigned short int Roomba500::SENSPACK_SIZE_CLIFF_FRONT_LEFT_SIGNAL = 2;
152 const unsigned short int Roomba500::SENSPACK_SIZE_CLIFF_FRONT_RIGHT_SIGNAL= 2;
153 const unsigned short int Roomba500::SENSPACK_SIZE_CLIFF_RIGHT_SIGNAL = 2;
154 const unsigned short int Roomba500::SENSPACK_SIZE_CHARGE_SOURCES = 1;
155 const unsigned short int Roomba500::SENSPACK_SIZE_OI_MODE = 1;
156 const unsigned short int Roomba500::SENSPACK_SIZE_SONG_NUMBER = 1;
157 const unsigned short int Roomba500::SENSPACK_SIZE_SONG_PLAYING = 1;
158 const unsigned short int Roomba500::SENSPACK_SIZE_STREAM_PACKETS = 1;
159 const unsigned short int Roomba500::SENSPACK_SIZE_REQ_VELOCITY = 2;
160 const unsigned short int Roomba500::SENSPACK_SIZE_REQ_RADIUS = 2;
161 const unsigned short int Roomba500::SENSPACK_SIZE_REQ_RIGHT_VELOCITY = 2;
162 const unsigned short int Roomba500::SENSPACK_SIZE_REQ_LEFT_VELOCITY = 2;
163 const unsigned short int Roomba500::SENSPACK_SIZE_RIGHT_ENCODER = 2;
164 const unsigned short int Roomba500::SENSPACK_SIZE_LEFT_ENCODER = 2;
165 const unsigned short int Roomba500::SENSPACK_SIZE_LIGHT_BUMPER = 1;
166 const unsigned short int Roomba500::SENSPACK_SIZE_LIGHT_BUMPER_LEFT = 2;
167 const unsigned short int Roomba500::SENSPACK_SIZE_LIGHT_BUMPER_FRONT_LEFT = 2;
168 const unsigned short int Roomba500::SENSPACK_SIZE_LIGHT_BUMPER_CENTER_LEFT = 2;
169 const unsigned short int Roomba500::SENSPACK_SIZE_LIGHT_BUMPER_CENTER_RIGHT = 2;
170 const unsigned short int Roomba500::SENSPACK_SIZE_LIGHT_BUMPER_FRONT_RIGHT = 2;
171 const unsigned short int Roomba500::SENSPACK_SIZE_LIGHT_BUMPER_RIGHT = 2;
172 const unsigned short int Roomba500::SENSPACK_SIZE_LEFT_MOTOR_CURRENT = 2;
173 const unsigned short int Roomba500::SENSPACK_SIZE_RIGHT_MOTOR_CURRENT = 2;
174 const unsigned short int Roomba500::SENSPACK_SIZE_BRUSH_MOTOR_CURRENT = 2;
175 const unsigned short int Roomba500::SENSPACK_SIZE_SIDE_BRUSH_MOTOR_CURRENT = 2;
176 const unsigned short int Roomba500::SENSPACK_SIZE_STASIS = 1;
177 
178 const float Roomba500::DIAMETER = 0.33;
179 const float Roomba500::BUMPER_X_OFFSET = 0.05;
180 const float Roomba500::AXLE_LENGTH = 0.235;
181 
182 const short int Roomba500::MAX_LIN_VEL_MM_S = 500;
183 const short int Roomba500::MAX_RADIUS_MM = 2000;
184 
185 const unsigned short int Roomba500::MAX_ENCODER_COUNT = 65535;
186 const short int Roomba500::MAX_PWM = 255;
187 
188 const unsigned short int Roomba500::STREAM_INTERVAL_MS = 15;
189 const unsigned short int Roomba500::MODE_CHANGE_WAIT_MS = 20;
190 
191 const unsigned char Roomba500::CHECKSUM_SIZE = 1;
192 
193 /// @endcond
194 
195 /// @cond INTERNALS
196 #pragma pack(push,1)
197 
198 typedef struct {
199  int16_t velocity;
200  int16_t radius;
201 } DriveParams;
202 
203 typedef struct {
204  int16_t left_velocity;
205  int16_t right_velocity;
206 } DriveDirectParams;
207 
208 typedef struct {
209  int16_t left_pwm;
210  int16_t right_pwm;
211 } DrivePWMParams;
212 
213 typedef struct {
214  uint8_t num_packets;
215  uint8_t packet_id;
216 } StreamOnePacketParams;
217 
218 #pragma pack(pop)
219 /// @endcond
220 
221 
222 /** @class Roomba500 "roomba_500.h"
223  * Roomba 500 series communication class.
224  * This class implements the serial communication with Roomba robots of the
225  * 500 series.
226  *
227  * RFCOMM by reading http://people.csail.mit.edu/albert/bluez-intro/.
228  *
229  * @author Tim Niemueller
230  */
231 
232 /** Constructor.
233  * @param conntype connection type
234  * @param device for CONN_SERIAL connection type this is the device file for
235  * the serial connection. For CONN_ROOTOOTH this is either a name pattern
236  * of a bluetooth device to query or a bluetooth address. The name can be the
237  * full name, or a pattern using shell globs (e.g. FireFly-*). The bluetooth
238  * address must be given in hexadecimal manner (e.g. 00:11:22:33:44:55).
239  * @param flags ConnectionFlags constants, joined with bit-wise "or" (|).
240  */
241 Roomba500::Roomba500(Roomba500::ConnectionType conntype, const char *device,
242  unsigned int flags)
243 {
244  __conntype = conntype;
245  __conn_flags = flags;
246 #ifndef HAVE_BLUEZ
247  if (__conntype == CONNTYPE_ROOTOOTH) {
248  throw Exception("Native RooTooth not available at compile time.");
249  }
250 #endif
251  __mode = MODE_OFF;
252  __fd = -1;
253  __packet_id = SENSPACK_GROUP_ALL;
254  __sensors_enabled = false;
255 
256  __device = strdup(device);
257 
258  __sensor_mutex = new Mutex();
259  __read_mutex = new Mutex();
260  __write_mutex = new Mutex();
261 
262  try {
263  open();
264  } catch (Exception &e) {
265  free(__device);
266  delete __write_mutex;
267  delete __read_mutex;
268  delete __sensor_mutex;
269  throw;
270  }
271 }
272 
273 
274 /** Destructor. */
276 {
277  close();
278  free(__device);
279  delete __write_mutex;
280  delete __read_mutex;
281  delete __sensor_mutex;
282 }
283 
284 
285 /** Open serial port. */
286 void
288 {
289  if (__conntype == CONNTYPE_SERIAL) {
290  struct termios param;
291 
292  __fd = ::open(__device, O_NOCTTY | O_RDWR);
293  if (__fd == -1) {
294  throw CouldNotOpenFileException(__device, errno, "Cannot open device file");
295  }
296 
297  if (tcgetattr(__fd, &param) == -1) {
298  Exception e(errno, "Getting the port parameters failed");
299  ::close(__fd);
300  __fd = -1;
301  throw e;
302  }
303 
304  cfsetospeed(&param, B115200);
305  cfsetispeed(&param, B115200);
306 
307  param.c_lflag &= ~(ICANON | ECHO | ECHOE | ECHOK | ECHONL | ISIG | IEXTEN);
308  param.c_iflag &= ~(INLCR | IGNCR | ICRNL | IGNBRK | PARMRK);
309 
310  // turn off hardware flow control
311  param.c_iflag &= ~(IXON | IXOFF | IXANY);
312  param.c_cflag &= ~CRTSCTS;
313 
314  param.c_cflag |= (CREAD | CLOCAL);
315 
316  // number of data bits: 8
317  param.c_cflag &= ~CS5 & ~CS6 & ~CS7 & ~CS8;
318  param.c_cflag |= CS8;
319 
320  // parity: none
321  param.c_cflag &= ~(PARENB | PARODD);
322  param.c_iflag &= ~(INPCK | ISTRIP);
323 
324  // stop bits: 1
325  param.c_cflag &= ~CSTOPB;
326 
327  //enable raw output
328  param.c_oflag &= ~OPOST;
329 
330  param.c_cc[VMIN] = 1;
331  param.c_cc[VTIME] = 0;
332 
333  tcflush(__fd, TCIOFLUSH);
334 
335  if (tcsetattr(__fd, TCSANOW, &param) != 0) {
336  Exception e(errno, "Setting the port parameters failed");
337  ::close(__fd);
338  __fd = -1;
339  throw e;
340  }
341  } else {
342 #ifndef HAVE_BLUEZ
343  throw Exception("Native RooTooth support unavailable at compile time.");
344 #else
345  struct hci_dev_info di;
346  inquiry_info *ii = NULL;
347  int max_rsp, num_rsp;
348  int dev_id, sock, len, flags;
349  char addrstr[19] = { 0 };
350  char name[248] = { 0 };
351  bdaddr_t baddr = _BDADDR_ANY;
352 
353  if ((dev_id = hci_get_route(NULL)) < 0) {
354  throw Exception("RooTooth: local bluetooth device is not available");
355  }
356 
357  if (hci_devinfo(dev_id, &di) < 0) {
358  throw Exception("RooTooth: cannot get local device info.");
359  }
360 
361  if ((sock = hci_open_dev(dev_id)) < 0) {
362  throw Exception("RooTooth: failed to open socket.");
363  }
364 
365  len = 8;
366  max_rsp = 255;
367  flags = IREQ_CACHE_FLUSH;
368  ii = (inquiry_info*)malloc(max_rsp * sizeof(inquiry_info));
369 
370  if (strcmp(__device, "") == 0) {
371  // we simply guess from the device class
372 
373  num_rsp = hci_inquiry(dev_id, len, max_rsp, NULL, &ii, flags);
374  if (num_rsp < 0) {
375  throw Exception(errno, "HCI inquiry failed");
376  }
377 
378  for (int i = 0; i < num_rsp; i++) {
379 
380  uint8_t b[6];
381  baswap((bdaddr_t *) b, &(ii+i)->bdaddr);
382 
383  ba2str(&(ii+i)->bdaddr, addrstr);
384  /*
385  printf("Comparing (0x%2.2x%2.2x%2.2x) (0x%2.2x%2.2x%2.2x) %s %s\n",
386  b[0], b[1], b[2],
387  ii[i].dev_class[0], ii[i].dev_class[1], ii[i].dev_class[2],
388  name, addrstr);
389  */
390 
391  // This checks for the RooTooth I have which identifies itself as
392  // FireFly-XXXX, where XXXX are the last four digits of the BT addr
393  if (// check OUI for Roving Networks
394  (b[0] == 0x00) && (b[1] == 0x06) && (b[2] == 0x66) &&
395 
396  // check device class
397  (ii[i].dev_class[0] == 0x00) &&
398  (ii[i].dev_class[1] == 0x1f) &&
399  (ii[i].dev_class[2] == 0x00) )
400  {
401  // verify the name
402  memset(name, 0, sizeof(name));
403  hci_read_remote_name(sock, &(ii+i)->bdaddr, sizeof(name), name, 0);
404 
405  if ( // Now check the advertised name
406  (fnmatch("FireFly-*", name, FNM_NOESCAPE) == 0) ||
407  (strcmp("RooTooth", name) == 0) )
408  {
409  // found a device which is likely a
410  ba2str(&(ii+i)->bdaddr, addrstr);
411  //printf("found A: %s %s\n", addrstr, name);
412  free(__device);
413  __device = strdup(addrstr);
414  bacpy(&baddr, &(ii+i)->bdaddr);
415  break;
416  }
417  }
418  }
419  } else {
420  bool is_bdaddr = (bachk(__device) == 0);
421 
422  if (is_bdaddr) {
423  //printf("Match by bdaddr\n");
424 
425  str2ba(__device, &baddr);
426  ba2str(&baddr, addrstr);
427 
428  //printf("found B: %s %s\n", addrstr, name);
429  } else {
430  //printf("Match by btname\n");
431 
432  num_rsp = hci_inquiry(dev_id, len, max_rsp, NULL, &ii, flags);
433  if (num_rsp < 0) {
434  throw Exception(errno, "HCI inquiry failed");
435  }
436 
437  // we have a name pattern to check
438  for (int i = 0; i < num_rsp; i++) {
439  ba2str(&(ii+i)->bdaddr, addrstr);
440  memset(name, 0, sizeof(name));
441  if (hci_read_remote_name(sock, &(ii+i)->bdaddr, sizeof(name),
442  name, 0) < 0)
443  {
444  strcpy(name, "[unknown]");
445  }
446  if (fnmatch(__device, name, FNM_NOESCAPE) == 0) {
447  // found the device
448  //printf("found C: %s %s\n", addrstr, name);
449  free(__device);
450  __device = strdup(addrstr);
451  bacpy(&baddr, &(ii+i)->bdaddr);
452  break;
453  }
454  }
455  }
456  }
457 
458  free(ii);
459  ::close(sock);
460 
461  if (bacmp(&baddr, &_BDADDR_ANY) == 0) {
462  throw Exception("Could not find RooTooth device.");
463  }
464  ba2str(&baddr, addrstr);
465 
466  // connect via RFCOMM
467  struct sockaddr_rc rcaddr = { 0 };
468 
469  // allocate a socket
470  __fd = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
471 
472  // set the connection parameters (who to connect to)
473  rcaddr.rc_family = AF_BLUETOOTH;
474  rcaddr.rc_channel = (uint8_t) 1;
475  bacpy(&rcaddr.rc_bdaddr, &baddr);
476 
477  // connect to server
478  if (connect(__fd, (struct sockaddr *)&rcaddr, sizeof(rcaddr)) < 0) {
479  throw Exception(errno, "Failed to connect to %s", addrstr);
480  }
481 
482  try {
483  // Set to passive mode to ensure that auto-detection does no harm
484  send(OPCODE_START);
485  usleep(MODE_CHANGE_WAIT_MS * 1000);
486  __mode = MODE_PASSIVE;
487  // disable sensors just in case
488  disable_sensors();
489  } catch (Exception &e) {
490  ::close(__fd);
491  __mode = MODE_OFF;
492  throw;
493  }
494 
495  if (flags & FLAG_FIREFLY_FASTMODE) {
496  const char *cmd_seq = "$$$";
497  if (write(__fd, cmd_seq, 3) != 3) {
498  ::close(__fd);
499  throw Exception(errno, "Roomba500 (RooTooth): Failed to send command "
500  "sequence to enable fast mode");
501  }
502 
503  timeval timeout = {1, 500000};
504  fd_set read_fds;
505  FD_ZERO(&read_fds);
506  FD_SET(__fd, &read_fds);
507 
508  int rv = 0;
509  rv = select(__fd + 1, &read_fds, NULL, NULL, &timeout);
510 
511  if (rv > 0) {
512  char cmd_reply[4];
513  if (read(__fd, cmd_reply, 4) == 4) {
514  if (strncmp(cmd_reply, "CMD", 3) == 0) {
515  // We entered command mode, enable fast mode
516  const char *cmd_fastmode = "F,1\r";
517  if (write(__fd, cmd_fastmode, 4) != 4) {
518  ::close(__fd);
519  throw Exception(errno, "Roomba500 (RooTooth): Failed to send fast "
520  "mode command sequence.");
521  } // else fast mode enabled
522  } // else invalid reply, again assume fast mode
523  } // assume fast mode
524  } // else assume already enabled fast mode
525  } // else do not enable fast mode, assume user knows what he is doing
526 
527 #endif
528  }
529 
530  try {
531  send(OPCODE_START);
532  usleep(MODE_CHANGE_WAIT_MS * 1000);
533  __mode = MODE_PASSIVE;
534  } catch (Exception &e) {
535  ::close(__fd);
536  __mode = MODE_OFF;
537  throw;
538  }
539 
540 }
541 
542 
543 /** Close serial connection. */
544 void
546 {
547  if (__fd >= 0) {
548  ::close(__fd);
549  __fd = -1;
550  }
551  __mode = MODE_OFF;
552 }
553 
554 /** Send instruction packet.
555  * @param opcode Opcode of command
556  * @param params parameter bytes of the message
557  * @param plength length of the params array
558  */
559 void
560 Roomba500::send(Roomba500::OpCode opcode,
561  const void *params, const size_t plength)
562 {
563  MutexLocker write_lock(__write_mutex);
564 
565  // Byte 0 and 1 must be 0xFF
566  __obuffer[0] = opcode;
567  __obuffer_length = 1;
568 
569  if (params && (plength > 0)) {
570  if (plength > (sizeof(__obuffer) - __obuffer_length)) {
571  throw Exception("Parameters for command %i too long, maximum length is %zu",
572  opcode, (sizeof(__obuffer) - __obuffer_length));
573  }
574  unsigned char *pbytes = (unsigned char *)params;
575  for (size_t i = 0; i < plength; ++i) {
576  __obuffer[1+i] = pbytes[i];
577  }
578  __obuffer_length += plength;
579  }
580 
581  int written = write(__fd, __obuffer, __obuffer_length);
582 
583  /*
584  printf("Wrote %i of %i bytes:\n", written, __obuffer_length);
585  for (int i = 0; i < __obuffer_length; ++i) {
586  printf("%2u %s", __obuffer[i], i == written ? "| " : "");
587  }
588  printf("\n");
589  */
590 
591  if ( written < 0 ) {
592  throw Exception(errno, "Failed to write Roomba 500 packet %i", opcode);
593  } else if (written < __obuffer_length) {
594  throw Exception("Failed to write Roomba 500 packet %i, "
595  "only %d of %d bytes sent",
596  opcode, written, __obuffer_length);
597  }
598 }
599 
600 
601 /** Receive a packet.
602  * @param index index in ibuffer to fill
603  * @param num_bytes number of bytes to read
604  * @param timeout_ms maximum wait time in miliseconds, 0 to wait indefinitely.
605  */
606 void
607 Roomba500::recv(size_t index, size_t num_bytes, unsigned int timeout_ms)
608 {
609  timeval timeout = {0, (suseconds_t)timeout_ms * 1000};
610 
611  fd_set read_fds;
612  FD_ZERO(&read_fds);
613  FD_SET(__fd, &read_fds);
614 
615  int rv = 0;
616  rv = select(__fd + 1, &read_fds, NULL, NULL, (timeout_ms > 0) ? &timeout : NULL);
617 
618  if ( rv == -1 ) {
619  throw Exception(errno, "Roomba500::recv(): select on file descriptor failed");
620  } else if ( rv == 0 ) {
621  throw TimeoutException("Timeout while waiting for incoming Roomba data");
622  }
623 
624  __ibuffer_length = 0;
625 
626  // get octets one by one
627  int bytes_read = 0;
628  while (bytes_read < (int)num_bytes) {
629  int rv = read(__fd, &__ibuffer[index] +bytes_read, num_bytes -bytes_read);
630  if (rv == -1) {
631  throw Exception(errno, "Roomba500::recv(): read failed");
632  }
633  bytes_read += rv;
634  }
635 
636  if (bytes_read < (int)num_bytes) {
637  throw Exception("Roomba500::recv(): failed to read packet data");
638  }
639 
640  __ibuffer_length = index + num_bytes;
641 }
642 
643 
644 /** Check if data is available.
645  * @return true if data is available and can be read, false otherwise
646  */
647 bool
649 {
650  if (!__sensors_enabled) {
651  throw Exception("Roomba 500 sensors have not been enabled.");
652  }
653 
654  timeval timeout = {0, 0};
655 
656  fd_set read_fds;
657  FD_ZERO(&read_fds);
658  FD_SET(__fd, &read_fds);
659 
660  int rv = 0;
661  rv = select(__fd + 1, &read_fds, NULL, NULL, &timeout);
662 
663  return (rv > 0);
664 }
665 
666 
667 /** Read sensor values.
668  * Enable sensors before using enable_sensors(). This method will block until new
669  * data has been read. You can call is_data_available() to check if this method
670  * will block or not.
671  */
672 void
674 {
675  MutexLocker read_lock(__read_mutex);
676 
677  if (!__sensors_enabled) {
678  throw Exception("Roomba 500 sensors have not been enabled.");
679  }
680 
681  bool done = false;
682  unsigned int skipped = 0;
683  while (!done) {
684  __ibuffer_length = 0;
685 
686  recv(__ibuffer_length, 1);
687  if (__ibuffer[0] != 19) {
688  ++skipped;
689  continue;
690  }
691 
692  recv(__ibuffer_length, 1);
693  if (__ibuffer[1] != __packet_length + 1) {
694  ++skipped;
695  continue;
696  }
697 
698  recv(__ibuffer_length, 1);
699  if (__ibuffer[2] != __packet_id) {
700  ++skipped;
701  continue;
702  }
703 
704  recv(__ibuffer_length, __packet_length);
705 
706  recv(__ibuffer_length++, 1);
707 
708  unsigned int sum = 0;
709  for (int i = 0; i < __ibuffer_length; ++i) {
710  sum += __ibuffer[i];
711  }
712 
713  if ((sum & 0xFF) != 0) {
714  __sensor_packet_received = false;
715  } else {
716  __sensor_mutex->lock();
717  memcpy(&__sensor_packet, &__ibuffer[3], sizeof(SensorPacketGroupAll));
718  __sensor_packet_received = true;
719  __sensor_mutex->unlock();
720  }
721 
722  done = true;
723  }
724 
725  /*
726  printf("Read %u bytes: ", __ibuffer_length);
727  for (int i = 0; i < __ibuffer_length; ++i) {
728  printf("%2u ", __ibuffer[i]);
729  }
730  printf(" (skipped %u)\n", skipped);
731  */
732 }
733 
734 /** Enable sensor data stream.
735  * For simplicity and efficiency only the single SENSPACK_GROUP_ALL packet can
736  * be streamed at this time. Make sure that the used connection is fast enough.
737  * 56700 bit/sec should suffice, but 115200 is strongly recommended. If using
738  * RooTooth make sure to use it in fast mode.
739  */
740 void
742 {
743  assert_connected();
744 
745  StreamOnePacketParams sp;
746  sp.num_packets = 1;
747  sp.packet_id = SENSPACK_GROUP_ALL;
748 
749  send(OPCODE_STREAM, &sp, sizeof(StreamOnePacketParams));
750 
751  __packet_id = SENSPACK_GROUP_ALL;
752  __packet_reply_id = 19;
753  __packet_length = get_packet_size(SENSPACK_GROUP_ALL);
754  __sensors_enabled = true;
755  __sensor_packet_received = false;
756 }
757 
758 
759 /** Disable sensor data stream. */
760 void
762 {
763  assert_connected();
764 
765  unsigned char streamstate = STREAM_DISABLE;
766 
767  send(OPCODE_PAUSE_RESUME_STREAM, &streamstate, 1);
768 
769  __sensors_enabled = false;
770  __sensor_packet_received = false;
771 }
772 
773 
774 /** Query sensor once.
775  * For simplicity and efficiency only the single SENSPACK_GROUP_ALL packet can
776  * be streamed at this time.
777  */
778 void
780 {
781  assert_connected();
782 
783  unsigned char p = SENSPACK_GROUP_ALL;
784 
785  send(OPCODE_QUERY, &p, 1);
786 
787  __packet_id = SENSPACK_GROUP_ALL;
788  __packet_reply_id = 0;
789  __packet_length = get_packet_size(SENSPACK_GROUP_ALL);
790  __sensor_packet_received = true;
791 
792 
793  __read_mutex->lock();
794  recv(0, __packet_length);
795  __read_mutex->unlock();
796 
797  __sensor_mutex->lock();
798  memcpy(&__sensor_packet, __ibuffer, sizeof(SensorPacketGroupAll));
799  __sensor_mutex->unlock();
800 }
801 
802 
803 /** Get latest sensor packet.
804  * @return sensor packet
805  */
808 {
809  MutexLocker lock(__sensor_mutex);
810  if (! __sensor_packet_received) {
811  throw Exception("No valid data received, yet.");
812  }
813 
814  return __sensor_packet;
815 }
816 
817 /** Set control mode.
818  * @param mode the mode can be either MODE_FULL or MODE_SAFE (recommended).
819  * MODE_OFF and MODE_PASSIVE cannot be set explicitly. To enter MODE_PASSIVE
820  * issue a command which transitions mode, like clean() or seek_dock().
821  * @exception Exception thrown if an invalid mode is passed
822  */
823 void
825 {
826  if (mode == MODE_PASSIVE) {
827  send(OPCODE_START);
828  } if (mode == MODE_SAFE) {
829  send(OPCODE_SAFE);
830  } else if (mode == MODE_FULL) {
831  send(OPCODE_FULL);
832  } else if (mode == MODE_OFF) {
833  throw Exception("Mode OFF cannot be set, use power_down() instead");
834  }
835 
836  usleep(MODE_CHANGE_WAIT_MS * 1000);
837  __mode = mode;
838 }
839 
840 
841 /** Start normal cleaning operation.
842  * Transitions to passive mode.
843  */
844 void
846 {
847  send(OPCODE_CLEAN);
848 
849  __mode = MODE_PASSIVE;
850 }
851 
852 
853 /** Start spot cleaning operation.
854  * Transitions to passive mode.
855  */
856 void
858 {
859  send(OPCODE_SPOT);
860 
861  __mode = MODE_PASSIVE;
862 }
863 
864 
865 /** Seek for the home base and dock.
866  * Transitions to passive mode.
867  */
868 void
870 {
871  assert_connected();
872 
873  send(OPCODE_SEEK_DOCK);
874  __mode = MODE_PASSIVE;
875 }
876 
877 
878 /** Powers down the Roomba.
879  * Transitions to passive mode.
880  */
881 void
883 {
884  assert_connected();
885 
886  send(OPCODE_POWER);
887  __mode = MODE_PASSIVE;
888 }
889 
890 
891 /** Stop moption of the Roomba.
892  * Available only in safe or full mode.
893  */
894 void
896 {
897  assert_control();
898  drive_pwm(0, 0);
899 }
900 
901 
902 
903 /** Drive Roomba straight.
904  * Available only in safe or full mode.
905  * @param velo_mm_per_sec desired velocity in m/sec
906  */
907 void
908 Roomba500::drive_straight(short int velo_mm_per_sec)
909 {
910  assert_control();
911 
912  if (velo_mm_per_sec < -MAX_LIN_VEL_MM_S) velo_mm_per_sec = -MAX_LIN_VEL_MM_S;
913  if (velo_mm_per_sec > MAX_LIN_VEL_MM_S) velo_mm_per_sec = MAX_LIN_VEL_MM_S;
914 
915  DriveParams dp;
916  dp.velocity = htons(velo_mm_per_sec);
917  dp.radius = htons(0x8000);
918 
919  send(OPCODE_DRIVE, &dp, sizeof(DriveParams));
920 }
921 
922 /** Turn robot on the spot.
923  * Available only in safe or full mode.
924  * @param direction turning direction
925  */
926 void
928 {
929  assert_control();
930 
931  DriveParams dp;
932  dp.velocity = htons(0);
933  dp.radius = (direction == TURN_CLOCKWISE) ? -1 : 1;
934 
935  send(OPCODE_DRIVE, &dp, sizeof(DriveParams));
936 }
937 
938 
939 /** Drive Roomba on an arc.
940  * Available only in safe or full mode.
941  * @param velo_mm_per_sec desired velocity in m/sec
942  * @param radius_mm desired radius of arc in m
943  */
944 void
945 Roomba500::drive_arc(short int velo_mm_per_sec, short int radius_mm)
946 {
947  assert_control();
948 
949  if (velo_mm_per_sec < -MAX_LIN_VEL_MM_S) velo_mm_per_sec = -MAX_LIN_VEL_MM_S;
950  if (velo_mm_per_sec > MAX_LIN_VEL_MM_S) velo_mm_per_sec = MAX_LIN_VEL_MM_S;
951 
952  if (radius_mm < -MAX_RADIUS_MM) radius_mm = -MAX_RADIUS_MM;
953  if (radius_mm > MAX_RADIUS_MM) radius_mm = MAX_RADIUS_MM;
954 
955  DriveParams dp;
956  dp.velocity = htons(velo_mm_per_sec);
957  dp.radius = htons(radius_mm);
958 
959  send(OPCODE_DRIVE, &dp, sizeof(DriveParams));
960 }
961 
962 /** Drive Roomba.
963  * Available only in safe or full mode.
964  * @param velo_mm_per_sec desired velocity in m/sec
965  * @param radius_mm desired radius of arc in m
966  */
967 void
968 Roomba500::drive(short int velo_mm_per_sec, short int radius_mm)
969 {
970  assert_control();
971 
972  if (velo_mm_per_sec < -MAX_LIN_VEL_MM_S) velo_mm_per_sec = -MAX_LIN_VEL_MM_S;
973  if (velo_mm_per_sec > MAX_LIN_VEL_MM_S) velo_mm_per_sec = MAX_LIN_VEL_MM_S;
974 
975  if (radius_mm < -MAX_RADIUS_MM) radius_mm = -MAX_RADIUS_MM;
976  if (radius_mm > MAX_RADIUS_MM) radius_mm = 0x8000; // drive straight
977 
978  DriveParams dp;
979  dp.velocity = htons(velo_mm_per_sec);
980  dp.radius = htons(radius_mm);
981 
982  send(OPCODE_DRIVE, &dp, sizeof(DriveParams));
983 }
984 
985 
986 /** Directly control wheel velocities.
987  * Available only in safe or full mode.
988  * @param left_mm_per_sec velocity of left wheel in m/sec
989  * @param right_mm_per_sec velocity of right wheel in m/sec
990  */
991 void
992 Roomba500::drive_direct(short int left_mm_per_sec,
993  short int right_mm_per_sec)
994 {
995  assert_control();
996 
997  if (left_mm_per_sec < -MAX_LIN_VEL_MM_S) left_mm_per_sec = -MAX_LIN_VEL_MM_S;
998  if (left_mm_per_sec > MAX_LIN_VEL_MM_S) left_mm_per_sec = MAX_LIN_VEL_MM_S;
999 
1000  if (right_mm_per_sec < -MAX_LIN_VEL_MM_S) right_mm_per_sec = -MAX_LIN_VEL_MM_S;
1001  if (right_mm_per_sec > MAX_LIN_VEL_MM_S) right_mm_per_sec = MAX_LIN_VEL_MM_S;
1002 
1003  DriveDirectParams dp;
1004  dp.left_velocity = htons(left_mm_per_sec);
1005  dp.right_velocity = htons(right_mm_per_sec);
1006 
1007  send(OPCODE_DRIVE, &dp, sizeof(DriveDirectParams));
1008 }
1009 
1010 
1011 /** Directly control wheel velocities via PWM.
1012  * Available only in safe or full mode.
1013  * @param left_wheel_pwm PWM parameter for left wheel
1014  * @param right_wheel_pwm PWM parameter for right wheel
1015  */
1016 void
1017 Roomba500::drive_pwm(short int left_wheel_pwm, short int right_wheel_pwm)
1018 {
1019  assert_control();
1020 
1021  if (left_wheel_pwm < -MAX_PWM) left_wheel_pwm = -MAX_PWM;
1022  if (left_wheel_pwm > MAX_PWM) left_wheel_pwm = MAX_PWM;
1023 
1024  if (right_wheel_pwm < -MAX_PWM) right_wheel_pwm = -MAX_PWM;
1025  if (right_wheel_pwm > MAX_PWM) right_wheel_pwm = MAX_PWM;
1026 
1027  DrivePWMParams dp;
1028  dp.left_pwm = htons(left_wheel_pwm);
1029  dp.right_pwm = htons(right_wheel_pwm);
1030 
1031  send(OPCODE_DRIVE, &dp, sizeof(DrivePWMParams));
1032 }
1033 
1034 
1035 /** Set motor states (brushes and vacuum).
1036  * Available only in safe or full mode.
1037  * @param main true to enable main brushes
1038  * @param side true to enable side brush
1039  * @param vacuum true to enable vacuuming
1040  * @param main_backward true to enable backward operation of main brushes
1041  * @param side_backward true to enable backward operation of side brush
1042  */
1043 void
1044 Roomba500::set_motors(bool main, bool side, bool vacuum,
1045  bool main_backward, bool side_backward)
1046 {
1047  assert_control();
1048 
1049  unsigned char param = 0;
1050  if (main) param |= MOTOR_MAIN_BRUSHES;
1051  if (side) param |= MOTOR_SIDE_BRUSH;
1052  if (vacuum) param |= MOTOR_VACUUM;
1053  if (main_backward) param |= MOTOR_MAIN_BRUSHES_BACKWARD;
1054  if (side_backward) param |= MOTOR_SIDE_BRUSH_BACKWARD;
1055 
1056  send(OPCODE_MOTORS, &param, 1);
1057 }
1058 
1059 
1060 /** Set LED status of main LEDs.
1061  * Available only in safe or full mode.
1062  * @param debris true to enable debris LED, false to disable
1063  * @param spot true to enable spot LED, false to disable
1064  * @param dock true to enable dock LED, false to disable
1065  * @param check_robot true to enable check_robot LED, false to disable
1066  * @param clean_color color of clean button LED from green (0) to red (255)
1067  * @param clean_intensity intensity of clean button LED from off (0) to
1068  * full intensity (255)
1069  */
1070 void
1071 Roomba500::set_leds(bool debris, bool spot, bool dock, bool check_robot,
1072  unsigned char clean_color, unsigned char clean_intensity)
1073 {
1074  assert_control();
1075 
1076  unsigned char param[3] = {0, clean_color, clean_intensity};
1077  if (debris) param[0] |= LED_DEBRIS;
1078  if (spot) param[0] |= LED_SPOT;
1079  if (dock) param[0] |= LED_DOCK;
1080  if (check_robot) param[0] |= LED_CHECK_ROBOT;
1081 
1082  send(OPCODE_LEDS, param, 3);
1083 }
1084 
1085 
1086 /** Set digit LEDs.
1087  * Available only in safe or full mode.
1088  * Note, that not all characters are availabe. You can use ASCII table entries
1089  * 32-39, 44-63, 65-96, and 123-126.
1090  * @param digits array of digit values
1091  */
1092 void
1093 Roomba500::set_digit_leds(const char digits[4])
1094 {
1095  assert_control();
1096 
1097  send(OPCODE_DIGIT_LEDS_ASCII, digits, 4);
1098 }
1099 
1100 
1101 /** Play a simple fanfare.
1102  * You can play this for example upon connection to inform the user.
1103  */
1104 void
1106 {
1107  unsigned char p[14];
1108  p[0] = 0;
1109  p[1] = 6;
1110 
1111  // C,E,G,G,E,G
1112  p[2] = 72;
1113  p[3] = 6;
1114 
1115  p[4] = 76;
1116  p[5] = 6;
1117 
1118  p[6] = 79;
1119  p[7] = 8;
1120 
1121  p[8] = 79;
1122  p[9] = 10;
1123 
1124  p[10] = 76;
1125  p[11] = 8;
1126 
1127  p[12] = 79;
1128  p[13] = 8;
1129 
1130  unsigned char play;
1131  play = 0;
1132 
1133  send(OPCODE_SONG, p, sizeof(p));
1134  send(OPCODE_PLAY, &play, 1);
1135 }
1136 
1137 
1138 /** Get size of packet.
1139  * @param packet ID of packet to query size for
1140  * @return size of packet
1141  * @exception Exception thrown for unknown packet IDs.
1142  */
1143 unsigned short int
1145 {
1146  switch (packet) {
1147  case SENSPACK_BUMPS_DROPS: return SENSPACK_SIZE_BUMPS_DROPS;
1148  case SENSPACK_WALL: return SENSPACK_SIZE_WALL;
1149  case SENSPACK_CLIFF_LEFT: return SENSPACK_SIZE_CLIFF_LEFT;
1150  case SENSPACK_CLIFF_FRONT_LEFT: return SENSPACK_SIZE_CLIFF_FRONT_LEFT;
1151  case SENSPACK_CLIFF_FRONT_RIGHT: return SENSPACK_SIZE_CLIFF_FRONT_RIGHT;
1152  case SENSPACK_CLIFF_RIGHT: return SENSPACK_SIZE_CLIFF_RIGHT;
1153  case SENSPACK_VIRTUAL_WALL: return SENSPACK_SIZE_VIRTUAL_WALL;
1154  case SENSPACK_WHEEL_OVERCURRENTS: return SENSPACK_SIZE_WHEEL_OVERCURRENTS;
1155  case SENSPACK_DIRT_DETECT: return SENSPACK_SIZE_DIRT_DETECT;
1156  case SENSPACK_IR_CHAR_OMNI: return SENSPACK_SIZE_IR_CHAR_OMNI;
1157  case SENSPACK_BUTTONS: return SENSPACK_SIZE_BUTTONS;
1158  case SENSPACK_DISTANCE: return SENSPACK_SIZE_DISTANCE;
1159  case SENSPACK_ANGLE: return SENSPACK_SIZE_ANGLE;
1160  case SENSPACK_CHARGING_STATE: return SENSPACK_SIZE_CHARGING_STATE;
1161  case SENSPACK_VOLTAGE: return SENSPACK_SIZE_VOLTAGE;
1162  case SENSPACK_CURRENT: return SENSPACK_SIZE_CURRENT;
1163  case SENSPACK_TEMPERATURE: return SENSPACK_SIZE_TEMPERATURE;
1164  case SENSPACK_BATTERY_CHARGE: return SENSPACK_SIZE_BATTERY_CHARGE;
1165  case SENSPACK_BATTERY_CAPACITY: return SENSPACK_SIZE_BATTERY_CAPACITY;
1166  case SENSPACK_WALL_SIGNAL: return SENSPACK_SIZE_WALL_SIGNAL;
1167  case SENSPACK_CLIFF_LEFT_SIGNAL: return SENSPACK_SIZE_CLIFF_LEFT_SIGNAL;
1168  case SENSPACK_CLIFF_FRONT_LEFT_SIGNAL:
1169  return SENSPACK_SIZE_CLIFF_FRONT_LEFT_SIGNAL;
1170  case SENSPACK_CLIFF_FRONT_RIGHT_SIGNAL:
1171  return SENSPACK_SIZE_CLIFF_FRONT_RIGHT_SIGNAL;
1172  case SENSPACK_CLIFF_RIGHT_SIGNAL: return SENSPACK_SIZE_CLIFF_RIGHT_SIGNAL;
1173  case SENSPACK_CHARGE_SOURCES: return SENSPACK_SIZE_CHARGE_SOURCES;
1174  case SENSPACK_OI_MODE: return SENSPACK_SIZE_OI_MODE;
1175  case SENSPACK_SONG_NUMBER: return SENSPACK_SIZE_SONG_NUMBER;
1176  case SENSPACK_SONG_PLAYING: return SENSPACK_SIZE_SONG_PLAYING;
1177  case SENSPACK_STREAM_PACKETS: return SENSPACK_SIZE_STREAM_PACKETS;
1178  case SENSPACK_REQ_VELOCITY: return SENSPACK_SIZE_REQ_VELOCITY;
1179  case SENSPACK_REQ_RADIUS: return SENSPACK_SIZE_REQ_RADIUS;
1180  case SENSPACK_REQ_RIGHT_VELOCITY: return SENSPACK_SIZE_REQ_RIGHT_VELOCITY;
1181  case SENSPACK_REQ_LEFT_VELOCITY: return SENSPACK_SIZE_REQ_LEFT_VELOCITY;
1182  case SENSPACK_RIGHT_ENCODER: return SENSPACK_SIZE_RIGHT_ENCODER;
1183  case SENSPACK_LEFT_ENCODER: return SENSPACK_SIZE_LEFT_ENCODER;
1184  case SENSPACK_LIGHT_BUMPER: return SENSPACK_SIZE_LIGHT_BUMPER;
1185  case SENSPACK_LIGHT_BUMPER_LEFT: return SENSPACK_SIZE_LIGHT_BUMPER_LEFT;
1186  case SENSPACK_LIGHT_BUMPER_FRONT_LEFT:
1187  return SENSPACK_SIZE_LIGHT_BUMPER_FRONT_LEFT;
1188  case SENSPACK_LIGHT_BUMPER_CENTER_LEFT:
1189  return SENSPACK_SIZE_LIGHT_BUMPER_CENTER_LEFT;
1190  case SENSPACK_LIGHT_BUMPER_CENTER_RIGHT:
1191  return SENSPACK_SIZE_LIGHT_BUMPER_CENTER_RIGHT;
1192  case SENSPACK_LIGHT_BUMPER_FRONT_RIGHT:
1193  return SENSPACK_SIZE_LIGHT_BUMPER_FRONT_RIGHT;
1194  case SENSPACK_LIGHT_BUMPER_RIGHT:
1195  return SENSPACK_SIZE_LIGHT_BUMPER_RIGHT;
1196  case SENSPACK_IR_CHAR_LEFT: return SENSPACK_SIZE_IR_CHAR_LEFT;
1197  case SENSPACK_IR_CHAR_RIGHT: return SENSPACK_SIZE_IR_CHAR_RIGHT;
1198  case SENSPACK_LEFT_MOTOR_CURRENT: return SENSPACK_SIZE_LEFT_MOTOR_CURRENT;
1199  case SENSPACK_RIGHT_MOTOR_CURRENT: return SENSPACK_SIZE_RIGHT_MOTOR_CURRENT;
1200  case SENSPACK_BRUSH_MOTOR_CURRENT: return SENSPACK_SIZE_BRUSH_MOTOR_CURRENT;
1201  case SENSPACK_SIDE_BRUSH_MOTOR_CURRENT:
1202  return SENSPACK_SIZE_SIDE_BRUSH_MOTOR_CURRENT;
1203  case SENSPACK_STASIS: return SENSPACK_SIZE_STASIS;
1204  case SENSPACK_GROUP_0: return SENSPACK_SIZE_GROUP_0;
1205  case SENSPACK_GROUP_1: return SENSPACK_SIZE_GROUP_1;
1206  case SENSPACK_GROUP_2: return SENSPACK_SIZE_GROUP_2;
1207  case SENSPACK_GROUP_3: return SENSPACK_SIZE_GROUP_3;
1208  case SENSPACK_GROUP_4: return SENSPACK_SIZE_GROUP_4;
1209  case SENSPACK_GROUP_5: return SENSPACK_SIZE_GROUP_5;
1210  case SENSPACK_GROUP_6: return SENSPACK_SIZE_GROUP_6;
1211  case SENSPACK_GROUP_ALL: return SENSPACK_SIZE_GROUP_ALL;
1212  case SENSPACK_GROUP_101: return SENSPACK_SIZE_GROUP_101;
1213  case SENSPACK_GROUP_106: return SENSPACK_SIZE_GROUP_106;
1214  case SENSPACK_GROUP_107: return SENSPACK_SIZE_GROUP_107;
1215  default:
1216  throw Exception("Roomba500:get_packet_size(): unknown packet ID %i", packet);
1217  }
1218 }
void play_fanfare()
Play a simple fanfare.
static const unsigned short int STREAM_INTERVAL_MS
Time in ms between.
Definition: roomba_500.h:372
static const unsigned char MOTOR_SIDE_BRUSH
Side brush motor bit.
Definition: roomba_500.h:290
void drive_direct(short int left_mm_per_sec, short int right_mm_per_sec)
Directly control wheel velocities.
Definition: roomba_500.cpp:992
static const unsigned char CHARGER_HOME_BASE
Home base charger bit.
Definition: roomba_500.h:296
File could not be opened.
Definition: system.h:53
static const unsigned char OVERCURRENT_WHEEL_RIGHT
Right wheel bit.
Definition: roomba_500.h:249
static const unsigned char CHARGING_SOURCE_INTERNAL
Internal socket.
Definition: roomba_500.h:254
static const short int MAX_LIN_VEL_MM_S
Maximum linear velocity.
Definition: roomba_500.h:367
void stop()
Stop moption of the Roomba.
Definition: roomba_500.cpp:895
static const unsigned char BUMPER_CENTER_RIGHT
Center right bumper.
Definition: roomba_500.h:259
void drive_arc(short int velo_mm_per_sec, short int radius_mm)
Drive Roomba on an arc.
Definition: roomba_500.cpp:945
bool is_data_available()
Check if data is available.
Definition: roomba_500.cpp:648
Roomba500(ConnectionType conntype, const char *device, unsigned int flags=0)
Constructor.
Definition: roomba_500.cpp:241
static const unsigned char WEEKDAY_LED_THU
Thursday.
Definition: roomba_500.h:272
static const unsigned char WHEEL_DROP_RIGHT
Right wheel drop bit.
Definition: roomba_500.h:244
void drive_pwm(short int left_wheel_pwm, short int right_wheel_pwm)
Directly control wheel velocities via PWM.
static const unsigned char BUTTON_DAY
Day button.
Definition: roomba_500.h:239
static const unsigned char WHEEL_DROP_LEFT
Left wheel drop bit.
Definition: roomba_500.h:243
static const unsigned short int MODE_CHANGE_WAIT_MS
Time in ms to wait after mode changes.
Definition: roomba_500.h:374
static const short int MAX_RADIUS_MM
Maximum drive radius.
Definition: roomba_500.h:368
static const unsigned char CHECKSUM_SIZE
Checksum byte size.
Definition: roomba_500.h:377
static const short int MAX_PWM
Maximum PWM value for wheels.
Definition: roomba_500.h:370
static const unsigned char LED_DEBRIS
Debris LED bit.
Definition: roomba_500.h:263
static const float DIAMETER
Robot diameter.
Definition: roomba_500.h:363
Fawkes library namespace.
void enable_sensors()
Enable sensor data stream.
Definition: roomba_500.cpp:741
void seek_dock()
Seek for the home base and dock.
Definition: roomba_500.cpp:869
Mutex locking helper.
Definition: mutex_locker.h:33
static const unsigned char LED_SPOT
Spot LED bit.
Definition: roomba_500.h:264
static const float BUMPER_X_OFFSET
X Offset of bumper.
Definition: roomba_500.h:364
The current system call has timed out before completion.
Definition: system.h:46
static const unsigned char BUTTON_CLEAN
Cleaning button.
Definition: roomba_500.h:234
static const unsigned char SCHEDULING_LED_PM
PM LED bit.
Definition: roomba_500.h:277
void drive_straight(short int velo_mm_per_sec)
Drive Roomba straight.
Definition: roomba_500.cpp:908
static const unsigned char DIGIT_LED_NORTH_EAST
Top right segment LED.
Definition: roomba_500.h:284
static const unsigned char BUTTON_DOCK
Dock button.
Definition: roomba_500.h:236
static const unsigned char BUTTON_HOUR
Hour button.
Definition: roomba_500.h:238
static const unsigned char BUMPER_LEFT
Left bumper.
Definition: roomba_500.h:256
static const unsigned char MOTOR_SIDE_BRUSH_BACKWARD
Side backward bit.
Definition: roomba_500.h:293
static const unsigned char DIGIT_LED_NORTH_WEST
Top left segment LED.
Definition: roomba_500.h:283
void power_down()
Powers down the Roomba.
Definition: roomba_500.cpp:882
void disable_sensors()
Disable sensor data stream.
Definition: roomba_500.cpp:761
static const unsigned char BUMPER_FRONT_LEFT
Front left bumper.
Definition: roomba_500.h:257
static const unsigned char SCHEDULING_LED_AM
AM LED bit.
Definition: roomba_500.h:278
static const unsigned char DIGIT_LED_SOUTH_EAST
Bottom right segment.
Definition: roomba_500.h:287
static const unsigned char LED_CHECK_ROBOT
Check robot LED bit.
Definition: roomba_500.h:266
static const unsigned char LED_DOCK
Dock LED bit.
Definition: roomba_500.h:265
Mode
Roomba 500 operation mode.
Definition: roomba_500.h:148
static const unsigned short int MAX_ENCODER_COUNT
Maximum encoder count.
Definition: roomba_500.h:371
void clean_spot()
Start spot cleaning operation.
Definition: roomba_500.cpp:857
static const unsigned char SCHEDULING_LED_SCHEDULE
Schedule LED bit.
Definition: roomba_500.h:280
static const unsigned char BUMPER_CENTER_LEFT
Center left bumper.
Definition: roomba_500.h:258
static const unsigned char BUTTON_SPOT
Spot cleaning button.
Definition: roomba_500.h:235
static const unsigned char OVERCURRENT_WHEEL_LEFT
Left wheel bit.
Definition: roomba_500.h:248
SensorPacketID
Roomba 500 sensor package IDs.
Definition: roomba_500.h:82
static const unsigned char MOTOR_MAIN_BRUSHES_BACKWARD
Main backward bit.
Definition: roomba_500.h:294
static const unsigned char CHARGER_INTERNAL
Internal charger bit.
Definition: roomba_500.h:297
static const unsigned char OVERCURRENT_MAIN_BRUSH
Main brush bit.
Definition: roomba_500.h:250
Base class for exceptions in Fawkes.
Definition: exception.h:36
ConnectionType
Connection type.
Definition: roomba_500.h:38
static const unsigned char DIGIT_LED_CENTER
Center segment LED.
Definition: roomba_500.h:285
static const unsigned char BUTTON_SCHEDULE
Schedule button.
Definition: roomba_500.h:240
void clean()
Start normal cleaning operation.
Definition: roomba_500.cpp:845
void close()
Close serial connection.
Definition: roomba_500.cpp:545
static const unsigned char WEEKDAY_LED_FRI
Friday.
Definition: roomba_500.h:273
void query_sensors()
Query sensor once.
Definition: roomba_500.cpp:779
void set_motors(bool main=true, bool side=true, bool vacuum=true, bool main_backward=false, bool side_backward=false)
Set motor states (brushes and vacuum).
static const unsigned char BUTTON_CLOCK
Clock button.
Definition: roomba_500.h:241
static const unsigned char WEEKDAY_LED_SAT
Saturday.
Definition: roomba_500.h:274
static const unsigned char BUTTON_MINUTE
Minute button.
Definition: roomba_500.h:237
const SensorPacketGroupAll get_sensor_packet() const
Get latest sensor packet.
Definition: roomba_500.cpp:807
static const unsigned char SCHEDULING_LED_CLOCK
Clock LED bit.
Definition: roomba_500.h:279
static const unsigned char MOTOR_MAIN_BRUSHES
Main brush motor bit.
Definition: roomba_500.h:292
static const unsigned char WEEKDAY_LED_TUE
Tuesday.
Definition: roomba_500.h:270
static const unsigned char OVERCURRENT_SIDE_BRUSH
Side brush bit.
Definition: roomba_500.h:251
static const unsigned char WEEKDAY_LED_WED
Wednesday.
Definition: roomba_500.h:271
static const unsigned char BUMPER_RIGHT
Right bumper.
Definition: roomba_500.h:261
static const unsigned char DIGIT_LED_SOUTH
Bottom segment LED.
Definition: roomba_500.h:288
void set_mode(Mode mode)
Set control mode.
Definition: roomba_500.cpp:824
static const unsigned char WEEKDAY_LED_MON
Monday.
Definition: roomba_500.h:269
~Roomba500()
Destructor.
Definition: roomba_500.cpp:275
static const unsigned char DIGIT_LED_NORTH
Top segment LED.
Definition: roomba_500.h:282
void read_sensors()
Read sensor values.
Definition: roomba_500.cpp:673
void set_leds(bool debris, bool spot, bool dock, bool check_robot, unsigned char clean_color, unsigned char clean_intensity)
Set LED status of main LEDs.
Struct for packet group with everything (SENSPACK_GROUP_ALL).
Definition: roomba_500.h:381
Mutex mutual exclusion lock.
Definition: mutex.h:32
static const unsigned char BUMP_RIGHT
Right bumper bit.
Definition: roomba_500.h:246
static const unsigned char CHARGING_SOURCE_HOME_BASE
Docking station.
Definition: roomba_500.h:253
void drive(short int velocity_mm_per_sec, short int radius_mm)
Drive Roomba.
Definition: roomba_500.cpp:968
static const unsigned char BUMPER_FRONT_RIGHT
Front right bumper.
Definition: roomba_500.h:260
static const unsigned char MOTOR_VACUUM
Vacuum motor bit.
Definition: roomba_500.h:291
void open()
Open serial port.
Definition: roomba_500.cpp:287
OpCode
Roomba 500 Command op codes.
Definition: roomba_500.h:50
void set_digit_leds(const char digits[4])
Set digit LEDs.
static const unsigned char DIGIT_LED_SOUTH_WEST
Bottom left segment.
Definition: roomba_500.h:286
static unsigned short int get_packet_size(SensorPacketID packet)
Get size of packet.
TurnDirection
Turning direction.
Definition: roomba_500.h:229
static const unsigned char SCHEDULING_LED_COLON
Colon LED bit.
Definition: roomba_500.h:276
static const unsigned char BUMP_LEFT
Left bumper bit.
Definition: roomba_500.h:245
static const unsigned char WEEKDAY_LED_SUN
Sunday.
Definition: roomba_500.h:268
void drive_turn(TurnDirection direction)
Turn robot on the spot.
Definition: roomba_500.cpp:927
static const float AXLE_LENGTH
Axle length.
Definition: roomba_500.h:365