Add KITProstheticHandDriver and example

source/RobotAPI/drivers/CMakeLists.txt

@@ -5,4 +5,5 @@ add_subdirectory(OptoForceUnit)
 add_subdirectory(OrientedTactileSensor)
 add_subdirectory(GamepadUnit)
 add_subdirectory(MetaWearIMU)
+add_subdirectory(KITProstheticHandDriver)
 

source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.cpp

+#include <sstream>
+
+#include "BLEProthesisInterface.h"
+#include "BLEProthesisInterfaceQtWorkerThread.h"
+
+BLEProthesisInterface::BLEProthesisInterface(const std::string &mac) :
+    _worker{new BLEProthesisInterfaceQtWorkerThread{mac, *this}}
+{
+    _worker->start();
+}
+
+BLEProthesisInterface::~BLEProthesisInterface()
+{
+    _worker->kill();
+    _worker->quit();
+    _worker->exit();
+    _worker->wait(1000); // ms
+    _worker->terminate();
+    _worker->wait();
+}
+
+std::int64_t BLEProthesisInterface::getThumbPWM() const
+{
+    return _thumbPWM;
+}
+
+std::int64_t BLEProthesisInterface::getThumbPos() const
+{
+    return _thumbPos;
+}
+
+std::int64_t BLEProthesisInterface::getFingerPWM() const
+{
+    return _fingerPWM;
+}
+
+std::int64_t BLEProthesisInterface::getFingerPos() const
+{
+    return _fingerPos;
+}
+
+BLEProthesisInterface::State BLEProthesisInterface::getState() const
+{
+    return _state;
+}
+
+void BLEProthesisInterface::sendRaw(const std::string &cmd)
+{
+    if(_state != State::Running)
+    {
+        throw std::invalid_argument
+        {
+            "BLEProthesisInterface::sendRaw( cmd = " + cmd +
+            " ): The UART service is not connected!"
+        };
+    }
+    _worker->sendCommand(cmd);
+}
+
+void BLEProthesisInterface::sendGrasp(std::uint64_t n)
+{
+    if(n > _maxG)
+    {
+        throw std::invalid_argument
+        {
+            "BLEProthesisInterface::sendGrasp( n = " + std::to_string(n) +
+                    " ): the maximal value for n is " + std::to_string(_maxG)
+        };
+    }
+    sendRaw("g" + std::to_string(n) + "\n");
+}
+
+void BLEProthesisInterface::sendThumbPWM(uint64_t v, int64_t maxPWM, uint64_t pos)
+{
+    if( v < _minV || v > _maxV )
+    {
+        throw std::invalid_argument
+        {
+            "sendThumbPWM( v = " + std::to_string(v) +
+                    " , maxPWM = " + std::to_string(maxPWM) +
+                    " , pos = " + std::to_string(pos) +
+                    " ): The interval for v is [" + std::to_string(_minV) +
+                    ", " + std::to_string(_maxV) + "]"
+        };
+    }
+    if( maxPWM < _minPWM || maxPWM > _maxPWM )
+    {
+        throw std::invalid_argument
+        {
+            "sendThumbPWM( v = " + std::to_string(v) +
+                    " , maxPWM = " + std::to_string(maxPWM) +
+                    " , pos = " + std::to_string(pos) +
+                    " ): The interval for maxPWM is [" + std::to_string(_minPWM) +
+                    ", " + std::to_string(_maxPWM) + "]"
+        };
+    }
+    if( pos > _maxPosT )
+    {
+        throw std::invalid_argument
+        {
+            "sendThumbPWM( v = " + std::to_string(v) +
+                    " , maxPWM = " + std::to_string(maxPWM) +
+                    " , pos = " + std::to_string(pos) +
+                    " ): The interval for pos is [0, " +
+                    std::to_string(_maxPosT) + "]"
+        };
+    }
+    std::stringstream str;
+    str << 'M' << 2 << ',' << v << ',' << maxPWM << ',' << pos << '\n';
+    sendRaw(str.str());
+}
+
+void BLEProthesisInterface::sendFingerPWM(uint64_t v, int64_t maxPWM, uint64_t pos)
+{
+    if( v < _minV || v > _maxV )
+    {
+        throw std::invalid_argument
+        {
+            "sendThumbPWM( v = " + std::to_string(v) +
+                    " , maxPWM = " + std::to_string(maxPWM) +
+                    " , pos = " + std::to_string(pos) +
+                    " ): The interval for v is [" + std::to_string(_minV) +
+                    ", " + std::to_string(_maxV) + "]"
+        };
+    }
+    if( maxPWM < _minPWM || maxPWM > _maxPWM )
+    {
+        throw std::invalid_argument
+        {
+            "sendThumbPWM( v = " + std::to_string(v) +
+                    " , maxPWM = " + std::to_string(maxPWM) +
+                    " , pos = " + std::to_string(pos) +
+                    " ): The interval for maxPWM is [" + std::to_string(_minPWM) +
+                    ", " + std::to_string(_maxPWM) + "]"
+        };
+    }
+    if( pos > _maxPosF )
+    {
+        throw std::invalid_argument
+        {
+            "sendThumbPWM( v = " + std::to_string(v) +
+                    " , maxPWM = " + std::to_string(maxPWM) +
+                    " , pos = " + std::to_string(pos) +
+                    " ): The interval for pos is [0, " +
+                    std::to_string(_maxPosF) + "]"
+        };
+    }
+    std::stringstream str;
+    str << 'M' << 3 << ',' << v << ',' << maxPWM << ',' << pos << '\n';
+    sendRaw(str.str());
+}

