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;