Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
joystick_thread.cpp
1 
2 /***************************************************************************
3  * joystick_thread.cpp - Robotino joystick thread
4  *
5  * Created: Sun Nov 13 16:07:40 2011
6  * Copyright 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 "joystick_thread.h"
24 
25 #include <interfaces/MotorInterface.h>
26 #include <interfaces/JoystickInterface.h>
27 
28 #include <cmath>
29 
30 #define CFG_PREFIX "/hardware/robotino/joystick/"
31 #define CFG_AXIS_FORWARD CFG_PREFIX"axis_forward"
32 #define CFG_AXIS_SIDEWARD CFG_PREFIX"axis_sideward"
33 #define CFG_AXIS_ROTATION CFG_PREFIX"axis_rotation"
34 
35 using namespace fawkes;
36 
37 /** @class RobotinoJoystickThread "joystick_thread.h"
38  * Robotino act hook integration thread.
39  * This thread integrates into the Fawkes main loop at the ACT hook and
40  * executes motion commands.
41  * @author Tim Niemueller
42  */
43 
44 /** Constructor. */
46  : Thread("RobotinoJoystickThread", Thread::OPMODE_WAITFORWAKEUP),
47  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SKILL)
48 {
49 }
50 
51 
52 void
54 {
55  cfg_axis_forward_ = config->get_uint(CFG_AXIS_FORWARD);
56  cfg_axis_sideward_ = config->get_uint(CFG_AXIS_SIDEWARD);
57  cfg_axis_rotation_ = config->get_uint(CFG_AXIS_ROTATION);
58 
59  cfg_max_vx_ = config->get_float(CFG_PREFIX"max_vx");
60  cfg_max_vy_ = config->get_float(CFG_PREFIX"max_vy");
61  cfg_max_omega_ = config->get_float(CFG_PREFIX"max_omega");
62 
63  motor_if_ = blackboard->open_for_reading<MotorInterface>("Robotino");
64  joystick_if_ =
66 }
67 
68 
69 bool
71 {
72  stop();
73  return true;
74 }
75 
76 void
78 {
79  blackboard->close(motor_if_);
80  blackboard->close(joystick_if_);
81 }
82 
83 void
84 RobotinoJoystickThread::send_transrot(float vx, float vy, float omega)
85 {
87  new MotorInterface::TransRotMessage(vx, vy, omega);
88  motor_if_->msgq_enqueue(msg);
89 }
90 
91 
92 void
93 RobotinoJoystickThread::stop()
94 {
95  send_transrot(0., 0., 0.);
96 }
97 
98 void
100 {
101  joystick_if_->read();
102 
103  if (joystick_if_->changed()) {
104  if (joystick_if_->num_axes() == 0) {
105  logger->log_debug(name(), "Joystick disconnected, stopping");
106  stop();
107  } else if (fabsf(joystick_if_->axis(cfg_axis_forward_)) < 0.2 &&
108  fabsf(joystick_if_->axis(cfg_axis_sideward_)) < 0.2 &&
109  fabsf(joystick_if_->axis(cfg_axis_rotation_)) < 0.2) {
110  stop();
111  } else {
112  float vx = joystick_if_->axis(cfg_axis_forward_) * cfg_max_vx_;
113  float vy = joystick_if_->axis(cfg_axis_sideward_) * cfg_max_vy_;
114  float omega = joystick_if_->axis(cfg_axis_rotation_) * cfg_max_omega_;
115 
116  send_transrot(vx, vy, omega);
117  }
118  }
119 }
float * axis() const
Get axis value.
TransRotMessage Fawkes BlackBoard Interface Message.
JoystickInterface Fawkes BlackBoard Interface.
virtual void init()
Initialize the thread.
Thread class encapsulation of pthreads.
Definition: thread.h:42
virtual void finalize()
Finalize the thread.
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.
Thread aspect to use blocked timing.
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:472
const char * name() const
Get name of thread.
Definition: thread.h:95
bool changed() const
Check if data has been changed.
Definition: interface.cpp:748
virtual void loop()
Code to execute in the thread.
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
Definition: interface.cpp:830
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual bool prepare_finalize_user()
Prepare finalization user implementation.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier)=0
Open interface for reading.
MotorInterface Fawkes BlackBoard Interface.
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:44
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
RobotinoJoystickThread()
Constructor.
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.