source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.h

+#pragma once
+
+#include <atomic>
+#include <string>
+#include <memory>
+
+class BLEProthesisInterfaceQtWorkerThread;
+class BLEProthesisInterfaceQtWorker;
+
+class BLEProthesisInterface
+{
+public:
+    BLEProthesisInterface(const std::string& mac);
+    ~BLEProthesisInterface();
+    enum class State
+    {
+        Created,
+        DiscoveringDevices,
+        DiscoveringDevicesDone,
+        Disconnected,
+        Connecting,
+        ConnectingDone,
+        DiscoveringServices,
+        DiscoveringServicesDone,
+        ConnectingService,
+        Running,
+        Killed
+    };
+    std::int64_t getThumbPWM() const;
+    std::int64_t getThumbPos() const;
+    std::int64_t getFingerPWM() const;
+    std::int64_t getFingerPos() const;
+
+    State getState() const;
+
+    void sendRaw(const std::string& cmd);
+
+    void sendGrasp(std::uint64_t n);
+    void sendThumbPWM(std::uint64_t v, std::int64_t maxPWM, std::uint64_t pos);
+    void sendFingerPWM(std::uint64_t v, std::int64_t maxPWM, std::uint64_t pos);
+private:
+    friend class BLEProthesisInterfaceQtWorker;
+    std::atomic_int64_t _thumbPWM{0};
+    std::atomic_int64_t _thumbPos{0};
+    std::atomic_int64_t _fingerPWM{0};
+    std::atomic_int64_t _fingerPos{0};
+
+    std::unique_ptr<BLEProthesisInterfaceQtWorkerThread> _worker;
+    std::atomic<State> _state{State::Created};
+
+    static constexpr std::uint64_t _maxG = 8;
+    static constexpr std::uint64_t _minV = 10;
+    static constexpr std::uint64_t _maxV = 200;
+    static constexpr std::int64_t _minPWM = -2999;
+    static constexpr std::int64_t _maxPWM =  2999;
+    static constexpr std::uint64_t _maxPosT = 100'000;
+    static constexpr std::uint64_t _maxPosF = 200'000;
+};
+
+inline std::string to_string(BLEProthesisInterface::State s)
+{
+    switch(s)
+    {
+    case BLEProthesisInterface::State::Created: return "Created";
+    case BLEProthesisInterface::State::DiscoveringDevices: return "DiscoveringDevices";
+    case BLEProthesisInterface::State::DiscoveringDevicesDone: return "DiscoveringDevicesDone";
+    case BLEProthesisInterface::State::Disconnected: return "Disconnected";
+    case BLEProthesisInterface::State::Connecting: return "Connecting";
+    case BLEProthesisInterface::State::ConnectingDone: return "ConnectingDone";
+    case BLEProthesisInterface::State::DiscoveringServices: return "DiscoveringServices";
+    case BLEProthesisInterface::State::DiscoveringServicesDone: return "DiscoveringServicesDone";
+    case BLEProthesisInterface::State::ConnectingService: return "ConnectingService";
+    case BLEProthesisInterface::State::Running: return "Running";
+    case BLEProthesisInterface::State::Killed: return "Killed";
+    }
+    return "UNKNOWN VALUE FOR BLEProthesisInterface::State: " + std::to_string(static_cast<int>(s));
+}
+
+
+
+
+
+
+
+//#include <QBluetoothDeviceDiscoveryAgent>
+//#include <QBluetoothDeviceInfo>
+//#include <QBluetoothSocket>
+//#include <QLowEnergyController>
+
+
+//class BLEProthesisInterface : public QObject
+//{
+//    Q_OBJECT
+//public:
+//    explicit BLEProthesisInterface(const QString& mac, QObject *parent = 0);
+//    ~BLEProthesisInterface();
+
+////    void startClient(const QBluetoothServiceInfo &remoteService);
+////    void stopClient();
+//private:
+//    void removeAll();
+//public slots:
+////    void sendMessage(const QString &message);
+
+//    void reset(QString mac);
+
+//signals:
+////    void messageReceived(const QString &sender, const QString &message);
+////    void connected(const QString &name);
+////    void disconnected();
+
+//private slots:
+//    void deviceDiscovered(const QBluetoothDeviceInfo &info);
+//    void deviceDiscoverFinished();
+//    void discoveryFinished();
+////    void readSocket();
+//    void connected();
+//    void disconnected()
+//    {
+
+//    }
+//    void stateChanged(QLowEnergyController::ControllerState state);
+//    void serviceDiscovered(const QBluetoothUuid &newService);
+//    void controllerError(QLowEnergyController::Error error);
+
+//    void serviceCharacteristicChanged(const QLowEnergyCharacteristic& cha, const QByteArray& arr)
+//    {
+
+//    }
+//    void serviceDescriptorWritten(const QLowEnergyDescriptor& des, const QByteArray& arr)
+//    {
+
+//    }
+//    void serviceStateChanged(const QLowEnergyService::ServiceState& state);
+
+//private:
+//    QBluetoothDeviceDiscoveryAgent* _discoveryAgent{nullptr};
+//    QString _mac;
+//    QBluetoothSocket* _socket{nullptr};
+//    QLowEnergyController* _leController{nullptr};
+//    QLowEnergyService* _service{nullptr};
+//    bool _connected{false};
+//    bool _reconnect{true};
+//    bool _serviceUp{false};
+//};

