23 #include "roomba_500.h" 25 #include <core/exceptions/system.h> 26 #include <core/threading/mutex.h> 27 #include <core/threading/mutex_locker.h> 35 #include <netinet/in.h> 39 # include <bluetooth/bluetooth.h> 40 # include <bluetooth/hci.h> 41 # include <bluetooth/hci_lib.h> 42 # include <bluetooth/rfcomm.h> 43 # include <sys/socket.h> 46 static const bdaddr_t _BDADDR_ANY = {{0, 0, 0, 0, 0, 0}};
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;
204 int16_t left_velocity;
205 int16_t right_velocity;
216 } StreamOnePacketParams;
244 __conntype = conntype;
245 __conn_flags = flags;
247 if (__conntype == CONNTYPE_ROOTOOTH) {
248 throw Exception(
"Native RooTooth not available at compile time.");
253 __packet_id = SENSPACK_GROUP_ALL;
254 __sensors_enabled =
false;
256 __device = strdup(device);
258 __sensor_mutex =
new Mutex();
259 __read_mutex =
new Mutex();
260 __write_mutex =
new Mutex();
266 delete __write_mutex;
268 delete __sensor_mutex;
279 delete __write_mutex;
281 delete __sensor_mutex;
289 if (__conntype == CONNTYPE_SERIAL) {
290 struct termios param;
292 __fd = ::open(__device, O_NOCTTY | O_RDWR);
297 if (tcgetattr(__fd, ¶m) == -1) {
298 Exception e(errno,
"Getting the port parameters failed");
304 cfsetospeed(¶m, B115200);
305 cfsetispeed(¶m, B115200);
307 param.c_lflag &= ~(ICANON | ECHO | ECHOE | ECHOK | ECHONL | ISIG | IEXTEN);
308 param.c_iflag &= ~(INLCR | IGNCR | ICRNL | IGNBRK | PARMRK);
311 param.c_iflag &= ~(IXON | IXOFF | IXANY);
312 param.c_cflag &= ~CRTSCTS;
314 param.c_cflag |= (CREAD | CLOCAL);
317 param.c_cflag &= ~CS5 & ~CS6 & ~CS7 & ~CS8;
318 param.c_cflag |= CS8;
321 param.c_cflag &= ~(PARENB | PARODD);
322 param.c_iflag &= ~(INPCK | ISTRIP);
325 param.c_cflag &= ~CSTOPB;
328 param.c_oflag &= ~OPOST;
330 param.c_cc[VMIN] = 1;
331 param.c_cc[VTIME] = 0;
333 tcflush(__fd, TCIOFLUSH);
335 if (tcsetattr(__fd, TCSANOW, ¶m) != 0) {
336 Exception e(errno,
"Setting the port parameters failed");
343 throw Exception(
"Native RooTooth support unavailable at compile time.");
345 struct hci_dev_info di;
346 inquiry_info *ii = NULL;
347 int max_rsp, num_rsp;
348 int dev_id, sock, len, flags;
349 char addrstr[19] = { 0 };
350 char name[248] = { 0 };
351 bdaddr_t baddr = _BDADDR_ANY;
353 if ((dev_id = hci_get_route(NULL)) < 0) {
354 throw Exception(
"RooTooth: local bluetooth device is not available");
357 if (hci_devinfo(dev_id, &di) < 0) {
358 throw Exception(
"RooTooth: cannot get local device info.");
361 if ((sock = hci_open_dev(dev_id)) < 0) {
362 throw Exception(
"RooTooth: failed to open socket.");
367 flags = IREQ_CACHE_FLUSH;
368 ii = (inquiry_info*)malloc(max_rsp *
sizeof(inquiry_info));
370 if (strcmp(__device,
"") == 0) {
373 num_rsp = hci_inquiry(dev_id, len, max_rsp, NULL, &ii, flags);
375 throw Exception(errno,
"HCI inquiry failed");
378 for (
int i = 0; i < num_rsp; i++) {
381 baswap((bdaddr_t *) b, &(ii+i)->bdaddr);
383 ba2str(&(ii+i)->bdaddr, addrstr);
394 (b[0] == 0x00) && (b[1] == 0x06) && (b[2] == 0x66) &&
397 (ii[i].dev_class[0] == 0x00) &&
398 (ii[i].dev_class[1] == 0x1f) &&
399 (ii[i].dev_class[2] == 0x00) )
402 memset(name, 0,
sizeof(name));
403 hci_read_remote_name(sock, &(ii+i)->bdaddr,
sizeof(name), name, 0);
406 (fnmatch(
"FireFly-*", name, FNM_NOESCAPE) == 0) ||
407 (strcmp(
"RooTooth", name) == 0) )
410 ba2str(&(ii+i)->bdaddr, addrstr);
413 __device = strdup(addrstr);
414 bacpy(&baddr, &(ii+i)->bdaddr);
420 bool is_bdaddr = (bachk(__device) == 0);
425 str2ba(__device, &baddr);
426 ba2str(&baddr, addrstr);
432 num_rsp = hci_inquiry(dev_id, len, max_rsp, NULL, &ii, flags);
434 throw Exception(errno,
"HCI inquiry failed");
438 for (
int i = 0; i < num_rsp; i++) {
439 ba2str(&(ii+i)->bdaddr, addrstr);
440 memset(name, 0,
sizeof(name));
441 if (hci_read_remote_name(sock, &(ii+i)->bdaddr,
sizeof(name),
444 strcpy(name,
"[unknown]");
446 if (fnmatch(__device, name, FNM_NOESCAPE) == 0) {
450 __device = strdup(addrstr);
451 bacpy(&baddr, &(ii+i)->bdaddr);
461 if (bacmp(&baddr, &_BDADDR_ANY) == 0) {
462 throw Exception(
"Could not find RooTooth device.");
464 ba2str(&baddr, addrstr);
467 struct sockaddr_rc rcaddr = { 0 };
470 __fd = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
473 rcaddr.rc_family = AF_BLUETOOTH;
474 rcaddr.rc_channel = (uint8_t) 1;
475 bacpy(&rcaddr.rc_bdaddr, &baddr);
478 if (connect(__fd, (
struct sockaddr *)&rcaddr,
sizeof(rcaddr)) < 0) {
479 throw Exception(errno,
"Failed to connect to %s", addrstr);
485 usleep(MODE_CHANGE_WAIT_MS * 1000);
486 __mode = MODE_PASSIVE;
495 if (flags & FLAG_FIREFLY_FASTMODE) {
496 const char *cmd_seq =
"$$$";
497 if (write(__fd, cmd_seq, 3) != 3) {
499 throw Exception(errno,
"Roomba500 (RooTooth): Failed to send command " 500 "sequence to enable fast mode");
503 timeval timeout = {1, 500000};
506 FD_SET(__fd, &read_fds);
509 rv = select(__fd + 1, &read_fds, NULL, NULL, &timeout);
513 if (read(__fd, cmd_reply, 4) == 4) {
514 if (strncmp(cmd_reply,
"CMD", 3) == 0) {
516 const char *cmd_fastmode =
"F,1\r";
517 if (write(__fd, cmd_fastmode, 4) != 4) {
519 throw Exception(errno,
"Roomba500 (RooTooth): Failed to send fast " 520 "mode command sequence.");
532 usleep(MODE_CHANGE_WAIT_MS * 1000);
533 __mode = MODE_PASSIVE;
561 const void *params,
const size_t plength)
566 __obuffer[0] = opcode;
567 __obuffer_length = 1;
569 if (params && (plength > 0)) {
570 if (plength > (
sizeof(__obuffer) - __obuffer_length)) {
571 throw Exception(
"Parameters for command %i too long, maximum length is %zu",
572 opcode, (
sizeof(__obuffer) - __obuffer_length));
574 unsigned char *pbytes = (
unsigned char *)params;
575 for (
size_t i = 0; i < plength; ++i) {
576 __obuffer[1+i] = pbytes[i];
578 __obuffer_length += plength;
581 int written = write(__fd, __obuffer, __obuffer_length);
592 throw Exception(errno,
"Failed to write Roomba 500 packet %i", opcode);
593 }
else if (written < __obuffer_length) {
594 throw Exception(
"Failed to write Roomba 500 packet %i, " 595 "only %d of %d bytes sent",
596 opcode, written, __obuffer_length);
607 Roomba500::recv(
size_t index,
size_t num_bytes,
unsigned int timeout_ms)
609 timeval timeout = {0, (suseconds_t)timeout_ms * 1000};
613 FD_SET(__fd, &read_fds);
616 rv = select(__fd + 1, &read_fds, NULL, NULL, (timeout_ms > 0) ? &timeout : NULL);
619 throw Exception(errno,
"Roomba500::recv(): select on file descriptor failed");
620 }
else if ( rv == 0 ) {
624 __ibuffer_length = 0;
628 while (bytes_read < (
int)num_bytes) {
629 int rv = read(__fd, &__ibuffer[index] +bytes_read, num_bytes -bytes_read);
631 throw Exception(errno,
"Roomba500::recv(): read failed");
636 if (bytes_read < (
int)num_bytes) {
637 throw Exception(
"Roomba500::recv(): failed to read packet data");
640 __ibuffer_length = index + num_bytes;
650 if (!__sensors_enabled) {
651 throw Exception(
"Roomba 500 sensors have not been enabled.");
654 timeval timeout = {0, 0};
658 FD_SET(__fd, &read_fds);
661 rv = select(__fd + 1, &read_fds, NULL, NULL, &timeout);
677 if (!__sensors_enabled) {
678 throw Exception(
"Roomba 500 sensors have not been enabled.");
682 unsigned int skipped = 0;
684 __ibuffer_length = 0;
686 recv(__ibuffer_length, 1);
687 if (__ibuffer[0] != 19) {
692 recv(__ibuffer_length, 1);
693 if (__ibuffer[1] != __packet_length + 1) {
698 recv(__ibuffer_length, 1);
699 if (__ibuffer[2] != __packet_id) {
704 recv(__ibuffer_length, __packet_length);
706 recv(__ibuffer_length++, 1);
708 unsigned int sum = 0;
709 for (
int i = 0; i < __ibuffer_length; ++i) {
713 if ((sum & 0xFF) != 0) {
714 __sensor_packet_received =
false;
716 __sensor_mutex->lock();
718 __sensor_packet_received =
true;
719 __sensor_mutex->unlock();
745 StreamOnePacketParams sp;
747 sp.packet_id = SENSPACK_GROUP_ALL;
749 send(OPCODE_STREAM, &sp,
sizeof(StreamOnePacketParams));
751 __packet_id = SENSPACK_GROUP_ALL;
752 __packet_reply_id = 19;
753 __packet_length = get_packet_size(SENSPACK_GROUP_ALL);
754 __sensors_enabled =
true;
755 __sensor_packet_received =
false;
765 unsigned char streamstate = STREAM_DISABLE;
767 send(OPCODE_PAUSE_RESUME_STREAM, &streamstate, 1);
769 __sensors_enabled =
false;
770 __sensor_packet_received =
false;
783 unsigned char p = SENSPACK_GROUP_ALL;
785 send(OPCODE_QUERY, &p, 1);
787 __packet_id = SENSPACK_GROUP_ALL;
788 __packet_reply_id = 0;
789 __packet_length = get_packet_size(SENSPACK_GROUP_ALL);
790 __sensor_packet_received =
true;
793 __read_mutex->lock();
794 recv(0, __packet_length);
795 __read_mutex->unlock();
797 __sensor_mutex->lock();
799 __sensor_mutex->unlock();
810 if (! __sensor_packet_received) {
811 throw Exception(
"No valid data received, yet.");
814 return __sensor_packet;
826 if (mode == MODE_PASSIVE) {
828 }
if (mode == MODE_SAFE) {
830 }
else if (mode == MODE_FULL) {
832 }
else if (mode == MODE_OFF) {
833 throw Exception(
"Mode OFF cannot be set, use power_down() instead");
836 usleep(MODE_CHANGE_WAIT_MS * 1000);
849 __mode = MODE_PASSIVE;
861 __mode = MODE_PASSIVE;
873 send(OPCODE_SEEK_DOCK);
874 __mode = MODE_PASSIVE;
887 __mode = MODE_PASSIVE;
912 if (velo_mm_per_sec < -MAX_LIN_VEL_MM_S) velo_mm_per_sec = -MAX_LIN_VEL_MM_S;
913 if (velo_mm_per_sec > MAX_LIN_VEL_MM_S) velo_mm_per_sec = MAX_LIN_VEL_MM_S;
916 dp.velocity = htons(velo_mm_per_sec);
917 dp.radius = htons(0x8000);
919 send(OPCODE_DRIVE, &dp,
sizeof(DriveParams));
932 dp.velocity = htons(0);
933 dp.radius = (direction == TURN_CLOCKWISE) ? -1 : 1;
935 send(OPCODE_DRIVE, &dp,
sizeof(DriveParams));
949 if (velo_mm_per_sec < -MAX_LIN_VEL_MM_S) velo_mm_per_sec = -MAX_LIN_VEL_MM_S;
950 if (velo_mm_per_sec > MAX_LIN_VEL_MM_S) velo_mm_per_sec = MAX_LIN_VEL_MM_S;
952 if (radius_mm < -MAX_RADIUS_MM) radius_mm = -MAX_RADIUS_MM;
953 if (radius_mm > MAX_RADIUS_MM) radius_mm = MAX_RADIUS_MM;
956 dp.velocity = htons(velo_mm_per_sec);
957 dp.radius = htons(radius_mm);
959 send(OPCODE_DRIVE, &dp,
sizeof(DriveParams));
972 if (velo_mm_per_sec < -MAX_LIN_VEL_MM_S) velo_mm_per_sec = -MAX_LIN_VEL_MM_S;
973 if (velo_mm_per_sec > MAX_LIN_VEL_MM_S) velo_mm_per_sec = MAX_LIN_VEL_MM_S;
975 if (radius_mm < -MAX_RADIUS_MM) radius_mm = -MAX_RADIUS_MM;
976 if (radius_mm > MAX_RADIUS_MM) radius_mm = 0x8000;
979 dp.velocity = htons(velo_mm_per_sec);
980 dp.radius = htons(radius_mm);
982 send(OPCODE_DRIVE, &dp,
sizeof(DriveParams));
993 short int right_mm_per_sec)
997 if (left_mm_per_sec < -MAX_LIN_VEL_MM_S) left_mm_per_sec = -MAX_LIN_VEL_MM_S;
998 if (left_mm_per_sec > MAX_LIN_VEL_MM_S) left_mm_per_sec = MAX_LIN_VEL_MM_S;
1000 if (right_mm_per_sec < -MAX_LIN_VEL_MM_S) right_mm_per_sec = -MAX_LIN_VEL_MM_S;
1001 if (right_mm_per_sec > MAX_LIN_VEL_MM_S) right_mm_per_sec = MAX_LIN_VEL_MM_S;
1003 DriveDirectParams dp;
1004 dp.left_velocity = htons(left_mm_per_sec);
1005 dp.right_velocity = htons(right_mm_per_sec);
1007 send(OPCODE_DRIVE, &dp,
sizeof(DriveDirectParams));
1021 if (left_wheel_pwm < -MAX_PWM) left_wheel_pwm = -MAX_PWM;
1022 if (left_wheel_pwm > MAX_PWM) left_wheel_pwm = MAX_PWM;
1024 if (right_wheel_pwm < -MAX_PWM) right_wheel_pwm = -MAX_PWM;
1025 if (right_wheel_pwm > MAX_PWM) right_wheel_pwm = MAX_PWM;
1028 dp.left_pwm = htons(left_wheel_pwm);
1029 dp.right_pwm = htons(right_wheel_pwm);
1031 send(OPCODE_DRIVE, &dp,
sizeof(DrivePWMParams));
1045 bool main_backward,
bool side_backward)
1049 unsigned char param = 0;
1050 if (main) param |= MOTOR_MAIN_BRUSHES;
1051 if (side) param |= MOTOR_SIDE_BRUSH;
1052 if (vacuum) param |= MOTOR_VACUUM;
1053 if (main_backward) param |= MOTOR_MAIN_BRUSHES_BACKWARD;
1054 if (side_backward) param |= MOTOR_SIDE_BRUSH_BACKWARD;
1056 send(OPCODE_MOTORS, ¶m, 1);
1072 unsigned char clean_color,
unsigned char clean_intensity)
1076 unsigned char param[3] = {0, clean_color, clean_intensity};
1077 if (debris) param[0] |= LED_DEBRIS;
1078 if (spot) param[0] |= LED_SPOT;
1079 if (dock) param[0] |= LED_DOCK;
1080 if (check_robot) param[0] |= LED_CHECK_ROBOT;
1082 send(OPCODE_LEDS, param, 3);
1097 send(OPCODE_DIGIT_LEDS_ASCII, digits, 4);
1107 unsigned char p[14];
1133 send(OPCODE_SONG, p,
sizeof(p));
1134 send(OPCODE_PLAY, &play, 1);
1147 case SENSPACK_BUMPS_DROPS:
return SENSPACK_SIZE_BUMPS_DROPS;
1148 case SENSPACK_WALL:
return SENSPACK_SIZE_WALL;
1149 case SENSPACK_CLIFF_LEFT:
return SENSPACK_SIZE_CLIFF_LEFT;
1150 case SENSPACK_CLIFF_FRONT_LEFT:
return SENSPACK_SIZE_CLIFF_FRONT_LEFT;
1151 case SENSPACK_CLIFF_FRONT_RIGHT:
return SENSPACK_SIZE_CLIFF_FRONT_RIGHT;
1152 case SENSPACK_CLIFF_RIGHT:
return SENSPACK_SIZE_CLIFF_RIGHT;
1153 case SENSPACK_VIRTUAL_WALL:
return SENSPACK_SIZE_VIRTUAL_WALL;
1154 case SENSPACK_WHEEL_OVERCURRENTS:
return SENSPACK_SIZE_WHEEL_OVERCURRENTS;
1155 case SENSPACK_DIRT_DETECT:
return SENSPACK_SIZE_DIRT_DETECT;
1156 case SENSPACK_IR_CHAR_OMNI:
return SENSPACK_SIZE_IR_CHAR_OMNI;
1157 case SENSPACK_BUTTONS:
return SENSPACK_SIZE_BUTTONS;
1158 case SENSPACK_DISTANCE:
return SENSPACK_SIZE_DISTANCE;
1159 case SENSPACK_ANGLE:
return SENSPACK_SIZE_ANGLE;
1160 case SENSPACK_CHARGING_STATE:
return SENSPACK_SIZE_CHARGING_STATE;
1161 case SENSPACK_VOLTAGE:
return SENSPACK_SIZE_VOLTAGE;
1162 case SENSPACK_CURRENT:
return SENSPACK_SIZE_CURRENT;
1163 case SENSPACK_TEMPERATURE:
return SENSPACK_SIZE_TEMPERATURE;
1164 case SENSPACK_BATTERY_CHARGE:
return SENSPACK_SIZE_BATTERY_CHARGE;
1165 case SENSPACK_BATTERY_CAPACITY:
return SENSPACK_SIZE_BATTERY_CAPACITY;
1166 case SENSPACK_WALL_SIGNAL:
return SENSPACK_SIZE_WALL_SIGNAL;
1167 case SENSPACK_CLIFF_LEFT_SIGNAL:
return SENSPACK_SIZE_CLIFF_LEFT_SIGNAL;
1168 case SENSPACK_CLIFF_FRONT_LEFT_SIGNAL:
1169 return SENSPACK_SIZE_CLIFF_FRONT_LEFT_SIGNAL;
1170 case SENSPACK_CLIFF_FRONT_RIGHT_SIGNAL:
1171 return SENSPACK_SIZE_CLIFF_FRONT_RIGHT_SIGNAL;
1172 case SENSPACK_CLIFF_RIGHT_SIGNAL:
return SENSPACK_SIZE_CLIFF_RIGHT_SIGNAL;
1173 case SENSPACK_CHARGE_SOURCES:
return SENSPACK_SIZE_CHARGE_SOURCES;
1174 case SENSPACK_OI_MODE:
return SENSPACK_SIZE_OI_MODE;
1175 case SENSPACK_SONG_NUMBER:
return SENSPACK_SIZE_SONG_NUMBER;
1176 case SENSPACK_SONG_PLAYING:
return SENSPACK_SIZE_SONG_PLAYING;
1177 case SENSPACK_STREAM_PACKETS:
return SENSPACK_SIZE_STREAM_PACKETS;
1178 case SENSPACK_REQ_VELOCITY:
return SENSPACK_SIZE_REQ_VELOCITY;
1179 case SENSPACK_REQ_RADIUS:
return SENSPACK_SIZE_REQ_RADIUS;
1180 case SENSPACK_REQ_RIGHT_VELOCITY:
return SENSPACK_SIZE_REQ_RIGHT_VELOCITY;
1181 case SENSPACK_REQ_LEFT_VELOCITY:
return SENSPACK_SIZE_REQ_LEFT_VELOCITY;
1182 case SENSPACK_RIGHT_ENCODER:
return SENSPACK_SIZE_RIGHT_ENCODER;
1183 case SENSPACK_LEFT_ENCODER:
return SENSPACK_SIZE_LEFT_ENCODER;
1184 case SENSPACK_LIGHT_BUMPER:
return SENSPACK_SIZE_LIGHT_BUMPER;
1185 case SENSPACK_LIGHT_BUMPER_LEFT:
return SENSPACK_SIZE_LIGHT_BUMPER_LEFT;
1186 case SENSPACK_LIGHT_BUMPER_FRONT_LEFT:
1187 return SENSPACK_SIZE_LIGHT_BUMPER_FRONT_LEFT;
1188 case SENSPACK_LIGHT_BUMPER_CENTER_LEFT:
1189 return SENSPACK_SIZE_LIGHT_BUMPER_CENTER_LEFT;
1190 case SENSPACK_LIGHT_BUMPER_CENTER_RIGHT:
1191 return SENSPACK_SIZE_LIGHT_BUMPER_CENTER_RIGHT;
1192 case SENSPACK_LIGHT_BUMPER_FRONT_RIGHT:
1193 return SENSPACK_SIZE_LIGHT_BUMPER_FRONT_RIGHT;
1194 case SENSPACK_LIGHT_BUMPER_RIGHT:
1195 return SENSPACK_SIZE_LIGHT_BUMPER_RIGHT;
1196 case SENSPACK_IR_CHAR_LEFT:
return SENSPACK_SIZE_IR_CHAR_LEFT;
1197 case SENSPACK_IR_CHAR_RIGHT:
return SENSPACK_SIZE_IR_CHAR_RIGHT;
1198 case SENSPACK_LEFT_MOTOR_CURRENT:
return SENSPACK_SIZE_LEFT_MOTOR_CURRENT;
1199 case SENSPACK_RIGHT_MOTOR_CURRENT:
return SENSPACK_SIZE_RIGHT_MOTOR_CURRENT;
1200 case SENSPACK_BRUSH_MOTOR_CURRENT:
return SENSPACK_SIZE_BRUSH_MOTOR_CURRENT;
1201 case SENSPACK_SIDE_BRUSH_MOTOR_CURRENT:
1202 return SENSPACK_SIZE_SIDE_BRUSH_MOTOR_CURRENT;
1203 case SENSPACK_STASIS:
return SENSPACK_SIZE_STASIS;
1204 case SENSPACK_GROUP_0:
return SENSPACK_SIZE_GROUP_0;
1205 case SENSPACK_GROUP_1:
return SENSPACK_SIZE_GROUP_1;
1206 case SENSPACK_GROUP_2:
return SENSPACK_SIZE_GROUP_2;
1207 case SENSPACK_GROUP_3:
return SENSPACK_SIZE_GROUP_3;
1208 case SENSPACK_GROUP_4:
return SENSPACK_SIZE_GROUP_4;
1209 case SENSPACK_GROUP_5:
return SENSPACK_SIZE_GROUP_5;
1210 case SENSPACK_GROUP_6:
return SENSPACK_SIZE_GROUP_6;
1211 case SENSPACK_GROUP_ALL:
return SENSPACK_SIZE_GROUP_ALL;
1212 case SENSPACK_GROUP_101:
return SENSPACK_SIZE_GROUP_101;
1213 case SENSPACK_GROUP_106:
return SENSPACK_SIZE_GROUP_106;
1214 case SENSPACK_GROUP_107:
return SENSPACK_SIZE_GROUP_107;
1216 throw Exception(
"Roomba500:get_packet_size(): unknown packet ID %i", packet);
void play_fanfare()
Play a simple fanfare.
static const unsigned short int STREAM_INTERVAL_MS
Time in ms between.
static const unsigned char MOTOR_SIDE_BRUSH
Side brush motor bit.
void drive_direct(short int left_mm_per_sec, short int right_mm_per_sec)
Directly control wheel velocities.
static const unsigned char CHARGER_HOME_BASE
Home base charger bit.
File could not be opened.
static const unsigned char OVERCURRENT_WHEEL_RIGHT
Right wheel bit.
static const unsigned char CHARGING_SOURCE_INTERNAL
Internal socket.
static const short int MAX_LIN_VEL_MM_S
Maximum linear velocity.
void stop()
Stop moption of the Roomba.
static const unsigned char BUMPER_CENTER_RIGHT
Center right bumper.
void drive_arc(short int velo_mm_per_sec, short int radius_mm)
Drive Roomba on an arc.
bool is_data_available()
Check if data is available.
Roomba500(ConnectionType conntype, const char *device, unsigned int flags=0)
Constructor.
static const unsigned char WEEKDAY_LED_THU
Thursday.
static const unsigned char WHEEL_DROP_RIGHT
Right wheel drop bit.
void drive_pwm(short int left_wheel_pwm, short int right_wheel_pwm)
Directly control wheel velocities via PWM.
static const unsigned char BUTTON_DAY
Day button.
static const unsigned char WHEEL_DROP_LEFT
Left wheel drop bit.
static const unsigned short int MODE_CHANGE_WAIT_MS
Time in ms to wait after mode changes.
static const short int MAX_RADIUS_MM
Maximum drive radius.
static const unsigned char CHECKSUM_SIZE
Checksum byte size.
static const short int MAX_PWM
Maximum PWM value for wheels.
static const unsigned char LED_DEBRIS
Debris LED bit.
static const float DIAMETER
Robot diameter.
Fawkes library namespace.
void enable_sensors()
Enable sensor data stream.
void seek_dock()
Seek for the home base and dock.
static const unsigned char LED_SPOT
Spot LED bit.
static const float BUMPER_X_OFFSET
X Offset of bumper.
The current system call has timed out before completion.
static const unsigned char BUTTON_CLEAN
Cleaning button.
static const unsigned char SCHEDULING_LED_PM
PM LED bit.
void drive_straight(short int velo_mm_per_sec)
Drive Roomba straight.
static const unsigned char DIGIT_LED_NORTH_EAST
Top right segment LED.
static const unsigned char BUTTON_DOCK
Dock button.
static const unsigned char BUTTON_HOUR
Hour button.
static const unsigned char BUMPER_LEFT
Left bumper.
static const unsigned char MOTOR_SIDE_BRUSH_BACKWARD
Side backward bit.
static const unsigned char DIGIT_LED_NORTH_WEST
Top left segment LED.
void power_down()
Powers down the Roomba.
void disable_sensors()
Disable sensor data stream.
static const unsigned char BUMPER_FRONT_LEFT
Front left bumper.
static const unsigned char SCHEDULING_LED_AM
AM LED bit.
static const unsigned char DIGIT_LED_SOUTH_EAST
Bottom right segment.
static const unsigned char LED_CHECK_ROBOT
Check robot LED bit.
static const unsigned char LED_DOCK
Dock LED bit.
Mode
Roomba 500 operation mode.
static const unsigned short int MAX_ENCODER_COUNT
Maximum encoder count.
void clean_spot()
Start spot cleaning operation.
static const unsigned char SCHEDULING_LED_SCHEDULE
Schedule LED bit.
static const unsigned char BUMPER_CENTER_LEFT
Center left bumper.
static const unsigned char BUTTON_SPOT
Spot cleaning button.
static const unsigned char OVERCURRENT_WHEEL_LEFT
Left wheel bit.
SensorPacketID
Roomba 500 sensor package IDs.
static const unsigned char MOTOR_MAIN_BRUSHES_BACKWARD
Main backward bit.
static const unsigned char CHARGER_INTERNAL
Internal charger bit.
static const unsigned char OVERCURRENT_MAIN_BRUSH
Main brush bit.
Base class for exceptions in Fawkes.
ConnectionType
Connection type.
static const unsigned char DIGIT_LED_CENTER
Center segment LED.
static const unsigned char BUTTON_SCHEDULE
Schedule button.
void clean()
Start normal cleaning operation.
void close()
Close serial connection.
static const unsigned char WEEKDAY_LED_FRI
Friday.
void query_sensors()
Query sensor once.
void set_motors(bool main=true, bool side=true, bool vacuum=true, bool main_backward=false, bool side_backward=false)
Set motor states (brushes and vacuum).
static const unsigned char BUTTON_CLOCK
Clock button.
static const unsigned char WEEKDAY_LED_SAT
Saturday.
static const unsigned char BUTTON_MINUTE
Minute button.
const SensorPacketGroupAll get_sensor_packet() const
Get latest sensor packet.
static const unsigned char SCHEDULING_LED_CLOCK
Clock LED bit.
static const unsigned char MOTOR_MAIN_BRUSHES
Main brush motor bit.
static const unsigned char WEEKDAY_LED_TUE
Tuesday.
static const unsigned char OVERCURRENT_SIDE_BRUSH
Side brush bit.
static const unsigned char WEEKDAY_LED_WED
Wednesday.
static const unsigned char BUMPER_RIGHT
Right bumper.
static const unsigned char DIGIT_LED_SOUTH
Bottom segment LED.
void set_mode(Mode mode)
Set control mode.
static const unsigned char WEEKDAY_LED_MON
Monday.
static const unsigned char DIGIT_LED_NORTH
Top segment LED.
void read_sensors()
Read sensor values.
void set_leds(bool debris, bool spot, bool dock, bool check_robot, unsigned char clean_color, unsigned char clean_intensity)
Set LED status of main LEDs.
Struct for packet group with everything (SENSPACK_GROUP_ALL).
Mutex mutual exclusion lock.
static const unsigned char BUMP_RIGHT
Right bumper bit.
static const unsigned char CHARGING_SOURCE_HOME_BASE
Docking station.
void drive(short int velocity_mm_per_sec, short int radius_mm)
Drive Roomba.
static const unsigned char BUMPER_FRONT_RIGHT
Front right bumper.
static const unsigned char MOTOR_VACUUM
Vacuum motor bit.
void open()
Open serial port.
OpCode
Roomba 500 Command op codes.
void set_digit_leds(const char digits[4])
Set digit LEDs.
static const unsigned char DIGIT_LED_SOUTH_WEST
Bottom left segment.
static unsigned short int get_packet_size(SensorPacketID packet)
Get size of packet.
TurnDirection
Turning direction.
static const unsigned char SCHEDULING_LED_COLON
Colon LED bit.
static const unsigned char BUMP_LEFT
Left bumper bit.
static const unsigned char WEEKDAY_LED_SUN
Sunday.
void drive_turn(TurnDirection direction)
Turn robot on the spot.
static const float AXLE_LENGTH
Axle length.