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> 56 :
Thread(
"Roomba500WorkerThread",
Thread::OPMODE_CONTINUOUS),
57 logger(logger), __roomba(roomba), __query_mode(query_mode)
59 __fresh_data_mutex =
new Mutex();
62 #ifdef USE_TIMETRACKER 64 __ttc_query = __tt.add_class(
"Query");
65 __ttc_loop = __tt.add_class(
"Loop");
68 if (! __query_mode) __roomba->enable_sensors();
74 if (! __query_mode) __roomba->disable_sensors();
75 delete __fresh_data_mutex;
81 #ifdef USE_TIMETRACKER 82 __tt.ping_start(__ttc_loop);
88 #ifdef USE_TIMETRACKER 89 __tt.ping_start(__ttc_query);
91 if (__query_mode) __roomba->query_sensors();
92 else __roomba->read_sensors();
93 #ifdef USE_TIMETRACKER 94 __tt.ping_end(__ttc_query);
96 __fresh_data = __roomba->has_sensor_packet();
98 logger->
log_warn(name(),
"Failed to read sensor info, exception follows");
104 #ifdef USE_TIMETRACKER 105 __tt.ping_end(__ttc_loop);
106 if (++__tt_count == 300) {
108 __tt.print_to_stdout();
119 __fresh_data_mutex->lock();
120 bool rv = __fresh_data;
121 __fresh_data =
false;
122 __fresh_data_mutex->unlock();
130 Mutex *__fresh_data_mutex;
131 #ifdef USE_TIMETRACKER 133 unsigned int __ttc_query;
134 unsigned int __ttc_loop;
135 unsigned int __tt_count;
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;
179 __roomba500_if = NULL;
181 __greeting_loop_count = 0;
184 __cfg_btsave =
false;
188 __cfg_btfast =
false;
189 __cfg_bttype =
"firefly";
190 __cfg_play_fanfare =
true;
191 __cfg_query_mode =
true;
194 __cfg_play_fanfare =
config->
get_bool(
"/hardware/roomba/play_fanfare");
198 __cfg_query_mode =
config->
get_bool(
"/hardware/roomba/query_mode");
201 if (__cfg_conntype ==
"rootooth") {
209 "trying auto-detect");
221 if (__cfg_bttype ==
"firefly") {
223 }
else if (__cfg_bttype ==
"mitsumi") {
226 "supported, please set outside of Fawkes or wait " 227 "until configuration timeout has passed.");
228 __cfg_btfast =
false;
232 __cfg_bttype.c_str());
236 __cfg_btfast =
false;
241 }
else if (__cfg_conntype ==
"serial") {
245 throw Exception(
"Unknown mode '%s', must be rootooth or serial",
246 __cfg_conntype.c_str());
254 __cfg_mode =
"passive";
258 if (__cfg_mode ==
"passive") {
260 }
else if (__cfg_mode ==
"safe") {
262 }
else if (__cfg_mode ==
"full") {
265 throw Exception(
"Unknown mode '%s', must be one of passive, safe, or full",
276 __led_if_check_robot =
278 __led_if_clean_color =
280 __led_if_clean_intensity =
282 __switch_if_vacuuming =
284 __switch_if_but_clean =
286 __switch_if_but_spot =
288 __switch_if_but_dock =
290 __switch_if_but_minute =
292 __switch_if_but_hour =
294 __switch_if_but_day =
296 __switch_if_but_schedule =
298 __switch_if_but_clock =
309 unsigned int flags = 0;
314 __roomba =
new Roomba500(conntype, __cfg_device.c_str(), flags);
325 __roomba->
set_leds(
false,
false,
false,
true, 0, 255);
338 Roomba500Thread::close_interfaces()
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);
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()) )
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.));
421 __led_if_clean_intensity->
set_intensity(led_clean_intensity);
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();
449 char intensity = 255;
451 switch (msg->
mode()) {
452 case Roomba500Interface::MODE_OFF:
457 case Roomba500Interface::MODE_PASSIVE:
462 case Roomba500Interface::MODE_SAFE:
467 case Roomba500Interface::MODE_FULL:
478 if (! was_controlled) {
486 __led_if_check_robot->
intensity() >= 0.5,
489 if (was_controlled) {
535 (msg->
main() != Roomba500Interface::BRUSHSTATE_OFF),
536 (msg->
side() != Roomba500Interface::BRUSHSTATE_OFF),
538 (msg->
main() == Roomba500Interface::BRUSHSTATE_BACKWARD),
539 (msg->
side() == Roomba500Interface::BRUSHSTATE_BACKWARD));
549 if (__greeting_loop_count < 50) {
550 if (++__greeting_loop_count == 50) {
551 __roomba->
set_leds(
false,
false,
false,
false, 0, 0);
553 __roomba->
set_leds(
false,
false,
false,
true,
554 0, __greeting_loop_count * 5);
571 int charge = (int)roundf(((
float)ntohs(sp.battery_charge) /
572 (float)ntohs(sp.battery_capacity)) * 100.);
575 if (charge != __battery_percent) {
577 snprintf(digits, 4,
"%u%%", charge);
579 __battery_percent = charge;
594 __roomba500_if->
set_wall(sp.virtual_wall == 1);
625 __roomba500_if->
set_distance((int16_t)ntohs(sp.distance));
628 __roomba500_if->
set_angle(- (int16_t)ntohs(sp.angle));
632 __roomba500_if->
set_current((
int)ntohs(sp.current));
641 (
float)ntohs(sp.battery_capacity));
656 __roomba500_if->
set_velocity((int16_t)ntohs(sp.velocity));
657 __roomba500_if->
set_radius((int16_t)ntohs(sp.radius));
694 __roomba500_if->
write();
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();
705 __battery_if->
write();
720 char intensity = 255;
733 __led_if_check_robot->
intensity() >= 0.5,
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.
static const unsigned short int STREAM_INTERVAL_MS
Time in ms between.
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.
void set_current(const int16_t new_current)
Set current value.
static const unsigned char OVERCURRENT_WHEEL_RIGHT
Right wheel bit.
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.
void set_temperature(const int8_t new_temperature)
Set temperature value.
ChargingState
Current charging state.
bool msgq_empty()
Check if queue is empty.
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.
void set_radius(const int16_t new_radius)
Set radius value.
static const unsigned char BUMPER_CENTER_RIGHT
Center right bumper.
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.
Control acquired, safety measures in place.
static const unsigned char WHEEL_DROP_RIGHT
Right wheel drop bit.
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.
static const unsigned char WHEEL_DROP_LEFT
Left wheel drop bit.
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.
void seek_dock()
Seek for the home base and dock.
Mode mode() const
Get mode value.
This is supposed to be the central clock in Fawkes.
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.
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.
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.
InfraredCharacter
Infrared character values.
static const unsigned char BUTTON_DOCK
Dock button.
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.
void set_voltage(const uint32_t new_voltage)
Set voltage value.
Thread class encapsulation of pthreads.
static const unsigned char BUMPER_LEFT
Left bumper.
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.
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.
static const unsigned char BUMPER_FRONT_LEFT
Front left bumper.
Worker thread for the Roomba 500 thread.
Use serial connection (device file).
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.
void set_encoder_counts_left(const uint16_t new_encoder_counts_left)
Set encoder_counts_left value.
Mode
Roomba 500 operation mode.
void set_temperature(const uint32_t new_temperature)
Set temperature value.
Clock * clock
By means of this member access to the clock is given.
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.
Thread aspect to use blocked timing.
static const unsigned char BUTTON_SPOT
Spot cleaning button.
static const unsigned char OVERCURRENT_WHEEL_LEFT
Left wheel bit.
void msgq_pop()
Erase first message from queue.
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.
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.
static const unsigned char OVERCURRENT_MAIN_BRUSH
Main brush bit.
Control acquired, safety measures disabled.
Base class for exceptions in Fawkes.
Message * msgq_first()
Get the first message from the message queue.
ConnectionType
Connection type.
static const unsigned char BUTTON_SCHEDULE
Schedule button.
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.
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.
static const unsigned char BUTTON_CLOCK
Clock button.
BrushState main() const
Get main value.
const char * name() const
Get name of thread.
static const unsigned char BUTTON_MINUTE
Minute button.
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.
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.
int16_t velocity() const
Get velocity value.
const SensorPacketGroupAll get_sensor_packet() const
Get latest sensor packet.
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.
void cancel()
Cancel a thread.
~WorkerThread()
Destructor.
void set_button_clean(const bool new_button_clean)
Set button_clean value.
BrushState side() const
Get side value.
static const unsigned char OVERCURRENT_SIDE_BRUSH
Side brush bit.
LedInterface Fawkes BlackBoard Interface.
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.
static const unsigned char BUMPER_RIGHT
Right bumper.
void set_cliff_front_right(const bool new_cliff_front_right)
Set cliff_front_right value.
void set_mode(Mode mode)
Set control mode.
void set_cliff_left_signal(const uint16_t new_cliff_left_signal)
Set cliff_left_signal value.
void join()
Join the thread.
Passive mode, no control, only listening.
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.
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).
Mutex mutual exclusion lock.
static const unsigned char BUMP_RIGHT
Right bumper bit.
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.
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.
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.
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.
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.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
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.
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.