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);
269RoombaJoystickThread::stop()
276RoombaJoystickThread::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.
unsigned int msgq_enqueue(Message *message, bool proxy=false)
Enqueue message at end of queue.
void read()
Read from BlackBoard into local copy.
bool refreshed() const
Check if data has been refreshed.
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.