Fawkes API  Fawkes Development Version
joystick_thread.cpp
00001 
00002 /***************************************************************************
00003  *  joystick_thread.cpp - Robotino joystick thread
00004  *
00005  *  Created: Sun Nov 13 16:07:40 2011
00006  *  Copyright  2011  Tim Niemueller [www.niemueller.de]
00007  *
00008  ****************************************************************************/
00009 
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version.
00014  *
00015  *  This program is distributed in the hope that it will be useful,
00016  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  *  GNU Library General Public License for more details.
00019  *
00020  *  Read the full text in the LICENSE.GPL file in the doc directory.
00021  */
00022 
00023 #include "joystick_thread.h"
00024 
00025 #include <interfaces/MotorInterface.h>
00026 #include <interfaces/JoystickInterface.h>
00027 
00028 #include <cmath>
00029 
00030 #define CFG_PREFIX "/hardware/robotino/joystick/"
00031 #define CFG_AXIS_FORWARD   CFG_PREFIX"axis_forward"
00032 #define CFG_AXIS_SIDEWARD  CFG_PREFIX"axis_sideward"
00033 #define CFG_AXIS_ROTATION  CFG_PREFIX"axis_rotation"
00034 
00035 using namespace fawkes;
00036 
00037 /** @class RobotinoJoystickThread "joystick_thread.h"
00038  * Robotino act hook integration thread.
00039  * This thread integrates into the Fawkes main loop at the ACT hook and
00040  * executes motion commands.
00041  * @author Tim Niemueller
00042  */
00043 
00044 /** Constructor. */
00045 RobotinoJoystickThread::RobotinoJoystickThread()
00046   : Thread("RobotinoJoystickThread", Thread::OPMODE_WAITFORWAKEUP),
00047     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SKILL)
00048 {
00049 }
00050 
00051 
00052 void
00053 RobotinoJoystickThread::init()
00054 {
00055   cfg_axis_forward_   = config->get_uint(CFG_AXIS_FORWARD);
00056   cfg_axis_sideward_  = config->get_uint(CFG_AXIS_SIDEWARD);
00057   cfg_axis_rotation_  = config->get_uint(CFG_AXIS_ROTATION);
00058 
00059   cfg_max_vx_    = config->get_float(CFG_PREFIX"max_vx");
00060   cfg_max_vy_    = config->get_float(CFG_PREFIX"max_vy");
00061   cfg_max_omega_ = config->get_float(CFG_PREFIX"max_omega");
00062 
00063   motor_if_ = blackboard->open_for_reading<MotorInterface>("Robotino");
00064   joystick_if_ =
00065     blackboard->open_for_reading<JoystickInterface>("Joystick");
00066 }
00067 
00068 
00069 bool
00070 RobotinoJoystickThread::prepare_finalize_user()
00071 {
00072   stop();
00073   return true;
00074 }
00075 
00076 void
00077 RobotinoJoystickThread::finalize()
00078 {
00079   blackboard->close(motor_if_);
00080   blackboard->close(joystick_if_);
00081 }
00082 
00083 void
00084 RobotinoJoystickThread::send_transrot(float vx, float vy, float omega)
00085 {
00086   MotorInterface::TransRotMessage *msg =
00087     new MotorInterface::TransRotMessage(vx, vy, omega);
00088   motor_if_->msgq_enqueue(msg);
00089 }
00090 
00091 
00092 void
00093 RobotinoJoystickThread::stop()
00094 {
00095   send_transrot(0., 0., 0.);
00096 }
00097 
00098 void
00099 RobotinoJoystickThread::loop()
00100 {
00101   joystick_if_->read();
00102 
00103   if (joystick_if_->changed()) {
00104     if (joystick_if_->num_axes() == 0) {
00105       logger->log_debug(name(), "Joystick disconnected, stopping");
00106       stop();
00107     } else if (fabsf(joystick_if_->axis(cfg_axis_forward_)) < 0.2 &&
00108                fabsf(joystick_if_->axis(cfg_axis_sideward_)) < 0.2 &&
00109                fabsf(joystick_if_->axis(cfg_axis_rotation_)) < 0.2) {
00110       stop();
00111     } else {
00112       float vx    = joystick_if_->axis(cfg_axis_forward_) * cfg_max_vx_;
00113       float vy    = joystick_if_->axis(cfg_axis_sideward_) * cfg_max_vy_;
00114       float omega = joystick_if_->axis(cfg_axis_rotation_) * cfg_max_omega_;
00115 
00116       send_transrot(vx, vy, omega);
00117     }
00118   }
00119 }