23 #include "thread_roomba_500.h"
24 #include <interfaces/Roomba500Interface.h>
26 #include <utils/time/wait.h>
27 #include <utils/math/angle.h>
28 #ifdef USE_TIMETRACKER
29 # include <utils/time/tracker.h>
32 #include <interfaces/LedInterface.h>
33 #include <interfaces/SwitchInterface.h>
34 #include <interfaces/BatteryInterface.h>
37 #include <netinet/in.h>
40 using namespace fawkes;
56 :
Thread(
"Roomba500WorkerThread",
Thread::OPMODE_CONTINUOUS),
57 logger(logger), clock(clock), __roomba(roomba),
58 __query_mode(query_mode)
60 __fresh_data_mutex =
new Mutex();
63 #ifdef USE_TIMETRACKER
65 __ttc_query = __tt.add_class(
"Query");
66 __ttc_loop = __tt.add_class(
"Loop");
69 if (! __query_mode) __roomba->enable_sensors();
75 if (! __query_mode) __roomba->disable_sensors();
76 delete __fresh_data_mutex;
82 #ifdef USE_TIMETRACKER
83 __tt.ping_start(__ttc_loop);
89 #ifdef USE_TIMETRACKER
90 __tt.ping_start(__ttc_query);
92 if (__query_mode) __roomba->query_sensors();
93 else __roomba->read_sensors();
94 #ifdef USE_TIMETRACKER
95 __tt.ping_end(__ttc_query);
97 __fresh_data = __roomba->has_sensor_packet();
99 logger->
log_warn(name(),
"Failed to read sensor info, exception follows");
105 #ifdef USE_TIMETRACKER
106 __tt.ping_end(__ttc_loop);
107 if (++__tt_count == 300) {
109 __tt.print_to_stdout();
118 bool has_fresh_data()
120 __fresh_data_mutex->lock();
121 bool rv = __fresh_data;
122 __fresh_data =
false;
123 __fresh_data_mutex->unlock();
132 Mutex *__fresh_data_mutex;
133 #ifdef USE_TIMETRACKER
135 unsigned int __ttc_query;
136 unsigned int __ttc_loop;
137 unsigned int __tt_count;
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;
181 __roomba500_if = NULL;
183 __greeting_loop_count = 0;
186 __cfg_btsave =
false;
190 __cfg_btfast =
false;
191 __cfg_bttype =
"firefly";
192 __cfg_play_fanfare =
true;
193 __cfg_query_mode =
true;
196 __cfg_play_fanfare =
config->
get_bool(
"/hardware/roomba/play_fanfare");
200 __cfg_query_mode =
config->
get_bool(
"/hardware/roomba/query_mode");
203 if (__cfg_conntype ==
"rootooth") {
211 "trying auto-detect");
223 if (__cfg_bttype ==
"firefly") {
225 }
else if (__cfg_bttype ==
"mitsumi") {
228 "supported, please set outside of Fawkes or wait "
229 "until configuration timeout has passed.");
230 __cfg_btfast =
false;
234 __cfg_bttype.c_str());
238 __cfg_btfast =
false;
243 }
else if (__cfg_conntype ==
"serial") {
247 throw Exception(
"Unknown mode '%s', must be rootooth or serial",
248 __cfg_conntype.c_str());
256 __cfg_mode =
"passive";
260 if (__cfg_mode ==
"passive") {
262 }
else if (__cfg_mode ==
"safe") {
264 }
else if (__cfg_mode ==
"full") {
267 throw Exception(
"Unknown mode '%s', must be one of passive, safe, or full",
278 __led_if_check_robot =
280 __led_if_clean_color =
282 __led_if_clean_intensity =
284 __switch_if_vacuuming =
286 __switch_if_but_clean =
288 __switch_if_but_spot =
290 __switch_if_but_dock =
292 __switch_if_but_minute =
294 __switch_if_but_hour =
296 __switch_if_but_day =
298 __switch_if_but_schedule =
300 __switch_if_but_clock =
311 unsigned int flags = 0;
316 __roomba =
new Roomba500(conntype, __cfg_device.c_str(), flags);
327 __roomba->
set_leds(
false,
false,
false,
true, 0, 255);
340 Roomba500Thread::close_interfaces()
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);
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()) )
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.));
423 __led_if_clean_intensity->
set_intensity(led_clean_intensity);
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();
451 char intensity = 255;
453 switch (msg->
mode()) {
454 case Roomba500Interface::MODE_OFF:
459 case Roomba500Interface::MODE_PASSIVE:
464 case Roomba500Interface::MODE_SAFE:
469 case Roomba500Interface::MODE_FULL:
480 if (! was_controlled) {
488 __led_if_check_robot->
intensity() >= 0.5,
491 if (was_controlled) {
537 (msg->
main() != Roomba500Interface::BRUSHSTATE_OFF),
538 (msg->
side() != Roomba500Interface::BRUSHSTATE_OFF),
540 (msg->
main() == Roomba500Interface::BRUSHSTATE_BACKWARD),
541 (msg->
side() == Roomba500Interface::BRUSHSTATE_BACKWARD));
551 if (__greeting_loop_count < 50) {
552 if (++__greeting_loop_count == 50) {
553 __roomba->
set_leds(
false,
false,
false,
false, 0, 0);
555 __roomba->
set_leds(
false,
false,
false,
true,
556 0, __greeting_loop_count * 5);
573 int charge = (int)roundf(((
float)ntohs(sp.battery_charge) /
574 (float)ntohs(sp.battery_capacity)) * 100.);
577 if (charge != __battery_percent) {
579 snprintf(digits, 4,
"%u%%", charge);
581 __battery_percent = charge;
596 __roomba500_if->
set_wall(sp.virtual_wall == 1);
627 __roomba500_if->
set_distance((int16_t)ntohs(sp.distance));
630 __roomba500_if->
set_angle(- (int16_t)ntohs(sp.angle));
634 __roomba500_if->
set_current((
int)ntohs(sp.current));
643 (
float)ntohs(sp.battery_capacity));
658 __roomba500_if->
set_velocity((int16_t)ntohs(sp.velocity));
659 __roomba500_if->
set_radius((int16_t)ntohs(sp.radius));
696 __roomba500_if->
write();
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();
707 __battery_if->
write();
722 char intensity = 255;
735 __led_if_check_robot->
intensity() >= 0.5,