#include <atomic>
#include <iomanip>
#include <iostream>
#include <string>
#include <sstream>
#include <vector>
std::atomic<bool> s_running { true };
std::atomic<size_t> s_runningThreadsCount { 0 };
void* gravityCompensationThread(void* a_arg)
{
s_runningThreadsCount += 1;
int deviceId = *(reinterpret_cast<int*>(a_arg));
{
std::cout <<
"error: failed to enable force rendering (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
s_running = false;
}
while (s_running)
{
{
std::cout <<
"error: failed to set force on device id " << deviceId <<
" (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
s_running = false;
break;
}
}
{
std::cout <<
"warning: failed to close the connection on device id " << deviceId <<
"(" <<
dhdErrorGetLastStr() <<
")" << std::endl;
}
s_runningThreadsCount -= 1;
return nullptr;
}
int main(int argc,
char* argv[])
{
std::cout.rdbuf()->pubsetbuf(nullptr, 512);
std::cout << std::nounitbuf;
std::ios_base::sync_with_stdio(false);
std::cout <<
"Multithreaded Gravity Compensation Example " <<
dhdGetSDKVersionStr() << std::endl;
std::cout << "Copyright (C) 2001-2023 Force Dimension" << std::endl;
std::cout << "All Rights Reserved." << std::endl << std::endl;
if (deviceCount < 1)
{
return -1;
}
std::cout << deviceCount << " device(s) detected" << std::endl;
std::vector<int> deviceIdsList;
for (int index = 0; index < deviceCount; index++ )
{
if (deviceId >= 0)
{
uint16_t serialNumber = 0;
std::string serialNumberString;
{
std::ostringstream serialNumberStringStream;
serialNumberStringStream << "S/N " << serialNumber;
serialNumberString = serialNumberStringStream.str();
}
std::cout <<
"starting gravity compensation thread for " <<
dhdGetSystemName() <<
" " << serialNumberString << std::endl;
deviceIdsList.push_back(deviceId);
{
return -1;
}
}
}
while (s_runningThreadsCount < deviceIdsList.size())
{
if (!s_running)
{
std::cout << "error: at least one thread quit with an error" << std::endl;
return -1;
}
}
std::cout << "press 'q' to quit" << std::endl << std::endl;
while (s_running)
{
{
{
case 'q':
{
s_running = false;
break;
}
}
}
}
while (s_runningThreadsCount > 0)
{
}
std::cout << "all threads finished" << std::endl;
return 0;
}
char __SDK dhdKbGet()
Definition: dhdc.cpp:4743
int __SDK dhdSetForceAndTorqueAndGripperForce(double fx, double fy, double fz, double tx, double ty, double tz, double fg, char ID=-1)
Definition: dhdc.cpp:2040
int __SDK dhdOpenID(char ID)
Definition: dhdc.cpp:411
int __SDK dhdEnableForce(uchar val, char ID=-1)
Definition: dhdc.cpp:4378
int __SDK dhdGetSerialNumber(ushort *sn, char ID=-1)
Definition: dhdc.cpp:540
int __SDK dhdClose(char ID=-1)
Definition: dhdc.cpp:443
int __SDK dhdStartThread(void *func(void *), void *arg, int priority)
Definition: dhdc.cpp:4778
void __SDK dhdSleep(double sec)
Definition: dhdc.cpp:4754
int __SDK dhdGetDeviceCount()
Definition: dhdc.cpp:226
@ DHD_NO_ERROR
Definition: dhdc.h:52
#define DHD_THREAD_PRIORITY_HIGH
Definition: dhdc.h:210
bool __SDK dhdKbHit()
Definition: dhdc.cpp:4731
const char *__SDK dhdGetSDKVersionStr()
Definition: dhdc.cpp:925
#define DHD_ON
Definition: dhdc.h:128
const char *__SDK dhdGetSystemName(char ID=-1)
Definition: dhdc.cpp:814
const char *__SDK dhdErrorGetLastStr()
Definition: dhdError.cpp:197