Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * roombajoy_thread.cpp - Roomba joystick control thread 00004 * 00005 * Created: Sat Jan 29 14:36:18 2011 00006 * Copyright 2006-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 "roombajoy_thread.h" 00024 #include <interfaces/Roomba500Interface.h> 00025 #include <interfaces/JoystickInterface.h> 00026 00027 #include <cstdlib> 00028 00029 #define CFG_PREFIX "/hardware/roomba/joystick/" 00030 #define CFG_BUT_MAIN_BRUSH CFG_PREFIX"but_main_brush" 00031 #define CFG_BUT_SIDE_BRUSH CFG_PREFIX"but_side_brush" 00032 #define CFG_BUT_VACUUMING CFG_PREFIX"but_vacuuming" 00033 #define CFG_BUT_DOCK CFG_PREFIX"but_dock" 00034 #define CFG_BUT_SPOT CFG_PREFIX"but_spot" 00035 #define CFG_BUT_MODE CFG_PREFIX"but_mode" 00036 #define CFG_AXIS_FORWARD CFG_PREFIX"axis_forward" 00037 #define CFG_AXIS_SIDEWARD CFG_PREFIX"axis_sideward" 00038 #define CFG_AXIS_SPEED CFG_PREFIX"axis_speed" 00039 00040 using namespace fawkes; 00041 00042 /** @class RoombaJoystickThread "roombajoy_thread.h" 00043 * Roomba joystick control thread. 00044 * Read joystick information from the blackboard and transform it into 00045 * commands for the Roomba plugin. 00046 * This is for demonstration purposes, but really this should be solved 00047 * at the skill and agent levels (easy to spot because this thread is 00048 * also hooked in at the skill hook). 00049 * 00050 * @author Tim Niemueller 00051 */ 00052 00053 /** Constructor. */ 00054 RoombaJoystickThread::RoombaJoystickThread() 00055 : Thread("RoombaJoy", Thread::OPMODE_WAITFORWAKEUP), 00056 BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SKILL) 00057 { 00058 } 00059 00060 00061 void 00062 RoombaJoystickThread::init() 00063 { 00064 __joy_if = NULL; 00065 __roomba500_if = NULL; 00066 00067 __cfg_but_main_brush = confval(CFG_BUT_MAIN_BRUSH, JoystickInterface::BUTTON_1); 00068 __cfg_but_side_brush = confval(CFG_BUT_SIDE_BRUSH, JoystickInterface::BUTTON_2); 00069 __cfg_but_vacuuming = confval(CFG_BUT_VACUUMING, JoystickInterface::BUTTON_3); 00070 __cfg_but_dock = confval(CFG_BUT_DOCK, JoystickInterface::BUTTON_4); 00071 __cfg_but_spot = confval(CFG_BUT_SPOT, JoystickInterface::BUTTON_5); 00072 __cfg_but_mode = confval(CFG_BUT_MODE, JoystickInterface::BUTTON_6); 00073 00074 __cfg_axis_forward = confval(CFG_AXIS_FORWARD, 0); 00075 __cfg_axis_sideward = confval(CFG_AXIS_SIDEWARD, 1); 00076 __cfg_axis_speed = confval(CFG_AXIS_SPEED, 2); 00077 00078 __cfg_min_radius = config->get_uint(CFG_PREFIX"min_radius"); 00079 __cfg_max_radius = config->get_uint(CFG_PREFIX"max_radius"); 00080 __cfg_max_velocity = config->get_uint(CFG_PREFIX"max_velocity"); 00081 00082 try { 00083 __roomba500_if = blackboard->open_for_reading<Roomba500Interface>("Roomba 500"); 00084 __joy_if = blackboard->open_for_reading<JoystickInterface>("Joystick"); 00085 00086 } catch (Exception &e) { 00087 blackboard->close(__roomba500_if); 00088 blackboard->close(__joy_if); 00089 throw; 00090 } 00091 00092 if (__cfg_axis_forward > __joy_if->maxlenof_axis()) { 00093 throw Exception("Invalid forward axis value %u, must be smaller than %u", 00094 __cfg_axis_forward, __joy_if->maxlenof_axis()); 00095 } 00096 if (__cfg_axis_sideward > __joy_if->maxlenof_axis()) { 00097 throw Exception("Invalid sideward axis value %u, must be smaller than %u", 00098 __cfg_axis_sideward, __joy_if->maxlenof_axis()); 00099 } 00100 if (__cfg_axis_speed > __joy_if->maxlenof_axis()) { 00101 logger->log_warn(name(), "Speed axis disabled, setting half max speed."); 00102 } 00103 00104 __last_velo = __cfg_max_velocity / 2; 00105 __main_brush_enabled = false; 00106 __side_brush_enabled = false; 00107 __vacuuming_enabled = false; 00108 00109 __strong_rumble = false; 00110 __weak_rumble = false; 00111 } 00112 00113 void 00114 RoombaJoystickThread::finalize() 00115 { 00116 blackboard->close(__roomba500_if); 00117 blackboard->close(__joy_if); 00118 } 00119 00120 00121 void 00122 RoombaJoystickThread::loop() 00123 { 00124 __joy_if->read(); 00125 __roomba500_if->read(); 00126 00127 if (__joy_if->supported_ff_effects() & JoystickInterface::JFF_RUMBLE) { 00128 uint16_t mlb = __roomba500_if->light_bump_left(); 00129 mlb = std::max(mlb, __roomba500_if->light_bump_front_left()); 00130 mlb = std::max(mlb, __roomba500_if->light_bump_center_left()); 00131 mlb = std::max(mlb, __roomba500_if->light_bump_center_right()); 00132 mlb = std::max(mlb, __roomba500_if->light_bump_front_right()); 00133 mlb = std::max(mlb, __roomba500_if->light_bump_right()); 00134 00135 if (__roomba500_if->is_bump_left() || __roomba500_if->is_bump_right()) { 00136 if (! __weak_rumble) { 00137 JoystickInterface::StartRumbleMessage *msg = 00138 new JoystickInterface::StartRumbleMessage(); 00139 00140 msg->set_strong_magnitude(0xFFFF); 00141 msg->set_weak_magnitude(0x8000); 00142 00143 __joy_if->msgq_enqueue(msg); 00144 __weak_rumble = true; 00145 __strong_rumble = false; 00146 } 00147 } else if ((mlb > 200) && !__strong_rumble) { 00148 JoystickInterface::StartRumbleMessage *msg = 00149 new JoystickInterface::StartRumbleMessage(); 00150 00151 float mf = (mlb / 1000.f); 00152 if (mf > 1) mf = 1; 00153 if (mf < 0.4) mf = 0.4; 00154 00155 msg->set_weak_magnitude((uint16_t)floorf(mf * 0xFFFF)); 00156 if (mf > 0.8) msg->set_strong_magnitude(0x8000); 00157 00158 __joy_if->msgq_enqueue(msg); 00159 00160 __weak_rumble = false; 00161 __strong_rumble = true; 00162 } else if (__weak_rumble || __strong_rumble) { 00163 JoystickInterface::StopRumbleMessage *msg = 00164 new JoystickInterface::StopRumbleMessage(); 00165 __joy_if->msgq_enqueue(msg); 00166 00167 __weak_rumble = __strong_rumble = false; 00168 } 00169 } 00170 00171 if (__joy_if->changed()) { 00172 if (__joy_if->num_axes() == 0) { 00173 logger->log_debug(name(), "Joystick disconnected, stopping"); 00174 stop(); 00175 } else if (__joy_if->pressed_buttons()) { 00176 00177 bool motor_state = false; 00178 00179 if (__joy_if->pressed_buttons() & __cfg_but_main_brush) { 00180 motor_state = true; 00181 __main_brush_enabled = ! __main_brush_enabled; 00182 } 00183 00184 if (__joy_if->pressed_buttons() & __cfg_but_side_brush) { 00185 motor_state = true; 00186 __side_brush_enabled = ! __side_brush_enabled; 00187 } 00188 00189 if (__joy_if->pressed_buttons() & __cfg_but_vacuuming) { 00190 motor_state = true; 00191 __vacuuming_enabled = ! __vacuuming_enabled; 00192 } 00193 00194 if (motor_state) { 00195 Roomba500Interface::SetMotorsMessage *sm = 00196 new Roomba500Interface::SetMotorsMessage( 00197 __vacuuming_enabled, 00198 __main_brush_enabled ? Roomba500Interface::BRUSHSTATE_FORWARD 00199 : Roomba500Interface::BRUSHSTATE_OFF, 00200 __side_brush_enabled ? Roomba500Interface::BRUSHSTATE_FORWARD 00201 : Roomba500Interface::BRUSHSTATE_OFF 00202 ); 00203 __roomba500_if->msgq_enqueue(sm); 00204 } 00205 00206 if (__joy_if->pressed_buttons() & __cfg_but_dock) { 00207 Roomba500Interface::DockMessage *dm = 00208 new Roomba500Interface::DockMessage(); 00209 __roomba500_if->msgq_enqueue(dm); 00210 } 00211 00212 if (__joy_if->pressed_buttons() & __cfg_but_spot) { 00213 /* 00214 Roomba500Interface::DockMessage *dm = 00215 new Roomba500Interface::DockMessage(); 00216 __roomba500_if->msgq_enqueue(dm); 00217 */ 00218 } 00219 00220 if (__joy_if->pressed_buttons() & __cfg_but_mode) { 00221 Roomba500Interface::SetModeMessage *sm = 00222 new Roomba500Interface::SetModeMessage(); 00223 00224 switch (__roomba500_if->mode()) { 00225 case Roomba500Interface::MODE_PASSIVE: 00226 sm->set_mode(Roomba500Interface::MODE_SAFE); break; 00227 case Roomba500Interface::MODE_SAFE: 00228 sm->set_mode(Roomba500Interface::MODE_FULL); break; 00229 case Roomba500Interface::MODE_FULL: 00230 sm->set_mode(Roomba500Interface::MODE_PASSIVE); break; 00231 default: 00232 sm->set_mode(Roomba500Interface::MODE_PASSIVE); break; 00233 } 00234 __roomba500_if->msgq_enqueue(sm); 00235 } 00236 00237 00238 } else if (__joy_if->axis(__cfg_axis_forward) == 0 && 00239 __joy_if->axis(__cfg_axis_sideward) == 0) { 00240 stop(); 00241 } else { 00242 float forward = __joy_if->axis(__cfg_axis_forward) * __cfg_max_velocity; 00243 float sideward = __joy_if->axis(__cfg_axis_sideward); 00244 float radius = copysignf(std::max(__cfg_min_radius, 00245 (int)(1. - fabs(sideward)) * 00246 __cfg_max_radius), 00247 sideward); 00248 float velocity = .5; 00249 if (__cfg_axis_speed < __joy_if->maxlenof_axis()) { 00250 velocity = __joy_if->axis(__cfg_axis_speed); 00251 } 00252 00253 int16_t velmm = (int16_t)roundf(forward * velocity); 00254 int16_t radmm = (int16_t)roundf(radius); 00255 // special case handling for "turn on place" 00256 if (fabsf(__joy_if->axis(__cfg_axis_forward)) < 0.1) { 00257 velmm = (int16_t)fabs(sideward * velocity) * __cfg_max_velocity; 00258 radmm = (int16_t)copysignf(1, sideward); 00259 } 00260 00261 /* 00262 logger->log_debug(name(), "Joystick (%f,%f,%f) Velo %f/%i Radius %f/%i", 00263 __joy_if->axis(__cfg_axis_forward), 00264 __joy_if->axis(__cfg_axis_sideward), 00265 __joy_if->axis(__cfg_axis_speed), 00266 velocity, velmm, radius, radmm); 00267 */ 00268 00269 __last_velo = velmm; 00270 00271 Roomba500Interface::DriveMessage *dm = 00272 new Roomba500Interface::DriveMessage(velmm, radmm); 00273 __roomba500_if->msgq_enqueue(dm); 00274 } 00275 } 00276 } 00277 00278 00279 void 00280 RoombaJoystickThread::stop() 00281 { 00282 Roomba500Interface::StopMessage *sm = new Roomba500Interface::StopMessage(); 00283 __roomba500_if->msgq_enqueue(sm); 00284 } 00285 00286 00287 unsigned int 00288 RoombaJoystickThread::confval(const char *path, unsigned int default_value) 00289 { 00290 try { 00291 return config->get_uint(path); 00292 } catch (Exception &e) { 00293 return default_value; 00294 } 00295 00296 }