Fawkes API  Fawkes Development Version
force_feedback.cpp
00001 
00002 /***************************************************************************
00003  *  force_feedback.cpp - Force feedback for joysticks using Linux input API
00004  *
00005  *  Created: Mon Feb 07 01:35:29 2011 (Super Bowl XLV)
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 "force_feedback.h"
00024 
00025 #include <fnmatch.h>
00026 #include <sys/types.h>
00027 #include <dirent.h>
00028 #include <cstdio>
00029 #include <cstdlib>
00030 #include <sys/stat.h>
00031 #include <fcntl.h>
00032 #include <unistd.h>
00033 #include <cstring>
00034 #include <cerrno>
00035 
00036 #include <core/exception.h>
00037 
00038 #define BITS_PER_LONG (sizeof(long) * 8)
00039 #define NBITS(x) ((((x)-1)/BITS_PER_LONG)+1)
00040 #define OFF(x)  ((x)%BITS_PER_LONG)
00041 #define BIT(x)  (1UL<<OFF(x))
00042 #define LONG(x) ((x)/BITS_PER_LONG)
00043 #define test_bit(bit, array)  ((array[LONG(bit)] >> OFF(bit)) & 1)
00044 
00045 using namespace fawkes;
00046 
00047 /** @class JoystickForceFeedback "force_feedback.h"
00048  * Cause force feedback on a joystick.
00049  * An instance of this class opens an input device which belongs to
00050  * the given device name. It searches all input devices to find the
00051  * correct device file. Once opened, it detects the available features
00052  * of the joystick and provides conventient access to it allowing for
00053  * rumbling effects, for instance.
00054  * @author Tim Niemueller
00055  *
00056  * @fn bool JoystickForceFeedback::is_rumbling()
00057  * Check if rumbling effect is active.
00058  * @return true if effect is active, false otherwise
00059  *
00060  * @fn bool JoystickForceFeedback::can_rumble()
00061  * Check if rumbling effect is supported.
00062  * @return true if effect is supported, false otherwise
00063  *
00064  * @fn bool JoystickForceFeedback::can_periodic()
00065  * Check if periodic effect is supported.
00066  * @return true if effect is supported, false otherwise
00067  *
00068  * @fn bool JoystickForceFeedback::can_constant()
00069  * Check if constant effect is supported.
00070  * @return true if effect is supported, false otherwise
00071  *
00072  * @fn bool JoystickForceFeedback::can_spring()
00073  * Check if spring effect is supported.
00074  * @return true if effect is supported, false otherwise
00075  *
00076  * @fn bool JoystickForceFeedback::can_friction()
00077  * Check if friction effect is supported.
00078  * @return true if effect is supported, false otherwise
00079  *
00080  * @fn bool JoystickForceFeedback::can_damper()
00081  * Check if damper effect is supported.
00082  * @return true if effect is supported, false otherwise
00083  *
00084  * @fn bool JoystickForceFeedback::can_inertia()
00085  * Check if inertia effect is supported.
00086  * @return true if effect is supported, false otherwise
00087  *
00088  * @fn bool JoystickForceFeedback::can_ramp()
00089  * Check if ramp effect is supported.
00090  * @return true if effect is supported, false otherwise
00091  *
00092  * @fn bool JoystickForceFeedback::can_square()
00093  * Check if square effect is supported.
00094  * @return true if effect is supported, false otherwise
00095  *
00096  * @fn bool JoystickForceFeedback::can_triangle()
00097  * Check if triangle effect is supported.
00098  * @return true if effect is supported, false otherwise
00099  *
00100  * @fn bool JoystickForceFeedback::can_sine()
00101  * Check if sine effect is supported.
00102  * @return true if effect is supported, false otherwise
00103  *
00104  * @fn bool JoystickForceFeedback::can_saw_up()
00105  * Check if upward saw effect is supported.
00106  * @return true if effect is supported, false otherwise
00107  *
00108  * @fn bool JoystickForceFeedback::can_saw_down()
00109  * Check if downward saw effect is supported.
00110  * @return true if effect is supported, false otherwise
00111  *
00112  * @fn bool JoystickForceFeedback::can_custom()
00113  * Check if custom effect is supported.
00114  * @return true if effect is supported, false otherwise
00115  *
00116  */
00117 
00118 /** Constructor.
00119  * @param device_name device name, note that this is not the device
00120  * file, but rather the event files are tried and the device name is
00121  * compared.
00122  */
00123 JoystickForceFeedback::JoystickForceFeedback(const char *device_name)
00124 {
00125   __fd = -1;
00126 
00127   DIR *d = opendir("/dev/input");
00128 
00129   if (d == NULL) {
00130     throw Exception("Could not open directory /dev/input");
00131   }
00132 
00133   struct dirent *de;
00134   while ((de = readdir(d)) != NULL) {
00135     if (fnmatch("event*", de->d_name, 0) != FNM_NOMATCH) {
00136       char *path;
00137       if (asprintf(&path, "/dev/input/%s", de->d_name) == -1) {
00138         continue;
00139       }
00140 
00141       __fd = open(path, O_RDWR);
00142       if (__fd == -1) {
00143         free(path);
00144         continue;
00145       }
00146       free(path);
00147 
00148       char name[256]= "Unknown";
00149       if(ioctl(__fd, EVIOCGNAME(sizeof(name)), name) < 0) {
00150         close(__fd);
00151         __fd = -1;
00152         continue;
00153       }
00154 
00155       if (strcmp(name, device_name) != 0) {
00156         close(__fd);
00157         __fd = -1;
00158         continue;
00159       }
00160 
00161       long features[NBITS(EV_MAX)];
00162       memset(features, 0, sizeof(features));
00163       if (ioctl(__fd, EVIOCGBIT(0, EV_MAX), features) < 0) {
00164         close(__fd);
00165         __fd = -1;
00166         throw Exception("Cannot get feedback feature vector");
00167       }
00168 
00169       if (! test_bit(EV_FF, features)) {
00170         close(__fd);
00171         __fd = -1;
00172         throw Exception("Device '%s' does not support force-feedback", device_name);
00173       }
00174 
00175       long ff_features[NBITS(FF_MAX)];
00176         
00177       memset(ff_features, 0, sizeof(ff_features));
00178       if (ioctl(__fd, EVIOCGBIT(EV_FF, FF_MAX), ff_features) < 0) {
00179         close(__fd);
00180         __fd = -1;
00181         throw Exception("Cannot get device force feedback feature vector");
00182       }
00183 
00184       long no_ff_features[NBITS(FF_MAX)];
00185       memset(no_ff_features, 0, sizeof(no_ff_features));
00186       if (memcmp(ff_features, no_ff_features, sizeof(no_ff_features)) == 0) {
00187         close(__fd);
00188         __fd = -1;
00189         throw Exception("Device has no force feedback features");
00190       }
00191 
00192       __can_rumble   = test_bit(FF_RUMBLE, ff_features);
00193       __can_periodic = test_bit(FF_PERIODIC, ff_features);
00194       __can_constant = test_bit(FF_CONSTANT, ff_features);
00195       __can_spring   = test_bit(FF_SPRING, ff_features);
00196       __can_friction = test_bit(FF_FRICTION, ff_features);
00197       __can_damper   = test_bit(FF_DAMPER, ff_features);
00198       __can_inertia  = test_bit(FF_INERTIA, ff_features);
00199       __can_ramp     = test_bit(FF_RAMP, ff_features);
00200       __can_square   = test_bit(FF_SQUARE, ff_features);
00201       __can_triangle = test_bit(FF_TRIANGLE, ff_features);
00202       __can_sine     = test_bit(FF_SINE, ff_features);
00203       __can_saw_up   = test_bit(FF_SAW_UP, ff_features);
00204       __can_saw_down = test_bit(FF_SAW_DOWN, ff_features);
00205       __can_custom   = test_bit(FF_CUSTOM, ff_features);
00206       
00207       if (ioctl(__fd, EVIOCGEFFECTS, &__num_effects) < 0) {
00208         __num_effects = 1;
00209       }
00210 
00211       break;
00212     }
00213   }
00214 
00215   closedir(d);
00216 
00217   if (__fd == -1) {
00218     throw Exception("Force feedback joystick '%s' not found", device_name);
00219   }
00220 
00221   memset(&__rumble, 0, sizeof(__rumble));
00222   __rumble.type = FF_RUMBLE;
00223   __rumble.id   = -1;
00224 }
00225 
00226 
00227 /** Destructor. */
00228 JoystickForceFeedback::~JoystickForceFeedback()
00229 {
00230   close(__fd);
00231 }
00232 
00233 
00234 /** Rumble the joystick.
00235 
00236  * This is the most basic force feedback for example in force feedback
00237  * joypads. Often such joysticks provide two effect magnitudes, a
00238  * strong heavier motor for larger effects, and a smaller one for
00239  * vibrating effects.
00240  * @param strong_magnitude magnitude to use on the larger motor
00241  * @param weak_magnitude magnitude to use on the smaller motor
00242  * @param direction direction of the effect, meaningful on joysticks
00243  * (rather than joypads)
00244  * @param length length of the effect in ms
00245  * @param delay delay before the effect starts in ms
00246  */
00247 void
00248 JoystickForceFeedback::rumble(uint16_t strong_magnitude, uint16_t weak_magnitude,
00249                               Direction direction, uint16_t length, uint16_t delay)
00250 {
00251   if ( (__rumble.id == -1) ||
00252        (__rumble.u.rumble.strong_magnitude != strong_magnitude) ||
00253        (__rumble.u.rumble.weak_magnitude != weak_magnitude) ||
00254        (__rumble.direction != direction) ||
00255        (__rumble.replay.length != length) ||
00256        (__rumble.replay.delay != length) )
00257   {
00258     // we need to upload
00259     __rumble.u.rumble.strong_magnitude = strong_magnitude;
00260     __rumble.u.rumble.weak_magnitude   = weak_magnitude;
00261     __rumble.direction = direction;
00262     __rumble.replay.length = length;
00263     __rumble.replay.delay = delay;
00264 
00265     if (ioctl(__fd, EVIOCSFF, &__rumble) < 0) {
00266       throw Exception("Failed to upload rumble effect");
00267     }
00268   }
00269 
00270   struct input_event play;
00271   play.type  = EV_FF;
00272   play.code  = __rumble.id;
00273   play.value = 1;
00274 
00275   if (write(__fd, &play, sizeof(play)) < 0) {
00276     throw Exception("Failed to start rumble effect");
00277   }
00278 }
00279 
00280 
00281 /** Stop rumbling. */
00282 void
00283 JoystickForceFeedback::stop_rumble()
00284 {
00285   if (__rumble.id != -1) {
00286     if (ioctl(__fd, EVIOCRMFF, __rumble.id) < 0) {
00287       throw Exception("Failed to stop rumble effect");
00288     }
00289     __rumble.id = -1;
00290   }
00291 }
00292 
00293 
00294 /** Stop all current effects. */
00295 void
00296 JoystickForceFeedback::stop_all()
00297 {
00298   stop_rumble();
00299 }