// boucle de manipulation ??
void startManipulation(){
ghHD = HD_INVALID_HANDLE;
gSchedulerCallback = HD_INVALID_HANDLE;
bool returnValue = true;
int devideID = 0;
HDErrorInfo error;
// Initialize the device. This needs to be called before any actions on the
// device.
ghHD = hdInitDevice(HD_DEFAULT_DEVICE);
if (HD_DEVICE_ERROR(error = hdGetError()))
returnValue = false;
hdEnable(HD_FORCE_OUTPUT);
hdEnable(HD_MAX_FORCE_CLAMPING);
// initialize amplitude for vibration
// TODO
// mise en attente du device
hdScheduleSynchronous(startManipulationCallBack, 0,
HD_DEFAULT_SCHEDULER_PRIORITY);
// synchronization du scheduer avec les "actions" ???
hdStartScheduler();
if (HD_DEVICE_ERROR(error = hdGetError()))
returnValue = false;
atexit(exit);
}
/******************************************************************************
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;
}
//name: identifier of the haptic device (exemple : "PHANToM 1")
//cameraView : camera matrix linked with the haptic device
//constraint : physical constraint/object linked with the haptic device
void HapticDevice::addDevice(char * name, btTransform & cameraView)
{
if(NB_DEVICES_MAX>=m_nbDevices+1)
{
HDErrorInfo error;
m_name = name;
//HapticData * data = new HapticData(hdInitDevice(name)); <-- name
HapticData * data = new HapticData(hdInitDevice(HD_DEFAULT_DEVICE));
m_hss[m_nbDevices].setData(data);
m_hss[m_nbDevices].m_effectors = m_effectors[m_nbDevices];
m_hss[m_nbDevices].m_constraints = m_constraints[m_nbDevices];
if (HD_DEVICE_ERROR(error = hdGetError()))
{
printf("Failed to initialize haptic device %i\n",data->m_id);
return ;
}
else
{
printf("Succed to initialize haptic device %i\n",data->m_id);
}
hdEnable(HD_FORCE_OUTPUT);
m_hss[m_nbDevices].m_cameraViews = &cameraView;
m_cameraViews[m_nbDevices] = &cameraView;
m_nbDevices++;
}
}
HHD Omni::setup()
{
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("Omni Initialized\n");
/* Schedule the haptic callback function for continuously monitoring the
button state and rendering the anchored spring force. */
gCallbackHandle = hdScheduleAsynchronous(OmniForceCallback, 0, HD_MAX_SCHEDULER_PRIORITY);
hdEnable(HD_FORCE_OUTPUT);
/* 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;
}
return hHD;
}
int _tmain(int argc, _TCHAR* argv[])
{
ghHD = HD_INVALID_HANDLE;
gSchedulerCallback = HD_INVALID_HANDLE;
bool returnValue = true;
int devideID = 0;
g_doExit = false;
//Initialize Haptic device
HDErrorInfo error;
// Initialize the device. This needs to be called before any actions on the
// device.
ghHD = hdInitDevice(HD_DEFAULT_DEVICE);
if (HD_DEVICE_ERROR(error = hdGetError()))
returnValue = false;
hdEnable(HD_FORCE_OUTPUT);
hdEnable(HD_MAX_FORCE_CLAMPING);
// Initialize amplitude for move vribration
hdGetDoublev(HD_NOMINAL_MAX_CONTINUOUS_FORCE, &gVibrationAmplitude);
gVibrationAmplitude *= 0.75;
gSchedulerCallback = hdScheduleAsynchronous(touchScene, 0,
HD_MAX_SCHEDULER_PRIORITY);
hdStartScheduler();
if (HD_DEVICE_ERROR(error = hdGetError()))
returnValue = false;
atexit(ExitHandler);
while(!g_doExit){
//printf("%i %i %i\n",g_button1,g_button2,g_button3);
}
return 0;
}
/*******************************************************************************
* 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;
}
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);
}
VOID CALLBACK DisableTimerProc(HWND hwnd,
UINT uMsg,
UINT_PTR idEvent,
DWORD dwTime)
{
#ifndef NO_HAPTICS
//KillTimer(NULL, idEvent);
hdEnable(HD_FORCE_OUTPUT);
#endif
CurrentlyDisablingDevice = false;
}
/******************************************************************************
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[])
{
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;
}
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();
}
bool BaseGeometryPatch::run()
{
/*
Wait until the user presses a key. Meanwhile, the scheduler
runs and applies forceVecs to the device.
*/
hdEnable(HD_FORCE_OUTPUT);
hdStartScheduler();
/* Check for errors and abort if so. */
if (HD_DEVICE_ERROR(error = hdGetError()))
{
hduPrintError(stderr, &error, "Failed to start scheduler");
fprintf(stderr, "\nPress CR to quit.\n");
return false;
}
return true;
}
//==========================================================================
void init()
{
if (!initPhantomLIB)
{
initPhantomLIB = true;
for (int i=0; i<PHANTOM_NUM_DEVICES_MAX; i++)
{
// init button data
phantomDevices[i].button = 0;
// init position data
phantomDevices[i].position[0] = 0.0;
phantomDevices[i].position[1] = 0.0;
phantomDevices[i].position[2] = 0.0;
// init rotation data
phantomDevices[i].rotation[0] = 1.0;
phantomDevices[i].rotation[1] = 0.0;
phantomDevices[i].rotation[2] = 0.0;
phantomDevices[i].rotation[3] = 0.0;
phantomDevices[i].rotation[4] = 1.0;
phantomDevices[i].rotation[5] = 0.0;
phantomDevices[i].rotation[6] = 0.0;
phantomDevices[i].rotation[7] = 0.0;
phantomDevices[i].rotation[8] = 1.0;
// init force data
phantomDevices[i].force[0] = 0.0;
phantomDevices[i].force[1] = 0.0;
phantomDevices[i].force[2] = 0.0;
// init torque data
phantomDevices[i].torque[0] = 0.0;
phantomDevices[i].torque[1] = 0.0;
phantomDevices[i].torque[2] = 0.0;
// init enable/disable data
phantomDevices[i].enabled = false;
// init phantom api initialized
phantomDevices[i].initialized = false;
}
//------------------------------------------------------------------
// INITIALIZE DEVICES
//------------------------------------------------------------------
HDErrorInfo error;
numPhantomDevices = 0;
// search for a first device
HHD hHD0 = hdInitDevice(HD_DEFAULT_DEVICE);
// check if device is available
if (HD_DEVICE_ERROR(error = hdGetError()))
{
return;
}
// enable forces
hdMakeCurrentDevice(hHD0);
hdEnable(HD_FORCE_OUTPUT);
// add device to list
phantomDevices[0].handle = hHD0;
phantomDevices[0].enabled = true;
numPhantomDevices++;
// search for a possible second device
HHD hHD1 = hdInitDevice("Phantom2");
// check if device is available
if (HD_DEVICE_ERROR(error = hdGetError()))
{
return;
}
// enable forces
hdMakeCurrentDevice(hHD1);
hdEnable(HD_FORCE_OUTPUT);
// add device to list
phantomDevices[1].handle = hHD1;
phantomDevices[1].enabled = true;
numPhantomDevices++;
}
return;
}
/*******************************************************************************
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;
}
