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 #include <interfaces/Roomba500Interface.h>
25 #include <interfaces/JoystickInterface.h>
26 
27 #include <cstdlib>
28 
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"
39 
40 using namespace fawkes;
41 
42 /** @class RoombaJoystickThread "roombajoy_thread.h"
43  * Roomba joystick control thread.
44  * Read joystick information from the blackboard and transform it into
45  * commands for the Roomba plugin.
46  * This is for demonstration purposes, but really this should be solved
47  * at the skill and agent levels (easy to spot because this thread is
48  * also hooked in at the skill hook).
49  *
50  * @author Tim Niemueller
51  */
52 
53 /** Constructor. */
55  : Thread("RoombaJoy", Thread::OPMODE_WAITFORWAKEUP),
56  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SKILL)
57 {
58 }
59 
60 
61 void
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, __joy_if->maxlenof_axis());
95  }
96  if (__cfg_axis_sideward > __joy_if->maxlenof_axis()) {
97  throw Exception("Invalid sideward axis value %u, must be smaller than %u",
98  __cfg_axis_sideward, __joy_if->maxlenof_axis());
99  }
100  if (__cfg_axis_speed > __joy_if->maxlenof_axis()) {
101  logger->log_warn(name(), "Speed axis disabled, setting half max speed.");
102  }
103 
104  __last_velo = __cfg_max_velocity / 2;
105  __main_brush_enabled = false;
106  __side_brush_enabled = false;
107  __vacuuming_enabled = false;
108 
109  __strong_rumble = false;
110  __weak_rumble = false;
111 }
112 
113 void
115 {
116  blackboard->close(__roomba500_if);
117  blackboard->close(__joy_if);
118 }
119 
120 
121 void
123 {
124  __joy_if->read();
125  __roomba500_if->read();
126 
127  if (__joy_if->supported_ff_effects() & JoystickInterface::JFF_RUMBLE) {
128  uint16_t mlb = __roomba500_if->light_bump_left();
129  mlb = std::max(mlb, __roomba500_if->light_bump_front_left());
130  mlb = std::max(mlb, __roomba500_if->light_bump_center_left());
131  mlb = std::max(mlb, __roomba500_if->light_bump_center_right());
132  mlb = std::max(mlb, __roomba500_if->light_bump_front_right());
133  mlb = std::max(mlb, __roomba500_if->light_bump_right());
134 
135  if (__roomba500_if->is_bump_left() || __roomba500_if->is_bump_right()) {
136  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) {
150 
151  float mf = (mlb / 1000.f);
152  if (mf > 1) mf = 1;
153  if (mf < 0.4) mf = 0.4;
154 
155  msg->set_weak_magnitude((uint16_t)floorf(mf * 0xFFFF));
156  if (mf > 0.8) msg->set_strong_magnitude(0x8000);
157 
158  __joy_if->msgq_enqueue(msg);
159 
160  __weak_rumble = false;
161  __strong_rumble = true;
162  } else if (__weak_rumble || __strong_rumble) {
165  __joy_if->msgq_enqueue(msg);
166 
167  __weak_rumble = __strong_rumble = false;
168  }
169  }
170 
171  if (__joy_if->changed()) {
172  if (__joy_if->num_axes() == 0) {
173  logger->log_debug(name(), "Joystick disconnected, stopping");
174  stop();
175  } else if (__joy_if->pressed_buttons()) {
176 
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) {
197  __vacuuming_enabled,
198  __main_brush_enabled ? Roomba500Interface::BRUSHSTATE_FORWARD
199  : Roomba500Interface::BRUSHSTATE_OFF,
200  __side_brush_enabled ? Roomba500Interface::BRUSHSTATE_FORWARD
201  : Roomba500Interface::BRUSHSTATE_OFF
202  );
203  __roomba500_if->msgq_enqueue(sm);
204  }
205 
206  if (__joy_if->pressed_buttons() & __cfg_but_dock) {
209  __roomba500_if->msgq_enqueue(dm);
210  }
211 
212  if (__joy_if->pressed_buttons() & __cfg_but_spot) {
213  /*
214  Roomba500Interface::DockMessage *dm =
215  new Roomba500Interface::DockMessage();
216  __roomba500_if->msgq_enqueue(dm);
217  */
218  }
219 
220  if (__joy_if->pressed_buttons() & __cfg_but_mode) {
223 
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;
231  default:
232  sm->set_mode(Roomba500Interface::MODE_PASSIVE); break;
233  }
234  __roomba500_if->msgq_enqueue(sm);
235  }
236 
237 
238  } else if (__joy_if->axis(__cfg_axis_forward) == 0 &&
239  __joy_if->axis(__cfg_axis_sideward) == 0) {
240  stop();
241  } else {
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)) *
246  __cfg_max_radius),
247  sideward);
248  float velocity = .5;
249  if (__cfg_axis_speed < __joy_if->maxlenof_axis()) {
250  velocity = __joy_if->axis(__cfg_axis_speed);
251  }
252 
253  int16_t velmm = (int16_t)roundf(forward * velocity);
254  int16_t radmm = (int16_t)roundf(radius);
255  // special case handling for "turn on place"
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);
259  }
260 
261  /*
262  logger->log_debug(name(), "Joystick (%f,%f,%f) Velo %f/%i Radius %f/%i",
263  __joy_if->axis(__cfg_axis_forward),
264  __joy_if->axis(__cfg_axis_sideward),
265  __joy_if->axis(__cfg_axis_speed),
266  velocity, velmm, radius, radmm);
267  */
268 
269  __last_velo = velmm;
270 
272  new Roomba500Interface::DriveMessage(velmm, radmm);
273  __roomba500_if->msgq_enqueue(dm);
274  }
275  }
276 }
277 
278 
279 void
280 RoombaJoystickThread::stop()
281 {
283  __roomba500_if->msgq_enqueue(sm);
284 }
285 
286 
287 unsigned int
288 RoombaJoystickThread::confval(const char *path, unsigned int default_value)
289 {
290  try {
291  return config->get_uint(path);
292  } catch (Exception &e) {
293  return default_value;
294  }
295 
296 }
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.
Fawkes library namespace.
Mode mode() const
Get mode value.
Thread class encapsulation of pthreads.
Definition: thread.h:42
uint16_t light_bump_center_left() const
Get light_bump_center_left 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.
Definition: logging.h:44
float * axis() const
Get axis value.
StopRumbleMessage Fawkes BlackBoard Interface Message.
DockMessage Fawkes BlackBoard Interface Message.
Thread aspect to use blocked timing.
bool is_bump_right() const
Get bump_right value.
uint8_t supported_ff_effects() const
Get supported_ff_effects value.
uint32_t pressed_buttons() const
Get pressed_buttons value.
Base class for exceptions in Fawkes.
Definition: exception.h:36
uint16_t light_bump_front_left() const
Get light_bump_front_left value.
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:477
SetMotorsMessage Fawkes BlackBoard Interface Message.
uint16_t light_bump_left() const
Get light_bump_left value.
void set_mode(const Mode new_mode)
Set mode value.
virtual void init()
Initialize the thread.
const char * name() const
Get name of thread.
Definition: thread.h:95
uint16_t light_bump_right() const
Get light_bump_right value.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
Roomba500Interface Fawkes BlackBoard Interface.
SetModeMessage Fawkes BlackBoard Interface Message.
bool changed() const
Check if data has been changed.
Definition: interface.cpp:796
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
Definition: interface.cpp:903
uint16_t light_bump_front_right() const
Get light_bump_front_right value.
StartRumbleMessage Fawkes BlackBoard Interface Message.
bool is_bump_left() const
Get bump_left value.
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, const char *owner=NULL)=0
Open interface for reading.
uint8_t num_axes() const
Get num_axes value.
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
size_t maxlenof_axis() const
Get maximum length of axis value.
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.
Definition: configurable.h:44
DriveMessage Fawkes BlackBoard Interface Message.
virtual void loop()
Code to execute in the thread.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual void close(Interface *interface)=0
Close interface.