source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorker.cpp

+#include "BLEProthesisInterfaceQtWorker.h"
+
+
+#define UARTSERVICEUUID "6e400001-b5a3-f393-e0a9-e50e24dcca9e"
+#define RXUUID "6e400002-b5a3-f393-e0a9-e50e24dcca9e"
+#define TXUUID "6e400003-b5a3-f393-e0a9-e50e24dcca9e"
+
+BLEProthesisInterfaceQtWorker::BLEProthesisInterfaceQtWorker(const QString& mac, BLEProthesisInterface &owner) :
+    _owner{&owner},
+    _mac{mac}
+{
+    _timer = startTimer(std::chrono::milliseconds{10});
+}
+
+BLEProthesisInterfaceQtWorker::~BLEProthesisInterfaceQtWorker()
+{
+    cleanup();
+}
+
+void BLEProthesisInterfaceQtWorker::kill()
+{
+    _killed = true;
+}
+
+void BLEProthesisInterfaceQtWorker::sendCommand(const std::string &cmd)
+{
+    std::lock_guard guard(_cmdMutex);
+    _cmd += QString::fromStdString(cmd);
+}
+
+void BLEProthesisInterfaceQtWorker::cleanup()
+{
+    //stop services etc
+    if(_timer != -1)
+    {
+        killTimer(_timer);
+        _timer = -1;
+    }
+    //disconnect
+    if(_control)
+    {
+        _control->disconnectFromDevice();
+    }
+
+    //delete
+    if(_service)
+    {
+        delete _service;
+        _service = nullptr;
+    }
+    if(_control)
+    {
+        delete _control;
+        _control = nullptr;
+    }
+    if(_deviceDiscoveryAgent)
+    {
+        delete _deviceDiscoveryAgent;
+        _deviceDiscoveryAgent = nullptr;
+    }
+}
+
+void BLEProthesisInterfaceQtWorker::timerEvent(QTimerEvent*)
+{
+    if(_killed)
+    {
+        qDebug() << '[' << _mac << ']' << " Stopping NOW!";
+        cleanup();
+        quit();
+        return;
+    }
+    //discovering
+    if(_owner->_state == State::Created)
+    {
+        _deviceDiscoveryAgent = new QBluetoothDeviceDiscoveryAgent;
+        connect(_deviceDiscoveryAgent, SIGNAL(deviceDiscovered(const QBluetoothDeviceInfo&)),
+                this, SLOT(deviceDiscovered(const QBluetoothDeviceInfo&)));
+        connect(_deviceDiscoveryAgent, SIGNAL(error(QBluetoothDeviceDiscoveryAgent::Error)),
+                this, SLOT(deviceDiscoverError(QBluetoothDeviceDiscoveryAgent::Error)));
+        connect(_deviceDiscoveryAgent, SIGNAL(finished()), this, SLOT(deviceDiscoverFinished()));
+        connect(_deviceDiscoveryAgent, SIGNAL(canceled()), this, SLOT(deviceDiscoverFinished()));
+        qDebug() << '[' << _mac << ']' << " State DiscoveringDevices";
+        _owner->_state = State::DiscoveringDevices;
+        _deviceDiscoveryAgent->start();
+    }
+    else if(_owner->_state == State::DiscoveringDevicesDone)
+    {
+        if(!_deviceDiscovered)
+        {
+            qDebug() << '[' << _mac << ']' << " Device discovering failed!";
+            kill();
+            return;
+        }
+        qDebug() << '[' << _mac << ']' << " State Disconnected";
+        _owner->_state = State::Disconnected;
+    }
+    else if(_owner->_state == State::Disconnected)
+    {
+        if(_service)
+        {
+            delete _service;
+            _service = nullptr;
+        }
+        if(_control)
+        {
+            delete _control;
+            _control = nullptr;
+        }
+        _serviceDiscovered = false;
+        _control = new QLowEnergyController(_currentDevice);
+        _control ->setRemoteAddressType(QLowEnergyController::RandomAddress);
+
+        connect(_control, SIGNAL(serviceDiscovered(QBluetoothUuid)),
+                this, SLOT(serviceDiscovered(QBluetoothUuid)));
+        connect(_control, SIGNAL(discoveryFinished()),
+                this, SLOT(serviceScanDone()));
+        connect(_control, SIGNAL(error(QLowEnergyController::Error)),
+                this, SLOT(controllerError(QLowEnergyController::Error)));
+        connect(_control, SIGNAL(connected()),
+                this, SLOT(deviceConnected()));
+        connect(_control, SIGNAL(disconnected()),
+                this, SLOT(deviceDisconnected()));
+        _control->connectToDevice();
+        qDebug() << '[' << _mac << ']' << " State Connecting";
+        _owner->_state = State::Connecting;
+    }
+    else if(_owner->_state == State::ConnectingDone)
+    {
+        _control->discoverServices();
+        qDebug() << '[' << _mac << ']' << " State DiscoveringServices";
+        _owner->_state = State::DiscoveringServices;
+    }
+    else if(_owner->_state == State::DiscoveringServicesDone)
+    {
+        if(!_serviceDiscovered)
+        {
+            qDebug() << '[' << _mac << ']' << " Service discovering failed!";
+            kill();
+            return;
+        }
+        _service = _control->createServiceObject(QBluetoothUuid(QUuid(UARTSERVICEUUID)));
+
+        connect(_service, SIGNAL(stateChanged(QLowEnergyService::ServiceState)),
+                this, SLOT(serviceStateChanged(QLowEnergyService::ServiceState)));
+        connect(_service, SIGNAL(characteristicChanged(QLowEnergyCharacteristic,QByteArray)),
+                this, SLOT(readData(QLowEnergyCharacteristic,QByteArray)));
+        connect(_service, SIGNAL(descriptorWritten(QLowEnergyDescriptor,QByteArray)),
+                this, SLOT(receiveDeviceDisconnec(QLowEnergyDescriptor,QByteArray)));
+
+        _service->discoverDetails();
+
+        qDebug() << '[' << _mac << ']' << " State ConnectingService";
+        _owner->_state = State::ConnectingService;
+    }
+    else if (_owner->_state == State::Running) //send data
+    {
+        std::lock_guard g(_cmdMutex);
+        if(_service && _cmd.size() != 0)
+        {
+            const QLowEnergyCharacteristic  RxChar = _service->characteristic(QBluetoothUuid(QUuid(RXUUID)));
+            QByteArray data;
+            data.append(_cmd);
+            _service->writeCharacteristic(RxChar, data, QLowEnergyService::WriteWithoutResponse);
+            qDebug() << '[' << _mac << ']' << " send: " << _cmd;
+            _cmd.clear();
+        }
+    }
+}
+
+void BLEProthesisInterfaceQtWorker::deviceDiscovered(const QBluetoothDeviceInfo &device)
+{
+    if (device.address().toString() == _mac)
+    {
+        qDebug() << '[' << _mac << ']' << " Discovered target device " << device.address().toString();
+        _currentDevice = device;
+        _deviceDiscovered = true;
+        qDebug() << '[' << _mac << ']' << " State DiscoveringDevicesDone";
+        _owner->_state = State::DiscoveringDevicesDone;
+        _deviceDiscoveryAgent->stop();
+    }
+    else
+    {
+        qDebug() << '[' << _mac << ']' << " Discovered device " << device.address().toString();
+    }
+}
+
+void BLEProthesisInterfaceQtWorker::deviceDiscoverFinished()
+{
+    qDebug() << '[' << _mac << ']' << " Discovering of services done.";
+    _owner->_state = State::DiscoveringDevicesDone;
+}
+
+void BLEProthesisInterfaceQtWorker::deviceDiscoverError(QBluetoothDeviceDiscoveryAgent::Error error)
+{
+    if (error == QBluetoothDeviceDiscoveryAgent::PoweredOffError)
+    {
+        qDebug() << '[' << _mac << ']' << "The Bluetooth adaptor is powered off, power it on before doing discovery.";
+    }
+    else if (error == QBluetoothDeviceDiscoveryAgent::InputOutputError)
+    {
+        qDebug() << '[' << _mac << ']' << "Writing or reading from the device resulted in an error.";
+    }
+    else
+    {
+        qDebug() << '[' << _mac << ']' << "An unknown error has occurred.";
+    }
+    kill();
+}
+
+void BLEProthesisInterfaceQtWorker::serviceDiscovered(const QBluetoothUuid &gatt)
+{
+    qDebug() << '[' << _mac << ']' << " Discovered service " << gatt.toString();
+    if(gatt==QBluetoothUuid(QUuid(UARTSERVICEUUID)))
+    {
+        qDebug() << '[' << _mac << ']' << "Discovered UART service " << gatt.toString();
+        _serviceDiscovered =true;
+        _owner->_state = State::DiscoveringServicesDone;
+    }
+}
+
+void BLEProthesisInterfaceQtWorker::serviceDiscoverFinished()
+{
+    qDebug() << '[' << _mac << ']' << " State DiscoveringServicesDone";
+    _owner->_state = State::DiscoveringServicesDone;
+}
+
+void BLEProthesisInterfaceQtWorker::controllerError(QLowEnergyController::Error error)
+{
+    qDebug() << '[' << _mac << ']' << " Cannot connect to remote device.";
+    qWarning() << '[' << _mac << ']' << " Controller Error:" << error;
+    kill();
+}
+
+void BLEProthesisInterfaceQtWorker::receiveDeviceDisconnec(const QLowEnergyDescriptor &d, const QByteArray &value)
+{
+    if (d.isValid() && d == _notificationDescTx && value == QByteArray("0000"))
+    {
+        qDebug() << '[' << _mac << ']' << "Device requests disconnect.";
+        //disabled notifications -> assume disconnect intent
+        _control->disconnectFromDevice();
+        if(_service)
+        {
+            delete _service;
+            _service = nullptr;
+        }
+    }
+}
+
+void BLEProthesisInterfaceQtWorker::serviceStateChanged(QLowEnergyService::ServiceState s)
+{
+    // A descriptoc can only be written if the service is in the ServiceDiscovered state
+    qDebug() << '[' << _mac << ']' << " serviceStateChanged -> " << s;
+    switch (s)
+    {
+        case QLowEnergyService::ServiceDiscovered:
+        {
+
+            //looking for the TX characteristic
+            const QLowEnergyCharacteristic TxChar = _service->characteristic(QBluetoothUuid(QUuid(TXUUID)));
+            if (!TxChar.isValid())
+            {
+                qDebug() << '[' << _mac << ']' << " Tx characteristic not found";
+                break;
+            }
+
+            //looking for the RX characteristic
+            const QLowEnergyCharacteristic  RxChar = _service->characteristic(QBluetoothUuid(QUuid(RXUUID)));
+            if (!RxChar.isValid())
+            {
+                qDebug() << '[' << _mac << ']' << " Rx characteristic not found";
+                break;
+            }
+
+            // Bluetooth LE spec Where a characteristic can be notified, a Client Characteristic Configuration descriptor
+            // shall be included in that characteristic as required by the Bluetooth Core Specification
+            // Tx notify is enabled
+            _notificationDescTx = TxChar.descriptor(QBluetoothUuid::ClientCharacteristicConfiguration);
+            if (_notificationDescTx.isValid())
+            {
+                // enable notification
+                _service->writeDescriptor(_notificationDescTx, QByteArray::fromHex("0100"));
+                qDebug() << '[' << _mac << ']' << " State Running";
+                _owner->_state = State::Running;
+            }
+        }
+        break;
+        default:
+        break;
+    }
+}
+
+void BLEProthesisInterfaceQtWorker::deviceConnected()
+{
+    qDebug() << '[' << _mac << ']' << " State ConnectingDone";
+    _owner->_state = State::ConnectingDone;
+}
+
+void BLEProthesisInterfaceQtWorker::deviceDisconnected()
+{
+    qDebug() << '[' << _mac << ']' << " State Disconnected";
+    _owner->_state = State::Disconnected;
+}
+
+void BLEProthesisInterfaceQtWorker::readData(const QLowEnergyCharacteristic &c, const QByteArray &value)
+{
+    // ignore any other characteristic change
+    if (c.uuid() != QBluetoothUuid(QUuid(TXUUID)))
+        return;
+    //qDebug() << '[' << _mac << ']' << " received : " << value;
+
+    _valueAkk += QString{value};
+    if(!_valueAkk.contains('\n'))
+    {
+        return;
+    }
+    auto listPacks = _valueAkk.split('\n');
+    auto listVals = listPacks.at(listPacks.size() - 2).split(' ');
+
+    _owner->_thumbPos = listVals.at(0).toLong();
+    _owner->_thumbPWM = listVals.at(1).toLong();
+    _owner->_fingerPos = listVals.at(2).toLong();
+    _owner->_fingerPWM = listVals.at(3).toLong();
+
+    _valueAkk = listPacks.back();
+}

