#include <atomic>
#include <iomanip>
#include <iostream>
#include <string>
#include <sstream>
#include <vector>
#include "dhdc.h"
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)
{
if (
dhdSetForceAndTorqueAndGripperForce (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, deviceId) <
DHD_NO_ERROR)
{
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);
if (
dhdStartThread(gravityCompensationThread, &(deviceIdsList.back()),
DHD_THREAD_PRIORITY_HIGH) < 0)
{
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;
}
int __SDK dhdOpenID(char ID)
int __SDK dhdEnableForce(uchar val, char ID=-1)
int __SDK dhdGetSerialNumber(ushort *sn, char ID=-1)
int __SDK dhdClose(char ID=-1)
int __SDK dhdStartThread(void *func(void *), void *arg, int priority)
void __SDK dhdSleep(double sec)
int __SDK dhdGetDeviceCount()
const char *__SDK dhdGetSDKVersionStr()
const char *__SDK dhdErrorGetLastStr()
const char *__SDK dhdGetSystemName(char ID=-1)