Joystick.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    RobotAPI::ArmarXObjects::GamepadUnit
 * @author     Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <cstdint>
 
#include <fcntl.h>
#include <sys/poll.h>
#include <sys/stat.h>
#include <unistd.h>
 
#include <ArmarXCore/core/Component.h>
 
#include <linux/joystick.h>
 
namespace armarx
{
 
    class Joystick
    {
 
    private:
        int fd = -1;
        int fdEvent = -1;
        js_event event;
 
    public:
        std::vector<int16_t> axis;
        std::vector<bool> buttonsPressed;
        int numberOfAxis;
        int numberOfButtons;
        std::string name;
 
        bool
        open(std::string const& deviceName, std::string const& deviceEventName)
        {
 
            fd = ::open(deviceName.c_str(), O_RDONLY);
 
            if (!deviceEventName.empty())
            {
                ARMARX_INFO << "Force feedback enabled";
                fdEvent = ::open(deviceEventName.c_str(), O_RDWR | O_CLOEXEC);
 
                ARMARX_CHECK(fdEvent != -1);
            }
            else
            {
                ARMARX_INFO << "Force feedback disabled";
            }
 
            if (fd != -1)
            {
                // ARMARX_INFO << "before";
                // executeEffect();
                // ARMARX_INFO << "after";
 
 
                ioctl(fd, JSIOCGAXES, &numberOfAxis);
                ioctl(fd, JSIOCGBUTTONS, &numberOfButtons);
                name.resize(255);
                ioctl(fd, JSIOCGNAME(255), &name[0]);
                axis.resize(numberOfAxis, 0);
                name = name.c_str();
                buttonsPressed.resize(numberOfButtons, false);
            }
 
            ARMARX_INFO << "execute effect";
            executeEffect();
 
            return fd != -1;
        }
 
        bool
        opened() const
        {
            return fd != -1;
        }
 
        bool
        pollEvent()
        {
            int bytes = read(fd, &event, sizeof(event));
 
            // NOTE if this condition is not met, we're probably out of sync and this
            // Joystick instance is likely unusable
            if (bytes == -1 || bytes != sizeof(event))
            {
                return false;
            }
 
            if (event.type & JS_EVENT_BUTTON)
            {
                buttonsPressed[event.number] = event.value != 0;
            }
            else if (event.type & JS_EVENT_AXIS)
            {
                axis[event.number] = event.value;
            }
            return true;
        }
 
        void
        executeEffect(int gain = 100, const int nTimes = 1)
        {
            // this feature is disabled
            if (fdEvent < 0)
                return;
 
            // see https://docs.kernel.org/input/ff.html
 
 
            // https://xnux.eu/devices/feature/vibrator.html
 
            int ret;
            // pollfd pfds[1];
            int effects;
 
            // fd = open_event_dev("vibrator", O_RDWR | O_CLOEXEC);
            // syscall_error(fd < 0, "Can't open vibrator event device");
 
            ret = ioctl(fdEvent, EVIOCGEFFECTS, &effects);
            ARMARX_CHECK(ret >= 0);
            // syscall_error(ret < 0, "EVIOCGEFFECTS failed");
 
            // ARMARX_CHECK(effects & FF_RUMBLE);
 
            // Set the gain of the device
            {
                // int gain; between 0 and 100
                struct input_event ie; // structure used to communicate with the driver
 
                ie.type = EV_FF;
                ie.code = FF_GAIN;
                ie.value = 0xFFFFUL * gain / 100;
 
                if (write(fdEvent, &ie, sizeof(ie)) == -1)
                {
                    perror("set gain");
                }
            }
 
 
            ff_effect e;
            // e.type = FF_RUMBLE;
            // e.id = -1;
            // e.replay.length = 5000;
            // e.replay.delay = 500;
            // e.u.rumble.strong_magnitude = 1;
 
            e.type = FF_PERIODIC;
            e.id = -1;
            e.replay.length = 5000;
            e.replay.delay = 500;
            e.u.periodic.waveform = FF_SQUARE;
            e.u.periodic.period = 1000;
            e.u.periodic.magnitude = 0xFF;
            e.u.periodic.offset = 0xFF;
 
            ret = ioctl(fdEvent, EVIOCSFF, &e);
            ARMARX_CHECK(ret >= 0);
 
            // syscall_error(ret < 0, "EVIOCSFF failed");
 
            ARMARX_INFO << VAROUT(e.id);
 
            input_event play;
            play.type = EV_FF;
            play.code = static_cast<std::uint16_t>(e.id);
            play.value = 1;
 
            ret = write(fdEvent, &play, sizeof(play));
            ARMARX_CHECK(ret >= 0);
 
            ARMARX_INFO << "Executing effect";
 
 
            for (int i = 0; i < 5; i++)
            {
 
                input_event statusIe; // structure used to communicate with the driver
                statusIe.type = EV_FF_STATUS;
                statusIe.code = e.id;
                // statusIe.value = 0;
 
                ret = write(fdEvent, &statusIe, sizeof(statusIe));
 
                // ARMARX_CHECK()
                // ret should be FF_STATUS_PLAYING
                ARMARX_INFO << VAROUT(ret);
 
                sleep(1);
            }
 
 
            // syscall_error(ret < 0, "write failed");
 
            ARMARX_INFO << "Executing effect";
            // sleep(6);
 
 
            input_event stop;
            stop.type = EV_FF;
            stop.code = e.id;
            stop.value = 0;
            [[maybe_unused]] const int stopStatus =
                write(fdEvent, static_cast<const void*>(&stop), sizeof(stop));
 
 
            ret = ioctl(fdEvent, EVIOCRMFF, e.id);
            ARMARX_CHECK(ret >= 0);
 
            // syscall_error(ret < 0, "EVIOCRMFF failed");
 
            // close(fdEvent);
 
 
            /**/
            // Set the gain of the device
            // {
            //     // int gain; between 0 and 100
            //     struct input_event ie; // structure used to communicate with the driver
 
            //     ie.type = EV_FF;
            //     ie.code = FF_GAIN;
            //     ie.value = 0xFFFFUL * gain / 100;
 
            //     if (write(fd, &ie, sizeof(ie)) == -1)
            //     {
            //         perror("set gain");
            //     }
            // }
 
 
            // struct input_event play;
            // struct input_event stop;
 
            // // upload request to device
            // ff_effect effect;
            // const auto uploadStatus = ioctl(fd, EVIOCSFF, &effect);
 
            // // Play n times
            // play.type = EV_FF;
            // play.code = effect.id;
            // play.value = nTimes;
 
            // const int playStatus = write(fd, static_cast<const void*>(&play), sizeof(play));
 
            // // Stop an effect
            // stop.type = EV_FF;
            // stop.code = effect.id;
            // stop.value = 0;
            // const int stopStatus = write(fd, static_cast<const void*>(&stop), sizeof(stop));
 
            // // remove effect
            // ioctl(fd, EVIOCRMFF, effect.id);
        }
 
        void
        close()
        {
            ::close(fd);
            fd = -1;
        }
    };
} // namespace armarx