Fawkes API Fawkes Development Version
roombajoy_thread.cpp
1
2/***************************************************************************
3 * roombajoy_thread.cpp - Roomba joystick control thread
4 *
5 * Created: Sat Jan 29 14:36:18 2011
6 * Copyright 2006-2011 Tim Niemueller [www.niemueller.de]
7 *
8 ****************************************************************************/
9
10/* This program is free software; you can redistribute it and/or modify
11 * it under the terms of the GNU General Public License as published by
12 * the Free Software Foundation; either version 2 of the License, or
13 * (at your option) any later version.
14 *
15 * This program is distributed in the hope that it will be useful,
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 * GNU Library General Public License for more details.
19 *
20 * Read the full text in the LICENSE.GPL file in the doc directory.
21 */
22
23#include "roombajoy_thread.h"
24
25#include <interfaces/JoystickInterface.h>
26#include <interfaces/Roomba500Interface.h>
27
28#include <cstdlib>
29
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"
40
41using namespace fawkes;
42
43/** @class RoombaJoystickThread "roombajoy_thread.h"
44 * Roomba joystick control thread.
45 * Read joystick information from the blackboard and transform it into
46 * commands for the Roomba plugin.
47 * This is for demonstration purposes, but really this should be solved
48 * at the skill and agent levels (easy to spot because this thread is
49 * also hooked in at the skill hook).
50 *
51 * @author Tim Niemueller
52 */
53
54/** Constructor. */
56: Thread("RoombaJoy", Thread::OPMODE_WAITFORWAKEUP),
58{
59}
60
61void
63{
64 joy_if_ = NULL;
65 roomba500_if_ = NULL;
66
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);
73
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);
77
78 cfg_min_radius_ = config->get_uint(CFG_PREFIX "min_radius");
79 cfg_max_radius_ = config->get_uint(CFG_PREFIX "max_radius");
80 cfg_max_velocity_ = config->get_uint(CFG_PREFIX "max_velocity");
81
82 try {
83 roomba500_if_ = blackboard->open_for_reading<Roomba500Interface>("Roomba 500");
84 joy_if_ = blackboard->open_for_reading<JoystickInterface>("Joystick");
85
86 } catch (Exception &e) {
87 blackboard->close(roomba500_if_);
88 blackboard->close(joy_if_);
89 throw;
90 }
91
92 if (cfg_axis_forward_ > joy_if_->maxlenof_axis()) {
93 throw Exception("Invalid forward axis value %u, must be smaller than %u",
94 cfg_axis_forward_,
95 joy_if_->maxlenof_axis());
96 }
97 if (cfg_axis_sideward_ > joy_if_->maxlenof_axis()) {
98 throw Exception("Invalid sideward axis value %u, must be smaller than %u",
99 cfg_axis_sideward_,
100 joy_if_->maxlenof_axis());
101 }
102 if (cfg_axis_speed_ > joy_if_->maxlenof_axis()) {
103 logger->log_warn(name(), "Speed axis disabled, setting half max speed.");
104 }
105
106 last_velo_ = cfg_max_velocity_ / 2;
107 main_brush_enabled_ = false;
108 side_brush_enabled_ = false;
109 vacuuming_enabled_ = false;
110
111 strong_rumble_ = false;
112 weak_rumble_ = false;
113}
114
115void
117{
118 blackboard->close(roomba500_if_);
119 blackboard->close(joy_if_);
120}
121
122void
124{
125 joy_if_->read();
126 roomba500_if_->read();
127
128 if (joy_if_->supported_ff_effects() & JoystickInterface::JFF_RUMBLE) {
129 uint16_t mlb = roomba500_if_->light_bump_left();
130 mlb = std::max(mlb, roomba500_if_->light_bump_front_left());
131 mlb = std::max(mlb, roomba500_if_->light_bump_center_left());
132 mlb = std::max(mlb, roomba500_if_->light_bump_center_right());
133 mlb = std::max(mlb, roomba500_if_->light_bump_front_right());
134 mlb = std::max(mlb, roomba500_if_->light_bump_right());
135
136 if (roomba500_if_->is_bump_left() || roomba500_if_->is_bump_right()) {
137 if (!weak_rumble_) {
139
140 msg->set_strong_magnitude(0xFFFF);
141 msg->set_weak_magnitude(0x8000);
142
143 joy_if_->msgq_enqueue(msg);
144 weak_rumble_ = true;
145 strong_rumble_ = false;
146 }
147 } else if ((mlb > 200) && !strong_rumble_) {
149
150 float mf = (mlb / 1000.f);
151 if (mf > 1)
152 mf = 1;
153 if (mf < 0.4)
154 mf = 0.4;
155
156 msg->set_weak_magnitude((uint16_t)floorf(mf * 0xFFFF));
157 if (mf > 0.8)
158 msg->set_strong_magnitude(0x8000);
159
160 joy_if_->msgq_enqueue(msg);
161
162 weak_rumble_ = false;
163 strong_rumble_ = true;
164 } else if (weak_rumble_ || strong_rumble_) {
166 joy_if_->msgq_enqueue(msg);
167
168 weak_rumble_ = strong_rumble_ = false;
169 }
170 }
171
172 if (joy_if_->refreshed()) {
173 if (joy_if_->num_axes() == 0) {
174 logger->log_debug(name(), "Joystick disconnected, stopping");
175 stop();
176 } else if (joy_if_->pressed_buttons()) {
177 bool motor_state = false;
178
179 if (joy_if_->pressed_buttons() & cfg_but_main_brush_) {
180 motor_state = true;
181 main_brush_enabled_ = !main_brush_enabled_;
182 }
183
184 if (joy_if_->pressed_buttons() & cfg_but_side_brush_) {
185 motor_state = true;
186 side_brush_enabled_ = !side_brush_enabled_;
187 }
188
189 if (joy_if_->pressed_buttons() & cfg_but_vacuuming_) {
190 motor_state = true;
191 vacuuming_enabled_ = !vacuuming_enabled_;
192 }
193
194 if (motor_state) {
196 new Roomba500Interface::SetMotorsMessage(vacuuming_enabled_,
197 main_brush_enabled_
198 ? Roomba500Interface::BRUSHSTATE_FORWARD
199 : Roomba500Interface::BRUSHSTATE_OFF,
200 side_brush_enabled_
201 ? Roomba500Interface::BRUSHSTATE_FORWARD
202 : Roomba500Interface::BRUSHSTATE_OFF);
203 roomba500_if_->msgq_enqueue(sm);
204 }
205
206 if (joy_if_->pressed_buttons() & cfg_but_dock_) {
208 roomba500_if_->msgq_enqueue(dm);
209 }
210
211 if (joy_if_->pressed_buttons() & cfg_but_spot_) {
212 /*
213 Roomba500Interface::DockMessage *dm =
214 new Roomba500Interface::DockMessage();
215 roomba500_if_->msgq_enqueue(dm);
216 */
217 }
218
219 if (joy_if_->pressed_buttons() & cfg_but_mode_) {
221
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;
227 }
228 roomba500_if_->msgq_enqueue(sm);
229 }
230
231 } else if (joy_if_->axis(cfg_axis_forward_) == 0 && joy_if_->axis(cfg_axis_sideward_) == 0) {
232 stop();
233 } else {
234 float forward = joy_if_->axis(cfg_axis_forward_) * cfg_max_velocity_;
235 float sideward = joy_if_->axis(cfg_axis_sideward_);
236 float radius =
237 copysignf(std::max(cfg_min_radius_, (int)(1. - fabsf(sideward)) * cfg_max_radius_),
238 sideward);
239 float velocity = .5;
240 if (cfg_axis_speed_ < joy_if_->maxlenof_axis()) {
241 velocity = joy_if_->axis(cfg_axis_speed_);
242 }
243
244 int16_t velmm = (int16_t)roundf(forward * velocity);
245 int16_t radmm = (int16_t)roundf(radius);
246 // special case handling for "turn on place"
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);
250 }
251
252 /*
253 logger->log_debug(name(), "Joystick (%f,%f,%f) Velo %f/%i Radius %f/%i",
254 joy_if_->axis(cfg_axis_forward_),
255 joy_if_->axis(cfg_axis_sideward_),
256 joy_if_->axis(cfg_axis_speed_),
257 velocity, velmm, radius, radmm);
258 */
259
260 last_velo_ = velmm;
261
263 roomba500_if_->msgq_enqueue(dm);
264 }
265 }
266}
267
268void
269RoombaJoystickThread::stop()
270{
272 roomba500_if_->msgq_enqueue(sm);
273}
274
275unsigned int
276RoombaJoystickThread::confval(const char *path, unsigned int default_value)
277{
278 try {
279 return config->get_uint(path);
280 } catch (Exception &e) {
281 return default_value;
282 }
283}
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.
Definition: blackboard.h:44
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.
Definition: configurable.h:41
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.
Definition: exception.h:36
unsigned int msgq_enqueue(Message *message, bool proxy=false)
Enqueue message at end of queue.
Definition: interface.cpp:915
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:479
bool refreshed() const
Check if data has been refreshed.
Definition: interface.cpp:811
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.
Definition: logging.h:41
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.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
Fawkes library namespace.