source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorker.h

+#pragma once
+
+#include <mutex>
+
+#include <QBluetoothDeviceDiscoveryAgent>
+#include <QBluetoothDeviceInfo>
+#include <QLowEnergyController>
+#include <QLowEnergyService>
+
+#include "BLEProthesisInterface.h"
+
+#include <QThread>
+
+
+class BLEProthesisInterfaceQtWorker : public QThread
+{
+    Q_OBJECT
+public:
+    using State = BLEProthesisInterface::State;
+    BLEProthesisInterfaceQtWorker(const QString& mac, BLEProthesisInterface& owner);
+    ~BLEProthesisInterfaceQtWorker();
+
+    void kill();
+    void sendCommand(const std::string& cmd);
+private:
+    void cleanup();
+protected:
+    void timerEvent(QTimerEvent *event) override;
+signals:
+    void resultReady(const QString &s);
+private slots:
+    //dev discover
+    void deviceDiscovered(const QBluetoothDeviceInfo &device);
+    void deviceDiscoverFinished();
+    void deviceDiscoverError(QBluetoothDeviceDiscoveryAgent::Error);
+    void deviceConnected();
+    void deviceDisconnected();
+    //service discovery
+    void serviceDiscovered(const QBluetoothUuid &gatt);
+    void serviceDiscoverFinished();
+    void controllerError(QLowEnergyController::Error error);
+    //communication
+    void receiveDeviceDisconnec(const QLowEnergyDescriptor &d, const QByteArray &value);
+
+    void serviceStateChanged(QLowEnergyService::ServiceState s);
+    void readData(const QLowEnergyCharacteristic &c,const QByteArray &value);
+private:
+//    friend class BLEProthesisInterface;
+    BLEProthesisInterface*              _owner;
+    QString                             _mac;
+
+    //management (sensor/control)
+    int                                 _timer{-1};
+    std::atomic_bool                    _killed{false};
+
+    enum class SensorValue
+    {
+        ThumbPWM  = 0,
+        ThumbPos  = 1,
+        FingerPWM = 2,
+        FingerPos = 3
+    };
+    SensorValue                         _nextSensorValue{SensorValue::ThumbPos};
+    QString                             _valueAkk;
+
+    QString                             _cmd;
+    std::mutex                          _cmdMutex;
+    //ble discover
+    QBluetoothDeviceDiscoveryAgent*     _deviceDiscoveryAgent{nullptr};
+    QBluetoothDeviceInfo                _currentDevice;
+    bool                                _deviceDiscovered{false};
+    //ble dev
+    QLowEnergyController*               _control{nullptr};
+    //ble ser
+    bool                                _serviceDiscovered{false};
+    QLowEnergyDescriptor                _notificationDescTx;
+    QLowEnergyService*                  _service{nullptr};
+};

