23 #include "roombajoy_thread.h" 24 #include <interfaces/Roomba500Interface.h> 25 #include <interfaces/JoystickInterface.h> 29 #define CFG_PREFIX "/hardware/roomba/joystick/" 30 #define CFG_BUT_MAIN_BRUSH CFG_PREFIX"but_main_brush" 31 #define CFG_BUT_SIDE_BRUSH CFG_PREFIX"but_side_brush" 32 #define CFG_BUT_VACUUMING CFG_PREFIX"but_vacuuming" 33 #define CFG_BUT_DOCK CFG_PREFIX"but_dock" 34 #define CFG_BUT_SPOT CFG_PREFIX"but_spot" 35 #define CFG_BUT_MODE CFG_PREFIX"but_mode" 36 #define CFG_AXIS_FORWARD CFG_PREFIX"axis_forward" 37 #define CFG_AXIS_SIDEWARD CFG_PREFIX"axis_sideward" 38 #define CFG_AXIS_SPEED CFG_PREFIX"axis_speed" 65 __roomba500_if = NULL;
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",
97 throw Exception(
"Invalid sideward axis value %u, must be smaller than %u",
104 __last_velo = __cfg_max_velocity / 2;
105 __main_brush_enabled =
false;
106 __side_brush_enabled =
false;
107 __vacuuming_enabled =
false;
109 __strong_rumble =
false;
110 __weak_rumble =
false;
125 __roomba500_if->
read();
136 if (! __weak_rumble) {
144 __weak_rumble =
true;
145 __strong_rumble =
false;
147 }
else if ((mlb > 200) && !__strong_rumble) {
151 float mf = (mlb / 1000.f);
153 if (mf < 0.4) mf = 0.4;
160 __weak_rumble =
false;
161 __strong_rumble =
true;
162 }
else if (__weak_rumble || __strong_rumble) {
167 __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 __main_brush_enabled ? Roomba500Interface::BRUSHSTATE_FORWARD
199 : Roomba500Interface::BRUSHSTATE_OFF,
200 __side_brush_enabled ? Roomba500Interface::BRUSHSTATE_FORWARD
201 : Roomba500Interface::BRUSHSTATE_OFF
224 switch (__roomba500_if->
mode()) {
225 case Roomba500Interface::MODE_PASSIVE:
226 sm->
set_mode(Roomba500Interface::MODE_SAFE);
break;
227 case Roomba500Interface::MODE_SAFE:
228 sm->
set_mode(Roomba500Interface::MODE_FULL);
break;
229 case Roomba500Interface::MODE_FULL:
230 sm->
set_mode(Roomba500Interface::MODE_PASSIVE);
break;
232 sm->
set_mode(Roomba500Interface::MODE_PASSIVE);
break;
238 }
else if (__joy_if->
axis(__cfg_axis_forward) == 0 &&
239 __joy_if->
axis(__cfg_axis_sideward) == 0) {
242 float forward = __joy_if->
axis(__cfg_axis_forward) * __cfg_max_velocity;
243 float sideward = __joy_if->
axis(__cfg_axis_sideward);
244 float radius = copysignf(std::max(__cfg_min_radius,
245 (
int)(1. - fabs(sideward)) *
249 if (__cfg_axis_speed < __joy_if->maxlenof_axis()) {
250 velocity = __joy_if->
axis(__cfg_axis_speed);
253 int16_t velmm = (int16_t)roundf(forward * velocity);
254 int16_t radmm = (int16_t)roundf(radius);
256 if (fabsf(__joy_if->
axis(__cfg_axis_forward)) < 0.1) {
257 velmm = (int16_t)fabs(sideward * velocity) * __cfg_max_velocity;
258 radmm = (int16_t)copysignf(1, sideward);
280 RoombaJoystickThread::stop()
288 RoombaJoystickThread::confval(
const char *path,
unsigned int default_value)
293 return default_value;
uint16_t light_bump_center_right() const
Get light_bump_center_right value.
virtual void finalize()
Finalize the thread.
StopMessage Fawkes BlackBoard Interface Message.
JoystickInterface Fawkes BlackBoard Interface.
Fawkes library namespace.
Mode mode() const
Get mode value.
Thread class encapsulation of pthreads.
uint16_t light_bump_center_left() const
Get light_bump_center_left value.
void set_weak_magnitude(const uint16_t new_weak_magnitude)
Set weak_magnitude value.
Logger * logger
This is the Logger member used to access the logger.
float * axis() const
Get axis value.
StopRumbleMessage Fawkes BlackBoard Interface Message.
DockMessage Fawkes BlackBoard Interface Message.
Thread aspect to use blocked timing.
bool is_bump_right() const
Get bump_right value.
uint8_t supported_ff_effects() const
Get supported_ff_effects value.
uint32_t pressed_buttons() const
Get pressed_buttons value.
Base class for exceptions in Fawkes.
uint16_t light_bump_front_left() const
Get light_bump_front_left value.
void read()
Read from BlackBoard into local copy.
SetMotorsMessage Fawkes BlackBoard Interface Message.
uint16_t light_bump_left() const
Get light_bump_left value.
void set_mode(const Mode new_mode)
Set mode value.
virtual void init()
Initialize the thread.
const char * name() const
Get name of thread.
uint16_t light_bump_right() const
Get light_bump_right value.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
Roomba500Interface Fawkes BlackBoard Interface.
SetModeMessage Fawkes BlackBoard Interface Message.
bool changed() const
Check if data has been changed.
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
uint16_t light_bump_front_right() const
Get light_bump_front_right value.
StartRumbleMessage Fawkes BlackBoard Interface Message.
bool is_bump_left() const
Get bump_left value.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
RoombaJoystickThread()
Constructor.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
uint8_t num_axes() const
Get num_axes value.
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
size_t maxlenof_axis() const
Get maximum length of axis value.
void set_strong_magnitude(const uint16_t new_strong_magnitude)
Set strong_magnitude value.
Configuration * config
This is the Configuration member used to access the configuration.
DriveMessage Fawkes BlackBoard Interface Message.
virtual void loop()
Code to execute in the thread.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual void close(Interface *interface)=0
Close interface.