23 #include "roomba_500.h" 25 #include <core/exceptions/system.h> 26 #include <core/threading/mutex_locker.h> 27 #include <netinet/in.h> 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> 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;
196 #pragma pack(push, 1) 206 int16_t left_velocity;
207 int16_t right_velocity;
220 } StreamOnePacketParams;
247 conntype_ = conntype;
251 throw Exception(
"Native RooTooth not available at compile time.");
257 sensors_enabled_ =
false;
273 struct termios param;
275 fd_ =
::open(device_.c_str(), O_NOCTTY | O_RDWR);
280 if (tcgetattr(fd_, ¶m) == -1) {
281 Exception e(errno,
"Getting the port parameters failed");
287 cfsetospeed(¶m, B115200);
288 cfsetispeed(¶m, B115200);
290 param.c_lflag &= ~(ICANON | ECHO | ECHOE | ECHOK | ECHONL | ISIG | IEXTEN);
291 param.c_iflag &= ~(INLCR | IGNCR | ICRNL | IGNBRK | PARMRK);
294 param.c_iflag &= ~(IXON | IXOFF | IXANY);
295 param.c_cflag &= ~CRTSCTS;
297 param.c_cflag |= (CREAD | CLOCAL);
300 param.c_cflag &= ~CS5 & ~CS6 & ~CS7 & ~CS8;
301 param.c_cflag |= CS8;
304 param.c_cflag &= ~(PARENB | PARODD);
305 param.c_iflag &= ~(INPCK | ISTRIP);
308 param.c_cflag &= ~CSTOPB;
311 param.c_oflag &= ~OPOST;
313 param.c_cc[VMIN] = 1;
314 param.c_cc[VTIME] = 0;
316 tcflush(fd_, TCIOFLUSH);
318 if (tcsetattr(fd_, TCSANOW, ¶m) != 0) {
319 Exception e(errno,
"Setting the port parameters failed");
326 throw Exception(
"Native RooTooth support unavailable at compile time.");
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;
336 if ((dev_id = hci_get_route(NULL)) < 0) {
337 throw Exception(
"RooTooth: local bluetooth device is not available");
340 if (hci_devinfo(dev_id, &di) < 0) {
341 throw Exception(
"RooTooth: cannot get local device info.");
344 if ((sock = hci_open_dev(dev_id)) < 0) {
345 throw Exception(
"RooTooth: failed to open socket.");
350 flags = IREQ_CACHE_FLUSH;
351 ii = (inquiry_info *)malloc(max_rsp *
sizeof(inquiry_info));
353 if (device_.empty()) {
356 num_rsp = hci_inquiry(dev_id, len, max_rsp, NULL, &ii, flags);
358 throw Exception(errno,
"HCI inquiry failed");
361 for (
int i = 0; i < num_rsp; i++) {
363 baswap((bdaddr_t *)b, &(ii + i)->bdaddr);
365 ba2str(&(ii + i)->bdaddr, addrstr);
376 (b[0] == 0x00) && (b[1] == 0x06) && (b[2] == 0x66) &&
379 (ii[i].dev_class[0] == 0x00) && (ii[i].dev_class[1] == 0x1f)
380 && (ii[i].dev_class[2] == 0x00)) {
382 memset(name, 0,
sizeof(name));
383 hci_read_remote_name(sock, &(ii + i)->bdaddr,
sizeof(name), name, 0);
386 (fnmatch(
"FireFly-*", name, FNM_NOESCAPE) == 0) || (strcmp(
"RooTooth", name) == 0)) {
388 ba2str(&(ii + i)->bdaddr, addrstr);
391 bacpy(&baddr, &(ii + i)->bdaddr);
397 bool is_bdaddr = (bachk(device_.c_str()) == 0);
402 str2ba(device_.c_str(), &baddr);
403 ba2str(&baddr, addrstr);
409 num_rsp = hci_inquiry(dev_id, len, max_rsp, NULL, &ii, flags);
411 throw Exception(errno,
"HCI inquiry failed");
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]");
421 if (fnmatch(device_.c_str(), name, FNM_NOESCAPE) == 0) {
425 bacpy(&baddr, &(ii + i)->bdaddr);
435 if (bacmp(&baddr, &_BDADDR_ANY) == 0) {
436 throw Exception(
"Could not find RooTooth device.");
438 ba2str(&baddr, addrstr);
441 struct sockaddr_rc rcaddr = {0};
444 fd_ = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
447 rcaddr.rc_family = AF_BLUETOOTH;
448 rcaddr.rc_channel = (uint8_t)1;
449 bacpy(&rcaddr.rc_bdaddr, &baddr);
452 if (connect(fd_, (
struct sockaddr *)&rcaddr,
sizeof(rcaddr)) < 0) {
453 throw Exception(errno,
"Failed to connect to %s", addrstr);
470 const char *cmd_seq =
"$$$";
471 if (write(fd_, cmd_seq, 3) != 3) {
474 "Roomba500 (RooTooth): Failed to send command " 475 "sequence to enable fast mode");
478 timeval timeout = {1, 500000};
481 FD_SET(fd_, &read_fds);
484 rv = select(fd_ + 1, &read_fds, NULL, NULL, &timeout);
488 if (read(fd_, cmd_reply, 4) == 4) {
489 if (strncmp(cmd_reply,
"CMD", 3) == 0) {
491 const char *cmd_fastmode =
"F,1\r";
492 if (write(fd_, cmd_fastmode, 4) != 4) {
495 "Roomba500 (RooTooth): Failed to send fast " 496 "mode command sequence.");
534 Roomba500::send(
Roomba500::OpCode opcode,
const void *params,
const size_t plength)
539 obuffer_[0] = opcode;
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",
546 (
sizeof(obuffer_) - obuffer_length_));
548 unsigned char *pbytes = (
unsigned char *)params;
549 for (
size_t i = 0; i < plength; ++i) {
550 obuffer_[1 + i] = pbytes[i];
552 obuffer_length_ += plength;
555 int written = write(fd_, obuffer_, obuffer_length_);
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",
582 Roomba500::recv(
size_t index,
size_t num_bytes,
unsigned int timeout_ms)
584 timeval timeout = {0, (suseconds_t)timeout_ms * 1000};
588 FD_SET(fd_, &read_fds);
591 rv = select(fd_ + 1, &read_fds, NULL, NULL, (timeout_ms > 0) ? &timeout : NULL);
594 throw Exception(errno,
"Roomba500::recv(): select on file descriptor failed");
595 }
else if (rv == 0) {
603 while (bytes_read < (
int)num_bytes) {
604 int rv = read(fd_, &ibuffer_[index] + bytes_read, num_bytes - bytes_read);
606 throw Exception(errno,
"Roomba500::recv(): read failed");
611 if (bytes_read < (
int)num_bytes) {
612 throw Exception(
"Roomba500::recv(): failed to read packet data");
615 ibuffer_length_ = index + num_bytes;
624 if (!sensors_enabled_) {
625 throw Exception(
"Roomba 500 sensors have not been enabled.");
628 timeval timeout = {0, 0};
632 FD_SET(fd_, &read_fds);
635 rv = select(fd_ + 1, &read_fds, NULL, NULL, &timeout);
650 if (!sensors_enabled_) {
651 throw Exception(
"Roomba 500 sensors have not been enabled.");
659 recv(ibuffer_length_, 1);
660 if (ibuffer_[0] != 19) {
665 recv(ibuffer_length_, 1);
666 if (ibuffer_[1] != packet_length_ + 1) {
671 recv(ibuffer_length_, 1);
672 if (ibuffer_[2] != packet_id_) {
677 recv(ibuffer_length_, packet_length_);
679 recv(ibuffer_length_++, 1);
681 unsigned int sum = 0;
682 for (
int i = 0; i < ibuffer_length_; ++i) {
686 if ((sum & 0xFF) != 0) {
687 sensor_packet_received_ =
false;
689 sensor_mutex_.
lock();
691 sensor_packet_received_ =
true;
718 StreamOnePacketParams sp;
725 packet_reply_id_ = 19;
727 sensors_enabled_ =
true;
728 sensor_packet_received_ =
false;
741 sensors_enabled_ =
false;
742 sensor_packet_received_ =
false;
759 packet_reply_id_ = 0;
761 sensor_packet_received_ =
true;
764 recv(0, packet_length_);
767 sensor_mutex_.
lock();
779 if (!sensor_packet_received_) {
780 throw Exception(
"No valid data received, yet.");
783 return sensor_packet_;
803 throw Exception(
"Mode OFF cannot be set, use power_down() instead");
881 dp.velocity = htons(velo_mm_per_sec);
882 dp.radius = htons(0x8000);
897 dp.velocity = htons(0);
924 dp.velocity = htons(velo_mm_per_sec);
925 dp.radius = htons(radius_mm);
951 dp.velocity = htons(velo_mm_per_sec);
952 dp.radius = htons(radius_mm);
977 DriveDirectParams dp;
978 dp.left_velocity = htons(left_mm_per_sec);
979 dp.right_velocity = htons(right_mm_per_sec);
999 if (right_wheel_pwm < -
MAX_PWM)
1001 if (right_wheel_pwm >
MAX_PWM)
1005 dp.left_pwm = htons(left_wheel_pwm);
1006 dp.right_pwm = htons(right_wheel_pwm);
1024 unsigned char param = 0;
1054 unsigned char clean_color,
1055 unsigned char clean_intensity)
1059 unsigned char param[3] = {0, clean_color, clean_intensity};
1092 unsigned char p[14];
1191 default:
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 streamed sensor packets.
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.
Caster wheel stasis (forward.
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.
Omnidirectional IR receiver.
bool is_data_available()
Check if data is available.
Roomba500(ConnectionType conntype, const char *device, unsigned int flags=0)
Constructor.
Control acquired, safety measures in place.
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.
Right cliff signal value.
static const float DIAMETER
Robot diameter.
Fawkes library namespace.
void unlock()
Unlock the mutex.
void enable_sensors()
Enable sensor data stream.
void seek_dock()
Seek for the home base and dock.
Available charge sources.
static const unsigned char LED_SPOT
Spot LED bit.
Use BlueZ to find and connect to RooTooth.
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.
Front right cliff signal value.
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.
Use serial connection (device file).
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.
Front right bumper signal.
static const unsigned char OVERCURRENT_MAIN_BRUSH
Main brush bit.
Control acquired, safety measures disabled.
Requested right velocity.
Initiate communication with Roomba.
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.
Center right bumper signal.
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.
Side brush motor current.
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.
Pause or resume streaming data.
Center left bumper signal.
static const unsigned char WEEKDAY_LED_TUE
Tuesday.
Enable fast mode, assume FireFly RooTooth.
Front left bumper signal.
static const unsigned char OVERCURRENT_SIDE_BRUSH
Side brush bit.
Front left cliff signal value.
static const unsigned char WEEKDAY_LED_WED
Wednesday.
Front right cliff sensor.
static const unsigned char BUMPER_RIGHT
Right bumper.
static const unsigned char DIGIT_LED_SOUTH
Bottom segment LED.
Number of stream packets.
void set_mode(Mode mode)
Set control mode.
static const unsigned char WEEKDAY_LED_MON
Monday.
Passive mode, no control, only listening.
static const unsigned char DIGIT_LED_NORTH
Top segment LED.
void lock()
Lock this mutex.
void read_sensors()
Read sensor values.
Start normal cleaning mission.
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).
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.
Ascii control of digit LEDs.
static const float AXLE_LENGTH
Axle length.