diff --git a/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.cpp b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.cpp
index 9e1eff8bdd7f2353e54d460140917f24bdbd98fa..e8c024398fb5aa539b09cfa043fd28dea1e14cc3 100644
--- a/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.cpp
+++ b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.cpp
@@ -3,8 +3,9 @@
#include "BLEProthesisInterface.h"
#include "BLEProthesisInterfaceQtWorkerThread.h"
-BLEProthesisInterface::BLEProthesisInterface(const std::string& mac) :
- _worker{new BLEProthesisInterfaceQtWorkerThread{mac, *this}}
+BLEProthesisInterface::BLEProthesisInterface(const std::string& mac, SensorValueProtocol protocol) :
+ _worker{new BLEProthesisInterfaceQtWorkerThread{mac, *this}},
+ _protocol{protocol}
{
_worker->start();
}
@@ -149,3 +150,43 @@ void BLEProthesisInterface::sendFingerPWM(uint64_t v, uint64_t mxPWM, uint64_t p
str << 'M' << 3 << ',' << v << ',' << mxPWM << ',' << pos << '\n';
sendRaw(str.str());
}
+
+void BLEProthesisInterface::verboseReceive(bool b)
+{
+ _verboseReceive = b;
+}
+
+void BLEProthesisInterface::verboseSend(bool b)
+{
+ _verboseSend = b;
+}
+
+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));
+}
diff --git a/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.h b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.h
index 9daab0a2b38947e20c18f9a72abd90498ff30ee8..cf96515661f2e6497c1de67386010d0563fb83ac 100644
--- a/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.h
+++ b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.h
@@ -10,8 +10,6 @@ class BLEProthesisInterfaceQtWorker;
class BLEProthesisInterface
{
public:
- BLEProthesisInterface(const std::string& mac);
- ~BLEProthesisInterface();
enum class State
{
Created,
@@ -26,6 +24,15 @@ public:
Running,
Killed
};
+ enum class SensorValueProtocol
+ {
+ tpos_tpwm_fpos_fpwm,
+ mx_pos_pwm
+ };
+public:
+ BLEProthesisInterface(const std::string& mac, SensorValueProtocol protocol = SensorValueProtocol::mx_pos_pwm);
+ ~BLEProthesisInterface();
+
std::int64_t getThumbPWM() const;
std::int64_t getThumbPos() const;
std::int64_t getFingerPWM() const;
@@ -38,15 +45,23 @@ public:
void sendGrasp(std::uint64_t n);
void sendThumbPWM(std::uint64_t v, std::uint64_t maxPWM, std::uint64_t pos);
void sendFingerPWM(std::uint64_t v, std::uint64_t maxPWM, std::uint64_t pos);
+
+ void verboseReceive(bool b = true);
+ void verboseSend(bool b = true);
private:
friend class BLEProthesisInterfaceQtWorker;
+ //sensor values
std::atomic_int64_t _thumbPWM{0};
std::atomic_int64_t _thumbPos{0};
std::atomic_int64_t _fingerPWM{0};
std::atomic_int64_t _fingerPos{0};
+ //management
std::unique_ptr<BLEProthesisInterfaceQtWorkerThread> _worker;
std::atomic<State> _state{State::Created};
+ std::atomic_bool _verboseReceive{false};
+ std::atomic_bool _verboseSend{true};
+ SensorValueProtocol _protocol;
public:
static constexpr std::uint64_t getMaxG()
{
@@ -74,100 +89,4 @@ public:
}
};
-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};
-//};
+std::string to_string(BLEProthesisInterface::State s);
diff --git a/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorker.cpp b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorker.cpp
index 0d00d3c520e2072949cc8bdff766256e89438079..adc9d2a8083de7461205cbdcad4f8cc3a1c6577b 100644
--- a/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorker.cpp
+++ b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorker.cpp
@@ -1,5 +1,6 @@
#include "BLEProthesisInterfaceQtWorker.h"
+#include <QRegularExpression>
#define UARTSERVICEUUID "6e400001-b5a3-f393-e0a9-e50e24dcca9e"
#define RXUUID "6e400002-b5a3-f393-e0a9-e50e24dcca9e"
@@ -161,7 +162,10 @@ void BLEProthesisInterfaceQtWorker::timerEvent(QTimerEvent*)
QByteArray data;
data.append(_cmd);
_service->writeCharacteristic(RxChar, data, QLowEnergyService::WriteWithoutResponse);
- qDebug() << '[' << _mac << ']' << " send: " << _cmd;
+ if (_owner->_verboseSend)
+ {
+ qDebug() << '[' << _mac << ']' << " send: " << _cmd;
+ }
_cmd.clear();
}
}
@@ -301,23 +305,26 @@ void BLEProthesisInterfaceQtWorker::deviceDisconnected()
_owner->_state = State::Disconnected;
}
-void BLEProthesisInterfaceQtWorker::readData(const QLowEnergyCharacteristic& c, const QByteArray& value)
+
+template<>
+void BLEProthesisInterfaceQtWorker::consumeData<BLEProthesisInterface::SensorValueProtocol::tpos_tpwm_fpos_fpwm>()
{
- // ignore any other characteristic change
- if (c.uuid() != QBluetoothUuid(QUuid(TXUUID)))
+ if (!_valueAkk.contains('\n'))
{
+ if (_owner->_verboseReceive)
+ {
+ qDebug() << '[' << _mac << ']' << " data does not contain \\n -> no new sensor values\n Buffer:\n" << _valueAkk;
+ }
return;
}
- //qDebug() << '[' << _mac << ']' << " received : " << value;
+ auto listPacks = _valueAkk.split('\n');
- _valueAkk += QString{value};
- if (!_valueAkk.contains('\n'))
+ if (_owner->_verboseReceive)
{
- return;
+ qDebug() << '[' << _mac << ']' << " parsing " << listPacks.at(listPacks.size() - 2);
}
- auto listPacks = _valueAkk.split('\n');
- auto listVals = listPacks.at(listPacks.size() - 2).split(' ');
+ 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();
@@ -325,3 +332,80 @@ void BLEProthesisInterfaceQtWorker::readData(const QLowEnergyCharacteristic& c,
_valueAkk = listPacks.back();
}
+
+template<>
+void BLEProthesisInterfaceQtWorker::consumeData<BLEProthesisInterface::SensorValueProtocol::mx_pos_pwm>()
+{
+ if (!_valueAkk.contains('\n'))
+ {
+ if (_owner->_verboseReceive)
+ {
+ qDebug() << '[' << _mac << ']' << " data does not contain \\n -> no new sensor values\n Buffer:\n" << _valueAkk;
+ }
+ return;
+ }
+ auto listPacks = _valueAkk.split('\n');
+
+ static const QRegularExpression m2(R"(^M2:[ \t]+Pos.:[ \t]+(-?[1-9][0-9]*)[ \t]+PWM:[ \t]+(-?[1-9][0-9]*)[ \t]+$)");
+ static const QRegularExpression m3(R"(^M3:[ \t]+Pos.:[ \t]+(-?[1-9][0-9]*)[ \t]+PWM:[ \t]+(-?[1-9][0-9]*)[ \t]+$)");
+
+ for (int i = 0; i < listPacks.size() - 1; ++i)
+ {
+ if (listPacks.at(i).size() == 0)
+ {
+ continue;
+ }
+ if (_owner->_verboseReceive)
+ {
+ qDebug() << '[' << _mac << ']' << " parsing " << listPacks.at(i);
+ }
+ if (const auto matchM2 = m2.match(listPacks.at(i)); matchM2.hasMatch())
+ {
+ _owner->_thumbPos = matchM2.captured(1).toLong();
+ _owner->_thumbPWM = matchM2.captured(2).toLong();
+ if (_owner->_verboseReceive)
+ {
+ qDebug() << '[' << _mac << ']' << " updated M2";
+ }
+ }
+ else if (const auto matchM3 = m3.match(listPacks.at(i)); matchM3.hasMatch())
+ {
+ _owner->_fingerPos = matchM3.captured(1).toLong();
+ _owner->_fingerPWM = matchM3.captured(2).toLong();
+ if (_owner->_verboseReceive)
+ {
+ qDebug() << '[' << _mac << ']' << " updated M3";
+ }
+ }
+ else
+ {
+ qWarning() << "unknown format for data: " << listPacks.at(i) << "\nSkipping";
+ }
+ }
+ _valueAkk = listPacks.back();
+}
+
+void BLEProthesisInterfaceQtWorker::readData(const QLowEnergyCharacteristic& c, const QByteArray& value)
+{
+ // ignore any other characteristic change
+ if (c.uuid() != QBluetoothUuid(QUuid(TXUUID)))
+ {
+ return;
+ }
+ if (_owner->_verboseReceive)
+ {
+ qDebug() << '[' << _mac << ']' << " received : " << value;
+ }
+
+ _valueAkk.append(value.data());
+
+ switch (_owner->_protocol)
+ {
+ case BLEProthesisInterface::SensorValueProtocol::tpos_tpwm_fpos_fpwm:
+ consumeData<BLEProthesisInterface::SensorValueProtocol::tpos_tpwm_fpos_fpwm>();
+ break;
+ case BLEProthesisInterface::SensorValueProtocol::mx_pos_pwm:
+ consumeData<BLEProthesisInterface::SensorValueProtocol::mx_pos_pwm>();
+ break;
+ }
+}
diff --git a/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorker.h b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorker.h
index f4b84d716b238e30127d9253e5ab13b93147b834..ce14c53a2c4e987e6c5cfb3a5e1faed21616b06e 100644
--- a/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorker.h
+++ b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorker.h
@@ -45,7 +45,8 @@ private slots:
void serviceStateChanged(QLowEnergyService::ServiceState s);
void readData(const QLowEnergyCharacteristic& c, const QByteArray& value);
private:
- // friend class BLEProthesisInterface;
+ template<BLEProthesisInterface::SensorValueProtocol P> void consumeData();
+
BLEProthesisInterface* _owner;
QString _mac;
@@ -53,14 +54,6 @@ private:
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;
diff --git a/source/RobotAPI/drivers/KITProstheticHandDriver/example/KITProstheticHandDriverExampleNoArmarX.cpp b/source/RobotAPI/drivers/KITProstheticHandDriver/example/KITProstheticHandDriverExampleNoArmarX.cpp
index 7bd337ded20e5fb03b9dff2308d54e9d0be2461c..3b04f047f8524aa3e8a71733e92bdc6b7de1626f 100644
--- a/source/RobotAPI/drivers/KITProstheticHandDriver/example/KITProstheticHandDriverExampleNoArmarX.cpp
+++ b/source/RobotAPI/drivers/KITProstheticHandDriver/example/KITProstheticHandDriverExampleNoArmarX.cpp
@@ -5,11 +5,17 @@
#include <iostream>
#include <thread>
+[[maybe_unused]] static constexpr auto old = "DF:70:E8:81:DB:D6";
+[[maybe_unused]] static constexpr auto dlr = "DF:A2:F5:48:E7:50";
+[[maybe_unused]] static constexpr auto arches = "XX:XX:XX:XX:XX:XX";
+
int main(int argc, char* argv[])
{
QCoreApplication a(argc, argv);
{
- BLEProthesisInterface iface{"DF:70:E8:81:DB:D6"};
+ BLEProthesisInterface iface{dlr};
+ iface.verboseReceive(false);
+ iface.verboseSend(true);
std::size_t dcCounter = 0;
while (iface.getState() != BLEProthesisInterface::State::Running)
{
@@ -28,9 +34,9 @@ int main(int argc, char* argv[])
std::this_thread::sleep_for(std::chrono::milliseconds{100});
}
- auto printAndSleep = [&]
+ auto printAndSleep = [&](std::size_t n = 25)
{
- for (std::size_t i2 = 0; i2 < 25; ++i2)
+ for (std::size_t i2 = 0; i2 < n; ++i2)
{
std::cout << iface.getThumbPos() << "\t"
<< iface.getThumbPWM() << "\t"
@@ -46,7 +52,7 @@ int main(int argc, char* argv[])
printAndSleep();
}
iface.sendThumbPWM(200, 2999, 0);
- printAndSleep();
+ printAndSleep(1);
iface.sendFingerPWM(200, 2999, 0);
printAndSleep();
}