Fawkes API Fawkes Development Version
thread_roomba_500.cpp
1
2/***************************************************************************
3 * thread_roomba_500.cpp - Roomba 500 thread
4 *
5 * Created: Sun Jan 02 12:47:35 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 "thread_roomba_500.h"
24
25#include <interfaces/Roomba500Interface.h>
26#include <utils/math/angle.h>
27#include <utils/time/wait.h>
28#ifdef USE_TIMETRACKER
29# include <utils/time/tracker.h>
30#endif
31
32#include <interfaces/BatteryInterface.h>
33#include <interfaces/LedInterface.h>
34#include <interfaces/SwitchInterface.h>
35// include <interfaces/MotorInterface.h>
36
37#include <netinet/in.h>
38
39#include <cstdio>
40
41using namespace fawkes;
42
43/** Worker thread for the Roomba 500 thread.
44 * @author Tim Niemueller
45 */
47{
48public:
49 /** Constructor.
50 * @param logger logger
51 * @param clock clock
52 * @param roomba refptr to Roomba500 instance
53 * @param query_mode true to query data instead of streaming it.
54 */
58 bool query_mode)
59 : Thread("Roomba500WorkerThread", Thread::OPMODE_CONTINUOUS),
60 logger(logger),
61 roomba_(roomba),
62 query_mode_(query_mode)
63 {
64 fresh_data_mutex_ = new Mutex();
65 time_wait_ = new TimeWait(clock, Roomba500::STREAM_INTERVAL_MS * 1000);
66
67#ifdef USE_TIMETRACKER
68 tt_count_ = 0;
69 ttc_query_ = tt_.add_class("Query");
70 ttc_loop_ = tt_.add_class("Loop");
71#endif
72
73 if (!query_mode_)
74 roomba_->enable_sensors();
75 }
76
77 /** Destructor. */
79 {
80 if (!query_mode_)
81 roomba_->disable_sensors();
82 delete fresh_data_mutex_;
83 delete time_wait_;
84 }
85
86 virtual void
88 {
89#ifdef USE_TIMETRACKER
90 tt_.ping_start(ttc_loop_);
91#endif
92
93 //time_wait_->mark_start();
94
95 try {
96#ifdef USE_TIMETRACKER
97 tt_.ping_start(ttc_query_);
98#endif
99 if (query_mode_)
100 roomba_->query_sensors();
101 else
102 roomba_->read_sensors();
103#ifdef USE_TIMETRACKER
104 tt_.ping_end(ttc_query_);
105#endif
106 fresh_data_ = roomba_->has_sensor_packet();
107 } catch (Exception &e) {
108 logger->log_warn(name(), "Failed to read sensor info, exception follows");
109 logger->log_warn(name(), e);
110 }
111
112 //time_wait_->wait_systime();
113
114#ifdef USE_TIMETRACKER
115 tt_.ping_end(ttc_loop_);
116 if (++tt_count_ == 300) {
117 tt_count_ = 0;
118 tt_.print_to_stdout();
119 tt_.reset();
120 }
121#endif
122 }
123
124 /** Check if fresh data is available.
125 * @return true if fresh data is available, false otherwise.
126 */
127 bool
129 {
130 fresh_data_mutex_->lock();
131 bool rv = fresh_data_;
132 fresh_data_ = false;
133 fresh_data_mutex_->unlock();
134 return rv;
135 }
136
137private:
138 Logger * logger;
139 RefPtr<Roomba500> roomba_;
140 TimeWait * time_wait_;
141 Mutex * fresh_data_mutex_;
142#ifdef USE_TIMETRACKER
143 TimeTracker tt_;
144 unsigned int ttc_query_;
145 unsigned int ttc_loop_;
146 unsigned int tt_count_;
147#endif
148
149private:
150 bool fresh_data_;
151 bool query_mode_;
152};
153
154/** @class Roomba500Thread "thread_roomba_500.h"
155 * Roomba 500 integration thread.
156 * This thread integrates the Roomba 500 robot into Fawkes. The thread
157 * hooks in at the ACT hook and executes received commands on the hardware.
158 * @author Tim Niemueller
159 */
160
161/** Constructor. */
163: Thread("Roomba500", Thread::OPMODE_WAITFORWAKEUP),
165{
166}
167
168void
170{
171 led_if_debris_ = NULL;
172 led_if_spot_ = NULL;
173 led_if_dock_ = NULL;
174 led_if_check_robot_ = NULL;
175 led_if_clean_color_ = NULL;
176 led_if_clean_intensity_ = NULL;
177 switch_if_vacuuming_ = NULL;
178 switch_if_but_clean_ = NULL;
179 switch_if_but_spot_ = NULL;
180 switch_if_but_dock_ = NULL;
181 switch_if_but_minute_ = NULL;
182 switch_if_but_hour_ = NULL;
183 switch_if_but_day_ = NULL;
184 switch_if_but_schedule_ = NULL;
185 switch_if_but_clock_ = NULL;
186 //motor_if_ = NULL;
187 battery_if_ = NULL;
188 roomba500_if_ = NULL;
189
190 greeting_loop_count_ = 0;
191
192 cfg_device_ = "";
193 cfg_btsave_ = false;
194
196 cfg_conntype_ = config->get_string("/hardware/roomba/connection_type");
197 cfg_btfast_ = false;
198 cfg_bttype_ = "firefly";
199 cfg_play_fanfare_ = true;
200 cfg_query_mode_ = true;
201
202 try {
203 cfg_play_fanfare_ = config->get_bool("/hardware/roomba/play_fanfare");
204 } catch (Exception &e) {
205 }
206
207 try {
208 cfg_query_mode_ = config->get_bool("/hardware/roomba/query_mode");
209 } catch (Exception &e) {
210 }
211
212 if (cfg_conntype_ == "rootooth") {
213 try {
214 cfg_device_ = config->get_string("/hardware/roomba/btaddr");
215 } catch (Exception &e) {
216 try {
217 cfg_device_ = config->get_string("/hardware/roomba/btname");
218 } catch (Exception &e2) {
220 "Neither bluetooth name nor address set, "
221 "trying auto-detect");
222 }
223 }
224 try {
225 cfg_btfast_ = config->get_bool("/hardware/roomba/btfast");
226 } catch (Exception &e) {
227 }
228
229 try {
230 cfg_bttype_ = config->get_string("/hardware/roomba/bttype");
231 } catch (Exception &e) {
232 logger->log_info(name(), "RooTooth type not set, assuming 'firefly'");
233 }
234 if (cfg_bttype_ == "firefly") {
235 // we're cool
236 } else if (cfg_bttype_ == "mitsumi") {
237 if (cfg_btfast_) {
239 "Fast mode setting for Mitsumi RooTooth not "
240 "supported, please set outside of Fawkes or wait "
241 "until configuration timeout has passed.");
242 cfg_btfast_ = false;
243 }
244 } else {
245 logger->log_warn(name(), "Unknown RooTooth hardware type '%s' set", cfg_bttype_.c_str());
246 if (cfg_btfast_) {
248 "Fast mode setting only supported for "
249 "FireFly RooTooth");
250 cfg_btfast_ = false;
251 }
252 }
253
255 } else if (cfg_conntype_ == "serial") {
256 cfg_device_ = config->get_string("/hardware/roomba/device");
258 } else {
259 throw Exception("Unknown mode '%s', must be rootooth or serial", cfg_conntype_.c_str());
260 }
261
262 try {
263 cfg_btsave_ = config->get_bool("/hardware/roomba/btsave");
264 } catch (Exception &e) {
265 }
266
268 cfg_mode_ = "passive";
269 try {
270 cfg_mode_ = config->get_string("/hardware/roomba/mode");
271 } catch (Exception &e) {
272 }
273 if (cfg_mode_ == "passive") {
275 } else if (cfg_mode_ == "safe") {
277 } else if (cfg_mode_ == "full") {
279 } else {
280 throw Exception("Unknown mode '%s', must be one of passive, safe, or full", cfg_mode_.c_str());
281 }
282
283 try {
284 roomba500_if_ = blackboard->open_for_writing<Roomba500Interface>("Roomba 500");
285 led_if_debris_ = blackboard->open_for_writing<LedInterface>("Roomba LED Debris");
286 led_if_spot_ = blackboard->open_for_writing<LedInterface>("Roomba LED Spot");
287 led_if_dock_ = blackboard->open_for_writing<LedInterface>("Roomba LED Dock");
288 led_if_check_robot_ = blackboard->open_for_writing<LedInterface>("Roomba LED Check Robot");
289 led_if_clean_color_ = blackboard->open_for_writing<LedInterface>("Roomba LED Clean Color");
290 led_if_clean_intensity_ =
291 blackboard->open_for_writing<LedInterface>("Roomba LED Clean Intensity");
292 switch_if_vacuuming_ = blackboard->open_for_writing<SwitchInterface>("Roomba Vacuuming");
293 switch_if_but_clean_ = blackboard->open_for_writing<SwitchInterface>("Roomba Button Clean");
294 switch_if_but_spot_ = blackboard->open_for_writing<SwitchInterface>("Roomba Button Spot");
295 switch_if_but_dock_ = blackboard->open_for_writing<SwitchInterface>("Roomba Button Dock");
296 switch_if_but_minute_ = blackboard->open_for_writing<SwitchInterface>("Roomba Button Minute");
297 switch_if_but_hour_ = blackboard->open_for_writing<SwitchInterface>("Roomba Button Hour");
298 switch_if_but_day_ = blackboard->open_for_writing<SwitchInterface>("Roomba Button Day");
299 switch_if_but_schedule_ =
300 blackboard->open_for_writing<SwitchInterface>("Roomba Button Schedule");
301 switch_if_but_clock_ = blackboard->open_for_writing<SwitchInterface>("Roomba Button Clock");
302 //motor_if_ = blackboard->open_for_writing<MotorInterface>("Roomba Motor");
303 battery_if_ = blackboard->open_for_writing<BatteryInterface>("Roomba Battery");
304 } catch (Exception &e) {
305 close_interfaces();
306 throw;
307 }
308
309 wt_ = NULL;
310 try {
311 unsigned int flags = 0;
312 if (conntype == Roomba500::CONNTYPE_ROOTOOTH) {
313 logger->log_debug(name(), "Connecting via RooTooth, this may take a while");
314 if (cfg_btfast_)
316 }
317 roomba_ = new Roomba500(conntype, cfg_device_.c_str(), flags);
318
319 if (cfg_btsave_) {
321 "Saving Bluetooth address %s. Will be used for "
322 "next connection.",
323 roomba_->get_device().c_str());
324 config->set_string("/hardware/roomba/btaddr", roomba_->get_device().c_str());
325 }
326
327 roomba_->set_mode(mode);
328 if (roomba_->is_controlled()) {
329 if (cfg_play_fanfare_)
330 roomba_->play_fanfare();
331 roomba_->set_leds(false, false, false, true, 0, 255);
332 }
333 wt_ = new WorkerThread(logger, clock, roomba_, cfg_query_mode_);
334 } catch (Exception &e) {
335 close_interfaces();
336 roomba_.clear();
337 delete wt_;
338 throw;
339 }
340 wt_->start();
341}
342
343void
344Roomba500Thread::close_interfaces()
345{
346 blackboard->close(led_if_debris_);
347 blackboard->close(led_if_spot_);
348 blackboard->close(led_if_dock_);
349 blackboard->close(led_if_check_robot_);
350 blackboard->close(led_if_clean_color_);
351 blackboard->close(led_if_clean_intensity_);
352 blackboard->close(switch_if_vacuuming_);
353 blackboard->close(switch_if_but_clean_);
354 blackboard->close(switch_if_but_spot_);
355 blackboard->close(switch_if_but_dock_);
356 blackboard->close(switch_if_but_minute_);
357 blackboard->close(switch_if_but_hour_);
358 blackboard->close(switch_if_but_day_);
359 blackboard->close(switch_if_but_schedule_);
360 blackboard->close(switch_if_but_clock_);
361 //blackboard->close(motor_if_);
362 blackboard->close(battery_if_);
363 blackboard->close(roomba500_if_);
364}
365
366void
368{
369 wt_->cancel();
370 wt_->join();
371 delete wt_;
373 roomba_.clear();
374 close_interfaces();
375}
376
377float
378Roomba500Thread::led_process(fawkes::LedInterface *iface)
379{
380 float intensity = iface->intensity();
381 while (!iface->msgq_empty()) {
383 intensity = 1.0;
384 } else if (iface->msgq_first_is<LedInterface::TurnOffMessage>()) {
385 intensity = 0.0;
386 }
387 iface->msgq_pop();
388 }
389 return intensity;
390}
391
392void
394{
395 // process actuation
396 float led_debris = led_process(led_if_debris_);
397 float led_spot = led_process(led_if_spot_);
398 float led_dock = led_process(led_if_dock_);
399 float led_check_robot = led_process(led_if_check_robot_);
400 float led_clean_color = led_process(led_if_clean_color_);
401 float led_clean_intensity = led_process(led_if_clean_intensity_);
402
403 if ((led_debris != led_if_debris_->intensity()) || (led_spot != led_if_spot_->intensity())
404 || (led_dock != led_if_dock_->intensity())
405 || (led_check_robot != led_if_check_robot_->intensity())
406 || (led_clean_color != led_if_clean_color_->intensity())
407 || (led_clean_intensity != led_if_clean_intensity_->intensity())) {
408 try {
409 roomba_->set_leds(led_debris > 0.5,
410 led_spot > 0.5,
411 led_dock > 0.5,
412 led_check_robot > 0.5,
413 (char)roundf(led_clean_color * 255.),
414 (char)roundf(led_clean_intensity * 255.));
415 } catch (Exception &e) {
416 logger->log_warn(name(), "Failed to set LEDs, exception follows");
417 logger->log_warn(name(), e);
418 }
419
420 led_if_debris_->set_intensity(led_debris);
421 led_if_spot_->set_intensity(led_spot);
422 led_if_dock_->set_intensity(led_dock);
423 led_if_check_robot_->set_intensity(led_check_robot);
424 led_if_clean_color_->set_intensity(led_clean_color);
425 led_if_clean_intensity_->set_intensity(led_clean_intensity);
426
427 led_if_debris_->write();
428 led_if_spot_->write();
429 led_if_dock_->write();
430 led_if_check_robot_->write();
431 led_if_clean_color_->write();
432 led_if_clean_intensity_->write();
433 }
434
435 while (!roomba500_if_->msgq_empty()) {
436 if (roomba500_if_->msgq_first_is<Roomba500Interface::StopMessage>()) {
437 try {
438 roomba_->stop();
439 //roomba_->set_motors(false, false, false, false, false);
440 //logger->log_debug(name(), "Stopped");
441 } catch (Exception &e) {
442 logger->log_warn(name(), "Failed to stop robot, exception follows");
443 logger->log_warn(name(), e);
444 }
445 } else if (roomba500_if_->msgq_first_is<Roomba500Interface::SetModeMessage>()) {
446 Roomba500Interface::SetModeMessage *msg = roomba500_if_->msgq_first(msg);
447
448 Roomba500::Mode mode = roomba_->get_mode();
449 unsigned char color = 0;
450 unsigned char intensity = 255;
451
452 switch (msg->mode()) {
453 case Roomba500Interface::MODE_OFF:
454 logger->log_debug(name(), "Switching off");
455 mode = Roomba500::MODE_OFF;
456 intensity = 0;
457 break;
458 case Roomba500Interface::MODE_PASSIVE:
459 logger->log_debug(name(), "Switching to passive mode");
461 color = 0;
462 break;
463 case Roomba500Interface::MODE_SAFE:
464 logger->log_debug(name(), "Switching to safe mode");
466 color = 128;
467 break;
468 case Roomba500Interface::MODE_FULL:
469 logger->log_debug(name(), "Switching to full mode");
471 color = 255;
472 break;
473 default: logger->log_warn(name(), "Invalid mode %i received, ignoring", msg->mode());
474 }
475 try {
476 bool was_controlled = roomba_->is_controlled();
477 if (!was_controlled) {
478 // set first
479 roomba_->set_mode(mode);
480 }
481 if (roomba_->is_controlled()) {
482 roomba_->set_leds(led_if_debris_->intensity() >= 0.5,
483 led_if_spot_->intensity() >= 0.5,
484 led_if_dock_->intensity() >= 0.5,
485 led_if_check_robot_->intensity() >= 0.5,
486 color,
487 intensity);
488 }
489 if (was_controlled) {
490 roomba_->set_mode(mode);
491 }
492 } catch (Exception &e) {
493 logger->log_warn(name(), "Cannot set mode, exception follows");
494 logger->log_warn(name(), e);
495 }
496
497 } else if (roomba500_if_->msgq_first_is<Roomba500Interface::DockMessage>()) {
498 try {
499 roomba_->seek_dock();
500 logger->log_debug(name(), "Docking");
501 } catch (Exception &e) {
502 logger->log_warn(name(), "Failed to seek dock, exception follows");
503 logger->log_warn(name(), e);
504 }
505 } else if (roomba500_if_->msgq_first_is<Roomba500Interface::DriveStraightMessage>()) {
506 Roomba500Interface::DriveStraightMessage *msg = roomba500_if_->msgq_first(msg);
507
508 try {
509 roomba_->drive_straight(msg->velocity());
510 } catch (Exception &e) {
511 logger->log_warn(name(), "Failed to drive straight, exception follows");
512 logger->log_warn(name(), e);
513 }
514 } else if (roomba500_if_->msgq_first_is<Roomba500Interface::DriveMessage>()) {
515 Roomba500Interface::DriveMessage *msg = roomba500_if_->msgq_first(msg);
516
517 try {
518 roomba_->drive(msg->velocity(), msg->radius());
519 } catch (Exception &e) {
520 logger->log_warn(name(), "Failed to drive, exception follows");
521 logger->log_warn(name(), e);
522 }
523
524 } else if (roomba500_if_->msgq_first_is<Roomba500Interface::SetMotorsMessage>()) {
525 Roomba500Interface::SetMotorsMessage *msg = roomba500_if_->msgq_first(msg);
526
527 try {
528 roomba_->set_motors((msg->main() != Roomba500Interface::BRUSHSTATE_OFF),
529 (msg->side() != Roomba500Interface::BRUSHSTATE_OFF),
530 msg->is_vacuuming(),
531 (msg->main() == Roomba500Interface::BRUSHSTATE_BACKWARD),
532 (msg->side() == Roomba500Interface::BRUSHSTATE_BACKWARD));
533 } catch (Exception &e) {
534 logger->log_warn(name(), "Failed to set motors, exception follows");
535 logger->log_warn(name(), e);
536 }
537 }
538 roomba500_if_->msgq_pop();
539 }
540
541 if (roomba_->is_controlled()) {
542 if (greeting_loop_count_ < 50) {
543 if (++greeting_loop_count_ == 50) {
544 roomba_->set_leds(false, false, false, false, 0, 0);
545 } else {
546 roomba_->set_leds(false, false, false, true, 0, greeting_loop_count_ * 5);
547 }
548 }
549 }
550}
551
552/** Write data to blackboard.
553 * To be called by the RoombaSensorThread during the sensor hook to
554 * write new data to the blackboard if available.
555 */
556void
558{
559 if (wt_->has_fresh_data()) {
561
562 int charge =
563 (int)roundf(((float)ntohs(sp.battery_charge) / (float)ntohs(sp.battery_capacity)) * 100.);
564
565 if (roomba_->is_controlled()) {
566 if (charge != battery_percent_) {
567 char digits[4];
568 snprintf(digits, 4, "%d%%", charge);
569 roomba_->set_digit_leds(digits);
570 battery_percent_ = charge;
571 }
572 }
573
574 roomba500_if_->set_mode((Roomba500Interface::Mode)sp.mode);
579 roomba500_if_->set_cliff_left(sp.cliff_left == 1);
580 roomba500_if_->set_cliff_front_left(sp.cliff_front_left == 1);
581 roomba500_if_->set_cliff_front_right(sp.cliff_front_right == 1);
582 roomba500_if_->set_cliff_right(sp.cliff_right == 1);
583 roomba500_if_->set_wall(sp.virtual_wall == 1);
589 roomba500_if_->set_dirt_detect(sp.dirt_detect == 1);
596 roomba500_if_->set_button_day(sp.buttons & Roomba500::BUTTON_DAY);
599
600 switch_if_but_clean_->set_enabled(sp.buttons & Roomba500::BUTTON_CLEAN);
601 switch_if_but_spot_->set_enabled(sp.buttons & Roomba500::BUTTON_SPOT);
602 switch_if_but_dock_->set_enabled(sp.buttons & Roomba500::BUTTON_DOCK);
603 switch_if_but_minute_->set_enabled(sp.buttons & Roomba500::BUTTON_MINUTE);
604 switch_if_but_hour_->set_enabled(sp.buttons & Roomba500::BUTTON_HOUR);
605 switch_if_but_day_->set_enabled(sp.buttons & Roomba500::BUTTON_DAY);
606 switch_if_but_schedule_->set_enabled(sp.buttons & Roomba500::BUTTON_SCHEDULE);
607 switch_if_but_clock_->set_enabled(sp.buttons & Roomba500::BUTTON_CLOCK);
608
609 // Convert mm to m for distance
610 roomba500_if_->set_distance((int16_t)ntohs(sp.distance));
611 // invert because in Fawkes positive angles go counter-clockwise, while
612 // for the Roomba they go clockwise
613 roomba500_if_->set_angle(-(int16_t)ntohs(sp.angle));
615 roomba500_if_->set_voltage(ntohs(sp.voltage));
616 roomba500_if_->set_current((int)ntohs(sp.current));
617 roomba500_if_->set_temperature((int)sp.temperature);
618 roomba500_if_->set_battery_charge(ntohs(sp.battery_charge));
619 roomba500_if_->set_battery_capacity(ntohs(sp.battery_capacity));
620
621 battery_if_->set_voltage(ntohs(sp.voltage));
622 battery_if_->set_current((int)ntohs(sp.current));
623 battery_if_->set_temperature((char)sp.temperature);
624 battery_if_->set_absolute_soc((float)ntohs(sp.battery_charge)
625 / (float)ntohs(sp.battery_capacity));
626 battery_if_->set_relative_soc(battery_if_->absolute_soc());
627
628 roomba500_if_->set_wall_signal(ntohs(sp.wall_signal));
629 roomba500_if_->set_cliff_left_signal(ntohs(sp.cliff_left_signal));
632 roomba500_if_->set_cliff_right_signal(ntohs(sp.cliff_right_signal));
637 roomba500_if_->set_song_number(sp.song_number);
638 roomba500_if_->set_song_playing(sp.song_playing == 1);
639
640 roomba500_if_->set_velocity((int16_t)ntohs(sp.velocity));
641 roomba500_if_->set_radius((int16_t)ntohs(sp.radius));
642 roomba500_if_->set_velocity_right((int16_t)ntohs(sp.velocity_right));
643 roomba500_if_->set_velocity_left((int16_t)ntohs(sp.velocity_left));
644 roomba500_if_->set_encoder_counts_left(ntohs(sp.encoder_counts_left));
645 roomba500_if_->set_encoder_counts_right(ntohs(sp.encoder_counts_right));
646
653
654 roomba500_if_->set_light_bump_left(ntohs(sp.light_bump_left));
655 roomba500_if_->set_light_bump_front_left(ntohs(sp.light_bump_front_left));
656 roomba500_if_->set_light_bump_center_left(ntohs(sp.light_bump_center_left));
658 roomba500_if_->set_light_bump_front_right(ntohs(sp.light_bump_front_right));
659 roomba500_if_->set_light_bump_right(ntohs(sp.light_bump_right));
660
663
664 roomba500_if_->set_left_motor_current((int)ntohs(sp.left_motor_current));
665 roomba500_if_->set_right_motor_current((int)ntohs(sp.right_motor_current));
666 roomba500_if_->set_side_brush_current((int)ntohs(sp.side_brush_current));
667 roomba500_if_->set_main_brush_current((int)ntohs(sp.main_brush_current));
668 roomba500_if_->set_caster_stasis(sp.stasis == 1);
669
670 roomba500_if_->write();
671
672 switch_if_but_clean_->write();
673 switch_if_but_spot_->write();
674 switch_if_but_dock_->write();
675 switch_if_but_minute_->write();
676 switch_if_but_hour_->write();
677 switch_if_but_day_->write();
678 switch_if_but_schedule_->write();
679 switch_if_but_clock_->write();
680
681 battery_if_->write();
682 }
683}
684
685/** Set mode and indicate with LED.
686 * This will set the mode and if successful also set the color and intensity
687 * of the clean LED indicating the mode.
688 * @param mode mode to set
689 * @exception Exception may be thrown if mode setting fails
690 */
691void
692Roomba500Thread::set_mode(Roomba500::Mode mode)
693{
694 unsigned char color = 0;
695 unsigned char intensity = 255;
696
697 switch (mode) {
698 case Roomba500::MODE_OFF: intensity = 0; break;
699 case Roomba500::MODE_PASSIVE: color = 0; break;
700 case Roomba500::MODE_SAFE: color = 128; break;
701 case Roomba500::MODE_FULL: color = 255; break;
702 }
703
704 roomba_->set_mode(mode);
705 roomba_->set_leds(led_if_debris_->intensity() >= 0.5,
706 led_if_spot_->intensity() >= 0.5,
707 led_if_dock_->intensity() >= 0.5,
708 led_if_check_robot_->intensity() >= 0.5,
709 color,
710 intensity);
711}
Worker thread for the Roomba 500 thread.
bool has_fresh_data()
Check if fresh data is available.
WorkerThread(fawkes::Logger *logger, fawkes::Clock *clock, fawkes::RefPtr< Roomba500 > roomba, bool query_mode)
Constructor.
virtual void loop()
Code to execute in the thread.
virtual void loop()
Code to execute in the thread.
virtual void init()
Initialize the thread.
void write_blackboard()
Write data to blackboard.
virtual void finalize()
Finalize the thread.
Roomba500Thread()
Constructor.
Roomba 500 series communication class.
Definition: roomba_500.h:33
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 BUTTON_CLEAN
Cleaning button.
Definition: roomba_500.h:232
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
static const unsigned char BUTTON_CLOCK
Clock button.
Definition: roomba_500.h:239
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 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
bool has_sensor_packet() const
Check if sensor packet is availabe.
Definition: roomba_500.h:516
Mode get_mode() const
Get current mode.
Definition: roomba_500.h:468
static const unsigned char BUTTON_MINUTE
Minute button.
Definition: roomba_500.h:235
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).
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
bool is_controlled() const
Check if robot is being controlled.
Definition: roomba_500.h:476
void drive_straight(short int velo_mm_per_sec)
Drive Roomba straight.
Definition: roomba_500.cpp:871
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
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 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 WHEEL_DROP_LEFT
Left wheel drop bit.
Definition: roomba_500.h:241
std::string get_device() const
Get device string.
Definition: roomba_500.h:460
static const unsigned char BUMPER_FRONT_LEFT
Front left bumper.
Definition: roomba_500.h:255
void drive(short int velocity_mm_per_sec, short int radius_mm)
Drive Roomba.
Definition: roomba_500.cpp:936
static const unsigned char BUTTON_DAY
Day button.
Definition: roomba_500.h:237
void play_fanfare()
Play a simple fanfare.
void stop()
Stop moption of the Roomba.
Definition: roomba_500.cpp:860
void set_digit_leds(const char digits[4])
Set digit LEDs.
void disable_sensors()
Disable sensor data stream.
Definition: roomba_500.cpp:733
static const unsigned char BUTTON_HOUR
Hour button.
Definition: roomba_500.h:236
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 CHARGER_INTERNAL
Internal charger bit.
Definition: roomba_500.h:295
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 char OVERCURRENT_WHEEL_RIGHT
Right wheel bit.
Definition: roomba_500.h:247
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
BatteryInterface Fawkes BlackBoard Interface.
void set_voltage(const uint32_t new_voltage)
Set voltage value.
void set_temperature(const uint32_t new_temperature)
Set temperature value.
void set_current(const uint32_t new_current)
Set current value.
void set_relative_soc(const float new_relative_soc)
Set relative_soc value.
float absolute_soc() const
Get absolute_soc value.
void set_absolute_soc(const float new_absolute_soc)
Set absolute_soc value.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:42
This is supposed to be the central clock in Fawkes.
Definition: clock.h:35
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual void set_string(const char *path, std::string &s)=0
Set new value in configuration of type string.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
Definition: exception.h:36
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:351
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1215
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1200
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:501
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1062
TurnOffMessage Fawkes BlackBoard Interface Message.
Definition: LedInterface.h:107
TurnOnMessage Fawkes BlackBoard Interface Message.
Definition: LedInterface.h:87
LedInterface Fawkes BlackBoard Interface.
Definition: LedInterface.h:34
float intensity() const
Get intensity value.
void set_intensity(const float new_intensity)
Set intensity value.
Interface for logging.
Definition: logger.h:42
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
Mutex mutual exclusion lock.
Definition: mutex.h:33
void lock()
Lock this mutex.
Definition: mutex.cpp:87
void unlock()
Unlock the mutex.
Definition: mutex.cpp:131
void clear()
Set underlying instance to 0, decrementing reference count of existing instance appropriately.
Definition: refptr.h:447
DockMessage Fawkes BlackBoard Interface Message.
DriveMessage Fawkes BlackBoard Interface Message.
int16_t velocity() const
Get velocity value.
int16_t radius() const
Get radius value.
DriveStraightMessage Fawkes BlackBoard Interface Message.
SetModeMessage Fawkes BlackBoard Interface Message.
SetMotorsMessage Fawkes BlackBoard Interface Message.
bool is_vacuuming() const
Get vacuuming value.
StopMessage Fawkes BlackBoard Interface Message.
Roomba500Interface Fawkes BlackBoard Interface.
void set_cliff_left_signal(const uint16_t new_cliff_left_signal)
Set cliff_left_signal value.
void set_cliff_right(const bool new_cliff_right)
Set cliff_right value.
void set_wall_signal(const uint16_t new_wall_signal)
Set wall_signal value.
void set_button_hour(const bool new_button_hour)
Set button_hour value.
void set_side_brush_current(const int16_t new_side_brush_current)
Set side_brush_current value.
void set_velocity_left(const int16_t new_velocity_left)
Set velocity_left value.
void set_song_playing(const bool new_song_playing)
Set song_playing value.
void set_ir_opcode_right(const InfraredCharacter new_ir_opcode_right)
Set ir_opcode_right value.
void set_encoder_counts_left(const uint16_t new_encoder_counts_left)
Set encoder_counts_left value.
void set_wall(const bool new_wall)
Set wall value.
void set_button_dock(const bool new_button_dock)
Set button_dock value.
void set_light_bump_front_right(const uint16_t new_light_bump_front_right)
Set light_bump_front_right value.
void set_overcurrent_left_wheel(const bool new_overcurrent_left_wheel)
Set overcurrent_left_wheel value.
void set_cliff_front_right_signal(const uint16_t new_cliff_front_right_signal)
Set cliff_front_right_signal value.
void set_internal_charger_available(const bool new_internal_charger_available)
Set internal_charger_available value.
void set_button_spot(const bool new_button_spot)
Set button_spot value.
void set_button_clock(const bool new_button_clock)
Set button_clock value.
ChargingState
Current charging state.
void set_light_bump_front_left(const uint16_t new_light_bump_front_left)
Set light_bump_front_left value.
void set_bumper_center_right(const bool new_bumper_center_right)
Set bumper_center_right value.
void set_cliff_front_right(const bool new_cliff_front_right)
Set cliff_front_right value.
void set_cliff_front_left(const bool new_cliff_front_left)
Set cliff_front_left value.
void set_dirt_detect(const bool new_dirt_detect)
Set dirt_detect value.
void set_light_bump_right(const uint16_t new_light_bump_right)
Set light_bump_right value.
void set_bump_left(const bool new_bump_left)
Set bump_left value.
void set_angle(const int16_t new_angle)
Set angle value.
void set_button_clean(const bool new_button_clean)
Set button_clean value.
void set_encoder_counts_right(const uint16_t new_encoder_counts_right)
Set encoder_counts_right value.
void set_button_minute(const bool new_button_minute)
Set button_minute value.
void set_velocity(const int16_t new_velocity)
Set velocity value.
void set_main_brush_current(const int16_t new_main_brush_current)
Set main_brush_current value.
void set_radius(const int16_t new_radius)
Set radius value.
void set_button_day(const bool new_button_day)
Set button_day value.
void set_bumper_front_right(const bool new_bumper_front_right)
Set bumper_front_right value.
void set_left_motor_current(const int16_t new_left_motor_current)
Set left_motor_current value.
void set_overcurrent_right_wheel(const bool new_overcurrent_right_wheel)
Set overcurrent_right_wheel value.
void set_caster_stasis(const bool new_caster_stasis)
Set caster_stasis value.
void set_button_schedule(const bool new_button_schedule)
Set button_schedule value.
void set_ir_opcode_left(const InfraredCharacter new_ir_opcode_left)
Set ir_opcode_left value.
void set_light_bump_center_left(const uint16_t new_light_bump_center_left)
Set light_bump_center_left value.
void set_song_number(const uint8_t new_song_number)
Set song_number value.
void set_cliff_right_signal(const uint16_t new_cliff_right_signal)
Set cliff_right_signal value.
InfraredCharacter
Infrared character values.
void set_light_bump_left(const uint16_t new_light_bump_left)
Set light_bump_left value.
void set_cliff_front_left_signal(const uint16_t new_cliff_front_left_signal)
Set cliff_front_left_signal value.
void set_temperature(const int8_t new_temperature)
Set temperature value.
void set_home_base_charger_available(const bool new_home_base_charger_available)
Set home_base_charger_available value.
void set_bump_right(const bool new_bump_right)
Set bump_right value.
void set_bumper_right(const bool new_bumper_right)
Set bumper_right value.
void set_wheel_drop_right(const bool new_wheel_drop_right)
Set wheel_drop_right value.
void set_cliff_left(const bool new_cliff_left)
Set cliff_left value.
void set_voltage(const uint16_t new_voltage)
Set voltage value.
void set_overcurrent_side_brush(const bool new_overcurrent_side_brush)
Set overcurrent_side_brush value.
void set_right_motor_current(const int16_t new_right_motor_current)
Set right_motor_current value.
void set_overcurrent_main_brush(const bool new_overcurrent_main_brush)
Set overcurrent_main_brush value.
void set_ir_opcode_omni(const InfraredCharacter new_ir_opcode_omni)
Set ir_opcode_omni value.
void set_wheel_drop_left(const bool new_wheel_drop_left)
Set wheel_drop_left value.
void set_distance(const int16_t new_distance)
Set distance value.
void set_battery_capacity(const uint16_t new_battery_capacity)
Set battery_capacity value.
void set_bumper_left(const bool new_bumper_left)
Set bumper_left value.
void set_light_bump_center_right(const uint16_t new_light_bump_center_right)
Set light_bump_center_right value.
void set_bumper_front_left(const bool new_bumper_front_left)
Set bumper_front_left value.
void set_velocity_right(const int16_t new_velocity_right)
Set velocity_right value.
void set_mode(const Mode new_mode)
Set mode value.
void set_current(const int16_t new_current)
Set current value.
void set_bumper_center_left(const bool new_bumper_center_left)
Set bumper_center_left value.
void set_charging_state(const ChargingState new_charging_state)
Set charging_state value.
void set_battery_charge(const uint16_t new_battery_charge)
Set battery_charge value.
SwitchInterface Fawkes BlackBoard Interface.
void set_enabled(const bool new_enabled)
Set enabled value.
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
void start(bool wait=true)
Call this method to start the thread.
Definition: thread.cpp:499
void join()
Join the thread.
Definition: thread.cpp:597
void cancel()
Cancel a thread.
Definition: thread.cpp:646
@ OPMODE_CONTINUOUS
operate in continuous mode (default)
Definition: thread.h:57
Time tracking utility.
Definition: tracker.h:37
Time wait utility.
Definition: wait.h:33
Fawkes library namespace.
Struct for packet group with everything (SENSPACK_GROUP_ALL).
Definition: roomba_500.h:379
uint8_t cliff_front_right
Front right cliff sensor.
Definition: roomba_500.h:384
uint16_t encoder_counts_left
Encoder counts for left wheel.
Definition: roomba_500.h:416
uint8_t ir_opcode_right
Right IR receiver character.
Definition: roomba_500.h:426
uint8_t mode
Open Interface mode.
Definition: roomba_500.h:408
uint16_t cliff_left_signal
Raw left cliff signal.
Definition: roomba_500.h:401
uint8_t cliff_right
Right cliff sensor.
Definition: roomba_500.h:385
uint16_t light_bump_center_right
Raw center right light bumper signal.
Definition: roomba_500.h:422
uint16_t light_bump_front_right
Raw front right light bumper signal.
Definition: roomba_500.h:423
uint8_t virtual_wall
Wall sensor.
Definition: roomba_500.h:386
uint16_t wall_signal
Raw wall signal.
Definition: roomba_500.h:400
uint8_t bumps_wheeldrops
Bumps and wheeldrops bits.
Definition: roomba_500.h:380
int16_t current
Current in mA.
Definition: roomba_500.h:396
uint16_t cliff_front_right_signal
Raw front right cliff signal.
Definition: roomba_500.h:403
int16_t velocity
Velocity in mm/sec.
Definition: roomba_500.h:412
int16_t velocity_left
Velocity of left wheel in mm/sec.
Definition: roomba_500.h:415
uint16_t encoder_counts_right
Encoder counts for right wheel.
Definition: roomba_500.h:417
int16_t radius
Radius in mm.
Definition: roomba_500.h:413
uint8_t light_bumper
Light bumper bits.
Definition: roomba_500.h:418
uint8_t ir_opcode_omni
Omni IR receiver character.
Definition: roomba_500.h:390
uint8_t charging_state
Charging state.
Definition: roomba_500.h:394
int16_t angle
Turned angle in degree.
Definition: roomba_500.h:393
int16_t velocity_right
Velocity of right wheel in mm/sec.
Definition: roomba_500.h:414
uint8_t overcurrents
Overcurrents bits.
Definition: roomba_500.h:387
int8_t temperature
Temperature in deg C.
Definition: roomba_500.h:397
uint8_t song_number
Song number.
Definition: roomba_500.h:409
uint16_t voltage
Voltage in mV.
Definition: roomba_500.h:395
uint8_t cliff_left
Left cliff sensor.
Definition: roomba_500.h:382
uint8_t buttons
Buttons bits.
Definition: roomba_500.h:391
uint16_t battery_charge
Battery charge in mAh.
Definition: roomba_500.h:398
int16_t left_motor_current
Raw left motor current signal.
Definition: roomba_500.h:427
uint8_t cliff_front_left
Front left cliff sensor.
Definition: roomba_500.h:383
int16_t side_brush_current
Raw side brush motor current signal.
Definition: roomba_500.h:430
uint16_t light_bump_front_left
Raw front left light bumper signal.
Definition: roomba_500.h:420
uint16_t light_bump_center_left
Raw center left light bumper signal.
Definition: roomba_500.h:421
uint16_t light_bump_left
Raw left light bumper signal.
Definition: roomba_500.h:419
uint8_t charger_available
Available chargers bits.
Definition: roomba_500.h:407
uint16_t cliff_right_signal
Raw right cliff signal.
Definition: roomba_500.h:404
uint8_t song_playing
Song playing byte.
Definition: roomba_500.h:410
int16_t main_brush_current
Raw main brush motor current signal.
Definition: roomba_500.h:429
uint16_t battery_capacity
Battery capacity in mAh.
Definition: roomba_500.h:399
int16_t right_motor_current
Raw right motor current signal.
Definition: roomba_500.h:428
uint16_t cliff_front_left_signal
Raw front left cliff signal.
Definition: roomba_500.h:402
int16_t distance
Traveled distance in mm.
Definition: roomba_500.h:392
uint8_t dirt_detect
Dirt detect sensor.
Definition: roomba_500.h:388
uint8_t ir_opcode_left
Left IR receiver character.
Definition: roomba_500.h:425
uint8_t stasis
Castor stasis.
Definition: roomba_500.h:431
uint16_t light_bump_right
Raw right light bumper signal.
Definition: roomba_500.h:424