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