void CybPhantom::showDeviceInformation()
{
cout<<"Used Device: "<<hdGetString(HD_DEVICE_MODEL_TYPE)<<endl;
cout<<"Version Driver: "<<hdGetString(HD_DEVICE_DRIVER_VERSION)<<endl;
cout<<"Device Vendor: "<<hdGetString(HD_DEVICE_VENDOR)<<endl;
cout<<"Serial Number: "<<hdGetString(HD_DEVICE_SERIAL_NUMBER)<<endl;
}
void Phantom::Open() {
HDErrorInfo error;
_Device = hdInitDevice(HD_DEFAULT_DEVICE);
if (HD_DEVICE_ERROR(error = hdGetError()))
{
//hduPrintError(stderr, &error, "Failed to initialize haptic device");
cout<< "Failed to initialize haptic device"<< endl; //: %s", &error);
return;
}
printf("Found %s.\n\n", hdGetString(HD_DEVICE_MODEL_TYPE));
hdEnable(HD_FORCE_OUTPUT);
}
/******************************************************************************
Main function.
******************************************************************************/
int main(int argc, char* argv[])
{
HDErrorInfo error;
printf("Starting application\n");
atexit(exitHandler);
// Initialize the device. This needs to be called before any other
// actions on the device are performed.
ghHD = hdInitDevice(HD_DEFAULT_DEVICE);
if (HD_DEVICE_ERROR(error = hdGetError()))
{
hduPrintError(stderr, &error, "Failed to initialize haptic device");
fprintf(stderr, "\nPress any key to quit.\n");
getchar();
exit(-1);
}
printf("Found device %s\n",hdGetString(HD_DEVICE_MODEL_TYPE));
hdEnable(HD_FORCE_OUTPUT);
hdEnable(HD_MAX_FORCE_CLAMPING);
hdStartScheduler();
if (HD_DEVICE_ERROR(error = hdGetError()))
{
hduPrintError(stderr, &error, "Failed to start scheduler");
fprintf(stderr, "\nPress any key to quit.\n");
getchar();
exit(-1);
}
initGlut(argc, argv);
// Get the workspace dimensions.
HDdouble maxWorkspace[6];
hdGetDoublev(HD_MAX_WORKSPACE_DIMENSIONS, maxWorkspace);
// Low/left/back point of device workspace.
hduVector3Dd LLB(maxWorkspace[0], maxWorkspace[1], maxWorkspace[2]);
// Top/right/front point of device workspace.
hduVector3Dd TRF(maxWorkspace[3], maxWorkspace[4], maxWorkspace[5]);
initGraphics(LLB, TRF);
// Application loop.
CoulombForceField();
printf("Done\n");
return 0;
}
int main(int argc, char** argv) {
////////////////////////////////////////////////////////////////
// Init Phantom
////////////////////////////////////////////////////////////////
HDErrorInfo error;
HHD hHD;
hHD = hdInitDevice(HD_DEFAULT_DEVICE);
if (HD_DEVICE_ERROR(error = hdGetError())) {
//hduPrintError(stderr, &error, "Failed to initialize haptic device");
ROS_ERROR("Failed to initialize haptic device"); //: %s", &error);
return -1;
}
ROS_INFO("Found %s.", hdGetString(HD_DEVICE_MODEL_TYPE));
hdEnable(HD_FORCE_OUTPUT);
hdStartScheduler();
if (HD_DEVICE_ERROR(error = hdGetError())) {
ROS_ERROR("Failed to start the scheduler"); //, &error);
return -1;
}
HHD_Auto_Calibration();
////////////////////////////////////////////////////////////////
// Init ROS
////////////////////////////////////////////////////////////////
ros::init(argc, argv, "omni_haptic_node");
OmniState state;
PhantomROS omni_ros;
omni_ros.init(&state);
hdScheduleAsynchronous(omni_state_callback, &state,
HD_MAX_SCHEDULER_PRIORITY);
////////////////////////////////////////////////////////////////
// Loop and publish
////////////////////////////////////////////////////////////////
pthread_t publish_thread;
pthread_create(&publish_thread, NULL, ros_publish, (void*) &omni_ros);
pthread_join(publish_thread, NULL);
ROS_INFO("Ending Session....");
hdStopScheduler();
hdDisableDevice(hHD);
return 0;
}
int phantomOpen()
{
HDErrorInfo error;
Device = hdInitDevice(HD_DEFAULT_DEVICE);
if (HD_DEVICE_ERROR(error = hdGetError()))
{
//hduPrintError(stderr, &error, "Failed to initialize haptic device");
ROS_ERROR("Failed to initialize haptic device"); //: %s", &error);
return -1;
}
ROS_INFO("Found %s.\n\n", hdGetString(HD_DEVICE_MODEL_TYPE));
hdEnable(HD_FORCE_OUTPUT);
hdStartScheduler();
if (HD_DEVICE_ERROR(error = hdGetError()))
{
ROS_ERROR("Failed to start the scheduler");//, &error);
return -1;
}
phantomCalibrate();
}
//==========================================================================
int hdPhantomGetType(int a_deviceID,
char* a_typeName)
{
// check id
if ((a_deviceID < 0) || (a_deviceID >= numPhantomDevices)) { return (-1); }
// check if servo started
if (!servoStarted) { hdPhantomStartServo(); }
// check if enabled
if (!phantomDevices[a_deviceID].enabled) { return (-1); }
// retrieve handle
HHD hHD = phantomDevices[a_deviceID].handle;
// activate ith device
hdMakeCurrentDevice(hHD);
// read device model
const char* typeName = hdGetString(HD_DEVICE_MODEL_TYPE);
strcpy(a_typeName, typeName);
}
bool BaseGeometryPatch::setup()
{
/* Initialize the device, must be done before attempting to call any hd
functions. Passing in HD_DEFAULT_DEVICE causes the default device to be
initialized. */
hHD = hdInitDevice(HD_DEFAULT_DEVICE);
if (HD_DEVICE_ERROR(error = hdGetError()))
{
hduPrintError(stderr, &error, "Failed to initialize haptic device");
fprintf(stderr, "\nPress any key to quit.\n");
getch();
return false;
}
printf("Hello Haptic Device!\n");
printf("Found device model: %s.\n\n", hdGetString(HD_DEVICE_MODEL_TYPE));
/* Schedule the main callback that will render forceVecs to the device. */
hHapticEffect = hdScheduleAsynchronous(patchCallback, this, HD_DEFAULT_SCHEDULER_PRIORITY);
return true;
}
/******************************************************************************
Collects statistics about the update rate of the haptic device.
******************************************************************************/
int main(int argc, char* argv[])
{
HDErrorInfo error;
HHD hHD = hdInitDevice(HD_DEFAULT_DEVICE);
if (HD_DEVICE_ERROR(error = hdGetError()))
{
hduPrintError(stderr, &error, "Failed to initialize haptic device");
fprintf(stderr, "\nPress any key to quit.\n");
getch();
return -1;
}
HDstring model = hdGetString(HD_DEVICE_MODEL_TYPE);
std::cout << "Initialized: " << model << std::endl;
HDSchedulerHandle hServoCallback = hdScheduleAsynchronous(
ServoSchedulerCallback, 0, HD_MAX_SCHEDULER_PRIORITY);
if (HD_DEVICE_ERROR(error = hdGetError()))
{
std::cerr << error << std::endl;
std::cerr << "Failed to schedule servoloop callback" << std::endl;
hdDisableDevice(hHD);
return -1;
}
hdSetSchedulerRate(TARGET_SERVOLOOP_RATE);
if (HD_DEVICE_ERROR(error = hdGetError()))
{
std::cerr << error << std::endl;
std::cerr << "Failed to set servoloop rate" << std::endl;
hdDisableDevice(hHD);
return -1;
}
hdDisable(HD_FORCE_OUTPUT);
hdStartScheduler();
if (HD_DEVICE_ERROR(error = hdGetError()))
{
std::cerr << error << std::endl;
std::cerr << "Failed to start servoloop" << std::endl;
hdDisableDevice(hHD);
return -1;
}
std::cout << "Printing servoloop rate stats. All numbers are in Hz units" << std::endl;
std::cout << std::endl;
char fileName[256];
sprintf(fileName, "%s Rate Stats.txt", model);
std::ofstream fout(fileName, std::ios::out | std::ios::app);
for (int nRuns = 0; nRuns < 10 && !_kbhit(); nRuns++)
{
Sleep(1000);
hdScheduleSynchronous(CopyUpdateRateStats, 0, HD_MIN_SCHEDULER_PRIORITY);
// Prints some stats about the rate as well as log it to file.
PrintUpdateRateStats(std::cout);
PrintUpdateRateStats(fout);
if (!hdWaitForCompletion(hServoCallback, HD_WAIT_CHECK_STATUS))
{
std::cerr << "Error occurred during main loop" << std::endl;
std::cerr << "Press any key to quit." << std::endl;
getch();
break;
}
}
hdStopScheduler();
hdUnschedule(hServoCallback);
hdDisableDevice(hHD);
return 0;
}
/*******************************************************************************
Main function.
Initializes the device, starts the schedule, creates a schedule callback
to handle gravity well forces, waits for the user to press a button, exits
the application.
*******************************************************************************/
int main(int argc, char* argv[])
{
int KeyInfo;
HDErrorInfo error;
HDSchedulerHandle hGravityWell;
/* Initialize the device, must be done before attempting to call any hd
functions. Passing in HD_DEFAULT_DEVICE causes the default device to be
initialized. */
HHD hHD = hdInitDevice(HD_DEFAULT_DEVICE);
hIOMutex = CreateMutex(NULL, FALSE, NULL);
kill = 0;
recording = 0;
if (HD_DEVICE_ERROR(error = hdGetError()))
{
hduPrintError(stderr, &error, "Failed to initialize haptic device");
fprintf(stderr, "\nPress any key to quit.\n");
getch();
return -1;
}
printf("Hello Haptic Device!\n");
printf("Found device model: %s.\n\n", hdGetString(HD_DEVICE_MODEL_TYPE));
/* Schedule the main callback that will render forces to the device. */
hGravityWell = hdScheduleAsynchronous(
gravityWellCallback, 0,
HD_MAX_SCHEDULER_PRIORITY);
hdEnable(HD_FORCE_OUTPUT);
hdStartScheduler();
_beginthread( recordingLoop );
/* Check for errors and abort if so. */
if (HD_DEVICE_ERROR(error = hdGetError()))
{
hduPrintError(stderr, &error, "Failed to start scheduler");
fprintf(stderr, "\nPress any key to quit.\n");
return -1;
}
/* Wait until the user presses a key. Meanwhile, the scheduler
runs and applies forces to the device. */
printf("Press R to record data\n");
printf("Press Q to quit.\n\n");
count = 0;
//while (!_kbhit())
do
{
KeyInfo = _getch();
if ( tolower( KeyInfo ) == 'q')
{
kill = 1;
}
else
{
if ( tolower( KeyInfo ) == 'r' )
{
if ( recording == 0) {
printf("Data recording ON\n");
recording = 1;
}
else
{
printf("Data recording OFF\n");
recording = 0;
}
}
}
/* Periodically check if the gravity well callback has exited. */
if (!hdWaitForCompletion(hGravityWell, HD_WAIT_CHECK_STATUS))
{
kill = 1;
fprintf(stderr, "Press any key to quit.\n");
getch();
break;
}
// printf("\n%d", count);
} while (!kill);
/* For cleanup, unschedule callback and stop the scheduler. */
hdStopScheduler();
hdUnschedule(hGravityWell);
/* Disable the device. */
hdDisableDevice(hHD);
return 0;
}