HDCallbackCode HDCALLBACK HapticDevice::sScheduleOut(void *pUserData)
{
HapticSynchronizer * hs = (HapticSynchronizer *) pUserData;
for(unsigned int i=0;i<NB_DEVICES_MAX;i++)
{
if(hs[i].m_data!=NULL)
{
hdMakeCurrentDevice(hs[i].m_data->m_id);
hdBeginFrame(hs[i].m_data->m_id);
hdGetDoublev(HD_CURRENT_POSITION, hs[i].m_data->m_position);
hdGetDoublev(HD_CURRENT_FORCE, hs[i].m_data->m_force);
hdGetDoublev(HD_CURRENT_TRANSFORM, hs[i].m_data->m_transform);
hdGetIntegerv(HD_CURRENT_BUTTONS, &hs[i].m_data->m_buttons);
hdGetDoublev(HD_CURRENT_VELOCITY, hs[i].m_data->m_velocity);
hdEndFrame(hs[i].m_data->m_id);
hs[i].m_free.m_position = hs[i].m_data->m_position;
for(int j=0;j<16;j++)
hs[i].m_free.m_transform[j] = hs[i].m_data->m_transform[j];
hs[i].m_free.m_buttons=hs[i].m_data->m_buttons;
hs[i].m_free.m_force = hs[i].m_data->m_force;
hs[i].m_free.m_velocity = hs[i].m_data->m_velocity;
}
}
return HD_CALLBACK_DONE;
}
//==========================================================================
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);
}
//==========================================================================
int hdPhantomGetWorkspaceRadius(const int a_deviceID,
double *a_workspaceRadius)
{
// 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 workspace of device
double size[6];
hdGetDoublev(HD_USABLE_WORKSPACE_DIMENSIONS, size);
double sizeX = size[3] - size[0];
double sizeY = size[4] - size[1];
double sizeZ = size[5] - size[2];
double radius = 0.5 * sqrt(sizeX * sizeX +
sizeY * sizeY +
sizeZ * sizeZ);
// convert value to [m]
phantomDevices[a_deviceID].workspaceRadius = 0.001 * radius;
// return estimated workspace radius
*a_workspaceRadius = phantomDevices[a_deviceID].workspaceRadius;
// success
return (0);
}
//==========================================================================
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;
}
//==========================================================================
HDCallbackCode HDCALLBACK servoPhantomDevices(void* pUserData)
{
for (int i=0; i<PHANTOM_NUM_DEVICES_MAX; i++)
{
// for each activated phantom device
if (phantomDevices[i].enabled)
{
// retrieve handle
HHD hHD = phantomDevices[i].handle;
// activate ith device
hdMakeCurrentDevice(hHD);
// start sending commands
hdBeginFrame(hHD);
// retrieve the position and orientation of the end-effector.
double frame[16];
hdGetDoublev(HD_CURRENT_TRANSFORM, frame);
// convert position from [mm] to [m]
frame[12] = frame[12] * 0.001;
frame[13] = frame[13] * 0.001;
frame[14] = frame[14] * 0.001;
phantomDevices[i].position[0] = frame[12];
phantomDevices[i].position[1] = frame[13];
phantomDevices[i].position[2] = frame[14];
phantomDevices[i].rotation[0] = frame[0];
phantomDevices[i].rotation[1] = frame[1];
phantomDevices[i].rotation[2] = frame[2];
phantomDevices[i].rotation[3] = frame[4];
phantomDevices[i].rotation[4] = frame[5];
phantomDevices[i].rotation[5] = frame[6];
phantomDevices[i].rotation[6] = frame[8];
phantomDevices[i].rotation[7] = frame[9];
phantomDevices[i].rotation[8] = frame[10];
// read linear velocity
double vel[3];
hdGetDoublev(HD_CURRENT_VELOCITY, vel);
// convert position from [mm] to [m]
vel[0] = vel[0] * 0.001;
vel[1] = vel[1] * 0.001;
vel[2] = vel[2] * 0.001;
phantomDevices[i].linearVelocity[0] = vel[0];
phantomDevices[i].linearVelocity[1] = vel[1];
phantomDevices[i].linearVelocity[2] = vel[2];
// read user buttons
int buttons;
hdGetIntegerv(HD_CURRENT_BUTTONS, &buttons);
phantomDevices[i].button = buttons;
// send force to end-effector
double force[3];
force[0] = phantomDevices[i].force[0];
force[1] = phantomDevices[i].force[1];
force[2] = phantomDevices[i].force[2];
hdSetDoublev(HD_CURRENT_FORCE, force);
// send torque to end-effector
double torque[3];
torque[0] = phantomDevices[i].torque[0];
torque[1] = phantomDevices[i].torque[1];
torque[2] = phantomDevices[i].torque[2];
hdSetDoublev(HD_CURRENT_TORQUE, torque);
// flush commands
hdEndFrame(hHD);
}
}
return (HD_CALLBACK_CONTINUE);
}
/*******************************************************************************
Main function.
*******************************************************************************/
int main(int argc, char *argv[])
{
HHD hHD;
HHLRC hHLRC;
HDErrorInfo error;
HLuint spring;
/* The code snippet provided to you by SensAble should be executed near
application startup. Once a deployment license has been validated,
it will remain in effect until the application is shutdown.
Be sure to place the HD deployment license code before the call
to hdInitDevice, and the HL deployment license code before the call
to hlCreateContext.
NOTE THAT THE FOLLOWING IS FOR DEMONSTRATION ONLY. THE LICENSES
ARE NOT VALID. */
hdDeploymentLicense(
"ABC Software, Inc.",
"HapticsGold",
"F1312D97ECCC754D5BE4BEE7E831BC27ACF809E9B850D9576F1A856AF70DD3A879B4D3DC7F922BDB2C639DA4A565CA5FC598D8AF34EA010B13A8C232B78F22C");
hlDeploymentLicense(
"ABC Software, Inc.",
"HapticsGold",
"A653A1EEAFF7B87C952754672FDB1AA16A9035ADE1CFCA6394FE869BAFECE0B7A32251502DDF220B7BA27979695041AE59DCEA007605027D471801F4BF26C24");
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;
}
hdMakeCurrentDevice(hHD);
hHLRC = hlCreateContext(hHD);
hlMakeCurrent(hHLRC);
hlDisable(HL_USE_GL_MODELVIEW);
spring = hlGenEffects(1);
// Add a callback to handle button down in the collision thread.
hlAddEventCallback(HL_EVENT_1BUTTONDOWN, HL_OBJECT_ANY, HL_CLIENT_THREAD,
buttonCB, &spring);
hlAddEventCallback(HL_EVENT_1BUTTONUP, HL_OBJECT_ANY, HL_CLIENT_THREAD,
buttonCB, &spring);
printf("Press and hold the primary stylus button to feel the spring effect.\n");
printf("Press any key to quit.\n\n");
// Run the main loop.
while (!_kbhit())
{
hlBeginFrame();
// Poll for events. Note that client thread event callbacks get
// dispatched from within a frame here, so we can safely start/stop
// effects from the event callback directly.
hlCheckEvents();
hlEndFrame();
}
hlDeleteEffects(spring, 1);
hlDeleteContext(hHLRC);
hdDisableDevice(hHD);
return 0;
}
/*******************************************************************************
Main function.
*******************************************************************************/
int main(int argc, char *argv[])
{
HHD hHD;
HHLRC hHLRC;
HDErrorInfo error;
HLuint friction, spring;
HLerror frameError;
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;
}
hdMakeCurrentDevice(hHD);
hHLRC = hlCreateContext(hHD);
hlMakeCurrent(hHLRC);
hlDisable(HL_USE_GL_MODELVIEW);
spring = hlGenEffects(1);
/* Add a callback to handle button down in the collision thread. */
hlAddEventCallback(HL_EVENT_1BUTTONDOWN, HL_OBJECT_ANY, HL_CLIENT_THREAD,
buttonCB, &spring);
hlAddEventCallback(HL_EVENT_1BUTTONUP, HL_OBJECT_ANY, HL_CLIENT_THREAD,
buttonCB, &spring);
hlAddEventCallback(HL_EVENT_2BUTTONDOWN, HL_OBJECT_ANY, HL_CLIENT_THREAD,
buttonCB, 0);
/* Start an ambient friction effect. */
friction = hlGenEffects(1);
hlBeginFrame();
hlEffectd(HL_EFFECT_PROPERTY_GAIN, 0.2);
hlEffectd(HL_EFFECT_PROPERTY_MAGNITUDE, 0.5);
hlStartEffect(HL_EFFECT_FRICTION, friction);
hlEndFrame();
printf("Move around to feel the ambient stick-slip friction.\n\n");
printf("Press and hold the primary stylus button to feel the spring effect.\n\n");
printf("Press the second stylus button to trigger an impulse.\n\n");
/* Run the main loop. */
while (!_kbhit())
{
hlBeginFrame();
/* Poll for events. Note that client thread event callbacks get
dispatched from within a frame here, so we can safely start/stop
effects from the event callback directly */
hlCheckEvents();
hlEndFrame();
/* Check for any errors. */
while (HL_ERROR(frameError = hlGetError()))
{
fprintf(stderr, "HL Error: %s\n", frameError.errorCode);
if (frameError.errorCode == HL_DEVICE_ERROR)
{
hduPrintError(stderr, &frameError.errorInfo,
"Error during haptic rendering\n");
}
}
}
/* Stop the friction effect. */
hlBeginFrame();
hlStopEffect(friction);
hlEndFrame();
hlDeleteEffects(friction, 1);
hlDeleteEffects(spring, 1);
hlDeleteContext(hHLRC);
hdDisableDevice(hHD);
return 0;
}