23 #include "thread_roomba_500.h" 25 #include <interfaces/Roomba500Interface.h> 26 #include <utils/math/angle.h> 27 #include <utils/time/wait.h> 28 #ifdef USE_TIMETRACKER 29 # include <utils/time/tracker.h> 32 #include <interfaces/BatteryInterface.h> 33 #include <interfaces/LedInterface.h> 34 #include <interfaces/SwitchInterface.h> 37 #include <netinet/in.h> 59 :
Thread(
"Roomba500WorkerThread",
Thread::OPMODE_CONTINUOUS),
62 query_mode_(query_mode)
64 fresh_data_mutex_ =
new Mutex();
67 #ifdef USE_TIMETRACKER 69 ttc_query_ = tt_.add_class(
"Query");
70 ttc_loop_ = tt_.add_class(
"Loop");
74 roomba_->enable_sensors();
81 roomba_->disable_sensors();
82 delete fresh_data_mutex_;
89 #ifdef USE_TIMETRACKER 90 tt_.ping_start(ttc_loop_);
96 #ifdef USE_TIMETRACKER 97 tt_.ping_start(ttc_query_);
100 roomba_->query_sensors();
102 roomba_->read_sensors();
103 #ifdef USE_TIMETRACKER 104 tt_.ping_end(ttc_query_);
106 fresh_data_ = roomba_->has_sensor_packet();
108 logger->
log_warn(name(),
"Failed to read sensor info, exception follows");
114 #ifdef USE_TIMETRACKER 115 tt_.ping_end(ttc_loop_);
116 if (++tt_count_ == 300) {
118 tt_.print_to_stdout();
130 fresh_data_mutex_->lock();
131 bool rv = fresh_data_;
133 fresh_data_mutex_->unlock();
141 Mutex * fresh_data_mutex_;
142 #ifdef USE_TIMETRACKER 144 unsigned int ttc_query_;
145 unsigned int ttc_loop_;
146 unsigned int tt_count_;
171 led_if_debris_ = NULL;
174 led_if_check_robot_ = NULL;
175 led_if_clean_color_ = NULL;
176 led_if_clean_intensity_ = NULL;
177 switch_if_vacuuming_ = NULL;
178 switch_if_but_clean_ = NULL;
179 switch_if_but_spot_ = NULL;
180 switch_if_but_dock_ = NULL;
181 switch_if_but_minute_ = NULL;
182 switch_if_but_hour_ = NULL;
183 switch_if_but_day_ = NULL;
184 switch_if_but_schedule_ = NULL;
185 switch_if_but_clock_ = NULL;
188 roomba500_if_ = NULL;
190 greeting_loop_count_ = 0;
198 cfg_bttype_ =
"firefly";
199 cfg_play_fanfare_ =
true;
200 cfg_query_mode_ =
true;
203 cfg_play_fanfare_ =
config->
get_bool(
"/hardware/roomba/play_fanfare");
208 cfg_query_mode_ =
config->
get_bool(
"/hardware/roomba/query_mode");
212 if (cfg_conntype_ ==
"rootooth") {
220 "Neither bluetooth name nor address set, " 221 "trying auto-detect");
234 if (cfg_bttype_ ==
"firefly") {
236 }
else if (cfg_bttype_ ==
"mitsumi") {
239 "Fast mode setting for Mitsumi RooTooth not " 240 "supported, please set outside of Fawkes or wait " 241 "until configuration timeout has passed.");
245 logger->
log_warn(
name(),
"Unknown RooTooth hardware type '%s' set", cfg_bttype_.c_str());
248 "Fast mode setting only supported for " 255 }
else if (cfg_conntype_ ==
"serial") {
259 throw Exception(
"Unknown mode '%s', must be rootooth or serial", cfg_conntype_.c_str());
268 cfg_mode_ =
"passive";
273 if (cfg_mode_ ==
"passive") {
275 }
else if (cfg_mode_ ==
"safe") {
277 }
else if (cfg_mode_ ==
"full") {
280 throw Exception(
"Unknown mode '%s', must be one of passive, safe, or full", cfg_mode_.c_str());
290 led_if_clean_intensity_ =
299 switch_if_but_schedule_ =
311 unsigned int flags = 0;
317 roomba_ =
new Roomba500(conntype, cfg_device_.c_str(), flags);
321 "Saving Bluetooth address %s. Will be used for " 329 if (cfg_play_fanfare_)
331 roomba_->
set_leds(
false,
false,
false,
true, 0, 255);
344 Roomba500Thread::close_interfaces()
396 float led_debris = led_process(led_if_debris_);
397 float led_spot = led_process(led_if_spot_);
398 float led_dock = led_process(led_if_dock_);
399 float led_check_robot = led_process(led_if_check_robot_);
400 float led_clean_color = led_process(led_if_clean_color_);
401 float led_clean_intensity = led_process(led_if_clean_intensity_);
403 if ((led_debris != led_if_debris_->
intensity()) || (led_spot != led_if_spot_->
intensity())
404 || (led_dock != led_if_dock_->
intensity())
405 || (led_check_robot != led_if_check_robot_->
intensity())
406 || (led_clean_color != led_if_clean_color_->
intensity())
407 || (led_clean_intensity != led_if_clean_intensity_->
intensity())) {
412 led_check_robot > 0.5,
413 (
char)roundf(led_clean_color * 255.),
414 (
char)roundf(led_clean_intensity * 255.));
427 led_if_debris_->
write();
428 led_if_spot_->
write();
429 led_if_dock_->
write();
430 led_if_check_robot_->
write();
431 led_if_clean_color_->
write();
432 led_if_clean_intensity_->
write();
449 unsigned char color = 0;
450 unsigned char intensity = 255;
452 switch (msg->
mode()) {
453 case Roomba500Interface::MODE_OFF:
458 case Roomba500Interface::MODE_PASSIVE:
463 case Roomba500Interface::MODE_SAFE:
468 case Roomba500Interface::MODE_FULL:
477 if (!was_controlled) {
489 if (was_controlled) {
528 roomba_->
set_motors((msg->
main() != Roomba500Interface::BRUSHSTATE_OFF),
529 (msg->
side() != Roomba500Interface::BRUSHSTATE_OFF),
531 (msg->
main() == Roomba500Interface::BRUSHSTATE_BACKWARD),
532 (msg->
side() == Roomba500Interface::BRUSHSTATE_BACKWARD));
542 if (greeting_loop_count_ < 50) {
543 if (++greeting_loop_count_ == 50) {
544 roomba_->
set_leds(
false,
false,
false,
false, 0, 0);
546 roomba_->
set_leds(
false,
false,
false,
true, 0, greeting_loop_count_ * 5);
563 (int)roundf(((
float)ntohs(sp.battery_charge) / (float)ntohs(sp.battery_capacity)) * 100.);
566 if (charge != battery_percent_) {
568 snprintf(digits, 4,
"%d%%", charge);
570 battery_percent_ = charge;
583 roomba500_if_->
set_wall(sp.virtual_wall == 1);
610 roomba500_if_->
set_distance((int16_t)ntohs(sp.distance));
613 roomba500_if_->
set_angle(-(int16_t)ntohs(sp.angle));
616 roomba500_if_->
set_current((
int)ntohs(sp.current));
625 / (
float)ntohs(sp.battery_capacity));
640 roomba500_if_->
set_velocity((int16_t)ntohs(sp.velocity));
641 roomba500_if_->
set_radius((int16_t)ntohs(sp.radius));
670 roomba500_if_->
write();
672 switch_if_but_clean_->
write();
673 switch_if_but_spot_->
write();
674 switch_if_but_dock_->
write();
675 switch_if_but_minute_->
write();
676 switch_if_but_hour_->
write();
677 switch_if_but_day_->
write();
678 switch_if_but_schedule_->
write();
679 switch_if_but_clock_->
write();
681 battery_if_->
write();
694 unsigned char color = 0;
695 unsigned char intensity = 255;
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 streamed sensor packets.
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.
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.
std::string get_device() const
Get device string.
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.