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
46static const bdaddr_t _BDADDR_ANY = {{0, 0, 0, 0, 0, 0}};
47#endif
48
49using namespace fawkes;
50
51/// @cond SELFEXPLAINING
52const unsigned char Roomba500::BUTTON_CLEAN = 1;
53const unsigned char Roomba500::BUTTON_SPOT = 2;
54const unsigned char Roomba500::BUTTON_DOCK = 4;
55const unsigned char Roomba500::BUTTON_MINUTE = 8;
56const unsigned char Roomba500::BUTTON_HOUR = 16;
57const unsigned char Roomba500::BUTTON_DAY = 32;
58const unsigned char Roomba500::BUTTON_SCHEDULE = 64;
59const unsigned char Roomba500::BUTTON_CLOCK = 128;
60
61const unsigned char Roomba500::WHEEL_DROP_LEFT = 8;
62const unsigned char Roomba500::WHEEL_DROP_RIGHT = 4;
63const unsigned char Roomba500::BUMP_LEFT = 2;
64const unsigned char Roomba500::BUMP_RIGHT = 1;
65
66const unsigned char Roomba500::OVERCURRENT_WHEEL_LEFT = 16;
67const unsigned char Roomba500::OVERCURRENT_WHEEL_RIGHT = 8;
68const unsigned char Roomba500::OVERCURRENT_MAIN_BRUSH = 4;
69const unsigned char Roomba500::OVERCURRENT_SIDE_BRUSH = 1;
70
71const unsigned char Roomba500::CHARGING_SOURCE_HOME_BASE = 2;
72const unsigned char Roomba500::CHARGING_SOURCE_INTERNAL = 1;
73
74const unsigned char Roomba500::BUMPER_LEFT = 1;
75const unsigned char Roomba500::BUMPER_FRONT_LEFT = 2;
76const unsigned char Roomba500::BUMPER_CENTER_LEFT = 4;
77const unsigned char Roomba500::BUMPER_CENTER_RIGHT = 8;
78const unsigned char Roomba500::BUMPER_FRONT_RIGHT = 16;
79const unsigned char Roomba500::BUMPER_RIGHT = 32;
80
81const unsigned char Roomba500::LED_DEBRIS = 1;
82const unsigned char Roomba500::LED_SPOT = 2;
83const unsigned char Roomba500::LED_DOCK = 4;
84const unsigned char Roomba500::LED_CHECK_ROBOT = 8;
85
86const unsigned char Roomba500::WEEKDAY_LED_SUN = 1;
87const unsigned char Roomba500::WEEKDAY_LED_MON = 2;
88const unsigned char Roomba500::WEEKDAY_LED_TUE = 4;
89const unsigned char Roomba500::WEEKDAY_LED_WED = 8;
90const unsigned char Roomba500::WEEKDAY_LED_THU = 16;
91const unsigned char Roomba500::WEEKDAY_LED_FRI = 32;
92const unsigned char Roomba500::WEEKDAY_LED_SAT = 64;
93
94const unsigned char Roomba500::SCHEDULING_LED_COLON = 1;
95const unsigned char Roomba500::SCHEDULING_LED_PM = 2;
96const unsigned char Roomba500::SCHEDULING_LED_AM = 4;
97const unsigned char Roomba500::SCHEDULING_LED_CLOCK = 8;
98const unsigned char Roomba500::SCHEDULING_LED_SCHEDULE = 16;
99
100const unsigned char Roomba500::DIGIT_LED_NORTH = 1;
101const unsigned char Roomba500::DIGIT_LED_NORTH_WEST = 32;
102const unsigned char Roomba500::DIGIT_LED_NORTH_EAST = 2;
103const unsigned char Roomba500::DIGIT_LED_CENTER = 64;
104const unsigned char Roomba500::DIGIT_LED_SOUTH_WEST = 16;
105const unsigned char Roomba500::DIGIT_LED_SOUTH_EAST = 4;
106const unsigned char Roomba500::DIGIT_LED_SOUTH = 8;
107
108const unsigned char Roomba500::MOTOR_SIDE_BRUSH = 1;
109const unsigned char Roomba500::MOTOR_VACUUM = 2;
110const unsigned char Roomba500::MOTOR_MAIN_BRUSHES = 4;
111const unsigned char Roomba500::MOTOR_SIDE_BRUSH_BACKWARD = 8;
112const unsigned char Roomba500::MOTOR_MAIN_BRUSHES_BACKWARD = 16;
113
114const unsigned char Roomba500::CHARGER_HOME_BASE = 2;
115const unsigned char Roomba500::CHARGER_INTERNAL = 1;
116
117const unsigned short int Roomba500::SENSPACK_SIZE_GROUP_0 = 26;
118const unsigned short int Roomba500::SENSPACK_SIZE_GROUP_1 = 10;
119const unsigned short int Roomba500::SENSPACK_SIZE_GROUP_2 = 6;
120const unsigned short int Roomba500::SENSPACK_SIZE_GROUP_3 = 10;
121const unsigned short int Roomba500::SENSPACK_SIZE_GROUP_4 = 14;
122const unsigned short int Roomba500::SENSPACK_SIZE_GROUP_5 = 12;
123const unsigned short int Roomba500::SENSPACK_SIZE_GROUP_6 = 52;
124const unsigned short int Roomba500::SENSPACK_SIZE_GROUP_ALL = 80;
125const unsigned short int Roomba500::SENSPACK_SIZE_GROUP_101 = 28;
126const unsigned short int Roomba500::SENSPACK_SIZE_GROUP_106 = 12;
127const unsigned short int Roomba500::SENSPACK_SIZE_GROUP_107 = 9;
128const unsigned short int Roomba500::SENSPACK_SIZE_BUMPS_DROPS = 1;
129const unsigned short int Roomba500::SENSPACK_SIZE_WALL = 1;
130const unsigned short int Roomba500::SENSPACK_SIZE_CLIFF_LEFT = 1;
131const unsigned short int Roomba500::SENSPACK_SIZE_CLIFF_FRONT_LEFT = 1;
132const unsigned short int Roomba500::SENSPACK_SIZE_CLIFF_FRONT_RIGHT = 1;
133const unsigned short int Roomba500::SENSPACK_SIZE_CLIFF_RIGHT = 1;
134const unsigned short int Roomba500::SENSPACK_SIZE_VIRTUAL_WALL = 1;
135const unsigned short int Roomba500::SENSPACK_SIZE_WHEEL_OVERCURRENTS = 1;
136const unsigned short int Roomba500::SENSPACK_SIZE_DIRT_DETECT = 1;
137const unsigned short int Roomba500::SENSPACK_SIZE_IR_CHAR_OMNI = 1;
138const unsigned short int Roomba500::SENSPACK_SIZE_IR_CHAR_LEFT = 1;
139const unsigned short int Roomba500::SENSPACK_SIZE_IR_CHAR_RIGHT = 1;
140const unsigned short int Roomba500::SENSPACK_SIZE_BUTTONS = 1;
141const unsigned short int Roomba500::SENSPACK_SIZE_DISTANCE = 2;
142const unsigned short int Roomba500::SENSPACK_SIZE_ANGLE = 2;
143const unsigned short int Roomba500::SENSPACK_SIZE_CHARGING_STATE = 1;
144const unsigned short int Roomba500::SENSPACK_SIZE_VOLTAGE = 2;
145const unsigned short int Roomba500::SENSPACK_SIZE_CURRENT = 2;
146const unsigned short int Roomba500::SENSPACK_SIZE_TEMPERATURE = 1;
147const unsigned short int Roomba500::SENSPACK_SIZE_BATTERY_CHARGE = 2;
148const unsigned short int Roomba500::SENSPACK_SIZE_BATTERY_CAPACITY = 2;
149const unsigned short int Roomba500::SENSPACK_SIZE_WALL_SIGNAL = 2;
150const unsigned short int Roomba500::SENSPACK_SIZE_CLIFF_LEFT_SIGNAL = 2;
151const unsigned short int Roomba500::SENSPACK_SIZE_CLIFF_FRONT_LEFT_SIGNAL = 2;
152const unsigned short int Roomba500::SENSPACK_SIZE_CLIFF_FRONT_RIGHT_SIGNAL = 2;
153const unsigned short int Roomba500::SENSPACK_SIZE_CLIFF_RIGHT_SIGNAL = 2;
154const unsigned short int Roomba500::SENSPACK_SIZE_CHARGE_SOURCES = 1;
155const unsigned short int Roomba500::SENSPACK_SIZE_OI_MODE = 1;
156const unsigned short int Roomba500::SENSPACK_SIZE_SONG_NUMBER = 1;
157const unsigned short int Roomba500::SENSPACK_SIZE_SONG_PLAYING = 1;
158const unsigned short int Roomba500::SENSPACK_SIZE_STREAM_PACKETS = 1;
159const unsigned short int Roomba500::SENSPACK_SIZE_REQ_VELOCITY = 2;
160const unsigned short int Roomba500::SENSPACK_SIZE_REQ_RADIUS = 2;
161const unsigned short int Roomba500::SENSPACK_SIZE_REQ_RIGHT_VELOCITY = 2;
162const unsigned short int Roomba500::SENSPACK_SIZE_REQ_LEFT_VELOCITY = 2;
163const unsigned short int Roomba500::SENSPACK_SIZE_RIGHT_ENCODER = 2;
164const unsigned short int Roomba500::SENSPACK_SIZE_LEFT_ENCODER = 2;
165const unsigned short int Roomba500::SENSPACK_SIZE_LIGHT_BUMPER = 1;
166const unsigned short int Roomba500::SENSPACK_SIZE_LIGHT_BUMPER_LEFT = 2;
167const unsigned short int Roomba500::SENSPACK_SIZE_LIGHT_BUMPER_FRONT_LEFT = 2;
168const unsigned short int Roomba500::SENSPACK_SIZE_LIGHT_BUMPER_CENTER_LEFT = 2;
169const unsigned short int Roomba500::SENSPACK_SIZE_LIGHT_BUMPER_CENTER_RIGHT = 2;
170const unsigned short int Roomba500::SENSPACK_SIZE_LIGHT_BUMPER_FRONT_RIGHT = 2;
171const unsigned short int Roomba500::SENSPACK_SIZE_LIGHT_BUMPER_RIGHT = 2;
172const unsigned short int Roomba500::SENSPACK_SIZE_LEFT_MOTOR_CURRENT = 2;
173const unsigned short int Roomba500::SENSPACK_SIZE_RIGHT_MOTOR_CURRENT = 2;
174const unsigned short int Roomba500::SENSPACK_SIZE_BRUSH_MOTOR_CURRENT = 2;
175const unsigned short int Roomba500::SENSPACK_SIZE_SIDE_BRUSH_MOTOR_CURRENT = 2;
176const unsigned short int Roomba500::SENSPACK_SIZE_STASIS = 1;
177
178const float Roomba500::DIAMETER = 0.33;
179const float Roomba500::BUMPER_X_OFFSET = 0.05;
180const float Roomba500::AXLE_LENGTH = 0.235;
181
182const short int Roomba500::MAX_LIN_VEL_MM_S = 500;
183const short int Roomba500::MAX_RADIUS_MM = 2000;
184
185const unsigned short int Roomba500::MAX_ENCODER_COUNT = 65535;
186const short int Roomba500::MAX_PWM = 255;
187
188const unsigned short int Roomba500::STREAM_INTERVAL_MS = 15;
189const unsigned short int Roomba500::MODE_CHANGE_WAIT_MS = 20;
190
191const unsigned char Roomba500::CHECKSUM_SIZE = 1;
192
193/// @endcond
194
195/// @cond INTERNALS
196#pragma pack(push, 1)
197
198typedef struct
199{
200 int16_t velocity;
201 int16_t radius;
202} DriveParams;
203
204typedef struct
205{
206 int16_t left_velocity;
207 int16_t right_velocity;
208} DriveDirectParams;
209
210typedef struct
211{
212 int16_t left_pwm;
213 int16_t right_pwm;
214} DrivePWMParams;
215
216typedef 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 */
244Roomba500::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. */
269void
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
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. */
518void
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 */
533void
534Roomba500::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 */
581void
582Roomba500::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 */
621bool
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 */
645void
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 */
713void
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. */
732void
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 */
749void
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 */
792void
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 */
813void
815{
816 send(OPCODE_CLEAN);
817
818 mode_ = MODE_PASSIVE;
819}
820
821/** Start spot cleaning operation.
822 * Transitions to passive mode.
823 */
824void
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 */
835void
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 */
847void
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 */
859void
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 */
870void
871Roomba500::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 */
891void
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 */
908void
909Roomba500::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 */
935void
936Roomba500::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 */
962void
963Roomba500::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 */
989void
990Roomba500::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 */
1019void
1020Roomba500::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)
1033 if (side_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 */
1049void
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 */
1078void
1079Roomba500::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 */
1089void
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 */
1127unsigned 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}
void read_sensors()
Read sensor values.
Definition: roomba_500.cpp:646
void seek_dock()
Seek for the home base and dock.
Definition: roomba_500.cpp:836
static const unsigned char CHARGING_SOURCE_HOME_BASE
Docking station.
Definition: roomba_500.h:251
static const unsigned char WEEKDAY_LED_WED
Wednesday.
Definition: roomba_500.h:269
static const unsigned char SCHEDULING_LED_AM
AM LED bit.
Definition: roomba_500.h:276
static const unsigned char LED_SPOT
Spot LED bit.
Definition: roomba_500.h:262
static const unsigned char BUTTON_CLEAN
Cleaning button.
Definition: roomba_500.h:232
OpCode
Roomba 500 Command op codes.
Definition: roomba_500.h:48
@ OPCODE_DIGIT_LEDS_ASCII
Ascii control of digit LEDs.
Definition: roomba_500.h:73
@ OPCODE_SPOT
Start spot cleaning.
Definition: roomba_500.h:55
@ OPCODE_PAUSE_RESUME_STREAM
Pause or resume streaming data.
Definition: roomba_500.h:70
@ OPCODE_DRIVE
Drive robot.
Definition: roomba_500.h:58
@ OPCODE_LEDS
Control LEDs.
Definition: roomba_500.h:60
@ OPCODE_SEEK_DOCK
Start seeking dock.
Definition: roomba_500.h:64
@ OPCODE_FULL
Enter full mode.
Definition: roomba_500.h:53
@ OPCODE_SONG
Register song.
Definition: roomba_500.h:61
@ OPCODE_SAFE
Enter safe mode.
Definition: roomba_500.h:52
@ OPCODE_PLAY
Play song.
Definition: roomba_500.h:62
@ OPCODE_START
Initiate communication with Roomba.
Definition: roomba_500.h:49
@ OPCODE_POWER
Power down Roomba.
Definition: roomba_500.h:54
@ OPCODE_STREAM
Start streaming of data.
Definition: roomba_500.h:68
@ OPCODE_QUERY
Query sensor info.
Definition: roomba_500.h:63
@ OPCODE_MOTORS
Set motor state.
Definition: roomba_500.h:59
@ OPCODE_CLEAN
Start normal cleaning mission.
Definition: roomba_500.h:56
static const unsigned char MOTOR_MAIN_BRUSHES
Main brush motor bit.
Definition: roomba_500.h:290
static const unsigned char DIGIT_LED_NORTH_WEST
Top left segment LED.
Definition: roomba_500.h:281
static const short int MAX_LIN_VEL_MM_S
Maximum linear velocity.
Definition: roomba_500.h:364
static const unsigned char BUMPER_CENTER_LEFT
Center left bumper.
Definition: roomba_500.h:256
@ FLAG_FIREFLY_FASTMODE
Enable fast mode, assume FireFly RooTooth.
Definition: roomba_500.h:44
static const unsigned char OVERCURRENT_WHEEL_LEFT
Left wheel bit.
Definition: roomba_500.h:246
static const unsigned char BUMP_RIGHT
Right bumper bit.
Definition: roomba_500.h:244
static const unsigned char OVERCURRENT_MAIN_BRUSH
Main brush bit.
Definition: roomba_500.h:248
static const unsigned char OVERCURRENT_SIDE_BRUSH
Side brush bit.
Definition: roomba_500.h:249
void drive_turn(TurnDirection direction)
Turn robot on the spot.
Definition: roomba_500.cpp:892
static const unsigned char BUTTON_CLOCK
Clock button.
Definition: roomba_500.h:239
static const unsigned char WEEKDAY_LED_MON
Monday.
Definition: roomba_500.h:267
const SensorPacketGroupAll get_sensor_packet() const
Get latest sensor packet.
Definition: roomba_500.cpp:776
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.
void close()
Close serial connection.
Definition: roomba_500.cpp:519
static const unsigned char WEEKDAY_LED_SUN
Sunday.
Definition: roomba_500.h:266
void clean()
Start normal cleaning operation.
Definition: roomba_500.cpp:814
void set_mode(Mode mode)
Set control mode.
Definition: roomba_500.cpp:793
void query_sensors()
Query sensor once.
Definition: roomba_500.cpp:750
static const unsigned char BUMPER_CENTER_RIGHT
Center right bumper.
Definition: roomba_500.h:257
~Roomba500()
Destructor.
Definition: roomba_500.cpp:263
static const unsigned char DIGIT_LED_SOUTH_WEST
Bottom left segment.
Definition: roomba_500.h:284
static const unsigned char BUTTON_MINUTE
Minute button.
Definition: roomba_500.h:235
static const unsigned char SCHEDULING_LED_PM
PM LED bit.
Definition: roomba_500.h:275
static const unsigned char WEEKDAY_LED_SAT
Saturday.
Definition: roomba_500.h:272
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 float BUMPER_X_OFFSET
X Offset of bumper.
Definition: roomba_500.h:361
ConnectionType
Connection type.
Definition: roomba_500.h:36
@ CONNTYPE_ROOTOOTH
Use BlueZ to find and connect to RooTooth.
Definition: roomba_500.h:38
@ CONNTYPE_SERIAL
Use serial connection (device file).
Definition: roomba_500.h:37
static const unsigned char MOTOR_SIDE_BRUSH
Side brush motor bit.
Definition: roomba_500.h:288
static const float DIAMETER
Robot diameter.
Definition: roomba_500.h:360
void drive_straight(short int velo_mm_per_sec)
Drive Roomba straight.
Definition: roomba_500.cpp:871
void clean_spot()
Start spot cleaning operation.
Definition: roomba_500.cpp:825
static unsigned short int get_packet_size(SensorPacketID packet)
Get size of packet.
Roomba500(ConnectionType conntype, const char *device, unsigned int flags=0)
Constructor.
Definition: roomba_500.cpp:244
@ STREAM_DISABLE
Stream disabled.
Definition: roomba_500.h:156
void drive_arc(short int velo_mm_per_sec, short int radius_mm)
Drive Roomba on an arc.
Definition: roomba_500.cpp:909
static const unsigned char CHARGING_SOURCE_INTERNAL
Internal socket.
Definition: roomba_500.h:252
static const unsigned char SCHEDULING_LED_SCHEDULE
Schedule LED bit.
Definition: roomba_500.h:278
bool is_data_available()
Check if data is available.
Definition: roomba_500.cpp:622
Mode
Roomba 500 operation mode.
Definition: roomba_500.h:146
@ MODE_OFF
No connection.
Definition: roomba_500.h:147
@ MODE_FULL
Control acquired, safety measures disabled.
Definition: roomba_500.h:150
@ MODE_PASSIVE
Passive mode, no control, only listening.
Definition: roomba_500.h:148
@ MODE_SAFE
Control acquired, safety measures in place.
Definition: roomba_500.h:149
void open()
Open serial port.
Definition: roomba_500.cpp:270
static const unsigned char LED_DEBRIS
Debris LED bit.
Definition: roomba_500.h:261
static const unsigned char BUTTON_DOCK
Dock button.
Definition: roomba_500.h:234
static const unsigned char BUMPER_FRONT_RIGHT
Front right bumper.
Definition: roomba_500.h:258
static const unsigned char WEEKDAY_LED_THU
Thursday.
Definition: roomba_500.h:270
static const unsigned char BUTTON_SCHEDULE
Schedule button.
Definition: roomba_500.h:238
static const unsigned short int STREAM_INTERVAL_MS
Time in ms between streamed sensor packets.
Definition: roomba_500.h:369
static const unsigned char DIGIT_LED_NORTH_EAST
Top right segment LED.
Definition: roomba_500.h:282
static const unsigned char SCHEDULING_LED_COLON
Colon LED bit.
Definition: roomba_500.h:274
static const unsigned char WHEEL_DROP_LEFT
Left wheel drop bit.
Definition: roomba_500.h:241
static const unsigned char DIGIT_LED_SOUTH
Bottom segment LED.
Definition: roomba_500.h:286
static const unsigned char DIGIT_LED_CENTER
Center segment LED.
Definition: roomba_500.h:283
static const unsigned char LED_DOCK
Dock LED bit.
Definition: roomba_500.h:263
static const unsigned char BUMPER_FRONT_LEFT
Front left bumper.
Definition: roomba_500.h:255
void power_down()
Powers down the Roomba.
Definition: roomba_500.cpp:848
static const unsigned char WEEKDAY_LED_TUE
Tuesday.
Definition: roomba_500.h:268
void drive(short int velocity_mm_per_sec, short int radius_mm)
Drive Roomba.
Definition: roomba_500.cpp:936
static const unsigned char DIGIT_LED_NORTH
Top segment LED.
Definition: roomba_500.h:280
static const unsigned char BUTTON_DAY
Day button.
Definition: roomba_500.h:237
static const unsigned short int MAX_ENCODER_COUNT
Maximum encoder count.
Definition: roomba_500.h:368
void play_fanfare()
Play a simple fanfare.
void stop()
Stop moption of the Roomba.
Definition: roomba_500.cpp:860
static const unsigned char SCHEDULING_LED_CLOCK
Clock LED bit.
Definition: roomba_500.h:277
void set_digit_leds(const char digits[4])
Set digit LEDs.
SensorPacketID
Roomba 500 sensor package IDs.
Definition: roomba_500.h:80
@ SENSPACK_BATTERY_CAPACITY
Battery capacity.
Definition: roomba_500.h:99
@ SENSPACK_CLIFF_FRONT_LEFT
Front left cliff sensor.
Definition: roomba_500.h:84
@ SENSPACK_WALL
Wall sensor.
Definition: roomba_500.h:82
@ SENSPACK_STASIS
Caster wheel stasis (forward.
Definition: roomba_500.h:129
@ SENSPACK_IR_CHAR_RIGHT
Right IR character.
Definition: roomba_500.h:124
@ SENSPACK_GROUP_107
Packet IDs 54-58.
Definition: roomba_500.h:142
@ SENSPACK_DIRT_DETECT
Dirt detection sensor.
Definition: roomba_500.h:89
@ SENSPACK_LIGHT_BUMPER_CENTER_LEFT
Center left bumper signal.
Definition: roomba_500.h:119
@ SENSPACK_CURRENT
Current.
Definition: roomba_500.h:96
@ SENSPACK_LIGHT_BUMPER_FRONT_RIGHT
Front right bumper signal.
Definition: roomba_500.h:121
@ SENSPACK_GROUP_5
Packet IDs 35-42.
Definition: roomba_500.h:137
@ SENSPACK_CLIFF_FRONT_RIGHT
Front right cliff sensor.
Definition: roomba_500.h:85
@ SENSPACK_OI_MODE
Open Interface mode.
Definition: roomba_500.h:106
@ SENSPACK_GROUP_1
Packet IDs 7-16.
Definition: roomba_500.h:133
@ SENSPACK_CLIFF_RIGHT_SIGNAL
Front right cliff signal value.
Definition: roomba_500.h:104
@ SENSPACK_RIGHT_MOTOR_CURRENT
Right motor current.
Definition: roomba_500.h:126
@ SENSPACK_TEMPERATURE
Temperature.
Definition: roomba_500.h:97
@ SENSPACK_VIRTUAL_WALL
Virtual wall detector.
Definition: roomba_500.h:87
@ SENSPACK_CLIFF_LEFT_SIGNAL
Left cliff signal value.
Definition: roomba_500.h:101
@ SENSPACK_SONG_NUMBER
Song number.
Definition: roomba_500.h:107
@ SENSPACK_CLIFF_RIGHT
Right cliff sensor.
Definition: roomba_500.h:86
@ SENSPACK_LIGHT_BUMPER_FRONT_LEFT
Front left bumper signal.
Definition: roomba_500.h:118
@ SENSPACK_LIGHT_BUMPER
Light bumper status.
Definition: roomba_500.h:116
@ SENSPACK_RIGHT_ENCODER
Right encoder value.
Definition: roomba_500.h:114
@ SENSPACK_SIDE_BRUSH_MOTOR_CURRENT
Side brush motor current.
Definition: roomba_500.h:128
@ SENSPACK_GROUP_3
Packet IDs 21-26.
Definition: roomba_500.h:135
@ SENSPACK_LEFT_MOTOR_CURRENT
Left motor current.
Definition: roomba_500.h:125
@ SENSPACK_LIGHT_BUMPER_RIGHT
Right bumper signal.
Definition: roomba_500.h:122
@ SENSPACK_GROUP_106
Packet IDs 46-51.
Definition: roomba_500.h:141
@ SENSPACK_GROUP_0
Packet IDs 7-26.
Definition: roomba_500.h:132
@ SENSPACK_LIGHT_BUMPER_CENTER_RIGHT
Center right bumper signal.
Definition: roomba_500.h:120
@ SENSPACK_GROUP_6
Packet IDs 7-42.
Definition: roomba_500.h:138
@ SENSPACK_GROUP_4
Packet IDs 27-34.
Definition: roomba_500.h:136
@ SENSPACK_SONG_PLAYING
Song playing indicator.
Definition: roomba_500.h:108
@ SENSPACK_STREAM_PACKETS
Number of stream packets.
Definition: roomba_500.h:109
@ SENSPACK_DISTANCE
Travelled distance.
Definition: roomba_500.h:92
@ SENSPACK_CHARGING_STATE
Charging state.
Definition: roomba_500.h:94
@ SENSPACK_VOLTAGE
Voltage.
Definition: roomba_500.h:95
@ SENSPACK_GROUP_101
Packet IDs 43-58.
Definition: roomba_500.h:140
@ SENSPACK_REQ_VELOCITY
Requested velocity.
Definition: roomba_500.h:110
@ SENSPACK_CLIFF_LEFT
Left cliff sensor.
Definition: roomba_500.h:83
@ SENSPACK_BRUSH_MOTOR_CURRENT
Brush motor current.
Definition: roomba_500.h:127
@ SENSPACK_BUTTONS
Button status.
Definition: roomba_500.h:91
@ SENSPACK_CLIFF_FRONT_LEFT_SIGNAL
Front left cliff signal value.
Definition: roomba_500.h:102
@ SENSPACK_GROUP_ALL
All packet IDs (7-58)
Definition: roomba_500.h:139
@ SENSPACK_ANGLE
Turned angle.
Definition: roomba_500.h:93
@ SENSPACK_CLIFF_FRONT_RIGHT_SIGNAL
Right cliff signal value.
Definition: roomba_500.h:103
@ SENSPACK_LEFT_ENCODER
Left encoder value.
Definition: roomba_500.h:115
@ SENSPACK_LIGHT_BUMPER_LEFT
Left bumper signal.
Definition: roomba_500.h:117
@ SENSPACK_IR_CHAR_OMNI
Omnidirectional IR receiver.
Definition: roomba_500.h:90
@ SENSPACK_REQ_RADIUS
Requested radius.
Definition: roomba_500.h:111
@ SENSPACK_IR_CHAR_LEFT
Left IR character.
Definition: roomba_500.h:123
@ SENSPACK_GROUP_2
Packet IDs 17-20.
Definition: roomba_500.h:134
@ SENSPACK_REQ_RIGHT_VELOCITY
Requested right velocity.
Definition: roomba_500.h:112
@ SENSPACK_BUMPS_DROPS
Bumper and wheel drops.
Definition: roomba_500.h:81
@ SENSPACK_REQ_LEFT_VELOCITY
Requested left velocity.
Definition: roomba_500.h:113
@ SENSPACK_BATTERY_CHARGE
Battery charge.
Definition: roomba_500.h:98
@ SENSPACK_WALL_SIGNAL
Wall signal value.
Definition: roomba_500.h:100
@ SENSPACK_WHEEL_OVERCURRENTS
Overcurrents.
Definition: roomba_500.h:88
@ SENSPACK_CHARGE_SOURCES
Available charge sources.
Definition: roomba_500.h:105
void disable_sensors()
Disable sensor data stream.
Definition: roomba_500.cpp:733
static const short int MAX_RADIUS_MM
Maximum drive radius.
Definition: roomba_500.h:365
static const unsigned char BUTTON_HOUR
Hour button.
Definition: roomba_500.h:236
void drive_pwm(short int left_wheel_pwm, short int right_wheel_pwm)
Directly control wheel velocities via PWM.
Definition: roomba_500.cpp:990
static const unsigned char MOTOR_SIDE_BRUSH_BACKWARD
Side backward bit.
Definition: roomba_500.h:291
static const unsigned char BUMP_LEFT
Left bumper bit.
Definition: roomba_500.h:243
static const unsigned char BUTTON_SPOT
Spot cleaning button.
Definition: roomba_500.h:233
static const unsigned char BUMPER_RIGHT
Right bumper.
Definition: roomba_500.h:259
static const unsigned char MOTOR_MAIN_BRUSHES_BACKWARD
Main backward bit.
Definition: roomba_500.h:292
static const unsigned char MOTOR_VACUUM
Vacuum motor bit.
Definition: roomba_500.h:289
static const unsigned char WEEKDAY_LED_FRI
Friday.
Definition: roomba_500.h:271
TurnDirection
Turning direction.
Definition: roomba_500.h:227
@ TURN_CLOCKWISE
Clockwise turning.
Definition: roomba_500.h:228
static const unsigned char CHARGER_INTERNAL
Internal charger bit.
Definition: roomba_500.h:295
static const float AXLE_LENGTH
Axle length.
Definition: roomba_500.h:362
static const unsigned char BUMPER_LEFT
Left bumper.
Definition: roomba_500.h:254
static const unsigned char WHEEL_DROP_RIGHT
Right wheel drop bit.
Definition: roomba_500.h:242
static const unsigned short int MODE_CHANGE_WAIT_MS
Time in ms to wait after mode changes.
Definition: roomba_500.h:371
static const unsigned char OVERCURRENT_WHEEL_RIGHT
Right wheel bit.
Definition: roomba_500.h:247
void drive_direct(short int left_mm_per_sec, short int right_mm_per_sec)
Directly control wheel velocities.
Definition: roomba_500.cpp:963
static const unsigned char CHECKSUM_SIZE
Checksum byte size.
Definition: roomba_500.h:374
static const unsigned char DIGIT_LED_SOUTH_EAST
Bottom right segment.
Definition: roomba_500.h:285
static const unsigned char LED_CHECK_ROBOT
Check robot LED bit.
Definition: roomba_500.h:264
static const unsigned char CHARGER_HOME_BASE
Home base charger bit.
Definition: roomba_500.h:294
void enable_sensors()
Enable sensor data stream.
Definition: roomba_500.cpp:714
static const short int MAX_PWM
Maximum PWM value for wheels.
Definition: roomba_500.h:367
File could not be opened.
Definition: system.h:53
Base class for exceptions in Fawkes.
Definition: exception.h:36
Mutex locking helper.
Definition: mutex_locker.h:34
void lock()
Lock this mutex.
Definition: mutex.cpp:87
void unlock()
Unlock the mutex.
Definition: mutex.cpp:131
The current system call has timed out before completion.
Definition: system.h:46
Fawkes library namespace.
Struct for packet group with everything (SENSPACK_GROUP_ALL).
Definition: roomba_500.h:379