From 38023712ae69ce8b32935503584434bde3669b29 Mon Sep 17 00:00:00 2001
From: Raphael Grimm <raphael.grimm@kit.edu>
Date: Wed, 28 Nov 2018 17:57:02 +0100
Subject: [PATCH] Add KITProstheticHandDriver and example
---
source/RobotAPI/drivers/CMakeLists.txt | 1 +
.../BLEProthesisInterface.cpp | 151 ++++++++
.../BLEProthesisInterface.h | 145 ++++++++
.../BLEProthesisInterfaceQtWorker.cpp | 325 ++++++++++++++++++
.../BLEProthesisInterfaceQtWorker.h | 78 +++++
.../BLEProthesisInterfaceQtWorkerThread.cpp | 33 ++
.../BLEProthesisInterfaceQtWorkerThread.h | 30 ++
.../KITProstheticHandDriver/CMakeLists.txt | 35 ++
.../example/CMakeLists.txt | 9 +
...KITProstheticHandDriverExampleNoArmarX.cpp | 46 +++
10 files changed, 853 insertions(+)
create mode 100644 source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.cpp
create mode 100644 source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.h
create mode 100644 source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorker.cpp
create mode 100644 source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorker.h
create mode 100644 source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorkerThread.cpp
create mode 100644 source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorkerThread.h
create mode 100644 source/RobotAPI/drivers/KITProstheticHandDriver/CMakeLists.txt
create mode 100644 source/RobotAPI/drivers/KITProstheticHandDriver/example/CMakeLists.txt
create mode 100644 source/RobotAPI/drivers/KITProstheticHandDriver/example/KITProstheticHandDriverExampleNoArmarX.cpp
diff --git a/source/RobotAPI/drivers/CMakeLists.txt b/source/RobotAPI/drivers/CMakeLists.txt
index 90331c6e0..c51339f3a 100644
--- a/source/RobotAPI/drivers/CMakeLists.txt
+++ b/source/RobotAPI/drivers/CMakeLists.txt
@@ -5,4 +5,5 @@ add_subdirectory(OptoForceUnit)
add_subdirectory(OrientedTactileSensor)
add_subdirectory(GamepadUnit)
add_subdirectory(MetaWearIMU)
+add_subdirectory(KITProstheticHandDriver)
diff --git a/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.cpp b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.cpp
new file mode 100644
index 000000000..90db3dadd
--- /dev/null
+++ b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.cpp
@@ -0,0 +1,151 @@
+#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());
+}
diff --git a/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.h b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.h
new file mode 100644
index 000000000..b3c3428a0
--- /dev/null
+++ b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.h
@@ -0,0 +1,145 @@
+#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};
+//};
diff --git a/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorker.cpp b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorker.cpp
new file mode 100644
index 000000000..046e5bdf0
--- /dev/null
+++ b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorker.cpp
@@ -0,0 +1,325 @@
+#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();
+}
diff --git a/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorker.h b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorker.h
new file mode 100644
index 000000000..322ab5a3f
--- /dev/null
+++ b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorker.h
@@ -0,0 +1,78 @@
+#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};
+};
diff --git a/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorkerThread.cpp b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorkerThread.cpp
new file mode 100644
index 000000000..5f665d077
--- /dev/null
+++ b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorkerThread.cpp
@@ -0,0 +1,33 @@
+#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;
+}
diff --git a/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorkerThread.h b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorkerThread.h
new file mode 100644
index 000000000..258194b17
--- /dev/null
+++ b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorkerThread.h
@@ -0,0 +1,30 @@
+#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};
+};
diff --git a/source/RobotAPI/drivers/KITProstheticHandDriver/CMakeLists.txt b/source/RobotAPI/drivers/KITProstheticHandDriver/CMakeLists.txt
new file mode 100644
index 000000000..e70462472
--- /dev/null
+++ b/source/RobotAPI/drivers/KITProstheticHandDriver/CMakeLists.txt
@@ -0,0 +1,35 @@
+#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)
diff --git a/source/RobotAPI/drivers/KITProstheticHandDriver/example/CMakeLists.txt b/source/RobotAPI/drivers/KITProstheticHandDriver/example/CMakeLists.txt
new file mode 100644
index 000000000..8d23177e3
--- /dev/null
+++ b/source/RobotAPI/drivers/KITProstheticHandDriver/example/CMakeLists.txt
@@ -0,0 +1,9 @@
+armarx_component_set_name("KITProstheticHandDriverExampleNoArmarX")
+
+set(COMPONENT_LIBS
+ KITProstheticHandDriver
+)
+
+set(EXE_SOURCE KITProstheticHandDriverExampleNoArmarX.cpp)
+
+armarx_add_component_executable("${EXE_SOURCE}")
diff --git a/source/RobotAPI/drivers/KITProstheticHandDriver/example/KITProstheticHandDriverExampleNoArmarX.cpp b/source/RobotAPI/drivers/KITProstheticHandDriver/example/KITProstheticHandDriverExampleNoArmarX.cpp
new file mode 100644
index 000000000..e544ffe12
--- /dev/null
+++ b/source/RobotAPI/drivers/KITProstheticHandDriver/example/KITProstheticHandDriverExampleNoArmarX.cpp
@@ -0,0 +1,46 @@
+#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;
+}
+
--
GitLab