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);
566 if (charge != battery_percent_) {
568 snprintf(digits, 4,
"%d%%", charge);
570 battery_percent_ = charge;
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;
Worker thread for the Roomba 500 thread.
~WorkerThread()
Destructor.
bool has_fresh_data()
Check if fresh data is available.
WorkerThread(fawkes::Logger *logger, fawkes::Clock *clock, fawkes::RefPtr< Roomba500 > roomba, bool query_mode)
Constructor.
virtual void loop()
Code to execute in the thread.
virtual void loop()
Code to execute in the thread.
virtual void init()
Initialize the thread.
void write_blackboard()
Write data to blackboard.
virtual void finalize()
Finalize the thread.
Roomba500Thread()
Constructor.
Roomba 500 series communication class.
void seek_dock()
Seek for the home base and dock.
static const unsigned char BUTTON_CLEAN
Cleaning button.
static const unsigned char BUMPER_CENTER_LEFT
Center left bumper.
@ FLAG_FIREFLY_FASTMODE
Enable fast mode, assume FireFly RooTooth.
static const unsigned char OVERCURRENT_WHEEL_LEFT
Left wheel bit.
static const unsigned char BUMP_RIGHT
Right bumper bit.
static const unsigned char OVERCURRENT_MAIN_BRUSH
Main brush bit.
static const unsigned char OVERCURRENT_SIDE_BRUSH
Side brush bit.
static const unsigned char BUTTON_CLOCK
Clock button.
const SensorPacketGroupAll get_sensor_packet() const
Get latest sensor packet.
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_mode(Mode mode)
Set control mode.
static const unsigned char BUMPER_CENTER_RIGHT
Center right bumper.
Mode get_mode() const
Get current mode.
static const unsigned char BUTTON_MINUTE
Minute button.
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).
ConnectionType
Connection type.
@ CONNTYPE_ROOTOOTH
Use BlueZ to find and connect to RooTooth.
@ CONNTYPE_SERIAL
Use serial connection (device file).
bool is_controlled() const
Check if robot is being controlled.
void drive_straight(short int velo_mm_per_sec)
Drive Roomba straight.
Mode
Roomba 500 operation mode.
@ MODE_FULL
Control acquired, safety measures disabled.
@ MODE_PASSIVE
Passive mode, no control, only listening.
@ MODE_SAFE
Control acquired, safety measures in place.
static const unsigned char BUTTON_DOCK
Dock button.
static const unsigned char BUMPER_FRONT_RIGHT
Front right bumper.
static const unsigned char BUTTON_SCHEDULE
Schedule button.
static const unsigned short int STREAM_INTERVAL_MS
Time in ms between streamed sensor packets.
static const unsigned char WHEEL_DROP_LEFT
Left wheel drop bit.
std::string get_device() const
Get device string.
static const unsigned char BUMPER_FRONT_LEFT
Front left bumper.
void drive(short int velocity_mm_per_sec, short int radius_mm)
Drive Roomba.
static const unsigned char BUTTON_DAY
Day button.
void play_fanfare()
Play a simple fanfare.
void stop()
Stop moption of the Roomba.
void set_digit_leds(const char digits[4])
Set digit LEDs.
static const unsigned char BUTTON_HOUR
Hour button.
static const unsigned char BUMP_LEFT
Left bumper bit.
static const unsigned char BUTTON_SPOT
Spot cleaning button.
static const unsigned char BUMPER_RIGHT
Right bumper.
static const unsigned char CHARGER_INTERNAL
Internal charger bit.
static const unsigned char BUMPER_LEFT
Left bumper.
static const unsigned char WHEEL_DROP_RIGHT
Right wheel drop bit.
static const unsigned char OVERCURRENT_WHEEL_RIGHT
Right wheel bit.
static const unsigned char CHARGER_HOME_BASE
Home base charger bit.
BatteryInterface Fawkes BlackBoard Interface.
void set_voltage(const uint32_t new_voltage)
Set voltage value.
void set_temperature(const uint32_t new_temperature)
Set temperature value.
void set_current(const uint32_t new_current)
Set current value.
void set_relative_soc(const float new_relative_soc)
Set relative_soc value.
float absolute_soc() const
Get absolute_soc value.
void set_absolute_soc(const float new_absolute_soc)
Set absolute_soc value.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Clock * clock
By means of this member access to the clock is given.
This is supposed to be the central clock in Fawkes.
Configuration * config
This is the Configuration member used to access the configuration.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
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.
Base class for exceptions in Fawkes.
bool msgq_first_is()
Check if first message has desired type.
void msgq_pop()
Erase first message from queue.
Message * msgq_first()
Get the first message from the message queue.
void write()
Write from local copy into BlackBoard memory.
bool msgq_empty()
Check if queue is empty.
TurnOffMessage Fawkes BlackBoard Interface Message.
TurnOnMessage Fawkes BlackBoard Interface Message.
LedInterface Fawkes BlackBoard Interface.
float intensity() const
Get intensity value.
void set_intensity(const float new_intensity)
Set intensity value.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
virtual void log_warn(const char *component, const char *format,...)
Log warning message.
Mutex mutual exclusion lock.
void clear()
Set underlying instance to 0, decrementing reference count of existing instance appropriately.
DockMessage Fawkes BlackBoard Interface Message.
DriveMessage Fawkes BlackBoard Interface Message.
int16_t velocity() const
Get velocity value.
int16_t radius() const
Get radius value.
DriveStraightMessage Fawkes BlackBoard Interface Message.
int16_t velocity() const
Get velocity value.
SetModeMessage Fawkes BlackBoard Interface Message.
Mode mode() const
Get mode value.
SetMotorsMessage Fawkes BlackBoard Interface Message.
BrushState main() const
Get main value.
bool is_vacuuming() const
Get vacuuming value.
BrushState side() const
Get side value.
StopMessage Fawkes BlackBoard Interface Message.
Roomba500Interface Fawkes BlackBoard Interface.
void set_cliff_left_signal(const uint16_t new_cliff_left_signal)
Set cliff_left_signal value.
void set_cliff_right(const bool new_cliff_right)
Set cliff_right value.
void set_wall_signal(const uint16_t new_wall_signal)
Set wall_signal value.
void set_button_hour(const bool new_button_hour)
Set button_hour value.
void set_side_brush_current(const int16_t new_side_brush_current)
Set side_brush_current value.
void set_velocity_left(const int16_t new_velocity_left)
Set velocity_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.
void set_encoder_counts_left(const uint16_t new_encoder_counts_left)
Set encoder_counts_left value.
void set_wall(const bool new_wall)
Set wall value.
void set_button_dock(const bool new_button_dock)
Set button_dock value.
void set_light_bump_front_right(const uint16_t new_light_bump_front_right)
Set light_bump_front_right value.
void set_overcurrent_left_wheel(const bool new_overcurrent_left_wheel)
Set overcurrent_left_wheel value.
void set_cliff_front_right_signal(const uint16_t new_cliff_front_right_signal)
Set cliff_front_right_signal value.
void set_internal_charger_available(const bool new_internal_charger_available)
Set internal_charger_available value.
void set_button_spot(const bool new_button_spot)
Set button_spot value.
void set_button_clock(const bool new_button_clock)
Set button_clock value.
ChargingState
Current charging state.
void set_light_bump_front_left(const uint16_t new_light_bump_front_left)
Set light_bump_front_left value.
void set_bumper_center_right(const bool new_bumper_center_right)
Set bumper_center_right value.
void set_cliff_front_right(const bool new_cliff_front_right)
Set cliff_front_right value.
void set_cliff_front_left(const bool new_cliff_front_left)
Set cliff_front_left value.
void set_dirt_detect(const bool new_dirt_detect)
Set dirt_detect value.
void set_light_bump_right(const uint16_t new_light_bump_right)
Set light_bump_right value.
void set_bump_left(const bool new_bump_left)
Set bump_left value.
void set_angle(const int16_t new_angle)
Set angle value.
void set_button_clean(const bool new_button_clean)
Set button_clean value.
void set_encoder_counts_right(const uint16_t new_encoder_counts_right)
Set encoder_counts_right value.
void set_button_minute(const bool new_button_minute)
Set button_minute value.
void set_velocity(const int16_t new_velocity)
Set velocity value.
void set_main_brush_current(const int16_t new_main_brush_current)
Set main_brush_current value.
void set_radius(const int16_t new_radius)
Set radius value.
void set_button_day(const bool new_button_day)
Set button_day value.
void set_bumper_front_right(const bool new_bumper_front_right)
Set bumper_front_right value.
void set_left_motor_current(const int16_t new_left_motor_current)
Set left_motor_current value.
void set_overcurrent_right_wheel(const bool new_overcurrent_right_wheel)
Set overcurrent_right_wheel value.
void set_caster_stasis(const bool new_caster_stasis)
Set caster_stasis value.
void set_button_schedule(const bool new_button_schedule)
Set button_schedule value.
void set_ir_opcode_left(const InfraredCharacter new_ir_opcode_left)
Set ir_opcode_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_number(const uint8_t new_song_number)
Set song_number value.
void set_cliff_right_signal(const uint16_t new_cliff_right_signal)
Set cliff_right_signal value.
InfraredCharacter
Infrared character values.
void set_light_bump_left(const uint16_t new_light_bump_left)
Set light_bump_left value.
void set_cliff_front_left_signal(const uint16_t new_cliff_front_left_signal)
Set cliff_front_left_signal value.
void set_temperature(const int8_t new_temperature)
Set temperature value.
void set_home_base_charger_available(const bool new_home_base_charger_available)
Set home_base_charger_available value.
void set_bump_right(const bool new_bump_right)
Set bump_right value.
void set_bumper_right(const bool new_bumper_right)
Set bumper_right value.
void set_wheel_drop_right(const bool new_wheel_drop_right)
Set wheel_drop_right value.
void set_cliff_left(const bool new_cliff_left)
Set cliff_left value.
void set_voltage(const uint16_t new_voltage)
Set voltage value.
void set_overcurrent_side_brush(const bool new_overcurrent_side_brush)
Set overcurrent_side_brush value.
void set_right_motor_current(const int16_t new_right_motor_current)
Set right_motor_current value.
void set_overcurrent_main_brush(const bool new_overcurrent_main_brush)
Set overcurrent_main_brush value.
void set_ir_opcode_omni(const InfraredCharacter new_ir_opcode_omni)
Set ir_opcode_omni value.
void set_wheel_drop_left(const bool new_wheel_drop_left)
Set wheel_drop_left value.
void set_distance(const int16_t new_distance)
Set distance value.
void set_battery_capacity(const uint16_t new_battery_capacity)
Set battery_capacity value.
void set_bumper_left(const bool new_bumper_left)
Set bumper_left value.
void set_light_bump_center_right(const uint16_t new_light_bump_center_right)
Set light_bump_center_right value.
void set_bumper_front_left(const bool new_bumper_front_left)
Set bumper_front_left value.
void set_velocity_right(const int16_t new_velocity_right)
Set velocity_right value.
void set_mode(const Mode new_mode)
Set mode value.
void set_current(const int16_t new_current)
Set current value.
void set_bumper_center_left(const bool new_bumper_center_left)
Set bumper_center_left value.
void set_charging_state(const ChargingState new_charging_state)
Set charging_state value.
void set_battery_charge(const uint16_t new_battery_charge)
Set battery_charge value.
SwitchInterface Fawkes BlackBoard Interface.
void set_enabled(const bool new_enabled)
Set enabled value.
Thread class encapsulation of pthreads.
const char * name() const
Get name of thread.
void start(bool wait=true)
Call this method to start the thread.
void join()
Join the thread.
void cancel()
Cancel a thread.
Fawkes library namespace.
Struct for packet group with everything (SENSPACK_GROUP_ALL).
uint8_t cliff_front_right
Front right cliff sensor.
uint16_t encoder_counts_left
Encoder counts for left wheel.
uint8_t ir_opcode_right
Right IR receiver character.
uint8_t mode
Open Interface mode.
uint16_t cliff_left_signal
Raw left cliff signal.
uint8_t cliff_right
Right cliff sensor.
uint16_t light_bump_center_right
Raw center right light bumper signal.
uint16_t light_bump_front_right
Raw front right light bumper signal.
uint8_t virtual_wall
Wall sensor.
uint16_t wall_signal
Raw wall signal.
uint8_t bumps_wheeldrops
Bumps and wheeldrops bits.
int16_t current
Current in mA.
uint16_t cliff_front_right_signal
Raw front right cliff signal.
int16_t velocity
Velocity in mm/sec.
int16_t velocity_left
Velocity of left wheel in mm/sec.
uint16_t encoder_counts_right
Encoder counts for right wheel.
int16_t radius
Radius in mm.
uint8_t light_bumper
Light bumper bits.
uint8_t ir_opcode_omni
Omni IR receiver character.
uint8_t charging_state
Charging state.
int16_t angle
Turned angle in degree.
int16_t velocity_right
Velocity of right wheel in mm/sec.
uint8_t overcurrents
Overcurrents bits.
int8_t temperature
Temperature in deg C.
uint8_t song_number
Song number.
uint16_t voltage
Voltage in mV.
uint8_t cliff_left
Left cliff sensor.
uint8_t buttons
Buttons bits.
uint16_t battery_charge
Battery charge in mAh.
int16_t left_motor_current
Raw left motor current signal.
uint8_t cliff_front_left
Front left cliff sensor.
int16_t side_brush_current
Raw side brush motor current signal.
uint16_t light_bump_front_left
Raw front left light bumper signal.
uint16_t light_bump_center_left
Raw center left light bumper signal.
uint16_t light_bump_left
Raw left light bumper signal.
uint8_t charger_available
Available chargers bits.
uint16_t cliff_right_signal
Raw right cliff signal.
uint8_t song_playing
Song playing byte.
int16_t main_brush_current
Raw main brush motor current signal.
uint16_t battery_capacity
Battery capacity in mAh.
int16_t right_motor_current
Raw right motor current signal.
uint16_t cliff_front_left_signal
Raw front left cliff signal.
int16_t distance
Traveled distance in mm.
uint8_t dirt_detect
Dirt detect sensor.
uint8_t ir_opcode_left
Left IR receiver character.
uint8_t stasis
Castor stasis.
uint16_t light_bump_right
Raw right light bumper signal.