void phantomClose()
{
fclose(data);
hdStopScheduler();
hdUnschedule(hPhantomMain);
hdDisableDevice(Device);
}
HapticDevice::~HapticDevice(void)
{
if(m_updateDeviceCallback != HD_INVALID_HANDLE)
{
hdStopScheduler();
hdUnschedule(m_updateDeviceCallback);
}
for(unsigned int i=0;i<m_nbDevices;i++)
{
if(m_constraints[i] != NULL)
delete m_constraints[i];
if(m_hss[i].m_data!=NULL)
{
if (m_hss[i].m_data->m_id != HD_INVALID_HANDLE)
{
hdDisableDevice(m_hss[i].m_data->m_id);
m_hss[i].m_data->m_id = HD_INVALID_HANDLE;
}
}
}
printf(" Haptic Divice Done . \n");
}
/******************************************************************************
This handler gets called when the process is exiting. Ensures that HDAPI is
properly shutdown
******************************************************************************/
void exitHandler()
{
hdStopScheduler();
hdUnschedule(gSchedulerCallback);
if (ghHD != HD_INVALID_HANDLE)
{
hdDisableDevice(ghHD);
ghHD = HD_INVALID_HANDLE;
}
}
/*******************************************************************************
* main function
Initializes the device, creates a callback to handles plane forces,
terminates upon key press.
******************************************************************************/
int main(int argc, char* argv[])
{
HDErrorInfo error;
// Initialize the default haptic device.
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;
}
// Start the servo scheduler and enable forces.
hdEnable(HD_FORCE_OUTPUT);
hdStartScheduler();
if (HD_DEVICE_ERROR(error = hdGetError()))
{
hduPrintError(stderr, &error, "Failed to start the scheduler");
fprintf(stderr, "\nPress any key to quit.\n");
getch();
return -1;
}
// Schedule the frictionless plane callback, which will then run at
// servoloop rates and command forces if the user penetrates the plane.
HDCallbackCode hPlaneCallback = hdScheduleAsynchronous(
FrictionlessPlaneCallback, 0, HD_DEFAULT_SCHEDULER_PRIORITY);
printf("Plane example.\n");
printf("Move the device up and down to feel a plane along Y=0.\n");
printf("Push hard against the plane to popthrough to the other side.\n");
printf("Press any key to quit.\n\n");
while (!_kbhit())
{
if (!hdWaitForCompletion(hPlaneCallback, HD_WAIT_CHECK_STATUS))
{
fprintf(stderr, "\nThe main scheduler callback has exited\n");
fprintf(stderr, "\nPress any key to quit.\n");
getch();
break;
}
}
// Cleanup and shutdown the haptic device, cleanup all callbacks.
hdStopScheduler();
hdUnschedule(hPlaneCallback);
hdDisableDevice(hHD);
return 0;
}
bool BaseGeometryPatch::terminate()
{
/* For cleanup, unschedule callback and stop the scheduler. */
hdStopScheduler();
if(hHapticEffect != NULL)
hdUnschedule(hHapticEffect);
/* Disable the device. */
hdDisableDevice(hHD);
return true;
}
/******************************************************************************
main function
Initializes the device, creates a callback to handle sphere forces, terminates
upon key press.
******************************************************************************/
int main(int argc, char* argv[])
{
HDErrorInfo error;
// Initialize the default haptic device.
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;
}
// Start the servo scheduler and enable forces.
hdEnable(HD_FORCE_OUTPUT);
hdStartScheduler();
if (HD_DEVICE_ERROR(error = hdGetError()))
{
hduPrintError(stderr, &error, "Failed to start scheduler");
fprintf(stderr, "\nPress any key to quit.\n");
getch();
return -1;
}
// Application loop - schedule our call to the main callback.
HDSchedulerHandle hSphereCallback = hdScheduleAsynchronous(
FrictionlessSphereCallback, 0, HD_DEFAULT_SCHEDULER_PRIORITY);
printf("Sphere example.\n");
printf("Move the device around to feel a frictionless sphere\n\n");
printf("Press any key to quit.\n\n");
while (!_kbhit())
{
if (!hdWaitForCompletion(hSphereCallback, HD_WAIT_CHECK_STATUS))
{
fprintf(stderr, "\nThe main scheduler callback has exited\n");
fprintf(stderr, "\nPress any key to quit.\n");
getch();
break;
}
}
// For cleanup, unschedule our callbacks and stop the servo loop.
hdStopScheduler();
hdUnschedule(hSphereCallback);
hdDisableDevice(hHD);
return 0;
}
/******************************************************************************
Main function.
******************************************************************************/
int main(int argc, char* argv[])
{
hHD = device.setup();
/* Start the main application loop. */
mainLoop();
/* Cleanup by stopping the haptics loop, unscheduling the asynchronous
callback, disabling the device. */
hdStopScheduler();
hdUnschedule(gCallbackHandle);
hdDisableDevice(hHD);
return 0;
}
/******************************************************************************
Main function.
******************************************************************************/
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;
}
printf("Anchored Spring Force Example\n");
/* Schedule the haptic callback function for continuously monitoring the
button state and rendering the anchored spring force. */
gCallbackHandle = hdScheduleAsynchronous(
AnchoredSpringForceCallback, 0, HD_MAX_SCHEDULER_PRIORITY);
hdEnable(HD_FORCE_OUTPUT);
/* Query the max closed loop control stiffness that the device
can handle. Using a value beyond this limit will likely cause the
device to buzz. */
hdGetDoublev(HD_NOMINAL_MAX_STIFFNESS, &gMaxStiffness);
/* Start the haptic rendering loop. */
hdStartScheduler();
if (HD_DEVICE_ERROR(error = hdGetError()))
{
hduPrintError(stderr, &error, "Failed to start scheduler");
fprintf(stderr, "\nPress any key to quit.\n");
getch();
return -1;
}
/* Start the main application loop. */
mainLoop();
/* Cleanup by stopping the haptics loop, unscheduling the asynchronous
callback, disabling the device. */
hdStopScheduler();
hdUnschedule(gCallbackHandle);
hdDisableDevice(hHD);
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;
}
//==========================================================================
void hdPhantomStopServo()
{
if (servoStarted)
{
// check if any device is enabled
bool deviceEnabled = false;
for (int i=0; i<PHANTOM_NUM_DEVICES_MAX; i++)
{
deviceEnabled = deviceEnabled || phantomDevices[i].enabled;
}
// if a device is still enabled, do not stop servo loop
if (deviceEnabled) { return; };
// stop servo controller
servoStarted = false;
hdStopScheduler();
}
}
/*******************************************************************************
Main function.
Sets up the device, runs main application loop, cleans up when finished.
*******************************************************************************/
int main(int argc, char* argv[])
{
HDSchedulerHandle hUpdateHandle = 0;
HDErrorInfo error;
/* Initialize the device, must be done before attempting to call any hd
functions. */
HHD hHD = hdInitDevice(HD_DEFAULT_DEVICE);
if (HD_DEVICE_ERROR(error = hdGetError()))
{
hduPrintError(stderr, &error, "Failed to initialize the device");
fprintf(stderr, "\nPress any key to quit.\n");
getch();
return -1;
}
/* Schedule the main scheduler callback that updates the device state. */
hUpdateHandle = hdScheduleAsynchronous(
updateDeviceCallback, 0, HD_MAX_SCHEDULER_PRIORITY);
/* Start the servo loop scheduler. */
hdStartScheduler();
if (HD_DEVICE_ERROR(error = hdGetError()))
{
hduPrintError(stderr, &error, "Failed to start the scheduler");
fprintf(stderr, "\nPress any key to quit.\n");
getch();
return -1;
}
/* Run the application loop. */
mainLoop();
/* For cleanup, unschedule callbacks and stop the servo loop. */
hdStopScheduler();
hdUnschedule(hUpdateHandle);
hdDisableDevice(hHD);
return 0;
}
/******************************************************************************
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;
}
void Phantom::Close() {
hdStopScheduler();
hdUnschedule(_hPhantomMain);
hdDisableDevice(_Device);
}
void phantomPause()
{
hdStopScheduler();
}
void ExitHandler(){
hdStopScheduler();
hdUnschedule(gSchedulerCallback);
}
static void exit(){
hdStopScheduler();
// arrêt du scheduler
hdUnschedule(gSchedulerCallback);
// suppression des informations à envoyer
}
