diff --git a/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.cpp b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.cpp
index 1321cdb8d4e62d923397d34b8e3fddde938f80cf..9e1eff8bdd7f2353e54d460140917f24bdbd98fa 100644
--- a/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.cpp
+++ b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.cpp
@@ -70,7 +70,7 @@ void BLEProthesisInterface::sendGrasp(std::uint64_t n)
sendRaw("g" + std::to_string(n) + "\n");
}
-void BLEProthesisInterface::sendThumbPWM(uint64_t v, int64_t mxPWM, uint64_t pos)
+void BLEProthesisInterface::sendThumbPWM(uint64_t v, uint64_t mxPWM, uint64_t pos)
{
if (v < getMinV() || v > getMaxV())
{
@@ -83,15 +83,15 @@ void BLEProthesisInterface::sendThumbPWM(uint64_t v, int64_t mxPWM, uint64_t pos
", " + std::to_string(getMaxV()) + "]"
};
}
- if (mxPWM < getMinPWM() || mxPWM > getMaxPWM())
+ if (mxPWM > getMaxPWM())
{
throw std::invalid_argument
{
"sendThumbPWM( v = " + std::to_string(v) +
" , mxPWM = " + std::to_string(mxPWM) +
" , pos = " + std::to_string(pos) +
- " ): The interval for mxPWM is [" + std::to_string(getMinPWM()) +
- ", " + std::to_string(getMaxPWM()) + "]"
+ " ): The interval for mxPWM is [0, , " +
+ std::to_string(getMaxPWM()) + "]"
};
}
if (pos > getMaxPosThumb())
@@ -110,7 +110,7 @@ void BLEProthesisInterface::sendThumbPWM(uint64_t v, int64_t mxPWM, uint64_t pos
sendRaw(str.str());
}
-void BLEProthesisInterface::sendFingerPWM(uint64_t v, int64_t mxPWM, uint64_t pos)
+void BLEProthesisInterface::sendFingerPWM(uint64_t v, uint64_t mxPWM, uint64_t pos)
{
if (v < getMinV() || v > getMaxV())
{
@@ -123,15 +123,15 @@ void BLEProthesisInterface::sendFingerPWM(uint64_t v, int64_t mxPWM, uint64_t po
", " + std::to_string(getMaxV()) + "]"
};
}
- if (mxPWM < getMinPWM() || mxPWM > getMaxPWM())
+ if (mxPWM > getMaxPWM())
{
throw std::invalid_argument
{
"sendThumbPWM( v = " + std::to_string(v) +
" , mxPWM = " + std::to_string(mxPWM) +
" , pos = " + std::to_string(pos) +
- " ): The interval for mxPWM is [" + std::to_string(getMinPWM()) +
- ", " + std::to_string(getMaxPWM()) + "]"
+ " ): The interval for mxPWM is [0, , " +
+ std::to_string(getMaxPWM()) + "]"
};
}
if (pos > maxPosF())
diff --git a/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.h b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.h
index d8504bd32151b24c59e3113688911aad89230ff8..9daab0a2b38947e20c18f9a72abd90498ff30ee8 100644
--- a/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.h
+++ b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.h
@@ -36,8 +36,8 @@ public:
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);
+ 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);
private:
friend class BLEProthesisInterfaceQtWorker;
std::atomic_int64_t _thumbPWM{0};
@@ -60,11 +60,7 @@ public:
{
return 200;
}
- static constexpr std::int64_t getMinPWM()
- {
- return -2999;
- }
- static constexpr std::int64_t getMaxPWM()
+ static constexpr std::uint64_t getMaxPWM()
{
return 2999;
}
diff --git a/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorker.cpp b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorker.cpp
index 046e5bdf07e727726bc990d6fd22a08474a2806c..0d00d3c520e2072949cc8bdff766256e89438079 100644
--- a/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorker.cpp
+++ b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorker.cpp
@@ -5,7 +5,7 @@
#define RXUUID "6e400002-b5a3-f393-e0a9-e50e24dcca9e"
#define TXUUID "6e400003-b5a3-f393-e0a9-e50e24dcca9e"
-BLEProthesisInterfaceQtWorker::BLEProthesisInterfaceQtWorker(const QString& mac, BLEProthesisInterface &owner) :
+BLEProthesisInterfaceQtWorker::BLEProthesisInterfaceQtWorker(const QString& mac, BLEProthesisInterface& owner) :
_owner{&owner},
_mac{mac}
{
@@ -22,7 +22,7 @@ void BLEProthesisInterfaceQtWorker::kill()
_killed = true;
}
-void BLEProthesisInterfaceQtWorker::sendCommand(const std::string &cmd)
+void BLEProthesisInterfaceQtWorker::sendCommand(const std::string& cmd)
{
std::lock_guard guard(_cmdMutex);
_cmd += QString::fromStdString(cmd);
@@ -31,29 +31,29 @@ void BLEProthesisInterfaceQtWorker::sendCommand(const std::string &cmd)
void BLEProthesisInterfaceQtWorker::cleanup()
{
//stop services etc
- if(_timer != -1)
+ if (_timer != -1)
{
killTimer(_timer);
_timer = -1;
}
//disconnect
- if(_control)
+ if (_control)
{
_control->disconnectFromDevice();
}
//delete
- if(_service)
+ if (_service)
{
delete _service;
_service = nullptr;
}
- if(_control)
+ if (_control)
{
delete _control;
_control = nullptr;
}
- if(_deviceDiscoveryAgent)
+ if (_deviceDiscoveryAgent)
{
delete _deviceDiscoveryAgent;
_deviceDiscoveryAgent = nullptr;
@@ -62,7 +62,7 @@ void BLEProthesisInterfaceQtWorker::cleanup()
void BLEProthesisInterfaceQtWorker::timerEvent(QTimerEvent*)
{
- if(_killed)
+ if (_killed)
{
qDebug() << '[' << _mac << ']' << " Stopping NOW!";
cleanup();
@@ -70,7 +70,7 @@ void BLEProthesisInterfaceQtWorker::timerEvent(QTimerEvent*)
return;
}
//discovering
- if(_owner->_state == State::Created)
+ if (_owner->_state == State::Created)
{
_deviceDiscoveryAgent = new QBluetoothDeviceDiscoveryAgent;
connect(_deviceDiscoveryAgent, SIGNAL(deviceDiscovered(const QBluetoothDeviceInfo&)),
@@ -83,9 +83,9 @@ void BLEProthesisInterfaceQtWorker::timerEvent(QTimerEvent*)
_owner->_state = State::DiscoveringDevices;
_deviceDiscoveryAgent->start();
}
- else if(_owner->_state == State::DiscoveringDevicesDone)
+ else if (_owner->_state == State::DiscoveringDevicesDone)
{
- if(!_deviceDiscovered)
+ if (!_deviceDiscovered)
{
qDebug() << '[' << _mac << ']' << " Device discovering failed!";
kill();
@@ -94,14 +94,14 @@ void BLEProthesisInterfaceQtWorker::timerEvent(QTimerEvent*)
qDebug() << '[' << _mac << ']' << " State Disconnected";
_owner->_state = State::Disconnected;
}
- else if(_owner->_state == State::Disconnected)
+ else if (_owner->_state == State::Disconnected)
{
- if(_service)
+ if (_service)
{
delete _service;
_service = nullptr;
}
- if(_control)
+ if (_control)
{
delete _control;
_control = nullptr;
@@ -124,15 +124,15 @@ void BLEProthesisInterfaceQtWorker::timerEvent(QTimerEvent*)
qDebug() << '[' << _mac << ']' << " State Connecting";
_owner->_state = State::Connecting;
}
- else if(_owner->_state == State::ConnectingDone)
+ else if (_owner->_state == State::ConnectingDone)
{
_control->discoverServices();
qDebug() << '[' << _mac << ']' << " State DiscoveringServices";
_owner->_state = State::DiscoveringServices;
}
- else if(_owner->_state == State::DiscoveringServicesDone)
+ else if (_owner->_state == State::DiscoveringServicesDone)
{
- if(!_serviceDiscovered)
+ if (!_serviceDiscovered)
{
qDebug() << '[' << _mac << ']' << " Service discovering failed!";
kill();
@@ -142,10 +142,10 @@ void BLEProthesisInterfaceQtWorker::timerEvent(QTimerEvent*)
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)));
+ connect(_service, SIGNAL(characteristicChanged(QLowEnergyCharacteristic, QByteArray)),
+ this, SLOT(readData(QLowEnergyCharacteristic, QByteArray)));
+ connect(_service, SIGNAL(descriptorWritten(QLowEnergyDescriptor, QByteArray)),
+ this, SLOT(receiveDeviceDisconnec(QLowEnergyDescriptor, QByteArray)));
_service->discoverDetails();
@@ -155,7 +155,7 @@ void BLEProthesisInterfaceQtWorker::timerEvent(QTimerEvent*)
else if (_owner->_state == State::Running) //send data
{
std::lock_guard g(_cmdMutex);
- if(_service && _cmd.size() != 0)
+ if (_service && _cmd.size() != 0)
{
const QLowEnergyCharacteristic RxChar = _service->characteristic(QBluetoothUuid(QUuid(RXUUID)));
QByteArray data;
@@ -167,7 +167,7 @@ void BLEProthesisInterfaceQtWorker::timerEvent(QTimerEvent*)
}
}
-void BLEProthesisInterfaceQtWorker::deviceDiscovered(const QBluetoothDeviceInfo &device)
+void BLEProthesisInterfaceQtWorker::deviceDiscovered(const QBluetoothDeviceInfo& device)
{
if (device.address().toString() == _mac)
{
@@ -207,13 +207,13 @@ void BLEProthesisInterfaceQtWorker::deviceDiscoverError(QBluetoothDeviceDiscover
kill();
}
-void BLEProthesisInterfaceQtWorker::serviceDiscovered(const QBluetoothUuid &gatt)
+void BLEProthesisInterfaceQtWorker::serviceDiscovered(const QBluetoothUuid& gatt)
{
qDebug() << '[' << _mac << ']' << " Discovered service " << gatt.toString();
- if(gatt==QBluetoothUuid(QUuid(UARTSERVICEUUID)))
+ if (gatt == QBluetoothUuid(QUuid(UARTSERVICEUUID)))
{
qDebug() << '[' << _mac << ']' << "Discovered UART service " << gatt.toString();
- _serviceDiscovered =true;
+ _serviceDiscovered = true;
_owner->_state = State::DiscoveringServicesDone;
}
}
@@ -231,14 +231,14 @@ void BLEProthesisInterfaceQtWorker::controllerError(QLowEnergyController::Error
kill();
}
-void BLEProthesisInterfaceQtWorker::receiveDeviceDisconnec(const QLowEnergyDescriptor &d, const QByteArray &value)
+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)
+ if (_service)
{
delete _service;
_service = nullptr;
@@ -285,7 +285,7 @@ void BLEProthesisInterfaceQtWorker::serviceStateChanged(QLowEnergyService::Servi
}
break;
default:
- break;
+ break;
}
}
@@ -301,15 +301,17 @@ void BLEProthesisInterfaceQtWorker::deviceDisconnected()
_owner->_state = State::Disconnected;
}
-void BLEProthesisInterfaceQtWorker::readData(const QLowEnergyCharacteristic &c, const QByteArray &value)
+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'))
+ if (!_valueAkk.contains('\n'))
{
return;
}
diff --git a/source/RobotAPI/drivers/KITProstheticHandDriver/example/KITProstheticHandDriverExampleNoArmarX.cpp b/source/RobotAPI/drivers/KITProstheticHandDriver/example/KITProstheticHandDriverExampleNoArmarX.cpp
index e544ffe128b2960d0e17beb51fd44bcfdbe64873..7bd337ded20e5fb03b9dff2308d54e9d0be2461c 100644
--- a/source/RobotAPI/drivers/KITProstheticHandDriver/example/KITProstheticHandDriverExampleNoArmarX.cpp
+++ b/source/RobotAPI/drivers/KITProstheticHandDriver/example/KITProstheticHandDriverExampleNoArmarX.cpp
@@ -5,22 +5,22 @@
#include <iostream>
#include <thread>
-int main(int argc, char *argv[])
+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)
+ while (iface.getState() != BLEProthesisInterface::State::Running)
{
std::cout << "Waiting for Running State: " << to_string(iface.getState()) << std::endl;
- if(iface.getState() == BLEProthesisInterface::State::Disconnected)
+ if (iface.getState() == BLEProthesisInterface::State::Disconnected)
{
++dcCounter;
}
- if(dcCounter > 50)
+ if (dcCounter > 50)
{
return 1;
}
@@ -28,18 +28,27 @@ int main(int argc, char *argv[])
std::this_thread::sleep_for(std::chrono::milliseconds{100});
}
- for(std::size_t i = 0; i < 10; ++i)
+ auto printAndSleep = [&]
{
- 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});
+ for (std::size_t i2 = 0; i2 < 25; ++i2)
+ {
+ std::cout << iface.getThumbPos() << "\t"
+ << iface.getThumbPWM() << "\t"
+ << iface.getFingerPos() << "\t"
+ << iface.getFingerPWM() << std::endl;
+ std::this_thread::sleep_for(std::chrono::milliseconds{100});
+ }
+ };
+
+ for (std::size_t i = 0; i < 4; ++i)
+ {
+ iface.sendGrasp(i % 2);
+ printAndSleep();
}
- iface.sendThumbPWM(200, -2999, 0);
- iface.sendFingerPWM(200, -2999, 0);
- std::this_thread::sleep_for(std::chrono::milliseconds{2500});
+ iface.sendThumbPWM(200, 2999, 0);
+ printAndSleep();
+ iface.sendFingerPWM(200, 2999, 0);
+ printAndSleep();
}
return 0;
}