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