Fawkes API  Fawkes Development Version
act_thread.cpp
00001 
00002 /***************************************************************************
00003  *  act_thread.cpp - Joystick thread to execute force feedback
00004  *
00005  *  Created: Mon Feb 07 21:29:22 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 "act_thread.h"
00024 #include "sensor_thread.h"
00025 #include "acquisition_thread.h"
00026 
00027 #include <interfaces/JoystickInterface.h>
00028 
00029 #include "force_feedback.h"
00030 
00031 using namespace fawkes;
00032 
00033 /** @class JoystickActThread "act_thread.h"
00034  * Joystick force feedback actuation thread.
00035  * This thread integrates into the Fawkes main loop at the act hook and
00036  * executes force feedback commands.
00037  * @author Tim Niemueller
00038  */
00039 
00040 
00041 /** Constructor.
00042  * @param aqt JoystickAcquisitionThread to get data from
00043  * @param senst sensor thread to share joystick interface with
00044  */
00045 JoystickActThread::JoystickActThread(JoystickAcquisitionThread *aqt,
00046                                      JoystickSensorThread *senst)
00047   : Thread("JoystickActThread", Thread::OPMODE_WAITFORWAKEUP),
00048     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_ACT)
00049 {
00050   __aqt   = aqt;
00051   __senst = senst;
00052 }
00053 
00054 
00055 void
00056 JoystickActThread::init()
00057 {
00058   __joystick_if = __senst->joystick_interface();
00059   __msgproc = new MessageProcessor(__aqt, __joystick_if);
00060 }
00061 
00062 
00063 void
00064 JoystickActThread::finalize()
00065 {
00066   delete __msgproc;
00067 }
00068 
00069 
00070 void
00071 JoystickActThread::loop()
00072 {
00073   __msgproc->process();
00074 }
00075 
00076 
00077 /** @class JoystickActThread::MessageProcessor "act_thread.h"
00078  * Process incoming messages.
00079  * Internal utility class.
00080  * @author Tim Niemueller
00081  */
00082 
00083 /** Constructor.
00084  * @param aqt acqusition thread to intsruct
00085  * @param joystick_if interface to listen on for messages
00086  */
00087 JoystickActThread::MessageProcessor::MessageProcessor(JoystickAcquisitionThread *aqt,
00088                                                       JoystickInterface *joystick_if)
00089 {
00090   __aqt = aqt;
00091   __joystick_if = joystick_if;
00092   __joystick_connected = false;
00093 }
00094 
00095 /** Process a single message.
00096  * @param msg message to process
00097  */
00098 void
00099 JoystickActThread::MessageProcessor::process_message(Message *msg)
00100 {
00101   JoystickForceFeedback *ff = __aqt->ff();
00102 
00103   if (! ff)  return;
00104 
00105   if (dynamic_cast<JoystickInterface::StartRumbleMessage *>(msg) != NULL) {
00106     JoystickInterface::StartRumbleMessage *srm =
00107       dynamic_cast<JoystickInterface::StartRumbleMessage *>(msg);
00108 
00109     ff->rumble(srm->strong_magnitude(), srm->weak_magnitude(),
00110                (JoystickForceFeedback::Direction)srm->direction(),
00111                srm->length(), srm->delay());
00112 
00113     uint8_t e = __joystick_if->ff_effects() | JoystickInterface::JFF_RUMBLE;
00114     __joystick_if->set_ff_effects(e);
00115     __joystick_if->write();
00116     
00117   }
00118   else if (dynamic_cast<JoystickInterface::StopRumbleMessage *>(msg) != NULL)
00119   {
00120     ff->stop_rumble();
00121     uint8_t e = __joystick_if->ff_effects() & ~JoystickInterface::JFF_RUMBLE;
00122     __joystick_if->set_ff_effects(e);
00123     __joystick_if->write();
00124 
00125   }
00126   else if (dynamic_cast<JoystickInterface::StopAllMessage *>(msg) != NULL)
00127   {
00128     ff->stop_all();
00129     __joystick_if->set_ff_effects(0);
00130     __joystick_if->write();
00131   }
00132 }
00133 
00134 
00135 /** Process message currently in the queue. */
00136 void
00137 JoystickActThread::MessageProcessor::process()
00138 {
00139   JoystickForceFeedback *ff = __aqt->ff();
00140 
00141   if (ff == NULL) {
00142     __joystick_if->msgq_flush();
00143     if (__joystick_connected) {
00144       __joystick_if->set_supported_ff_effects(0);
00145       __joystick_if->write();
00146       __joystick_connected = false;
00147     }
00148   } else  if (! __joystick_connected) {
00149     uint8_t effects = 0;
00150     if (ff->can_rumble()) {
00151       effects |= JoystickInterface::JFF_RUMBLE;
00152     }
00153     if (ff->can_periodic()) {
00154       effects |= JoystickInterface::JFF_PERIODIC;
00155     }
00156     if (ff->can_ramp()) {
00157       effects |= JoystickInterface::JFF_RAMP;
00158     }
00159     if (ff->can_spring()) {
00160       effects |= JoystickInterface::JFF_SPRING;
00161     }
00162     if (ff->can_friction()) {
00163       effects |= JoystickInterface::JFF_FRICTION;
00164     }
00165     if (ff->can_damper()) {
00166       effects |= JoystickInterface::JFF_DAMPER;
00167     }
00168     if (ff->can_inertia()) {
00169       effects |= JoystickInterface::JFF_INERTIA;
00170     }
00171     if (ff->can_constant()) {
00172       effects |= JoystickInterface::JFF_CONSTANT;
00173     }
00174     __joystick_if->set_supported_ff_effects(effects);    
00175     __joystick_if->write();
00176     __joystick_connected = true;
00177   }
00178 
00179   while (! __joystick_if->msgq_empty() ) {
00180 
00181     if (! __joystick_connected) {
00182       __joystick_if->msgq_flush();
00183       break;
00184     }
00185 
00186     process_message(__joystick_if->msgq_first());
00187 
00188     __joystick_if->msgq_pop();
00189   }
00190 }