Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
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), clock(clock), __roomba(roomba),
58  __query_mode(query_mode)
59  {
60  __fresh_data_mutex = new Mutex();
61  __time_wait = new TimeWait(clock, Roomba500::STREAM_INTERVAL_MS * 1000);
62 
63 #ifdef USE_TIMETRACKER
64  __tt_count = 0;
65  __ttc_query = __tt.add_class("Query");
66  __ttc_loop = __tt.add_class("Loop");
67 #endif
68 
69  if (! __query_mode) __roomba->enable_sensors();
70  }
71 
72  /** Destructor. */
74  {
75  if (! __query_mode) __roomba->disable_sensors();
76  delete __fresh_data_mutex;
77  delete __time_wait;
78  }
79 
80  virtual void loop()
81  {
82 #ifdef USE_TIMETRACKER
83  __tt.ping_start(__ttc_loop);
84 #endif
85 
86  //__time_wait->mark_start();
87 
88  try {
89 #ifdef USE_TIMETRACKER
90  __tt.ping_start(__ttc_query);
91 #endif
92  if (__query_mode) __roomba->query_sensors();
93  else __roomba->read_sensors();
94 #ifdef USE_TIMETRACKER
95  __tt.ping_end(__ttc_query);
96 #endif
97  __fresh_data = __roomba->has_sensor_packet();
98  } catch (Exception &e) {
99  logger->log_warn(name(), "Failed to read sensor info, exception follows");
100  logger->log_warn(name(), e);
101  }
102 
103  //__time_wait->wait_systime();
104 
105 #ifdef USE_TIMETRACKER
106  __tt.ping_end(__ttc_loop);
107  if (++__tt_count == 300) {
108  __tt_count = 0;
109  __tt.print_to_stdout();
110  __tt.reset();
111  }
112 #endif
113  }
114 
115  /** Check if fresh data is available.
116  * @return true if fresh data is available, false otherwise.
117  */
118  bool has_fresh_data()
119  {
120  __fresh_data_mutex->lock();
121  bool rv = __fresh_data;
122  __fresh_data = false;
123  __fresh_data_mutex->unlock();
124  return rv;
125  }
126 
127  private:
128  Logger *logger;
129  Clock *clock;
130  RefPtr<Roomba500> __roomba;
131  TimeWait *__time_wait;
132  Mutex *__fresh_data_mutex;
133 #ifdef USE_TIMETRACKER
134  TimeTracker __tt;
135  unsigned int __ttc_query;
136  unsigned int __ttc_loop;
137  unsigned int __tt_count;
138 #endif
139 
140  private:
141  bool __fresh_data;
142  bool __query_mode;
143 };
144 
145 
146 /** @class Roomba500Thread "thread_roomba_500.h"
147  * Roomba 500 integration thread.
148  * This thread integrates the Roomba 500 robot into Fawkes. The thread
149  * hooks in at the ACT hook and executes received commands on the hardware.
150  * @author Tim Niemueller
151  */
152 
153 /** Constructor. */
155  : Thread("Roomba500", Thread::OPMODE_WAITFORWAKEUP),
156  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_ACT)
157 {
158 }
159 
160 
161 void
163 {
164  __led_if_debris = NULL;
165  __led_if_spot = NULL;
166  __led_if_dock = NULL;
167  __led_if_check_robot = NULL;
168  __led_if_clean_color = NULL;
169  __led_if_clean_intensity = NULL;
170  __switch_if_vacuuming = NULL;
171  __switch_if_but_clean = NULL;
172  __switch_if_but_spot = NULL;
173  __switch_if_but_dock = NULL;
174  __switch_if_but_minute = NULL;
175  __switch_if_but_hour = NULL;
176  __switch_if_but_day = NULL;
177  __switch_if_but_schedule = NULL;
178  __switch_if_but_clock = NULL;
179  //__motor_if = NULL;
180  __battery_if = NULL;
181  __roomba500_if = NULL;
182 
183  __greeting_loop_count = 0;
184 
185  __cfg_device = "";
186  __cfg_btsave = false;
187 
188  Roomba500::ConnectionType conntype;
189  __cfg_conntype = config->get_string("/hardware/roomba/connection_type");
190  __cfg_btfast = false;
191  __cfg_bttype = "firefly";
192  __cfg_play_fanfare = true;
193  __cfg_query_mode = true;
194 
195  try {
196  __cfg_play_fanfare = config->get_bool("/hardware/roomba/play_fanfare");
197  } catch (Exception &e) {}
198 
199  try {
200  __cfg_query_mode = config->get_bool("/hardware/roomba/query_mode");
201  } catch (Exception &e) {}
202 
203  if (__cfg_conntype == "rootooth") {
204  try {
205  __cfg_device = config->get_string("/hardware/roomba/btaddr");
206  } catch (Exception &e) {
207  try {
208  __cfg_device = config->get_string("/hardware/roomba/btname");
209  } catch (Exception &e2) {
210  logger->log_info(name(), "Neither bluetooth name nor address set, "
211  "trying auto-detect");
212  }
213  }
214  try {
215  __cfg_btfast = config->get_bool("/hardware/roomba/btfast");
216  } catch (Exception &e) {}
217 
218  try {
219  __cfg_bttype = config->get_string("/hardware/roomba/bttype");
220  } catch (Exception &e) {
221  logger->log_info(name(), "RooTooth type not set, assuming 'firefly'");
222  }
223  if (__cfg_bttype == "firefly") {
224  // we're cool
225  } else if (__cfg_bttype == "mitsumi") {
226  if (__cfg_btfast) {
227  logger->log_warn(name(), "Fast mode setting for Mitsumi RooTooth not "
228  "supported, please set outside of Fawkes or wait "
229  "until configuration timeout has passed.");
230  __cfg_btfast = false;
231  }
232  } else {
233  logger->log_warn(name(), "Unknown RooTooth hardware type '%s' set",
234  __cfg_bttype.c_str());
235  if (__cfg_btfast) {
236  logger->log_warn(name(), "Fast mode setting only supported for "
237  "FireFly RooTooth");
238  __cfg_btfast = false;
239  }
240  }
241 
242  conntype = Roomba500::CONNTYPE_ROOTOOTH;
243  } else if (__cfg_conntype == "serial") {
244  __cfg_device = config->get_string("/hardware/roomba/device");
245  conntype = Roomba500::CONNTYPE_SERIAL;
246  } else {
247  throw Exception("Unknown mode '%s', must be rootooth or serial",
248  __cfg_conntype.c_str());
249  }
250 
251  try {
252  __cfg_btsave = config->get_bool("/hardware/roomba/btsave");
253  } catch (Exception &e) {}
254 
256  __cfg_mode = "passive";
257  try {
258  __cfg_mode = config->get_string("/hardware/roomba/mode");
259  } catch (Exception &e) {}
260  if (__cfg_mode == "passive") {
262  } else if (__cfg_mode == "safe") {
263  mode = Roomba500::MODE_SAFE;
264  } else if (__cfg_mode == "full") {
265  mode = Roomba500::MODE_FULL;
266  } else {
267  throw Exception("Unknown mode '%s', must be one of passive, safe, or full",
268  __cfg_mode.c_str());
269  }
270 
271 
272  try {
273  __roomba500_if = blackboard->open_for_writing<Roomba500Interface>("Roomba 500");
274  __led_if_debris =
275  blackboard->open_for_writing<LedInterface>("Roomba LED Debris");
276  __led_if_spot = blackboard->open_for_writing<LedInterface>("Roomba LED Spot");
277  __led_if_dock = blackboard->open_for_writing<LedInterface>("Roomba LED Dock");
278  __led_if_check_robot =
279  blackboard->open_for_writing<LedInterface>("Roomba LED Check Robot");
280  __led_if_clean_color =
281  blackboard->open_for_writing<LedInterface>("Roomba LED Clean Color");
282  __led_if_clean_intensity =
283  blackboard->open_for_writing<LedInterface>("Roomba LED Clean Intensity");
284  __switch_if_vacuuming =
285  blackboard->open_for_writing<SwitchInterface>("Roomba Vacuuming");
286  __switch_if_but_clean =
287  blackboard->open_for_writing<SwitchInterface>("Roomba Button Clean");
288  __switch_if_but_spot =
289  blackboard->open_for_writing<SwitchInterface>("Roomba Button Spot");
290  __switch_if_but_dock =
291  blackboard->open_for_writing<SwitchInterface>("Roomba Button Dock");
292  __switch_if_but_minute =
293  blackboard->open_for_writing<SwitchInterface>("Roomba Button Minute");
294  __switch_if_but_hour =
295  blackboard->open_for_writing<SwitchInterface>("Roomba Button Hour");
296  __switch_if_but_day =
297  blackboard->open_for_writing<SwitchInterface>("Roomba Button Day");
298  __switch_if_but_schedule =
299  blackboard->open_for_writing<SwitchInterface>("Roomba Button Schedule");
300  __switch_if_but_clock =
301  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) flags |= Roomba500::FLAG_FIREFLY_FASTMODE;
315  }
316  __roomba = new Roomba500(conntype, __cfg_device.c_str(), flags);
317 
318  if (__cfg_btsave) {
319  logger->log_debug(name(), "Saving Bluetooth address %s. Will be used for "
320  "next connection.", __roomba->get_device());
321  config->set_string("/hardware/roomba/btaddr", __roomba->get_device());
322  }
323 
324  __roomba->set_mode(mode);
325  if (__roomba->is_controlled()) {
326  if (__cfg_play_fanfare) __roomba->play_fanfare();
327  __roomba->set_leds(false, false, false, true, 0, 255);
328  }
329  __wt = new WorkerThread(logger, clock, __roomba, __cfg_query_mode);
330  } catch (Exception &e) {
331  close_interfaces();
332  __roomba.clear();
333  delete __wt;
334  throw;
335  }
336  __wt->start();
337 }
338 
339 void
340 Roomba500Thread::close_interfaces()
341 {
342  blackboard->close(__led_if_debris);
343  blackboard->close(__led_if_spot);
344  blackboard->close(__led_if_dock);
345  blackboard->close(__led_if_check_robot);
346  blackboard->close(__led_if_clean_color);
347  blackboard->close(__led_if_clean_intensity);
348  blackboard->close(__switch_if_vacuuming);
349  blackboard->close(__switch_if_but_clean);
350  blackboard->close(__switch_if_but_spot);
351  blackboard->close(__switch_if_but_dock);
352  blackboard->close(__switch_if_but_minute);
353  blackboard->close(__switch_if_but_hour);
354  blackboard->close(__switch_if_but_day);
355  blackboard->close(__switch_if_but_schedule);
356  blackboard->close(__switch_if_but_clock);
357  //blackboard->close(__motor_if);
358  blackboard->close(__battery_if);
359  blackboard->close(__roomba500_if);
360 }
361 
362 
363 void
365 {
366  __wt->cancel();
367  __wt->join();
368  delete __wt;
370  __roomba.clear();
371  close_interfaces();
372 }
373 
374 
375 float
376 Roomba500Thread::led_process(fawkes::LedInterface *iface)
377 {
378  float intensity = iface->intensity();
379  while (! iface->msgq_empty() ) {
381  intensity = 1.0;
382  } else if (iface->msgq_first_is<LedInterface::TurnOffMessage>()) {
383  intensity = 0.0;
384  }
385  iface->msgq_pop();
386  }
387  return intensity;
388 }
389 
390 void
392 {
393  // process actuation
394  float led_debris = led_process(__led_if_debris);
395  float led_spot = led_process(__led_if_spot);
396  float led_dock = led_process(__led_if_dock);
397  float led_check_robot = led_process(__led_if_check_robot);
398  float led_clean_color = led_process(__led_if_clean_color);
399  float led_clean_intensity = led_process(__led_if_clean_intensity);
400 
401  if ( (led_debris != __led_if_debris->intensity()) ||
402  (led_spot != __led_if_spot->intensity()) ||
403  (led_dock != __led_if_dock->intensity()) ||
404  (led_check_robot != __led_if_check_robot->intensity()) ||
405  (led_clean_color != __led_if_clean_color->intensity()) ||
406  (led_clean_intensity != __led_if_clean_intensity->intensity()) )
407  {
408  try {
409  __roomba->set_leds(led_debris > 0.5, led_spot > 0.5,
410  led_dock > 0.5, led_check_robot > 0.5,
411  (char)roundf(led_clean_color * 255.),
412  (char)roundf(led_clean_intensity * 255.));
413  } catch (Exception &e) {
414  logger->log_warn(name(), "Failed to set LEDs, exception follows");
415  logger->log_warn(name(), e);
416  }
417 
418  __led_if_debris->set_intensity(led_debris);
419  __led_if_spot->set_intensity(led_spot);
420  __led_if_dock->set_intensity(led_dock);
421  __led_if_check_robot->set_intensity(led_check_robot);
422  __led_if_clean_color->set_intensity(led_clean_color);
423  __led_if_clean_intensity->set_intensity(led_clean_intensity);
424 
425  __led_if_debris->write();
426  __led_if_spot->write();
427  __led_if_dock->write();
428  __led_if_check_robot->write();
429  __led_if_clean_color->write();
430  __led_if_clean_intensity->write();
431  }
432 
433  while (! __roomba500_if->msgq_empty() ) {
434  if (__roomba500_if->msgq_first_is<Roomba500Interface::StopMessage>())
435  {
436  try {
437  __roomba->stop();
438  //__roomba->set_motors(false, false, false, false, false);
439  //logger->log_debug(name(), "Stopped");
440  } catch (Exception &e) {
441  logger->log_warn(name(), "Failed to stop robot, exception follows");
442  logger->log_warn(name(), e);
443  }
444  } else if (__roomba500_if->msgq_first_is<Roomba500Interface::SetModeMessage>())
445  {
447  __roomba500_if->msgq_first(msg);
448 
449  Roomba500::Mode mode = __roomba->get_mode();
450  char color = 0;
451  char intensity = 255;
452 
453  switch (msg->mode()) {
454  case Roomba500Interface::MODE_OFF:
455  logger->log_debug(name(), "Switching off");
456  mode = Roomba500::MODE_OFF;
457  intensity = 0;
458  break;
459  case Roomba500Interface::MODE_PASSIVE:
460  logger->log_debug(name(), "Switching to passive mode");
462  color = 0;
463  break;
464  case Roomba500Interface::MODE_SAFE:
465  logger->log_debug(name(), "Switching to safe mode");
466  mode = Roomba500::MODE_SAFE;
467  color = 128;
468  break;
469  case Roomba500Interface::MODE_FULL:
470  logger->log_debug(name(), "Switching to full mode");
471  mode = Roomba500::MODE_FULL;
472  color = 255;
473  break;
474  default:
475  logger->log_warn(name(), "Invalid mode %i received, ignoring",
476  msg->mode());
477  }
478  try {
479  bool was_controlled = __roomba->is_controlled();
480  if (! was_controlled) {
481  // set first
482  __roomba->set_mode(mode);
483  }
484  if (__roomba->is_controlled()) {
485  __roomba->set_leds(__led_if_debris->intensity() >= 0.5,
486  __led_if_spot->intensity() >= 0.5,
487  __led_if_dock->intensity() >= 0.5,
488  __led_if_check_robot->intensity() >= 0.5,
489  color, intensity);
490  }
491  if (was_controlled) {
492  __roomba->set_mode(mode);
493  }
494  } catch (Exception &e) {
495  logger->log_warn(name(), "Cannot set mode, exception follows");
496  logger->log_warn(name(), e);
497  }
498 
499  } else if (__roomba500_if->msgq_first_is<Roomba500Interface::DockMessage>()) {
500  try {
501  __roomba->seek_dock();
502  logger->log_debug(name(), "Docking");
503  } catch (Exception &e) {
504  logger->log_warn(name(), "Failed to seek dock, exception follows");
505  logger->log_warn(name(), e);
506  }
507  } else if (__roomba500_if->msgq_first_is<Roomba500Interface::DriveStraightMessage>())
508  {
510  __roomba500_if->msgq_first(msg);
511 
512  try {
513  __roomba->drive_straight(msg->velocity());
514  } catch (Exception &e) {
515  logger->log_warn(name(), "Failed to drive straight, exception follows");
516  logger->log_warn(name(), e);
517  }
518  } else if (__roomba500_if->msgq_first_is<Roomba500Interface::DriveMessage>())
519  {
521  __roomba500_if->msgq_first(msg);
522 
523  try {
524  __roomba->drive(msg->velocity(), msg->radius());
525  } catch (Exception &e) {
526  logger->log_warn(name(), "Failed to drive, exception follows");
527  logger->log_warn(name(), e);
528  }
529 
530  } else if (__roomba500_if->msgq_first_is<Roomba500Interface::SetMotorsMessage>())
531  {
533  __roomba500_if->msgq_first(msg);
534 
535  try {
536  __roomba->set_motors(
537  (msg->main() != Roomba500Interface::BRUSHSTATE_OFF),
538  (msg->side() != Roomba500Interface::BRUSHSTATE_OFF),
539  msg->is_vacuuming(),
540  (msg->main() == Roomba500Interface::BRUSHSTATE_BACKWARD),
541  (msg->side() == Roomba500Interface::BRUSHSTATE_BACKWARD));
542  } catch (Exception &e) {
543  logger->log_warn(name(), "Failed to set motors, exception follows");
544  logger->log_warn(name(), e);
545  }
546  }
547  __roomba500_if->msgq_pop();
548  }
549 
550  if (__roomba->is_controlled()) {
551  if (__greeting_loop_count < 50) {
552  if (++__greeting_loop_count == 50) {
553  __roomba->set_leds(false, false, false, false, 0, 0);
554  } else {
555  __roomba->set_leds(false, false, false, true,
556  0, __greeting_loop_count * 5);
557  }
558  }
559  }
560 }
561 
562 
563 /** Write data to blackboard.
564  * To be called by the RoombaSensorThread during the sensor hook to
565  * write new data to the blackboard if available.
566  */
567 void
569 {
570  if (__wt->has_fresh_data()) {
572 
573  int charge = (int)roundf(((float)ntohs(sp.battery_charge) /
574  (float)ntohs(sp.battery_capacity)) * 100.);
575 
576  if (__roomba->is_controlled()) {
577  if (charge != __battery_percent) {
578  char digits[4];
579  snprintf(digits, 4, "%u%%", charge);
580  __roomba->set_digit_leds(digits);
581  __battery_percent = charge;
582  }
583  }
584 
585  __roomba500_if->set_mode((Roomba500Interface::Mode)sp.mode);
586  __roomba500_if->set_wheel_drop_left(
587  sp.bumps_wheeldrops & Roomba500::WHEEL_DROP_LEFT);
588  __roomba500_if->set_wheel_drop_right(
589  sp.bumps_wheeldrops & Roomba500::WHEEL_DROP_RIGHT);
590  __roomba500_if->set_bump_left(sp.bumps_wheeldrops & Roomba500::BUMP_LEFT);
591  __roomba500_if->set_bump_right(sp.bumps_wheeldrops & Roomba500::BUMP_RIGHT);
592  __roomba500_if->set_cliff_left(sp.cliff_left == 1);
593  __roomba500_if->set_cliff_front_left(sp.cliff_front_left == 1);
594  __roomba500_if->set_cliff_front_right(sp.cliff_front_right == 1);
595  __roomba500_if->set_cliff_right(sp.cliff_right == 1);
596  __roomba500_if->set_wall(sp.virtual_wall == 1);
597  __roomba500_if->set_overcurrent_left_wheel(
598  sp.overcurrents & Roomba500::OVERCURRENT_WHEEL_LEFT);
599  __roomba500_if->set_overcurrent_right_wheel(
600  sp.overcurrents & Roomba500::OVERCURRENT_WHEEL_RIGHT);
601  __roomba500_if->set_overcurrent_main_brush(
602  sp.overcurrents & Roomba500::OVERCURRENT_MAIN_BRUSH);
603  __roomba500_if->set_overcurrent_side_brush(
604  sp.overcurrents & Roomba500::OVERCURRENT_SIDE_BRUSH);
605  __roomba500_if->set_dirt_detect(sp.dirt_detect == 1);
606  __roomba500_if->set_ir_opcode_omni(
607  (Roomba500Interface::InfraredCharacter)sp.ir_opcode_omni);
608  __roomba500_if->set_button_clean(sp.buttons & Roomba500::BUTTON_CLEAN);
609  __roomba500_if->set_button_spot(sp.buttons & Roomba500::BUTTON_SPOT);
610  __roomba500_if->set_button_dock(sp.buttons & Roomba500::BUTTON_DOCK);
611  __roomba500_if->set_button_minute(sp.buttons & Roomba500::BUTTON_MINUTE);
612  __roomba500_if->set_button_hour(sp.buttons & Roomba500::BUTTON_HOUR);
613  __roomba500_if->set_button_day(sp.buttons & Roomba500::BUTTON_DAY);
614  __roomba500_if->set_button_schedule(sp.buttons & Roomba500::BUTTON_SCHEDULE);
615  __roomba500_if->set_button_clock(sp.buttons & Roomba500::BUTTON_CLOCK);
616 
617  __switch_if_but_clean->set_enabled(sp.buttons & Roomba500::BUTTON_CLEAN);
618  __switch_if_but_spot->set_enabled(sp.buttons & Roomba500::BUTTON_SPOT);
619  __switch_if_but_dock->set_enabled(sp.buttons & Roomba500::BUTTON_DOCK);
620  __switch_if_but_minute->set_enabled(sp.buttons & Roomba500::BUTTON_MINUTE);
621  __switch_if_but_hour->set_enabled(sp.buttons & Roomba500::BUTTON_HOUR);
622  __switch_if_but_day->set_enabled(sp.buttons & Roomba500::BUTTON_DAY);
623  __switch_if_but_schedule->set_enabled(sp.buttons & Roomba500::BUTTON_SCHEDULE);
624  __switch_if_but_clock->set_enabled(sp.buttons & Roomba500::BUTTON_CLOCK);
625 
626  // Convert mm to m for distance
627  __roomba500_if->set_distance((int16_t)ntohs(sp.distance));
628  // invert because in Fawkes positive angles go counter-clockwise, while
629  // for the Roomba they go clockwise
630  __roomba500_if->set_angle(- (int16_t)ntohs(sp.angle));
631  __roomba500_if->set_charging_state(
632  (Roomba500Interface::ChargingState)sp.charging_state);
633  __roomba500_if->set_voltage(ntohs(sp.voltage));
634  __roomba500_if->set_current((int)ntohs(sp.current));
635  __roomba500_if->set_temperature((int)sp.temperature);
636  __roomba500_if->set_battery_charge(ntohs(sp.battery_charge));
637  __roomba500_if->set_battery_capacity(ntohs(sp.battery_capacity));
638 
639  __battery_if->set_voltage(ntohs(sp.voltage));
640  __battery_if->set_current((int)ntohs(sp.current));
641  __battery_if->set_temperature((char)sp.temperature);
642  __battery_if->set_absolute_soc((float)ntohs(sp.battery_charge) /
643  (float)ntohs(sp.battery_capacity));
644  __battery_if->set_relative_soc(__battery_if->absolute_soc());
645 
646  __roomba500_if->set_wall_signal(ntohs(sp.wall_signal));
647  __roomba500_if->set_cliff_left_signal(ntohs(sp.cliff_left_signal));
648  __roomba500_if->set_cliff_front_left_signal(ntohs(sp.cliff_front_left_signal));
649  __roomba500_if->set_cliff_front_right_signal(ntohs(sp.cliff_front_right_signal));
650  __roomba500_if->set_cliff_right_signal(ntohs(sp.cliff_right_signal));
651  __roomba500_if->set_home_base_charger_available(
652  sp.charger_available & Roomba500::CHARGER_HOME_BASE);
653  __roomba500_if->set_internal_charger_available(
654  sp.charger_available & Roomba500::CHARGER_INTERNAL);
655  __roomba500_if->set_song_number(sp.song_number);
656  __roomba500_if->set_song_playing(sp.song_playing == 1);
657 
658  __roomba500_if->set_velocity((int16_t)ntohs(sp.velocity));
659  __roomba500_if->set_radius((int16_t)ntohs(sp.radius));
660  __roomba500_if->set_velocity_right((int16_t)ntohs(sp.velocity_right));
661  __roomba500_if->set_velocity_left((int16_t)ntohs(sp.velocity_left));
662  __roomba500_if->set_encoder_counts_left(ntohs(sp.encoder_counts_left));
663  __roomba500_if->set_encoder_counts_right(ntohs(sp.encoder_counts_right));
664 
665  __roomba500_if->set_bumper_left(
666  sp.light_bumper & Roomba500::BUMPER_LEFT);
667  __roomba500_if->set_bumper_front_left(
668  sp.light_bumper & Roomba500::BUMPER_FRONT_LEFT);
669  __roomba500_if->set_bumper_center_left(
670  sp.light_bumper & Roomba500::BUMPER_CENTER_LEFT);
671  __roomba500_if->set_bumper_center_right(
672  sp.light_bumper & Roomba500::BUMPER_CENTER_RIGHT);
673  __roomba500_if->set_bumper_front_right(
674  sp.light_bumper & Roomba500::BUMPER_FRONT_RIGHT);
675  __roomba500_if->set_bumper_right(
676  sp.light_bumper & Roomba500::BUMPER_RIGHT);
677 
678  __roomba500_if->set_light_bump_left(ntohs(sp.light_bump_left));
679  __roomba500_if->set_light_bump_front_left(ntohs(sp.light_bump_front_left));
680  __roomba500_if->set_light_bump_center_left(ntohs(sp.light_bump_center_left));
681  __roomba500_if->set_light_bump_center_right(ntohs(sp.light_bump_center_right));
682  __roomba500_if->set_light_bump_front_right(ntohs(sp.light_bump_front_right));
683  __roomba500_if->set_light_bump_right(ntohs(sp.light_bump_right));
684 
685  __roomba500_if->set_ir_opcode_left(
686  (Roomba500Interface::InfraredCharacter)sp.ir_opcode_left);
687  __roomba500_if->set_ir_opcode_right(
688  (Roomba500Interface::InfraredCharacter)sp.ir_opcode_right);
689 
690  __roomba500_if->set_left_motor_current((int)ntohs(sp.left_motor_current));
691  __roomba500_if->set_right_motor_current((int)ntohs(sp.right_motor_current));
692  __roomba500_if->set_side_brush_current((int)ntohs(sp.side_brush_current));
693  __roomba500_if->set_main_brush_current((int)ntohs(sp.main_brush_current));
694  __roomba500_if->set_caster_stasis(sp.stasis == 1);
695 
696  __roomba500_if->write();
697 
698  __switch_if_but_clean->write();
699  __switch_if_but_spot->write();
700  __switch_if_but_dock->write();
701  __switch_if_but_minute->write();
702  __switch_if_but_hour->write();
703  __switch_if_but_day->write();
704  __switch_if_but_schedule->write();
705  __switch_if_but_clock->write();
706 
707  __battery_if->write();
708  }
709 }
710 
711 
712 /** Set mode and indicate with LED.
713  * This will set the mode and if successful also set the color and intensity
714  * of the clean LED indicating the mode.
715  * @param mode mode to set
716  * @exception Exception may be thrown if mode setting fails
717  */
718 void
719 Roomba500Thread::set_mode(Roomba500::Mode mode)
720 {
721  char color = 0;
722  char intensity = 255;
723 
724  switch (mode) {
725  case Roomba500::MODE_OFF: intensity = 0; break;
726  case Roomba500::MODE_PASSIVE: color = 0; break;
727  case Roomba500::MODE_SAFE: color = 128; break;
728  case Roomba500::MODE_FULL: color = 255; break;
729  }
730 
731  __roomba->set_mode(mode);
732  __roomba->set_leds(__led_if_debris->intensity() >= 0.5,
733  __led_if_spot->intensity() >= 0.5,
734  __led_if_dock->intensity() >= 0.5,
735  __led_if_check_robot->intensity() >= 0.5,
736  color, intensity);
737 }