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 
41 using 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),
57  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SKILL)
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_,
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 
115 void
117 {
118  blackboard->close(roomba500_if_);
119  blackboard->close(joy_if_);
120 }
121 
122 void
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 
268 void
269 RoombaJoystickThread::stop()
270 {
272  roomba500_if_->msgq_enqueue(sm);
273 }
274 
275 unsigned int
276 RoombaJoystickThread::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.