23 #include "joystick_thread.h" 25 #include <interfaces/MotorInterface.h> 26 #include <interfaces/JoystickInterface.h> 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" 46 :
Thread(
"RobotinoJoystickThread",
Thread::OPMODE_WAITFORWAKEUP),
84 RobotinoJoystickThread::send_transrot(
float vx,
float vy,
float omega)
93 RobotinoJoystickThread::stop()
95 send_transrot(0., 0., 0.);
101 joystick_if_->
read();
104 if (joystick_if_->
num_axes() == 0) {
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) {
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_;
116 send_transrot(vx, vy, omega);
float * axis() const
Get axis value.
TransRotMessage Fawkes BlackBoard Interface Message.
JoystickInterface Fawkes BlackBoard Interface.
Fawkes library namespace.
virtual void init()
Initialize the thread.
Thread class encapsulation of pthreads.
virtual void finalize()
Finalize the thread.
Logger * logger
This is the Logger member used to access the logger.
uint8_t num_axes() const
Get num_axes value.
Thread aspect to use blocked timing.
void read()
Read from BlackBoard into local copy.
const char * name() const
Get name of thread.
bool changed() const
Check if data has been changed.
virtual void loop()
Code to execute in the thread.
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
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.
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.
virtual void close(Interface *interface)=0
Close interface.