#include <cmath>
#include <algorithm>
#include <iomanip>
#include <iostream>
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 << "Copyright (C) 2001-2023 Force Dimension" << std::endl;
std::cout << "All Rights Reserved." << std::endl << std::endl;
{
std::cout <<
"error: failed to open device (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
{
std::cout << "exiting..." << std::endl;
return -1;
}
std::cout <<
dhdGetSystemName() <<
" device detected" << std::endl << std::endl;
{
std::cout <<
"error: failed to initialize device (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
{
std::cout <<
"error: failed to start robotic regulation (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
double positionCenter[
DHD_MAX_DOF] = {};
{
std::cout <<
"error: failed to move the device (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
{
std::cout <<
"error: failed to disable integral term (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
std::cout << "press 'z' to zero the sensor" << std::endl;
std::cout << " 'k/K' to decrease/increase the sensor stiffness Kp" << std::endl;
std::cout << " 'q' to quit demo" << std::endl << std::endl;
const double MaxKp = 1.5 * Kp;
double fxOffset = 0.0;
double fyOffset = 0.0;
double fzOffset = 0.0;
std::cout << " Kp | force | fx fy fz" << std::endl;
std::cout << "------|-----------|----------------------" << std::endl;
bool running = true;
while (running)
{
double fx = 0.0;
double fy = 0.0;
double fz = 0.0;
{
std::cout <<
"error: failed to disable integral term (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
break;
}
double fxSensor = fx - fxOffset;
double fySensor = fy - fyOffset;
double fzSensor = fz - fzOffset;
double f = std::sqrt(fxSensor * fxSensor + fySensor * fySensor + fzSensor * fzSensor);
if (time - lastDisplayUpdateTime > 0.1)
{
lastDisplayUpdateTime = time;
std::cout << std::noshowpos << std::fixed << std::setprecision(2) << std::setw(5) << Kp << " | ";
std::cout << std::showpos << std::setprecision(2) << std::setw(5) << f << " [N] |";
std::cout << std::showpos << std::setprecision(2) << std::setw(6) << fxSensor << " " << fxSensor << " " << fzSensor << " [N] \r";
std::cout.flush();
}
{
std::cout << "error: regulation thread not running" << std::endl;
return -1;
}
{
{
case 'z':
{
fxOffset = fx;
fyOffset = fy;
fzOffset = fz;
break;
}
case 'k':
{
Kp = std::max(0.0, Kp - 0.1);
break;
}
case 'K':
{
Kp = std::min(MaxKp, Kp + 0.1);
break;
}
case 'q':
{
std::cout << std::endl << std::endl << "exiting at user's request" << std::endl;
running = false;
break;
}
}
{
std::cout <<
"error: failed to change sensor stiffness (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
break;
}
}
}
{
std::cout <<
"error: failed to close the connection (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
std::cout << "connection closed" << std::endl;
return 0;
}