Fawkes API
Fawkes Development Version
|
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 }