From e55e84a80c0087f307d5ab5e8893bc3401175083 Mon Sep 17 00:00:00 2001
From: Raphael Grimm <raphael.grimm@kit.edu>
Date: Fri, 14 Dec 2018 09:48:03 +0100
Subject: [PATCH] Add verbosity and sensor value protocol options to
BLEProthesisInterface
---
.../BLEProthesisInterface.cpp | 16 ++-
.../BLEProthesisInterface.h | 19 +++-
.../BLEProthesisInterfaceQtWorker.cpp | 104 ++++++++++++++++--
.../BLEProthesisInterfaceQtWorker.h | 11 +-
...KITProstheticHandDriverExampleNoArmarX.cpp | 14 ++-
5 files changed, 137 insertions(+), 27 deletions(-)
diff --git a/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.cpp b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.cpp
index 03112b769..e8c024398 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,6 +150,17 @@ 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)
diff --git a/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.h b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.h
index eb02ffbea..cf9651566 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()
{
diff --git a/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorker.cpp b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorker.cpp
index 0d00d3c52..adc9d2a80 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 f4b84d716..ce14c53a2 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 7bd337ded..3b04f047f 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();
}
--
GitLab