source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorkerThread.cpp

+#include "BLEProthesisInterfaceQtWorkerThread.h"
+
+
+BLEProthesisInterfaceQtWorkerThread::BLEProthesisInterfaceQtWorkerThread(const std::string &mac, BLEProthesisInterface &owner) :
+    _owner{&owner},
+    _mac{QString::fromStdString(mac)}
+{}
+
+BLEProthesisInterfaceQtWorkerThread::~BLEProthesisInterfaceQtWorkerThread()
+{}
+
+void BLEProthesisInterfaceQtWorkerThread::kill()
+{
+    _worker->kill();
+}
+
+void BLEProthesisInterfaceQtWorkerThread::sendCommand(const std::string &cmd)
+{
+    _worker->sendCommand(cmd);
+}
+
+void BLEProthesisInterfaceQtWorkerThread::run()
+{
+    _worker = new BLEProthesisInterfaceQtWorker{_mac, *_owner};
+    qDebug() << '[' << _mac << ']' << " Starting qt event loop.";
+    int r = exec();
+    if(_worker)
+    {
+        _worker->kill();
+        delete _worker;
+    }
+    qDebug() << '[' << _mac << ']' << " Sopped qt event loop -> " << r;
+}

source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorkerThread.h

+#pragma once
+
+#include <mutex>
+
+#include <QBluetoothDeviceDiscoveryAgent>
+#include <QBluetoothDeviceInfo>
+#include <QLowEnergyController>
+#include <QLowEnergyService>
+
+#include "BLEProthesisInterface.h"
+#include "BLEProthesisInterfaceQtWorker.h"
+
+#include <QThread>
+
+
+class BLEProthesisInterfaceQtWorkerThread : public QThread
+{
+    Q_OBJECT
+public:
+    BLEProthesisInterfaceQtWorkerThread(const std::string& mac, BLEProthesisInterface& owner);
+    ~BLEProthesisInterfaceQtWorkerThread();
+    void kill();
+    void sendCommand(const std::string& cmd);
+private:
+    void run() override;
+private:
+    BLEProthesisInterface*              _owner{nullptr};
+    QString                             _mac;
+    BLEProthesisInterfaceQtWorker*      _worker{nullptr};
+};

