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 read_sensors()
Read sensor values.
void seek_dock()
Seek for the home base and dock.
static const unsigned char CHARGING_SOURCE_HOME_BASE
Docking station.
static const unsigned char WEEKDAY_LED_WED
Wednesday.
static const unsigned char SCHEDULING_LED_AM
AM LED bit.
static const unsigned char LED_SPOT
Spot LED bit.
static const unsigned char BUTTON_CLEAN
Cleaning button.
OpCode
Roomba 500 Command op codes.
@ OPCODE_DIGIT_LEDS_ASCII
Ascii control of digit LEDs.
@ OPCODE_SPOT
Start spot cleaning.
@ OPCODE_PAUSE_RESUME_STREAM
Pause or resume streaming data.
@ OPCODE_DRIVE
Drive robot.
@ OPCODE_LEDS
Control LEDs.
@ OPCODE_SEEK_DOCK
Start seeking dock.
@ OPCODE_FULL
Enter full mode.
@ OPCODE_SONG
Register song.
@ OPCODE_SAFE
Enter safe mode.
@ OPCODE_START
Initiate communication with Roomba.
@ OPCODE_POWER
Power down Roomba.
@ OPCODE_STREAM
Start streaming of data.
@ OPCODE_QUERY
Query sensor info.
@ OPCODE_MOTORS
Set motor state.
@ OPCODE_CLEAN
Start normal cleaning mission.
static const unsigned char MOTOR_MAIN_BRUSHES
Main brush motor bit.
static const unsigned char DIGIT_LED_NORTH_WEST
Top left segment LED.
static const short int MAX_LIN_VEL_MM_S
Maximum linear velocity.
static const unsigned char BUMPER_CENTER_LEFT
Center left bumper.
@ FLAG_FIREFLY_FASTMODE
Enable fast mode, assume FireFly RooTooth.
static const unsigned char OVERCURRENT_WHEEL_LEFT
Left wheel bit.
static const unsigned char BUMP_RIGHT
Right bumper bit.
static const unsigned char OVERCURRENT_MAIN_BRUSH
Main brush bit.
static const unsigned char OVERCURRENT_SIDE_BRUSH
Side brush bit.
void drive_turn(TurnDirection direction)
Turn robot on the spot.
static const unsigned char BUTTON_CLOCK
Clock button.
static const unsigned char WEEKDAY_LED_MON
Monday.
const SensorPacketGroupAll get_sensor_packet() const
Get latest sensor packet.
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.
static const unsigned char WEEKDAY_LED_SUN
Sunday.
void clean()
Start normal cleaning operation.
void set_mode(Mode mode)
Set control mode.
void query_sensors()
Query sensor once.
static const unsigned char BUMPER_CENTER_RIGHT
Center right bumper.
static const unsigned char DIGIT_LED_SOUTH_WEST
Bottom left segment.
static const unsigned char BUTTON_MINUTE
Minute button.
static const unsigned char SCHEDULING_LED_PM
PM LED bit.
static const unsigned char WEEKDAY_LED_SAT
Saturday.
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.
ConnectionType
Connection type.
@ CONNTYPE_ROOTOOTH
Use BlueZ to find and connect to RooTooth.
@ CONNTYPE_SERIAL
Use serial connection (device file).
static const unsigned char MOTOR_SIDE_BRUSH
Side brush motor bit.
static const float DIAMETER
Robot diameter.
void drive_straight(short int velo_mm_per_sec)
Drive Roomba straight.
void clean_spot()
Start spot cleaning operation.
static unsigned short int get_packet_size(SensorPacketID packet)
Get size of packet.
Roomba500(ConnectionType conntype, const char *device, unsigned int flags=0)
Constructor.
@ STREAM_DISABLE
Stream disabled.
void drive_arc(short int velo_mm_per_sec, short int radius_mm)
Drive Roomba on an arc.
static const unsigned char CHARGING_SOURCE_INTERNAL
Internal socket.
static const unsigned char SCHEDULING_LED_SCHEDULE
Schedule LED bit.
bool is_data_available()
Check if data is available.
Mode
Roomba 500 operation mode.
@ MODE_FULL
Control acquired, safety measures disabled.
@ MODE_PASSIVE
Passive mode, no control, only listening.
@ MODE_SAFE
Control acquired, safety measures in place.
void open()
Open serial port.
static const unsigned char LED_DEBRIS
Debris LED bit.
static const unsigned char BUTTON_DOCK
Dock button.
static const unsigned char BUMPER_FRONT_RIGHT
Front right bumper.
static const unsigned char WEEKDAY_LED_THU
Thursday.
static const unsigned char BUTTON_SCHEDULE
Schedule button.
static const unsigned short int STREAM_INTERVAL_MS
Time in ms between streamed sensor packets.
static const unsigned char DIGIT_LED_NORTH_EAST
Top right segment LED.
static const unsigned char SCHEDULING_LED_COLON
Colon LED bit.
static const unsigned char WHEEL_DROP_LEFT
Left wheel drop bit.
static const unsigned char DIGIT_LED_SOUTH
Bottom segment LED.
static const unsigned char DIGIT_LED_CENTER
Center segment LED.
static const unsigned char LED_DOCK
Dock LED bit.
static const unsigned char BUMPER_FRONT_LEFT
Front left bumper.
void power_down()
Powers down the Roomba.
static const unsigned char WEEKDAY_LED_TUE
Tuesday.
void drive(short int velocity_mm_per_sec, short int radius_mm)
Drive Roomba.
static const unsigned char DIGIT_LED_NORTH
Top segment LED.
static const unsigned char BUTTON_DAY
Day button.
static const unsigned short int MAX_ENCODER_COUNT
Maximum encoder count.
void play_fanfare()
Play a simple fanfare.
void stop()
Stop moption of the Roomba.
static const unsigned char SCHEDULING_LED_CLOCK
Clock LED bit.
void set_digit_leds(const char digits[4])
Set digit LEDs.
SensorPacketID
Roomba 500 sensor package IDs.
@ SENSPACK_BATTERY_CAPACITY
Battery capacity.
@ SENSPACK_CLIFF_FRONT_LEFT
Front left cliff sensor.
@ SENSPACK_WALL
Wall sensor.
@ SENSPACK_STASIS
Caster wheel stasis (forward.
@ SENSPACK_IR_CHAR_RIGHT
Right IR character.
@ SENSPACK_GROUP_107
Packet IDs 54-58.
@ SENSPACK_DIRT_DETECT
Dirt detection sensor.
@ SENSPACK_LIGHT_BUMPER_CENTER_LEFT
Center left bumper signal.
@ SENSPACK_CURRENT
Current.
@ SENSPACK_LIGHT_BUMPER_FRONT_RIGHT
Front right bumper signal.
@ SENSPACK_GROUP_5
Packet IDs 35-42.
@ SENSPACK_CLIFF_FRONT_RIGHT
Front right cliff sensor.
@ SENSPACK_OI_MODE
Open Interface mode.
@ SENSPACK_GROUP_1
Packet IDs 7-16.
@ SENSPACK_CLIFF_RIGHT_SIGNAL
Front right cliff signal value.
@ SENSPACK_RIGHT_MOTOR_CURRENT
Right motor current.
@ SENSPACK_TEMPERATURE
Temperature.
@ SENSPACK_VIRTUAL_WALL
Virtual wall detector.
@ SENSPACK_CLIFF_LEFT_SIGNAL
Left cliff signal value.
@ SENSPACK_SONG_NUMBER
Song number.
@ SENSPACK_CLIFF_RIGHT
Right cliff sensor.
@ SENSPACK_LIGHT_BUMPER_FRONT_LEFT
Front left bumper signal.
@ SENSPACK_LIGHT_BUMPER
Light bumper status.
@ SENSPACK_RIGHT_ENCODER
Right encoder value.
@ SENSPACK_SIDE_BRUSH_MOTOR_CURRENT
Side brush motor current.
@ SENSPACK_GROUP_3
Packet IDs 21-26.
@ SENSPACK_LEFT_MOTOR_CURRENT
Left motor current.
@ SENSPACK_LIGHT_BUMPER_RIGHT
Right bumper signal.
@ SENSPACK_GROUP_106
Packet IDs 46-51.
@ SENSPACK_GROUP_0
Packet IDs 7-26.
@ SENSPACK_LIGHT_BUMPER_CENTER_RIGHT
Center right bumper signal.
@ SENSPACK_GROUP_6
Packet IDs 7-42.
@ SENSPACK_GROUP_4
Packet IDs 27-34.
@ SENSPACK_SONG_PLAYING
Song playing indicator.
@ SENSPACK_STREAM_PACKETS
Number of stream packets.
@ SENSPACK_DISTANCE
Travelled distance.
@ SENSPACK_CHARGING_STATE
Charging state.
@ SENSPACK_VOLTAGE
Voltage.
@ SENSPACK_GROUP_101
Packet IDs 43-58.
@ SENSPACK_REQ_VELOCITY
Requested velocity.
@ SENSPACK_CLIFF_LEFT
Left cliff sensor.
@ SENSPACK_BRUSH_MOTOR_CURRENT
Brush motor current.
@ SENSPACK_BUTTONS
Button status.
@ SENSPACK_CLIFF_FRONT_LEFT_SIGNAL
Front left cliff signal value.
@ SENSPACK_GROUP_ALL
All packet IDs (7-58)
@ SENSPACK_ANGLE
Turned angle.
@ SENSPACK_CLIFF_FRONT_RIGHT_SIGNAL
Right cliff signal value.
@ SENSPACK_LEFT_ENCODER
Left encoder value.
@ SENSPACK_LIGHT_BUMPER_LEFT
Left bumper signal.
@ SENSPACK_IR_CHAR_OMNI
Omnidirectional IR receiver.
@ SENSPACK_REQ_RADIUS
Requested radius.
@ SENSPACK_IR_CHAR_LEFT
Left IR character.
@ SENSPACK_GROUP_2
Packet IDs 17-20.
@ SENSPACK_REQ_RIGHT_VELOCITY
Requested right velocity.
@ SENSPACK_BUMPS_DROPS
Bumper and wheel drops.
@ SENSPACK_REQ_LEFT_VELOCITY
Requested left velocity.
@ SENSPACK_BATTERY_CHARGE
Battery charge.
@ SENSPACK_WALL_SIGNAL
Wall signal value.
@ SENSPACK_WHEEL_OVERCURRENTS
Overcurrents.
@ SENSPACK_CHARGE_SOURCES
Available charge sources.
void disable_sensors()
Disable sensor data stream.
static const short int MAX_RADIUS_MM
Maximum drive radius.
static const unsigned char BUTTON_HOUR
Hour button.
void drive_pwm(short int left_wheel_pwm, short int right_wheel_pwm)
Directly control wheel velocities via PWM.
static const unsigned char MOTOR_SIDE_BRUSH_BACKWARD
Side backward bit.
static const unsigned char BUMP_LEFT
Left bumper bit.
static const unsigned char BUTTON_SPOT
Spot cleaning button.
static const unsigned char BUMPER_RIGHT
Right bumper.
static const unsigned char MOTOR_MAIN_BRUSHES_BACKWARD
Main backward bit.
static const unsigned char MOTOR_VACUUM
Vacuum motor bit.
static const unsigned char WEEKDAY_LED_FRI
Friday.
TurnDirection
Turning direction.
@ TURN_CLOCKWISE
Clockwise turning.
static const unsigned char CHARGER_INTERNAL
Internal charger bit.
static const float AXLE_LENGTH
Axle length.
static const unsigned char BUMPER_LEFT
Left bumper.
static const unsigned char WHEEL_DROP_RIGHT
Right wheel drop bit.
static const unsigned short int MODE_CHANGE_WAIT_MS
Time in ms to wait after mode changes.
static const unsigned char OVERCURRENT_WHEEL_RIGHT
Right wheel bit.
void drive_direct(short int left_mm_per_sec, short int right_mm_per_sec)
Directly control wheel velocities.
static const unsigned char CHECKSUM_SIZE
Checksum byte size.
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 CHARGER_HOME_BASE
Home base charger bit.
void enable_sensors()
Enable sensor data stream.
static const short int MAX_PWM
Maximum PWM value for wheels.
File could not be opened.
Base class for exceptions in Fawkes.
void lock()
Lock this mutex.
void unlock()
Unlock the mutex.
The current system call has timed out before completion.
Fawkes library namespace.
Struct for packet group with everything (SENSPACK_GROUP_ALL).