Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
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 }
float * axis() const
Get axis value.
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.
size_t maxlenof_axis() const
Get maximum length of axis value.
uint16_t light_bump_center_left() const
Get light_bump_center_left value.
uint32_t pressed_buttons() const
Get pressed_buttons value.
Thread class encapsulation of pthreads.
Definition: thread.h:42
uint8_t supported_ff_effects() const
Get supported_ff_effects 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
uint8_t num_axes() const
Get num_axes value.
StopRumbleMessage Fawkes BlackBoard Interface Message.
DockMessage Fawkes BlackBoard Interface Message.
bool is_bump_left() const
Get bump_left value.
Thread aspect to use blocked timing.
Base class for exceptions in Fawkes.
Definition: exception.h:36
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:472
SetMotorsMessage Fawkes BlackBoard Interface Message.
uint16_t light_bump_front_left() const
Get light_bump_front_left value.
void set_mode(const Mode new_mode)
Set mode value.
virtual void init()
Initialize the thread.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
Roomba500Interface Fawkes BlackBoard Interface.
SetModeMessage Fawkes BlackBoard Interface Message.
const char * name() const
Get name of thread.
Definition: thread.h:95
uint16_t light_bump_front_right() const
Get light_bump_front_right value.
bool changed() const
Check if data has been changed.
Definition: interface.cpp:748
uint16_t light_bump_left() const
Get light_bump_left value.
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
Definition: interface.cpp:830
StartRumbleMessage Fawkes BlackBoard Interface Message.
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)=0
Open interface for reading.
bool is_bump_right() const
Get bump_right value.
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
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
Mode mode() const
Get mode value.
DriveMessage Fawkes BlackBoard Interface Message.
uint16_t light_bump_right() const
Get light_bump_right value.
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:43
virtual void close(Interface *interface)=0
Close interface.