source/RobotAPI/drivers/KITProstheticHandDriver/CMakeLists.txt

+#requires the Packages libqt5bluetooth5 qtconnectivity5-dev
+set(LIB_NAME    KITProstheticHandDriver)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+find_package(Qt5 COMPONENTS Core Bluetooth QUIET)
+
+set(CMAKE_AUTOMOC ON)
+
+armarx_build_if(Qt5_FOUND "Qt5 Core or Bluetooth not available")
+
+set(LIBS
+    ${Qt5Core_LIBRARIES}
+    ${Qt5Bluetooth_LIBRARIES}
+)
+
+include_directories(SYSTEM ${Qt5Core_INCLUDE_DIRS})
+include_directories(SYSTEM ${Qt5Bluetooth_INCLUDE_DIRS})
+
+set(LIB_FILES
+    BLEProthesisInterface.cpp
+    BLEProthesisInterfaceQtWorker.cpp
+    BLEProthesisInterfaceQtWorkerThread.cpp
+)
+
+set(LIB_HEADERS
+    BLEProthesisInterface.h
+    BLEProthesisInterfaceQtWorker.h
+    BLEProthesisInterfaceQtWorkerThread.h
+)
+
+armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+
+add_subdirectory(example)

source/RobotAPI/drivers/KITProstheticHandDriver/example/CMakeLists.txt

