Fawkes API
Fawkes Development Version
|
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 }