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