+armarx_component_set_name("KITProstheticHandDriverExampleNoArmarX")
+
+set(COMPONENT_LIBS
+    KITProstheticHandDriver
+)
+
+set(EXE_SOURCE KITProstheticHandDriverExampleNoArmarX.cpp)
+
+armarx_add_component_executable("${EXE_SOURCE}")

source/RobotAPI/drivers/KITProstheticHandDriver/example/KITProstheticHandDriverExampleNoArmarX.cpp

+#include <QCoreApplication>
+
+#include <RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.h>
+
+#include <iostream>
+#include <thread>
+
+int main(int argc, char *argv[])
+{
+    QCoreApplication a(argc, argv);
+    {
+        BLEProthesisInterface iface{"DF:70:E8:81:DB:D6"};
+        std::size_t dcCounter = 0;
+        while(iface.getState() != BLEProthesisInterface::State::Running)
+        {
+            std::cout << "Waiting for Running State: " << to_string(iface.getState()) << std::endl;
+
+            if(iface.getState() == BLEProthesisInterface::State::Disconnected)
+            {
+                ++dcCounter;
+            }
+
+            if(dcCounter > 50)
+            {
+                return 1;
+            }
+
+            std::this_thread::sleep_for(std::chrono::milliseconds{100});
+        }
+
+        for(std::size_t i = 0; i < 10; ++i)
+        {
+            iface.sendGrasp(i%2);
+            std::cout << iface.getThumbPos() << "\t"
+                      << iface.getThumbPWM() << "\t"
+                      << iface.getFingerPos() << "\t"
+                      << iface.getFingerPWM() << std::endl;
+            std::this_thread::sleep_for(std::chrono::milliseconds{2500});
+        }
+        iface.sendThumbPWM(200, -2999, 0);
+        iface.sendFingerPWM(200, -2999, 0);
+        std::this_thread::sleep_for(std::chrono::milliseconds{2500});
+    }
+    return 0;
+}
+