23 #include "roombajoy_thread.h"
25 #include <interfaces/JoystickInterface.h>
26 #include <interfaces/Roomba500Interface.h>
30 #define CFG_PREFIX "/hardware/roomba/joystick/"
31 #define CFG_BUT_MAIN_BRUSH CFG_PREFIX "but_main_brush"
32 #define CFG_BUT_SIDE_BRUSH CFG_PREFIX "but_side_brush"
33 #define CFG_BUT_VACUUMING CFG_PREFIX "but_vacuuming"
34 #define CFG_BUT_DOCK CFG_PREFIX "but_dock"
35 #define CFG_BUT_SPOT CFG_PREFIX "but_spot"
36 #define CFG_BUT_MODE CFG_PREFIX "but_mode"
37 #define CFG_AXIS_FORWARD CFG_PREFIX "axis_forward"
38 #define CFG_AXIS_SIDEWARD CFG_PREFIX "axis_sideward"
39 #define CFG_AXIS_SPEED CFG_PREFIX "axis_speed"
67 cfg_but_main_brush_ = confval(CFG_BUT_MAIN_BRUSH, JoystickInterface::BUTTON_1);
68 cfg_but_side_brush_ = confval(CFG_BUT_SIDE_BRUSH, JoystickInterface::BUTTON_2);
69 cfg_but_vacuuming_ = confval(CFG_BUT_VACUUMING, JoystickInterface::BUTTON_3);
70 cfg_but_dock_ = confval(CFG_BUT_DOCK, JoystickInterface::BUTTON_4);
71 cfg_but_spot_ = confval(CFG_BUT_SPOT, JoystickInterface::BUTTON_5);
72 cfg_but_mode_ = confval(CFG_BUT_MODE, JoystickInterface::BUTTON_6);
74 cfg_axis_forward_ = confval(CFG_AXIS_FORWARD, 0);
75 cfg_axis_sideward_ = confval(CFG_AXIS_SIDEWARD, 1);
76 cfg_axis_speed_ = confval(CFG_AXIS_SPEED, 2);
93 throw Exception(
"Invalid forward axis value %u, must be smaller than %u",
98 throw Exception(
"Invalid sideward axis value %u, must be smaller than %u",
106 last_velo_ = cfg_max_velocity_ / 2;
107 main_brush_enabled_ =
false;
108 side_brush_enabled_ =
false;
109 vacuuming_enabled_ =
false;
111 strong_rumble_ =
false;
112 weak_rumble_ =
false;
126 roomba500_if_->
read();
145 strong_rumble_ =
false;
147 }
else if ((mlb > 200) && !strong_rumble_) {
150 float mf = (mlb / 1000.f);
162 weak_rumble_ =
false;
163 strong_rumble_ =
true;
164 }
else if (weak_rumble_ || strong_rumble_) {
168 weak_rumble_ = strong_rumble_ =
false;
177 bool motor_state =
false;
181 main_brush_enabled_ = !main_brush_enabled_;
186 side_brush_enabled_ = !side_brush_enabled_;
191 vacuuming_enabled_ = !vacuuming_enabled_;
198 ? Roomba500Interface::BRUSHSTATE_FORWARD
199 : Roomba500Interface::BRUSHSTATE_OFF,
201 ? Roomba500Interface::BRUSHSTATE_FORWARD
202 : Roomba500Interface::BRUSHSTATE_OFF);
222 switch (roomba500_if_->
mode()) {
223 case Roomba500Interface::MODE_PASSIVE: sm->
set_mode(Roomba500Interface::MODE_SAFE);
break;
224 case Roomba500Interface::MODE_SAFE: sm->
set_mode(Roomba500Interface::MODE_FULL);
break;
225 case Roomba500Interface::MODE_FULL: sm->
set_mode(Roomba500Interface::MODE_PASSIVE);
break;
226 default: sm->
set_mode(Roomba500Interface::MODE_PASSIVE);
break;
231 }
else if (joy_if_->
axis(cfg_axis_forward_) == 0 && joy_if_->
axis(cfg_axis_sideward_) == 0) {
234 float forward = joy_if_->
axis(cfg_axis_forward_) * cfg_max_velocity_;
235 float sideward = joy_if_->
axis(cfg_axis_sideward_);
237 copysignf(std::max(cfg_min_radius_, (
int)(1. - fabsf(sideward)) * cfg_max_radius_),
240 if (cfg_axis_speed_ < joy_if_->maxlenof_axis()) {
241 velocity = joy_if_->
axis(cfg_axis_speed_);
244 int16_t velmm = (int16_t)roundf(forward * velocity);
245 int16_t radmm = (int16_t)roundf(radius);
247 if (fabsf(joy_if_->
axis(cfg_axis_forward_)) < 0.1) {
248 velmm = (int16_t)((
double)fabsf(sideward * velocity) * cfg_max_velocity_);
249 radmm = (int16_t)copysignf(1, sideward);
269 RoombaJoystickThread::stop()
276 RoombaJoystickThread::confval(
const char *path,
unsigned int default_value)
281 return default_value;
virtual void init()
Initialize the thread.
virtual void loop()
Code to execute in the thread.
RoombaJoystickThread()
Constructor.
virtual void finalize()
Finalize the thread.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Configuration * config
This is the Configuration member used to access the configuration.
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
Base class for exceptions in Fawkes.
bool changed() const
Check if data has been changed.
void read()
Read from BlackBoard into local copy.
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
StartRumbleMessage Fawkes BlackBoard Interface Message.
void set_strong_magnitude(const uint16_t new_strong_magnitude)
Set strong_magnitude value.
void set_weak_magnitude(const uint16_t new_weak_magnitude)
Set weak_magnitude value.
StopRumbleMessage Fawkes BlackBoard Interface Message.
JoystickInterface Fawkes BlackBoard Interface.
size_t maxlenof_axis() const
Get maximum length of axis value.
uint32_t pressed_buttons() const
Get pressed_buttons value.
uint8_t supported_ff_effects() const
Get supported_ff_effects value.
uint8_t num_axes() const
Get num_axes value.
float * axis() const
Get axis 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.
Logger * logger
This is the Logger member used to access the logger.
DockMessage Fawkes BlackBoard Interface Message.
DriveMessage Fawkes BlackBoard Interface Message.
SetModeMessage Fawkes BlackBoard Interface Message.
void set_mode(const Mode new_mode)
Set mode value.
SetMotorsMessage Fawkes BlackBoard Interface Message.
StopMessage Fawkes BlackBoard Interface Message.
Roomba500Interface Fawkes BlackBoard Interface.
uint16_t light_bump_front_left() const
Get light_bump_front_left value.
uint16_t light_bump_center_left() const
Get light_bump_center_left value.
bool is_bump_left() const
Get bump_left value.
Mode mode() const
Get mode value.
bool is_bump_right() const
Get bump_right value.
uint16_t light_bump_front_right() const
Get light_bump_front_right value.
uint16_t light_bump_center_right() const
Get light_bump_center_right value.
uint16_t light_bump_left() const
Get light_bump_left value.
uint16_t light_bump_right() const
Get light_bump_right value.
Thread class encapsulation of pthreads.
const char * name() const
Get name of thread.
Fawkes library namespace.