Fawkes API  Fawkes Development Version
thread_roomba_500.cpp
00001 
00002 /***************************************************************************
00003  *  thread_roomba_500.cpp - Roomba 500 thread
00004  *
00005  *  Created: Sun Jan 02 12:47:35 2011
00006  *  Copyright  2006-2010  Tim Niemueller [www.niemueller.de]
00007  *
00008  ****************************************************************************/
00009 
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version.
00014  *
00015  *  This program is distributed in the hope that it will be useful,
00016  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  *  GNU Library General Public License for more details.
00019  *
00020  *  Read the full text in the LICENSE.GPL file in the doc directory.
00021  */
00022 
00023 #include "thread_roomba_500.h"
00024 #include <interfaces/Roomba500Interface.h>
00025 
00026 #include <utils/time/wait.h>
00027 #include <utils/math/angle.h>
00028 #ifdef USE_TIMETRACKER
00029 #  include <utils/time/tracker.h>
00030 #endif
00031 
00032 #include <interfaces/LedInterface.h>
00033 #include <interfaces/SwitchInterface.h>
00034 #include <interfaces/BatteryInterface.h>
00035 // include <interfaces/MotorInterface.h>
00036 
00037 #include <netinet/in.h>
00038 #include <cstdio>
00039 
00040 using namespace fawkes;
00041 
00042 /** Worker thread for the Roomba 500 thread.
00043  * @author Tim Niemueller
00044  */
00045 class Roomba500Thread::WorkerThread : public fawkes::Thread
00046 {
00047  public:
00048   /** Constructor.
00049    * @param logger logger
00050    * @param clock clock
00051    * @param roomba refptr to Roomba500 instance
00052    * @param query_mode true to query data instead of streaming it.
00053    */
00054   WorkerThread(fawkes::Logger *logger, fawkes::Clock *clock,
00055                fawkes::RefPtr<Roomba500> roomba, bool query_mode)
00056     : Thread("Roomba500WorkerThread", Thread::OPMODE_CONTINUOUS),
00057       logger(logger), clock(clock), __roomba(roomba),
00058       __query_mode(query_mode)
00059   {
00060     __fresh_data_mutex = new Mutex();
00061     __time_wait = new TimeWait(clock, Roomba500::STREAM_INTERVAL_MS * 1000);
00062 
00063 #ifdef USE_TIMETRACKER
00064     __tt_count  = 0;
00065     __ttc_query = __tt.add_class("Query");
00066     __ttc_loop = __tt.add_class("Loop");
00067 #endif
00068 
00069     if (! __query_mode)  __roomba->enable_sensors();
00070   }
00071 
00072   /** Destructor. */
00073   ~WorkerThread()
00074   {
00075     if (! __query_mode)  __roomba->disable_sensors();
00076     delete __fresh_data_mutex;
00077     delete __time_wait;
00078   }
00079 
00080   virtual void loop()
00081   {
00082 #ifdef USE_TIMETRACKER
00083     __tt.ping_start(__ttc_loop);
00084 #endif
00085     
00086     //__time_wait->mark_start();
00087 
00088     try {
00089 #ifdef USE_TIMETRACKER
00090       __tt.ping_start(__ttc_query);
00091 #endif
00092       if (__query_mode)  __roomba->query_sensors();
00093       else               __roomba->read_sensors();
00094 #ifdef USE_TIMETRACKER
00095       __tt.ping_end(__ttc_query);
00096 #endif
00097       __fresh_data = __roomba->has_sensor_packet();
00098     } catch (Exception &e) {
00099       logger->log_warn(name(), "Failed to read sensor info, exception follows");
00100       logger->log_warn(name(), e);
00101     }
00102 
00103     //__time_wait->wait_systime();
00104 
00105 #ifdef USE_TIMETRACKER
00106       __tt.ping_end(__ttc_loop);
00107       if (++__tt_count == 300) {
00108         __tt_count = 0;
00109         __tt.print_to_stdout();
00110         __tt.reset();
00111       }
00112 #endif
00113   }
00114 
00115   /** Check if fresh data is available.
00116    * @return true if fresh data is available, false otherwise.
00117    */
00118   bool has_fresh_data()
00119   {
00120     __fresh_data_mutex->lock();
00121     bool rv = __fresh_data;
00122     __fresh_data = false;
00123     __fresh_data_mutex->unlock();
00124     return rv;
00125   }
00126 
00127  private:
00128   Logger            *logger;
00129   Clock             *clock;
00130   RefPtr<Roomba500>  __roomba;
00131   TimeWait          *__time_wait;
00132   Mutex             *__fresh_data_mutex;
00133 #ifdef USE_TIMETRACKER
00134   TimeTracker        __tt;
00135   unsigned int       __ttc_query;
00136   unsigned int       __ttc_loop;
00137   unsigned int       __tt_count;
00138 #endif
00139 
00140  private:
00141   bool __fresh_data;
00142   bool __query_mode;
00143 };
00144 
00145 
00146 /** @class Roomba500Thread "thread_roomba_500.h"
00147  * Roomba 500 integration thread.
00148  * This thread integrates the Roomba 500 robot into Fawkes. The thread
00149  * hooks in at the ACT hook and executes received commands on the hardware.
00150  * @author Tim Niemueller
00151  */
00152 
00153 /** Constructor. */
00154 Roomba500Thread::Roomba500Thread()
00155   : Thread("Roomba500", Thread::OPMODE_WAITFORWAKEUP),
00156     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_ACT)
00157 {
00158 }
00159 
00160 
00161 void
00162 Roomba500Thread::init()
00163 {
00164   __led_if_debris = NULL;
00165   __led_if_spot = NULL;
00166   __led_if_dock = NULL;
00167   __led_if_check_robot = NULL;
00168   __led_if_clean_color = NULL;
00169   __led_if_clean_intensity = NULL;
00170   __switch_if_vacuuming = NULL;
00171   __switch_if_but_clean = NULL;
00172   __switch_if_but_spot = NULL;
00173   __switch_if_but_dock = NULL;
00174   __switch_if_but_minute = NULL;
00175   __switch_if_but_hour = NULL;
00176   __switch_if_but_day = NULL;
00177   __switch_if_but_schedule = NULL;
00178   __switch_if_but_clock = NULL;
00179   //__motor_if = NULL;
00180   __battery_if = NULL;
00181   __roomba500_if = NULL;
00182 
00183   __greeting_loop_count = 0;
00184 
00185   __cfg_device = "";
00186   __cfg_btsave = false;
00187 
00188   Roomba500::ConnectionType conntype;
00189   __cfg_conntype = config->get_string("/hardware/roomba/connection_type");
00190   __cfg_btfast = false;
00191   __cfg_bttype = "firefly";
00192   __cfg_play_fanfare = true;
00193   __cfg_query_mode = true;
00194 
00195   try {
00196     __cfg_play_fanfare = config->get_bool("/hardware/roomba/play_fanfare");
00197   } catch (Exception &e) {}
00198 
00199   try {
00200     __cfg_query_mode = config->get_bool("/hardware/roomba/query_mode");
00201   } catch (Exception &e) {}
00202 
00203   if (__cfg_conntype == "rootooth") {
00204     try {
00205       __cfg_device = config->get_string("/hardware/roomba/btaddr");
00206     } catch (Exception &e) {
00207       try {
00208         __cfg_device = config->get_string("/hardware/roomba/btname");
00209       } catch (Exception &e2) {
00210         logger->log_info(name(), "Neither bluetooth name nor address set, "
00211                          "trying auto-detect");
00212       }
00213     }
00214     try {
00215       __cfg_btfast = config->get_bool("/hardware/roomba/btfast");
00216     } catch (Exception &e) {}
00217 
00218     try {
00219       __cfg_bttype = config->get_string("/hardware/roomba/bttype");
00220     } catch (Exception &e) {
00221       logger->log_info(name(), "RooTooth type not set, assuming 'firefly'");
00222     }
00223     if (__cfg_bttype == "firefly") {
00224       // we're cool
00225     } else if (__cfg_bttype == "mitsumi") {
00226       if (__cfg_btfast) {
00227         logger->log_warn(name(), "Fast mode setting for Mitsumi RooTooth not "
00228                          "supported, please set outside of Fawkes or wait "
00229                          "until configuration timeout has passed.");
00230         __cfg_btfast = false;
00231       }
00232     } else {
00233       logger->log_warn(name(), "Unknown RooTooth hardware type '%s' set",
00234                        __cfg_bttype.c_str());
00235       if (__cfg_btfast) {
00236         logger->log_warn(name(), "Fast mode setting only supported for "
00237                          "FireFly RooTooth");
00238         __cfg_btfast = false;
00239       }
00240     }
00241 
00242     conntype = Roomba500::CONNTYPE_ROOTOOTH;
00243   } else if (__cfg_conntype == "serial") {
00244     __cfg_device = config->get_string("/hardware/roomba/device");
00245     conntype = Roomba500::CONNTYPE_SERIAL;
00246   } else {
00247     throw Exception("Unknown mode '%s', must be rootooth or serial",
00248                     __cfg_conntype.c_str());
00249   }
00250 
00251   try {
00252     __cfg_btsave = config->get_bool("/hardware/roomba/btsave");
00253   } catch (Exception &e) {}
00254 
00255   Roomba500::Mode mode = Roomba500::MODE_PASSIVE;
00256   __cfg_mode = "passive";
00257   try {
00258     __cfg_mode = config->get_string("/hardware/roomba/mode");
00259   } catch (Exception &e) {}
00260   if (__cfg_mode == "passive") {
00261     mode = Roomba500::MODE_PASSIVE;
00262   } else if (__cfg_mode == "safe") {
00263     mode = Roomba500::MODE_SAFE;
00264   } else if (__cfg_mode == "full") {
00265     mode = Roomba500::MODE_FULL;
00266   } else {
00267     throw Exception("Unknown mode '%s', must be one of passive, safe, or full",
00268                     __cfg_mode.c_str());
00269   }
00270 
00271 
00272   try {
00273     __roomba500_if = blackboard->open_for_writing<Roomba500Interface>("Roomba 500");
00274     __led_if_debris =
00275       blackboard->open_for_writing<LedInterface>("Roomba LED Debris");
00276     __led_if_spot = blackboard->open_for_writing<LedInterface>("Roomba LED Spot");
00277     __led_if_dock = blackboard->open_for_writing<LedInterface>("Roomba LED Dock");
00278     __led_if_check_robot =
00279       blackboard->open_for_writing<LedInterface>("Roomba LED Check Robot");
00280     __led_if_clean_color =
00281       blackboard->open_for_writing<LedInterface>("Roomba LED Clean Color");
00282     __led_if_clean_intensity =
00283       blackboard->open_for_writing<LedInterface>("Roomba LED Clean Intensity");
00284     __switch_if_vacuuming =
00285       blackboard->open_for_writing<SwitchInterface>("Roomba Vacuuming");
00286     __switch_if_but_clean =
00287       blackboard->open_for_writing<SwitchInterface>("Roomba Button Clean");
00288     __switch_if_but_spot =
00289       blackboard->open_for_writing<SwitchInterface>("Roomba Button Spot");
00290     __switch_if_but_dock =
00291       blackboard->open_for_writing<SwitchInterface>("Roomba Button Dock");
00292     __switch_if_but_minute =
00293       blackboard->open_for_writing<SwitchInterface>("Roomba Button Minute");
00294     __switch_if_but_hour =
00295       blackboard->open_for_writing<SwitchInterface>("Roomba Button Hour");
00296     __switch_if_but_day =
00297       blackboard->open_for_writing<SwitchInterface>("Roomba Button Day");
00298     __switch_if_but_schedule =
00299       blackboard->open_for_writing<SwitchInterface>("Roomba Button Schedule");
00300     __switch_if_but_clock =
00301       blackboard->open_for_writing<SwitchInterface>("Roomba Button Clock");
00302     //__motor_if = blackboard->open_for_writing<MotorInterface>("Roomba Motor");
00303     __battery_if = blackboard->open_for_writing<BatteryInterface>("Roomba Battery");
00304   } catch (Exception &e) {
00305     close_interfaces();
00306     throw;
00307   }
00308 
00309   __wt = NULL;
00310   try {
00311     unsigned int flags = 0;
00312     if (conntype == Roomba500::CONNTYPE_ROOTOOTH) {
00313       logger->log_debug(name(), "Connecting via RooTooth, this may take a while");
00314       if (__cfg_btfast) flags |= Roomba500::FLAG_FIREFLY_FASTMODE;
00315     }
00316     __roomba = new Roomba500(conntype, __cfg_device.c_str(), flags);
00317 
00318     if (__cfg_btsave) {
00319       logger->log_debug(name(), "Saving Bluetooth address %s. Will be used for "
00320                         "next connection.", __roomba->get_device());
00321       config->set_string("/hardware/roomba/btaddr", __roomba->get_device());
00322     }
00323 
00324     __roomba->set_mode(mode);
00325     if (__roomba->is_controlled()) {
00326       if (__cfg_play_fanfare)  __roomba->play_fanfare();
00327       __roomba->set_leds(false, false, false, true, 0, 255);
00328     }
00329     __wt = new WorkerThread(logger, clock, __roomba, __cfg_query_mode);
00330   } catch (Exception &e) {
00331     close_interfaces();
00332     __roomba.clear();
00333     delete __wt;
00334     throw;
00335   }
00336   __wt->start();
00337 }
00338 
00339 void
00340 Roomba500Thread::close_interfaces()
00341 {
00342   blackboard->close(__led_if_debris);
00343   blackboard->close(__led_if_spot);
00344   blackboard->close(__led_if_dock);
00345   blackboard->close(__led_if_check_robot);
00346   blackboard->close(__led_if_clean_color);
00347   blackboard->close(__led_if_clean_intensity);
00348   blackboard->close(__switch_if_vacuuming);
00349   blackboard->close(__switch_if_but_clean);
00350   blackboard->close(__switch_if_but_spot);
00351   blackboard->close(__switch_if_but_dock);
00352   blackboard->close(__switch_if_but_minute);
00353   blackboard->close(__switch_if_but_hour);
00354   blackboard->close(__switch_if_but_day);
00355   blackboard->close(__switch_if_but_schedule);
00356   blackboard->close(__switch_if_but_clock);
00357   //blackboard->close(__motor_if);
00358   blackboard->close(__battery_if);
00359   blackboard->close(__roomba500_if);
00360 }
00361 
00362 
00363 void
00364 Roomba500Thread::finalize()
00365 {
00366   __wt->cancel();
00367   __wt->join();
00368   delete __wt;
00369   __roomba->set_mode(Roomba500::MODE_PASSIVE);
00370   __roomba.clear();
00371   close_interfaces();
00372 }
00373 
00374 
00375 float
00376 Roomba500Thread::led_process(fawkes::LedInterface *iface)
00377 {
00378   float intensity = iface->intensity();
00379   while (! iface->msgq_empty() ) {
00380     if (iface->msgq_first_is<LedInterface::TurnOnMessage>()) {
00381       intensity = 1.0;
00382     } else if (iface->msgq_first_is<LedInterface::TurnOffMessage>()) {
00383       intensity = 0.0;
00384     }
00385     iface->msgq_pop();
00386   }
00387   return intensity;
00388 }
00389 
00390 void
00391 Roomba500Thread::loop()
00392 {
00393   // process actuation
00394   float led_debris          = led_process(__led_if_debris);
00395   float led_spot            = led_process(__led_if_spot);
00396   float led_dock            = led_process(__led_if_dock);
00397   float led_check_robot     = led_process(__led_if_check_robot);
00398   float led_clean_color     = led_process(__led_if_clean_color);
00399   float led_clean_intensity = led_process(__led_if_clean_intensity);
00400 
00401   if ( (led_debris != __led_if_debris->intensity()) ||
00402        (led_spot != __led_if_spot->intensity()) ||
00403        (led_dock != __led_if_dock->intensity()) ||
00404        (led_check_robot != __led_if_check_robot->intensity()) ||
00405        (led_clean_color != __led_if_clean_color->intensity()) ||
00406        (led_clean_intensity != __led_if_clean_intensity->intensity()) )
00407   {
00408     try {
00409       __roomba->set_leds(led_debris > 0.5, led_spot > 0.5,
00410                          led_dock > 0.5, led_check_robot > 0.5,
00411                          (char)roundf(led_clean_color * 255.),
00412                          (char)roundf(led_clean_intensity * 255.));
00413     } catch (Exception &e) {
00414       logger->log_warn(name(), "Failed to set LEDs, exception follows");
00415       logger->log_warn(name(), e);
00416     }
00417 
00418     __led_if_debris->set_intensity(led_debris);
00419     __led_if_spot->set_intensity(led_spot);
00420     __led_if_dock->set_intensity(led_dock);
00421     __led_if_check_robot->set_intensity(led_check_robot);
00422     __led_if_clean_color->set_intensity(led_clean_color);
00423     __led_if_clean_intensity->set_intensity(led_clean_intensity);
00424 
00425     __led_if_debris->write();
00426     __led_if_spot->write();
00427     __led_if_dock->write();
00428     __led_if_check_robot->write();
00429     __led_if_clean_color->write();
00430     __led_if_clean_intensity->write();
00431   }
00432 
00433   while (! __roomba500_if->msgq_empty() ) {
00434     if (__roomba500_if->msgq_first_is<Roomba500Interface::StopMessage>())
00435     {
00436       try {
00437         __roomba->stop();
00438         //__roomba->set_motors(false, false, false, false, false);
00439         //logger->log_debug(name(), "Stopped");
00440       } catch (Exception &e) {
00441         logger->log_warn(name(), "Failed to stop robot, exception follows");
00442         logger->log_warn(name(), e);
00443       }
00444     } else  if (__roomba500_if->msgq_first_is<Roomba500Interface::SetModeMessage>())
00445     {
00446       Roomba500Interface::SetModeMessage *msg =
00447         __roomba500_if->msgq_first(msg);
00448 
00449       Roomba500::Mode mode = __roomba->get_mode();
00450       char color     =   0;
00451       char intensity = 255;
00452 
00453       switch (msg->mode()) {
00454       case Roomba500Interface::MODE_OFF:
00455         logger->log_debug(name(), "Switching off");
00456         mode = Roomba500::MODE_OFF;
00457         intensity = 0;
00458         break;
00459       case Roomba500Interface::MODE_PASSIVE:
00460         logger->log_debug(name(), "Switching to passive mode");
00461         mode = Roomba500::MODE_PASSIVE;
00462         color = 0;
00463         break;
00464       case Roomba500Interface::MODE_SAFE:
00465         logger->log_debug(name(), "Switching to safe mode");
00466         mode = Roomba500::MODE_SAFE;
00467         color = 128;
00468         break;
00469       case Roomba500Interface::MODE_FULL:
00470         logger->log_debug(name(), "Switching to full mode");
00471         mode = Roomba500::MODE_FULL;
00472         color = 255;
00473         break;
00474       default:
00475         logger->log_warn(name(), "Invalid mode %i received, ignoring",
00476                          msg->mode());
00477       }
00478       try {
00479         bool was_controlled = __roomba->is_controlled();
00480         if (! was_controlled) {
00481           // set first
00482           __roomba->set_mode(mode);
00483         }
00484         if (__roomba->is_controlled()) {
00485           __roomba->set_leds(__led_if_debris->intensity() >= 0.5,
00486                              __led_if_spot->intensity() >= 0.5,
00487                              __led_if_dock->intensity() >= 0.5,
00488                              __led_if_check_robot->intensity() >= 0.5,
00489                              color, intensity);
00490         }
00491         if (was_controlled) {
00492           __roomba->set_mode(mode);
00493         }
00494       } catch (Exception &e) {
00495         logger->log_warn(name(), "Cannot set mode, exception follows");
00496         logger->log_warn(name(), e);
00497       }
00498 
00499     } else if (__roomba500_if->msgq_first_is<Roomba500Interface::DockMessage>()) {
00500       try {
00501         __roomba->seek_dock();
00502         logger->log_debug(name(), "Docking");
00503       } catch (Exception &e) {
00504         logger->log_warn(name(), "Failed to seek dock, exception follows");
00505         logger->log_warn(name(), e);
00506       }
00507     } else  if (__roomba500_if->msgq_first_is<Roomba500Interface::DriveStraightMessage>())
00508     {
00509       Roomba500Interface::DriveStraightMessage *msg =
00510         __roomba500_if->msgq_first(msg);
00511 
00512       try {
00513         __roomba->drive_straight(msg->velocity());
00514       } catch (Exception &e) {
00515         logger->log_warn(name(), "Failed to drive straight, exception follows");
00516         logger->log_warn(name(), e);
00517       }
00518     } else  if (__roomba500_if->msgq_first_is<Roomba500Interface::DriveMessage>())
00519     {
00520       Roomba500Interface::DriveMessage *msg =
00521         __roomba500_if->msgq_first(msg);
00522 
00523       try {
00524         __roomba->drive(msg->velocity(), msg->radius());
00525       } catch (Exception &e) {
00526         logger->log_warn(name(), "Failed to drive, exception follows");
00527         logger->log_warn(name(), e);
00528       }
00529 
00530     } else  if (__roomba500_if->msgq_first_is<Roomba500Interface::SetMotorsMessage>())
00531     {
00532       Roomba500Interface::SetMotorsMessage *msg =
00533         __roomba500_if->msgq_first(msg);
00534 
00535       try {
00536         __roomba->set_motors(
00537           (msg->main() != Roomba500Interface::BRUSHSTATE_OFF),
00538           (msg->side() != Roomba500Interface::BRUSHSTATE_OFF),
00539           msg->is_vacuuming(),
00540           (msg->main() == Roomba500Interface::BRUSHSTATE_BACKWARD),
00541           (msg->side() == Roomba500Interface::BRUSHSTATE_BACKWARD));
00542       } catch (Exception &e) {
00543         logger->log_warn(name(), "Failed to set motors, exception follows");
00544         logger->log_warn(name(), e);
00545       }
00546     }
00547     __roomba500_if->msgq_pop();
00548   }
00549 
00550   if (__roomba->is_controlled()) {
00551     if (__greeting_loop_count < 50) {
00552       if (++__greeting_loop_count == 50) {
00553         __roomba->set_leds(false, false, false, false, 0, 0);
00554       } else {
00555         __roomba->set_leds(false, false, false, true,
00556                            0, __greeting_loop_count * 5);
00557       }
00558     }
00559   }
00560 }
00561 
00562 
00563 /** Write data to blackboard.
00564  * To be called by the RoombaSensorThread during the sensor hook to
00565  * write new data to the blackboard if available.
00566  */
00567 void
00568 Roomba500Thread::write_blackboard()
00569 {
00570   if (__wt->has_fresh_data()) {
00571     const Roomba500::SensorPacketGroupAll sp(__roomba->get_sensor_packet());
00572 
00573     int charge = (int)roundf(((float)ntohs(sp.battery_charge) /
00574                               (float)ntohs(sp.battery_capacity)) * 100.);
00575 
00576     if (__roomba->is_controlled()) {
00577       if (charge != __battery_percent) {
00578         char digits[4];
00579         snprintf(digits, 4, "%u%%", charge);
00580         __roomba->set_digit_leds(digits);
00581         __battery_percent = charge;
00582       }
00583     }
00584 
00585     __roomba500_if->set_mode((Roomba500Interface::Mode)sp.mode);
00586     __roomba500_if->set_wheel_drop_left(
00587         sp.bumps_wheeldrops & Roomba500::WHEEL_DROP_LEFT);
00588     __roomba500_if->set_wheel_drop_right(
00589         sp.bumps_wheeldrops & Roomba500::WHEEL_DROP_RIGHT);
00590     __roomba500_if->set_bump_left(sp.bumps_wheeldrops & Roomba500::BUMP_LEFT);
00591     __roomba500_if->set_bump_right(sp.bumps_wheeldrops & Roomba500::BUMP_RIGHT);
00592     __roomba500_if->set_cliff_left(sp.cliff_left == 1);
00593     __roomba500_if->set_cliff_front_left(sp.cliff_front_left == 1);
00594     __roomba500_if->set_cliff_front_right(sp.cliff_front_right == 1);
00595     __roomba500_if->set_cliff_right(sp.cliff_right == 1);
00596     __roomba500_if->set_wall(sp.virtual_wall == 1);
00597     __roomba500_if->set_overcurrent_left_wheel(
00598         sp.overcurrents & Roomba500::OVERCURRENT_WHEEL_LEFT);
00599     __roomba500_if->set_overcurrent_right_wheel(
00600         sp.overcurrents & Roomba500::OVERCURRENT_WHEEL_RIGHT);
00601     __roomba500_if->set_overcurrent_main_brush(
00602         sp.overcurrents & Roomba500::OVERCURRENT_MAIN_BRUSH);
00603     __roomba500_if->set_overcurrent_side_brush(
00604         sp.overcurrents & Roomba500::OVERCURRENT_SIDE_BRUSH);
00605     __roomba500_if->set_dirt_detect(sp.dirt_detect == 1);
00606     __roomba500_if->set_ir_opcode_omni(
00607         (Roomba500Interface::InfraredCharacter)sp.ir_opcode_omni);
00608     __roomba500_if->set_button_clean(sp.buttons & Roomba500::BUTTON_CLEAN);
00609     __roomba500_if->set_button_spot(sp.buttons & Roomba500::BUTTON_SPOT);
00610     __roomba500_if->set_button_dock(sp.buttons & Roomba500::BUTTON_DOCK);
00611     __roomba500_if->set_button_minute(sp.buttons & Roomba500::BUTTON_MINUTE);
00612     __roomba500_if->set_button_hour(sp.buttons & Roomba500::BUTTON_HOUR);
00613     __roomba500_if->set_button_day(sp.buttons & Roomba500::BUTTON_DAY);
00614     __roomba500_if->set_button_schedule(sp.buttons & Roomba500::BUTTON_SCHEDULE);
00615     __roomba500_if->set_button_clock(sp.buttons & Roomba500::BUTTON_CLOCK);
00616 
00617     __switch_if_but_clean->set_enabled(sp.buttons & Roomba500::BUTTON_CLEAN);
00618     __switch_if_but_spot->set_enabled(sp.buttons & Roomba500::BUTTON_SPOT);
00619     __switch_if_but_dock->set_enabled(sp.buttons & Roomba500::BUTTON_DOCK);
00620     __switch_if_but_minute->set_enabled(sp.buttons & Roomba500::BUTTON_MINUTE);
00621     __switch_if_but_hour->set_enabled(sp.buttons & Roomba500::BUTTON_HOUR);
00622     __switch_if_but_day->set_enabled(sp.buttons & Roomba500::BUTTON_DAY);
00623     __switch_if_but_schedule->set_enabled(sp.buttons & Roomba500::BUTTON_SCHEDULE);
00624     __switch_if_but_clock->set_enabled(sp.buttons & Roomba500::BUTTON_CLOCK);
00625 
00626     // Convert mm to m for distance
00627     __roomba500_if->set_distance((int16_t)ntohs(sp.distance));
00628     // invert because in Fawkes positive angles go counter-clockwise, while
00629     // for the Roomba they go clockwise
00630     __roomba500_if->set_angle(- (int16_t)ntohs(sp.angle));
00631     __roomba500_if->set_charging_state(
00632         (Roomba500Interface::ChargingState)sp.charging_state);
00633     __roomba500_if->set_voltage(ntohs(sp.voltage));
00634     __roomba500_if->set_current((int)ntohs(sp.current));
00635     __roomba500_if->set_temperature((int)sp.temperature);
00636     __roomba500_if->set_battery_charge(ntohs(sp.battery_charge));
00637     __roomba500_if->set_battery_capacity(ntohs(sp.battery_capacity));
00638 
00639     __battery_if->set_voltage(ntohs(sp.voltage));
00640     __battery_if->set_current((int)ntohs(sp.current));
00641     __battery_if->set_temperature((char)sp.temperature);
00642     __battery_if->set_absolute_soc((float)ntohs(sp.battery_charge) /
00643                                    (float)ntohs(sp.battery_capacity));
00644     __battery_if->set_relative_soc(__battery_if->absolute_soc());
00645 
00646     __roomba500_if->set_wall_signal(ntohs(sp.wall_signal));
00647     __roomba500_if->set_cliff_left_signal(ntohs(sp.cliff_left_signal));
00648     __roomba500_if->set_cliff_front_left_signal(ntohs(sp.cliff_front_left_signal));
00649     __roomba500_if->set_cliff_front_right_signal(ntohs(sp.cliff_front_right_signal));
00650     __roomba500_if->set_cliff_right_signal(ntohs(sp.cliff_right_signal));
00651     __roomba500_if->set_home_base_charger_available(
00652         sp.charger_available & Roomba500::CHARGER_HOME_BASE);
00653     __roomba500_if->set_internal_charger_available(
00654         sp.charger_available & Roomba500::CHARGER_INTERNAL);
00655     __roomba500_if->set_song_number(sp.song_number);
00656     __roomba500_if->set_song_playing(sp.song_playing == 1);
00657 
00658     __roomba500_if->set_velocity((int16_t)ntohs(sp.velocity));
00659     __roomba500_if->set_radius((int16_t)ntohs(sp.radius));
00660     __roomba500_if->set_velocity_right((int16_t)ntohs(sp.velocity_right));
00661     __roomba500_if->set_velocity_left((int16_t)ntohs(sp.velocity_left));
00662     __roomba500_if->set_encoder_counts_left(ntohs(sp.encoder_counts_left));
00663     __roomba500_if->set_encoder_counts_right(ntohs(sp.encoder_counts_right));
00664 
00665     __roomba500_if->set_bumper_left(
00666         sp.light_bumper & Roomba500::BUMPER_LEFT);
00667     __roomba500_if->set_bumper_front_left(
00668         sp.light_bumper & Roomba500::BUMPER_FRONT_LEFT);
00669     __roomba500_if->set_bumper_center_left(
00670         sp.light_bumper & Roomba500::BUMPER_CENTER_LEFT);
00671     __roomba500_if->set_bumper_center_right(
00672         sp.light_bumper & Roomba500::BUMPER_CENTER_RIGHT);
00673     __roomba500_if->set_bumper_front_right(
00674         sp.light_bumper & Roomba500::BUMPER_FRONT_RIGHT);
00675     __roomba500_if->set_bumper_right(
00676         sp.light_bumper & Roomba500::BUMPER_RIGHT);
00677 
00678     __roomba500_if->set_light_bump_left(ntohs(sp.light_bump_left));
00679     __roomba500_if->set_light_bump_front_left(ntohs(sp.light_bump_front_left));
00680     __roomba500_if->set_light_bump_center_left(ntohs(sp.light_bump_center_left));
00681     __roomba500_if->set_light_bump_center_right(ntohs(sp.light_bump_center_right));
00682     __roomba500_if->set_light_bump_front_right(ntohs(sp.light_bump_front_right));
00683     __roomba500_if->set_light_bump_right(ntohs(sp.light_bump_right));
00684 
00685     __roomba500_if->set_ir_opcode_left(
00686         (Roomba500Interface::InfraredCharacter)sp.ir_opcode_left);
00687     __roomba500_if->set_ir_opcode_right(
00688         (Roomba500Interface::InfraredCharacter)sp.ir_opcode_right);
00689 
00690     __roomba500_if->set_left_motor_current((int)ntohs(sp.left_motor_current));
00691     __roomba500_if->set_right_motor_current((int)ntohs(sp.right_motor_current));
00692     __roomba500_if->set_side_brush_current((int)ntohs(sp.side_brush_current));
00693     __roomba500_if->set_main_brush_current((int)ntohs(sp.main_brush_current));
00694     __roomba500_if->set_caster_stasis(sp.stasis == 1);
00695 
00696     __roomba500_if->write();
00697 
00698     __switch_if_but_clean->write();
00699     __switch_if_but_spot->write();
00700     __switch_if_but_dock->write();
00701     __switch_if_but_minute->write();
00702     __switch_if_but_hour->write();
00703     __switch_if_but_day->write();
00704     __switch_if_but_schedule->write();
00705     __switch_if_but_clock->write();
00706 
00707     __battery_if->write();
00708   }
00709 }
00710 
00711 
00712 /** Set mode and indicate with LED.
00713  * This will set the mode and if successful also set the color and intensity
00714  * of the clean LED indicating the mode.
00715  * @param mode mode to set
00716  * @exception Exception may be thrown if mode setting fails
00717  */
00718 void
00719 Roomba500Thread::set_mode(Roomba500::Mode mode)
00720 {
00721   char color     =   0;
00722   char intensity = 255;
00723 
00724   switch (mode) {
00725   case Roomba500::MODE_OFF:     intensity =   0; break;
00726   case Roomba500::MODE_PASSIVE: color     =   0; break;
00727   case Roomba500::MODE_SAFE:    color     = 128; break;
00728   case Roomba500::MODE_FULL:    color     = 255; break;
00729   }
00730 
00731   __roomba->set_mode(mode);
00732   __roomba->set_leds(__led_if_debris->intensity() >= 0.5,
00733                      __led_if_spot->intensity() >= 0.5,
00734                      __led_if_dock->intensity() >= 0.5,
00735                      __led_if_check_robot->intensity() >= 0.5,
00736                      color, intensity);
00737 }