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