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_->changed()) {
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 }
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:45
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:41
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:35
uint16_t light_bump_front_left() const
Get light_bump_front_left value.
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:472
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:100
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:777
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
Definition: interface.cpp:879
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:41
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.