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;