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"
40 using namespace fawkes;
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;
float * axis() const
Get axis 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.
size_t maxlenof_axis() const
Get maximum length of axis value.
uint16_t light_bump_center_left() const
Get light_bump_center_left value.
uint32_t pressed_buttons() const
Get pressed_buttons value.
Thread class encapsulation of pthreads.
uint8_t supported_ff_effects() const
Get supported_ff_effects 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.
uint8_t num_axes() const
Get num_axes value.
StopRumbleMessage Fawkes BlackBoard Interface Message.
DockMessage Fawkes BlackBoard Interface Message.
bool is_bump_left() const
Get bump_left value.
Thread aspect to use blocked timing.
Base class for exceptions in Fawkes.
void read()
Read from BlackBoard into local copy.
SetMotorsMessage Fawkes BlackBoard Interface Message.
uint16_t light_bump_front_left() const
Get light_bump_front_left value.
void set_mode(const Mode new_mode)
Set mode value.
virtual void init()
Initialize the thread.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
Roomba500Interface Fawkes BlackBoard Interface.
SetModeMessage Fawkes BlackBoard Interface Message.
const char * name() const
Get name of thread.
uint16_t light_bump_front_right() const
Get light_bump_front_right value.
bool changed() const
Check if data has been changed.
uint16_t light_bump_left() const
Get light_bump_left value.
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
StartRumbleMessage Fawkes BlackBoard Interface Message.
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)=0
Open interface for reading.
bool is_bump_right() const
Get bump_right value.
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
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.
Mode mode() const
Get mode value.
DriveMessage Fawkes BlackBoard Interface Message.
uint16_t light_bump_right() const
Get light_bump_right value.
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.