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;
}
/*******************************************************************************
* 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;
}
/******************************************************************************
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;
}
/******************************************************************************
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;
}
/*******************************************************************************
Main callback that calculates and sets the force.
*******************************************************************************/
HDCallbackCode HDCALLBACK CoulombCallback(void *data)
{
HHD hHD = hdGetCurrentDevice();
hdBeginFrame(hHD);
hduVector3Dd pos;
hdGetDoublev(HD_CURRENT_POSITION,pos);
hduVector3Dd forceVec;
forceVec = forceField(pos);
hdSetDoublev(HD_CURRENT_FORCE, forceVec);
hdEndFrame(hHD);
HDErrorInfo error;
if (HD_DEVICE_ERROR(error = hdGetError()))
{
hduPrintError(stderr, &error, "Error during scheduler callback");
if (hduIsSchedulerError(&error))
{
return HD_CALLBACK_DONE;
}
}
return HD_CALLBACK_CONTINUE;
}
void CybPhantom::HLIdleFunc()
{
HLerror error;
while (HL_ERROR(error = hlGetError()))
{
fprintf(stderr, "Error: %s\n", error.errorCode);
if (error.errorCode == HL_DEVICE_ERROR)
{
hduPrintError(stderr, &error.errorInfo,
"Error during haptic rendering\n");
}
}
if(cybCore->phantomOn && !flagWorkspaceUpdate){
hapticWorkspaceCalibration();
flagWorkspaceUpdate = true;
}
else if(!cybCore->phantomOn)
{
flagWorkspaceUpdate = false;
}
}
/*
Function: InitHL
Usage: InitHL();
---------------------------------------------------------------------------
Initializes haptic device and haptic rendering library.
*/
void InitHL()
{
HDErrorInfo error;
hHD = hdInitDevice(HD_DEFAULT_DEVICE);
if (HD_DEVICE_ERROR(error = hdGetError()))
{
hduPrintError(stderr, &error, "Failed to initialize haptic device");
fprintf(stderr, "Press any key to exit");
getchar();
exit(1);
}
// Create a haptic context for the device. The haptic context maintains
// the state that persists between frame intervals and is used for
// haptic rendering.
hHLRC = hlCreateContext(hHD);
hlMakeCurrent(hHLRC);
// generate a shape id to hold the haptic surface rep
mSurfaceShapeId = hlGenShapes(1);
// add a callback to handle motion while touching surface
hlAddEventCallback(HL_EVENT_TOUCH, mSurfaceShapeId, HL_CLIENT_THREAD,
OnTouchSurface, NULL);
hlAddEventCallback(HL_EVENT_UNTOUCH, mSurfaceShapeId, HL_CLIENT_THREAD,
OnTouchSurface, NULL);
// set motion tolerance to 0 so we get motion event when
// when not moving.
hlEventd(HL_EVENT_MOTION_LINEAR_TOLERANCE, 0);
hlAddEventCallback(HL_EVENT_MOTION, mSurfaceShapeId, HL_CLIENT_THREAD,
OnDragSurface, NULL);
}
/*******************************************************************************
Automatic Calibration of Phantom Device - No character inputs
*******************************************************************************/
void HHD_Auto_Calibration() {
int supportedCalibrationStyles;
HDErrorInfo error;
hdGetIntegerv(HD_CALIBRATION_STYLE, &supportedCalibrationStyles);
if (supportedCalibrationStyles & HD_CALIBRATION_ENCODER_RESET) {
calibrationStyle = HD_CALIBRATION_ENCODER_RESET;
ROS_INFO("HD_CALIBRATION_ENCODER_RESE..");
}
if (supportedCalibrationStyles & HD_CALIBRATION_INKWELL) {
calibrationStyle = HD_CALIBRATION_INKWELL;
ROS_INFO("HD_CALIBRATION_INKWELL..");
}
if (supportedCalibrationStyles & HD_CALIBRATION_AUTO) {
calibrationStyle = HD_CALIBRATION_AUTO;
ROS_INFO("HD_CALIBRATION_AUTO..");
}
if (calibrationStyle == HD_CALIBRATION_ENCODER_RESET) {
do {
hdUpdateCalibration(calibrationStyle);
ROS_INFO("Calibrating.. (put stylus in well)");
if (HD_DEVICE_ERROR(error = hdGetError())) {
hduPrintError(stderr, &error, "Reset encoders reset failed.");
break;
}
} while (hdCheckCalibration() != HD_CALIBRATION_OK);
ROS_INFO("Calibration complete.");
}
if (hdCheckCalibration() == HD_CALIBRATION_NEEDS_MANUAL_INPUT) {
ROS_INFO("Please place the device into the inkwell for calibration.");
}
}
int CybPhantom::initHL()
{
//TODO HABILITAR O OUTPUT FORCE NA HD.
HDErrorInfo error;
hapticDevice = hdInitDevice(HD_DEFAULT_DEVICE);
if(HD_DEVICE_ERROR(error = hdGetError()))
{
//Falta tratar o erro!
hduPrintError(stderr, &error, "Failed to initialize haptic device");
return -1;
}
checkCalibrationStatus(&stateCalibration);
hapticContext = hlCreateContext(hapticDevice);
hlMakeCurrent(hapticContext);
hlTouchableFace(HL_FRONT_AND_BACK);
hlEnable(HL_HAPTIC_CAMERA_VIEW);
//hlEnable(HL_ADAPTIVE_VIEWPORT);
//hdEnable(HD_FORCE_OUTPUT);
return 0;
}
/*******************************************************************************
Haptic sphere callback.
The sphere is oriented at 0,0,0 with radius 40, and provides a repelling force
if the device attempts to penetrate through it.
*******************************************************************************/
HDCallbackCode HDCALLBACK FrictionlessSphereCallback(void *data)
{
const double sphereRadius = 10.0;
const hduVector3Dd spherePosition(0,50,0);
// Stiffness, i.e. k value, of the sphere. Higher stiffness results
// in a harder surface.
const double sphereStiffness = .01;
hdBeginFrame(hdGetCurrentDevice());
// Get the position of the device.
hduVector3Dd position;
hdGetDoublev(HD_CURRENT_POSITION, position);
// Find the distance between the device and the center of the
// sphere.
double distance = (position-spherePosition).magnitude();
// If the user is within the sphere -- i.e. if the distance from the user to
// the center of the sphere is less than the sphere radius -- then the user
// is penetrating the sphere and a force should be commanded to repel him
// towards the surface.
if (distance > sphereRadius)
{
// Calculate the penetration distance.
double penetrationDistance = sphereRadius-distance;
// Create a unit vector in the direction of the force, this will always
// be outward from the center of the sphere through the user's
// position.
hduVector3Dd forceDirection = (position-spherePosition)/distance;
// Use F=kx to create a force vector that is away from the center of
// the sphere and proportional to the penetration distance, and scsaled
// by the object stiffness.
// Hooke's law explicitly:
double k = sphereStiffness;
hduVector3Dd x = penetrationDistance*forceDirection;
hduVector3Dd f = k*x;
hdSetDoublev(HD_CURRENT_FORCE, f);
}
hdEndFrame(hdGetCurrentDevice());
HDErrorInfo error;
if (HD_DEVICE_ERROR(error = hdGetError()))
{
hduPrintError(stderr, &error, "Error during main scheduler callback\n");
if (hduIsSchedulerError(&error))
{
return HD_CALLBACK_DONE;
}
}
return HD_CALLBACK_CONTINUE;
}
HDCallbackCode HDCALLBACK omni_state_callback(void *pUserData) {
OmniState *omni_state = static_cast<OmniState *>(pUserData);
if (hdCheckCalibration() == HD_CALIBRATION_NEEDS_UPDATE) {
ROS_DEBUG("Updating calibration...");
hdUpdateCalibration(calibrationStyle);
}
hdBeginFrame(hdGetCurrentDevice());
//Get angles, set forces
hdGetDoublev(HD_CURRENT_GIMBAL_ANGLES, omni_state->rot);
hdGetDoublev(HD_CURRENT_POSITION, omni_state->position);
hdGetDoublev(HD_CURRENT_JOINT_ANGLES, omni_state->joints);
hduVector3Dd vel_buff(0, 0, 0);
vel_buff = (omni_state->position * 3 - 4 * omni_state->pos_hist1
+ omni_state->pos_hist2) / 0.002; //mm/s, 2nd order backward dif
omni_state->velocity = (.2196 * (vel_buff + omni_state->inp_vel3)
+ .6588 * (omni_state->inp_vel1 + omni_state->inp_vel2)) / 1000.0
- (-2.7488 * omni_state->out_vel1 + 2.5282 * omni_state->out_vel2
- 0.7776 * omni_state->out_vel3); //cutoff freq of 20 Hz
omni_state->pos_hist2 = omni_state->pos_hist1;
omni_state->pos_hist1 = omni_state->position;
omni_state->inp_vel3 = omni_state->inp_vel2;
omni_state->inp_vel2 = omni_state->inp_vel1;
omni_state->inp_vel1 = vel_buff;
omni_state->out_vel3 = omni_state->out_vel2;
omni_state->out_vel2 = omni_state->out_vel1;
omni_state->out_vel1 = omni_state->velocity;
if (omni_state->lock == true) {
omni_state->force = 0.04 * (omni_state->lock_pos - omni_state->position)
- 0.001 * omni_state->velocity;
}
hdSetDoublev(HD_CURRENT_FORCE, omni_state->force);
//Get buttons
int nButtons = 0;
hdGetIntegerv(HD_CURRENT_BUTTONS, &nButtons);
omni_state->buttons[0] = (nButtons & HD_DEVICE_BUTTON_1) ? 1 : 0;
omni_state->buttons[1] = (nButtons & HD_DEVICE_BUTTON_2) ? 1 : 0;
hdEndFrame(hdGetCurrentDevice());
HDErrorInfo error;
if (HD_DEVICE_ERROR(error = hdGetError())) {
hduPrintError(stderr, &error, "Error during main scheduler callback");
if (hduIsSchedulerError(&error))
return HD_CALLBACK_DONE;
}
float t[7] = { 0., omni_state->joints[0], omni_state->joints[1],
omni_state->joints[2] - omni_state->joints[1], omni_state->rot[0],
omni_state->rot[1], omni_state->rot[2] };
for (int i = 0; i < 7; i++)
omni_state->thetas[i] = t[i];
return HD_CALLBACK_CONTINUE;
}
/*******************************************************************************
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;
}
void init(void)
{
glClearColor(0.0, 0.0, 0.0, 0.0);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
glOrtho(0.0, 1.0, 0.0, 1.0, -1.0, 1.0);
// Enable depth buffering to provide depth information for OpenHaptics.
glDepthFunc(GL_LEQUAL);
glEnable(GL_DEPTH_TEST);
// OpenHaptics setup follows:
// Create a haptic device instance.
HDErrorInfo error;
HHD hHD = hdInitDevice(HD_DEFAULT_DEVICE);
if (HD_DEVICE_ERROR(error = hdGetError()))
{
hduPrintError(stderr, &error, "Failed to initialize haptic device");
fprintf(stderr, "Press any key to exit");
getchar();
exit(-1);
}
if (HD_SUCCESS != hdGetError().errorCode)
{
fprintf(stderr, "Erorr initializing haptic device.\nPress any key to exit");
getchar();
exit(-1);
}
// Create a haptic rendering context and activate it.
HHLRC hHLRC = hlCreateContext(hHD);
hlMakeCurrent(hHLRC);
// Reserve an id for the shape
gMyShapeId = hlGenShapes(1);
// Specify the boundaries for the workspace of the haptic device
// in millimeters in the cordinates of the haptic device.
// The haptics engine will map the view volume to this workspace
hlWorkspace (-80, -80, -70, 80, 80, 20);
// Specify the haptic view volume (in this case it will be
// the same as the graphic view volume).
hlMatrixMode(HL_TOUCHWORKSPACE);
hlLoadIdentity();
hlOrtho (0.0, 1.0, 0.0, 1.0, -1.0, 1.0);
}
/*************************************************************
ADDED
*************************************************************/
HDCallbackCode HDCALLBACK hdBeginCB(void *data)
{
// calling hdBeginFrame increments the HD frame counter.
hdBeginFrame(hdGetCurrentDevice());
HDErrorInfo error;
if (HD_DEVICE_ERROR(error = hdGetError()))
{
hduPrintError(stderr, &error, "Error in hdBeginCB\n");
return HD_CALLBACK_DONE;
}
return HD_CALLBACK_CONTINUE;
}
/*******************************************************************************
Schedules the coulomb force callback.
*******************************************************************************/
void CoulombForceField()
{
gSchedulerCallback = hdScheduleAsynchronous(
CoulombCallback, 0, HD_DEFAULT_SCHEDULER_PRIORITY);
HDErrorInfo error;
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);
}
glutMainLoop(); // Enter GLUT main loop.
}
/*******************************************************************************
GLUT callback for idle state. Use this as an opportunity to request a redraw.
Checks for HLAPI errors that have occurred since the last idle check.
*******************************************************************************/
void glutIdle()
{
HLerror error;
while (HL_ERROR(error = hlGetError()))
{
fprintf(stderr, "HL Error: %s\n", error.errorCode);
if (error.errorCode == HL_DEVICE_ERROR)
{
hduPrintError(stderr, &error.errorInfo,
"Error during haptic rendering\n");
}
}
glutPostRedisplay();
}
/*******************************************************************************
GLUT callback for idle state. Use this as an opportunity to request a redraw.
Checks for HLAPI errors that have occurred since the last idle check.
*******************************************************************************/
void glutIdle()
{
HLerror error;
while (HL_ERROR(error = hlGetError()))
{
fprintf(stderr, "HL Error: %s\n", error.errorCode);
if (error.errorCode == HL_DEVICE_ERROR)
{
hduPrintError(stderr, &error.errorInfo,
"Error during haptic rendering\n");
}
}
char title[40];
sprintf(title, "Haptic Displacement Mapping %4.1f fps", DetermineFPS());
glutSetWindowTitle(title);
glutPostRedisplay();
}
/*******************************************************************************
Initialize the HDAPI. This involves initing a device configuration, enabling
forces, and scheduling a haptic thread callback for servicing the device.
*******************************************************************************/
void initHL()
{
HDErrorInfo error;
ghHD = hdInitDevice( HD_DEFAULT_DEVICE );
if (HD_DEVICE_ERROR(error = hdGetError()))
{
hduPrintError(stderr, &error, "Failed to initialize haptic device");
fprintf(stderr, "Press any key to exit");
getchar();
exit(-1);
}
/*************************************************************
ADDED
*************************************************************/
hdScheduleAsynchronous(hdBeginCB, 0, HD_MAX_SCHEDULER_PRIORITY);
hdScheduleAsynchronous(hdEndCB, 0, HD_MIN_SCHEDULER_PRIORITY);
/*************************************************************
END ADDED
*************************************************************/
ghHLRC = hlCreateContext(ghHD);
hlMakeCurrent(ghHLRC);
// Enable optimization of the viewing parameters when rendering
// geometry for OpenHaptics.
hlEnable(HL_HAPTIC_CAMERA_VIEW);
// Generate id for the shape.
gShapeId = hlGenShapes(1);
gPointId = hlGenShapes(1);
hlTouchableFace(HL_FRONT);
hlAddEventCallback(HL_EVENT_TOUCH,gPointId , HL_COLLISION_THREAD,
hlTouchCB, 0);
hlAddEventCallback(HL_EVENT_UNTOUCH,gPointId , HL_COLLISION_THREAD,
hlUnTouchCB, 0);
}
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;
}
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 phantomCalibrate()
{
int calibrationStyle;
int supportedCalibrationStyles;
HDErrorInfo error;
hdGetIntegerv(HD_CALIBRATION_STYLE, &supportedCalibrationStyles);
if (supportedCalibrationStyles & HD_CALIBRATION_ENCODER_RESET)
{
calibrationStyle = HD_CALIBRATION_ENCODER_RESET;
printf("HD_CALIBRATION_ENCODER_RESE..\n\n");
}
if (supportedCalibrationStyles & HD_CALIBRATION_INKWELL)
{
calibrationStyle = HD_CALIBRATION_INKWELL;
printf("HD_CALIBRATION_INKWELL..\n\n");
}
if (supportedCalibrationStyles & HD_CALIBRATION_AUTO)
{
calibrationStyle = HD_CALIBRATION_AUTO;
printf("HD_CALIBRATION_AUTO..\n\n");
}
do
{
hdUpdateCalibration(calibrationStyle);
printf("Calibrating.. (put stylus in well)\n");
if (HD_DEVICE_ERROR(error = hdGetError()))
{
hduPrintError(stderr, &error, "Reset encoders reset failed.");
break;
}
} while (hdCheckCalibration() != HD_CALIBRATION_OK);
printf("\n\nCalibration complete.\n");
}
HDCallbackCode HDCALLBACK hdEndCB(void *data)
{
static const HDdouble kRampUpRate = 0.0001;
static const HDdouble kImpulseLimit = 0.001;
hdSetDoublev(HD_SOFTWARE_FORCE_IMPULSE_LIMIT,&kImpulseLimit);
hdSetDoublev(HD_FORCE_RAMPING_RATE, &kRampUpRate );
hdGetDoublev(HD_CURRENT_FORCE, force);
//Create custom haptic layers. There are two layers here
//at DOP = 0.1 and DOP = 0.35 .
if (touchedHole && force[2]>=0.0)
{
if (probeDop > 0.0 && probeDop < 0.0125)
force[2] = 0.25;
else if (probeDop > 0.0 && probeDop < 0.025)
force[2] = 0.5;
else if (probeDop > 0.025 && probeDop < 0.05)
force[2] = 0.75;
else if (probeDop > 0.05 && probeDop < 0.075)
force[2] = 1.0;
else if (probeDop > 0.075 && probeDop < 0.09)
force[2] = 1.5;
else if (probeDop > 0.09 && probeDop < 0.1)
force[2] = 0.0;
else if (probeDop > 0.1 && probeDop < 0.2)
force[2] = 0.25;
else if (probeDop > 0.2 && probeDop < 0.225)
force[2] = 0.5;
else if (probeDop > 0.225 && probeDop < 0.25)
force[2] = 0.75;
else if (probeDop > 0.25 && probeDop < 0.275)
force[2] = 0.0;
else if (probeDop > 0.275 && probeDop < 0.3)
force[2] = 0.0;
else if (probeDop > 0.3 && probeDop < 0.35)
force[2] = 0.25;
else if (probeDop > 0.35 && probeDop < 0.4)
force[2] = 0.5;
else if (probeDop > 0.4)
force[2] = 0.5;
hdSetDoublev(HD_CURRENT_FORCE, force*forceScaler);
}
// calling hdEndFrame decrements the frame counter.
// when the beginFrame counter reaches 0, forces are rendered.
// Note that HL makes calls to hdBeginFrame & hdEndFrame internally
hdEndFrame(hdGetCurrentDevice());
HDErrorInfo error;
if (HD_DEVICE_ERROR(error = hdGetError()))
{
hduPrintError(stderr, &error, "Error in hdEndCB\n");
}
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;
}
HDCallbackCode HDCALLBACK phantom_state_callback(void *pUserData)
{
static bool lock_flag = true;
PhantomState *phantom_state = static_cast<PhantomState *>(pUserData);
hdBeginFrame(hdGetCurrentDevice());
//Get angles, set forces
hdGetDoublev(HD_CURRENT_GIMBAL_ANGLES, phantom_state->rot);
hdGetDoublev(HD_CURRENT_POSITION, phantom_state->position);
hdGetDoublev(HD_CURRENT_JOINT_ANGLES, phantom_state->joints);
hdGetDoublev(HD_CURRENT_TRANSFORM, phantom_state->hd_cur_transform);
hduVector3Dd vel_buff(0, 0, 0);
vel_buff = (phantom_state->position * 3 - 4 * phantom_state->pos_hist1 + phantom_state->pos_hist2) / 0.002; //mm/s, 2nd order backward dif
// phantom_state->velocity = 0.0985*(vel_buff+phantom_state->inp_vel3)+0.2956*(phantom_state->inp_vel1+phantom_state->inp_vel2)-(-0.5772*phantom_state->out_vel1+0.4218*phantom_state->out_vel2 - 0.0563*phantom_state->out_vel3); //cutoff freq of 200 Hz
phantom_state->velocity = (.2196 * (vel_buff + phantom_state->inp_vel3)
+ .6588 * (phantom_state->inp_vel1 + phantom_state->inp_vel2)) / 1000.0
- (-2.7488 * phantom_state->out_vel1 + 2.5282 * phantom_state->out_vel2 - 0.7776 * phantom_state->out_vel3); //cutoff freq of 20 Hz
phantom_state->pos_hist2 = phantom_state->pos_hist1;
phantom_state->pos_hist1 = phantom_state->position;
phantom_state->inp_vel3 = phantom_state->inp_vel2;
phantom_state->inp_vel2 = phantom_state->inp_vel1;
phantom_state->inp_vel1 = vel_buff;
phantom_state->out_vel3 = phantom_state->out_vel2;
phantom_state->out_vel2 = phantom_state->out_vel1;
phantom_state->out_vel1 = phantom_state->velocity;
// printf("position x, y, z: %f %f %f \node_", phantom_state->position[0], phantom_state->position[1], phantom_state->position[2]);
// printf("velocity x, y, z, time: %f %f %f \node_", phantom_state->velocity[0], phantom_state->velocity[1],phantom_state->velocity[2]);
if (phantom_state->lock == true)
{
lock_flag = true;
phantom_state->force = 0.04 * (phantom_state->lock_pos - phantom_state->position) - 0.001 * phantom_state->velocity;
}
else
{
if(lock_flag == true)
{
phantom_state->force.set(0.0, 0.0, 0.0);
lock_flag = false;
}
}
// Set force
hdSetDoublev(HD_CURRENT_FORCE, phantom_state->force);
// Set torque
hdSetDoublev(HD_CURRENT_TORQUE, phantom_state->torque);
//Get buttons
int nButtons = 0;
hdGetIntegerv(HD_CURRENT_BUTTONS, &nButtons);
phantom_state->buttons[0] = (nButtons & HD_DEVICE_BUTTON_1) ? 1 : 0;
phantom_state->buttons[1] = (nButtons & HD_DEVICE_BUTTON_2) ? 1 : 0;
hdEndFrame(hdGetCurrentDevice());
HDErrorInfo error;
if (HD_DEVICE_ERROR(error = hdGetError()))
{
hduPrintError(stderr, &error, "Error during main scheduler callback\n");
if (hduIsSchedulerError(&error))
return HD_CALLBACK_DONE;
}
float t[7] = {0., phantom_state->joints[0], phantom_state->joints[1], phantom_state->joints[2] - phantom_state->joints[1],
phantom_state->rot[0], phantom_state->rot[1], phantom_state->rot[2]};
for (int i = 0; i < 7; i++)
phantom_state->thetas[i] = t[i];
return HD_CALLBACK_CONTINUE;
}
/******************************************************************************
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;
}
/*******************************************************************************
Haptic plane callback. The plane is oriented along Y=0 and provides a
repelling force if the device attempts to penetrates through it.
*******************************************************************************/
HDCallbackCode HDCALLBACK FrictionlessPlaneCallback(void *data)
{
// Stiffnes, i.e. k value, of the plane. Higher stiffness results
// in a harder surface.
const double planeStiffness = .25;
// Amount of force the user needs to apply in order to pop through
// the plane.
const double popthroughForceThreshold = 5.0;
// Plane direction changes whenever the user applies sufficient
// force to popthrough it.
// 1 means the plane is facing +Y.
// -1 means the plane is facing -Y.
static int directionFlag = 1;
hdBeginFrame(hdGetCurrentDevice());
// Get the position of the device.
hduVector3Dd position;
hdGetDoublev(HD_CURRENT_POSITION, position);
// If the user has penetrated the plane, set the device force to
// repel the user in the direction of the surface normal of the plane.
// Penetration occurs if the plane is facing in +Y and the user's Y position
// is negative, or vice versa.
if ((position[1] <= 0 && directionFlag > 0) ||
(position[1] > 0) && (directionFlag < 0))
{
// Create a force vector repelling the user from the plane proportional
// to the penetration distance, using F=kx where k is the plane
// stiffness and x is the penetration vector. Since the plane is
// oriented at the Y=0, the force direction is always either directly
// upward or downward, i.e. either (0,1,0) or (0,-1,0).
double penetrationDistance = fabs(position[1]);
hduVector3Dd forceDirection(0,directionFlag,0);
// Hooke's law explicitly:
double k = planeStiffness;
hduVector3Dd x = penetrationDistance*forceDirection;
hduVector3Dd f = k*x;
// If the user applies sufficient force, pop through the plane
// by reversing its direction. Otherwise, apply the repel
// force.
if (f.magnitude() > popthroughForceThreshold)
{
f.set(0.0,0.0,0.0);
directionFlag = -directionFlag;
}
hdSetDoublev(HD_CURRENT_FORCE, f);
}
hdEndFrame(hdGetCurrentDevice());
// In case of error, terminate the callback.
HDErrorInfo error;
if (HD_DEVICE_ERROR(error = hdGetError()))
{
hduPrintError(stderr, &error, "Error detected during main scheduler callback\n");
if (hduIsSchedulerError(&error))
{
return HD_CALLBACK_DONE;
}
}
return HD_CALLBACK_CONTINUE;
}
/*******************************************************************************
Initialize the HDAPI. This involves initing a device configuration, enabling
forces, and scheduling a haptic thread callback for servicing the device.
*******************************************************************************/
void initHL()
{
HDErrorInfo error;
hHD = hdInitDevice(HD_DEFAULT_DEVICE);
if (HD_DEVICE_ERROR(error = hdGetError()))
{
hduPrintError(stderr, &error, "Failed to initialize haptic device");
fprintf(stderr, "Press any key to exit");
getchar();
exit(-1);
}
hHLRC = hlCreateContext(hHD);
hlMakeCurrent(hHLRC);
// Enable optimization of the viewing parameters when rendering
// geometry for OpenHaptics.
hlEnable(HL_HAPTIC_CAMERA_VIEW);
// Generate id's for the three shapes.
gSphereShapeId = hlGenShapes(1);
gTorusShapeId = hlGenShapes(1);
gTeapotShapeId = hlGenShapes(1);
// Setup event callbacks.
hlAddEventCallback(HL_EVENT_TOUCH, HL_OBJECT_ANY, HL_CLIENT_THREAD,
&touchShapeCallback, NULL);
hlAddEventCallback(HL_EVENT_UNTOUCH, HL_OBJECT_ANY, HL_CLIENT_THREAD,
&untouchShapeCallback, NULL);
hlAddEventCallback(HL_EVENT_TOUCH, gSphereShapeId, HL_CLIENT_THREAD,
&touchSphereCallback, NULL);
hlAddEventCallback(HL_EVENT_UNTOUCH, gSphereShapeId, HL_CLIENT_THREAD,
&untouchSphereCallback, NULL);
hlAddEventCallback(HL_EVENT_TOUCH, gTorusShapeId, HL_CLIENT_THREAD,
&touchTorusCallback, NULL);
hlAddEventCallback(HL_EVENT_UNTOUCH, gTorusShapeId, HL_CLIENT_THREAD,
&untouchTorusCallback, NULL);
hlAddEventCallback(HL_EVENT_TOUCH, gTeapotShapeId, HL_CLIENT_THREAD,
&touchTeapotCallback, NULL);
hlAddEventCallback(HL_EVENT_UNTOUCH, gTeapotShapeId, HL_CLIENT_THREAD,
&untouchTeapotCallback, NULL);
hlAddEventCallback(HL_EVENT_1BUTTONDOWN, HL_OBJECT_ANY, HL_CLIENT_THREAD,
&button1DownCallback, NULL);
hlAddEventCallback(HL_EVENT_1BUTTONUP, HL_OBJECT_ANY, HL_CLIENT_THREAD,
&button1UpCallback, NULL);
hlAddEventCallback(HL_EVENT_2BUTTONDOWN, HL_OBJECT_ANY, HL_CLIENT_THREAD,
&button2DownCallback, NULL);
hlAddEventCallback(HL_EVENT_2BUTTONUP, HL_OBJECT_ANY, HL_CLIENT_THREAD,
&button2UpCallback, NULL);
hlAddEventCallback(HL_EVENT_3BUTTONDOWN, HL_OBJECT_ANY, HL_CLIENT_THREAD,
&button3DownCallback, NULL);
hlAddEventCallback(HL_EVENT_3BUTTONUP, HL_OBJECT_ANY, HL_CLIENT_THREAD,
&button3UpCallback, NULL);
hlAddEventCallback(HL_EVENT_1BUTTONDOWN, gSphereShapeId, HL_CLIENT_THREAD,
&button1DownSphereCallback, NULL);
hlAddEventCallback(HL_EVENT_1BUTTONUP, gSphereShapeId, HL_CLIENT_THREAD,
&button1UpSphereCallback, NULL);
hlAddEventCallback(HL_EVENT_MOTION, HL_OBJECT_ANY, HL_CLIENT_THREAD,
&motionCallback, NULL);
hlAddEventCallback(HL_EVENT_MOTION, gSphereShapeId, HL_CLIENT_THREAD,
&motionOnSphereCallback, NULL);
hlAddEventCallback(HL_EVENT_CALIBRATION_UPDATE, HL_OBJECT_ANY,
HL_CLIENT_THREAD, &calibrationCallback, NULL);
hlAddEventCallback(HL_EVENT_CALIBRATION_INPUT, HL_OBJECT_ANY,
HL_CLIENT_THREAD, &calibrationCallback, NULL);
}
HDCallbackCode BaseGeometryPatch::patchCalc(){
HDErrorInfo error;
hduVector3Dd forceVec;
hduVector3Dd targForceVec;
hduVector3Dd loopForceVec;
hduVector3Dd patchMinusDeviceVec;
hduVector3Dd sensorVec;
double sensorRadius = 1.0;
if(simToPhantom == NULL){
return HD_CALLBACK_CONTINUE;
}
HHD hHD = hdGetCurrentDevice();
// reset all the variables that need to accumulate
for(int sensi = 0; sensi < SimToPhantom::NUM_SENSORS; ++sensi){
phantomToSim->forceMagnitude[sensi] = 0;
phantomToSim->forceVec[sensi][0] = 0;
phantomToSim->forceVec[sensi][1] = 0;
phantomToSim->forceVec[sensi][2] = 0;
}
// Begin haptics frame. ( In general, all state-related haptics calls
// should be made within a frame. )
hdBeginFrame(hHD);
/* Get the current devicePos of the device. */
hdGetDoublev(HD_CURRENT_POSITION, devicePos);
hdGetDoublev(HD_CURRENT_GIMBAL_ANGLES, deviceAngle);
hdGetDoublev(HD_CURRENT_TRANSFORM, transformMat);
forceVec[0] = 0;
forceVec[1] = 0;
forceVec[2] = 0;
if(simToPhantom->numTargets > SimToPhantom::MAX_TARGETS)
simToPhantom->numTargets = SimToPhantom::MAX_TARGETS;
// clear all the data structures
if(phantomToSim != NULL){
for(int i = 0; i < PhantomToSim::NUM_SENSORS; ++i){
phantomToSim->forceMagnitude[i] = 0;
phantomToSim->forceVec[i][0] = 0;
phantomToSim->forceVec[i][1] = 0;
phantomToSim->forceVec[i][2] = 0;
for(int j = 0; j < PhantomToSim::MAX_TARGETS; ++j){
phantomToSim->targForceMagnitude[j][i] = 0;
phantomToSim->targForceVec[j][i][0] = 0;
phantomToSim->targForceVec[j][i][1] = 0;
phantomToSim->targForceVec[j][i][2] = 0;
}
}
for(int j = 0; j < PhantomToSim::MAX_TARGETS; ++j){
phantomToSim->targForcesMagnitude[j] = 0;
}
}
/***/
//printf("BaseGeometryPatch::patchCalc() numtargets = %d\n", simToPhantom->numTargets);
for(int targi = 0; targi < simToPhantom->numTargets; ++targi){
patchPos[0] = simToPhantom->targetX[targi];
patchPos[1] = simToPhantom->targetY[targi];
patchPos[2] = simToPhantom->targetZ[targi];
patchRadius = simToPhantom->targetRadius[targi];
targForceVec[0] = 0;
targForceVec[1] = 0;
targForceVec[2] = 0;
for(int sensi = 0; sensi < SimToPhantom::NUM_SENSORS; ++sensi){
loopForceVec[0] = 0;
loopForceVec[1] = 0;
loopForceVec[2] = 0;
sensorVec[0] = simToPhantom->sensorX[sensi] + devicePos[0];
sensorVec[1] = simToPhantom->sensorY[sensi] + devicePos[1];
sensorVec[2] = simToPhantom->sensorZ[sensi] + devicePos[2];
sensorRadius = simToPhantom->sensorRadius[sensi];
// patchMinusDeviceVec = patchPos-devicePos <
// Create a vector from the device devicePos towards the sphere's center.
//hduVecSubtract(patchMinusDeviceVec, patchPos, devicePos);
hduVecSubtract(patchMinusDeviceVec, patchPos, sensorVec);
hduVector3Dd dirVec;
hduVecNormalize(dirVec, patchMinusDeviceVec);
// If the device position is within the sphere's surface
// center, apply a spring forceVec towards the surface. The forceVec
// calculation differs from a traditional gravitational body in that the
// closer the device is to the center, the less forceVec the well exerts;
// the device behaves as if a spring were connected between itself and
// the well's center. *
double penetrationDist = patchMinusDeviceVec.magnitude() - (patchRadius+sensorRadius);
if(penetrationDist < 0)
{
// > F = k * x <
// F: forceVec in Newtons (N)
// k: Stiffness of the well (N/mm)
// x: Vector from the device endpoint devicePos to the center
// of the well.
hduVecScale(dirVec, dirVec, penetrationDist);
hduVecScale(loopForceVec, dirVec, stiffnessK);
/***
for(int i = 0; i < 3; ++i)
if(loopForceVec[i] < 0)
loopForceVec[i] *= 0.5;
printf("targForceVec[%d][%d] = %.2f, %.2f, %.2f\r", targi, sensi, loopForceVec[0], loopForceVec[1], loopForceVec[2]);
/****/
}
if(phantomToSim != NULL){
phantomToSim->forceMagnitude[sensi] += hduVecMagnitude(loopForceVec);
phantomToSim->forceVec[sensi][0] += loopForceVec[0];
phantomToSim->forceVec[sensi][1] += loopForceVec[1];
phantomToSim->forceVec[sensi][2] += loopForceVec[2];
phantomToSim->targForceMagnitude[targi][sensi] = hduVecMagnitude(loopForceVec);
phantomToSim->targForceVec[targi][sensi][0] = loopForceVec[0];
phantomToSim->targForceVec[targi][sensi][1] = loopForceVec[1];
phantomToSim->targForceVec[targi][sensi][2] = loopForceVec[2];
}
hduVecAdd(targForceVec, targForceVec, loopForceVec);
hduVecAdd(forceVec, forceVec, loopForceVec);
}
if(phantomToSim != NULL){
phantomToSim->targForcesMagnitude[targi] = hduVecMagnitude(targForceVec);
}
}
if(phantomToSim != NULL){
for(int i = 0; i < 16; ++i){
phantomToSim->matrix[i] = transformMat[i];
}
}
// divide the forceVec the number of times that a force was added?
/* Send the forceVec to the device. */
if(simToPhantom->enabled){
hdSetDoublev(HD_CURRENT_FORCE, forceVec);
}
/* End haptics frame. */
hdEndFrame(hHD);
/* Check for errors and abort the callback if a scheduler error
is detected. */
if (HD_DEVICE_ERROR(error = hdGetError()))
{
hduPrintError(stderr, &error, "BaseGeometryPatch.calcPatch():\n");
if (hduIsSchedulerError(&error))
{
return HD_CALLBACK_DONE;
}
}
/* Signify that the callback should continue running, i.e. that
it will be called again the next scheduler tick. */
return HD_CALLBACK_CONTINUE;
}
/*******************************************************************************
This routine allows the device to provide information about the current
location of the stylus, and contains a mechanism for terminating the
application.
Pressing the button causes the application to display the current location
of the device.
Holding the button down for N iterations causes the application to exit.
*******************************************************************************/
void mainLoop(void)
{
static const int kTerminateCount = 1000;
int buttonHoldCount = 0;
/* Instantiate the structure used to capture data from the device. */
DeviceData currentData;
DeviceData prevData;
/* Perform a synchronous call to copy the most current device state. */
hdScheduleSynchronous(copyDeviceDataCallback,
¤tData, HD_MIN_SCHEDULER_PRIORITY);
memcpy(&prevData, ¤tData, sizeof(DeviceData));
printHelp();
/* Run the main loop until the gimbal button is held. */
while (1)
{
/* Perform a synchronous call to copy the most current device state.
This synchronous scheduler call ensures that the device state
is obtained in a thread-safe manner. */
hdScheduleSynchronous(copyDeviceDataCallback,
¤tData,
HD_MIN_SCHEDULER_PRIORITY);
/* If the user depresses the gimbal button, display the current
location information. */
if (currentData.m_buttonState && !prevData.m_buttonState)
{
fprintf(stdout, "Current position: (%g, %g, %g)\n",
currentData.m_devicePosition[0],
currentData.m_devicePosition[1],
currentData.m_devicePosition[2]);
}
else if (currentData.m_buttonState && prevData.m_buttonState)
{
/* Keep track of how long the user has been pressing the button.
If this exceeds N ticks, then terminate the application. */
buttonHoldCount++;
if (buttonHoldCount > kTerminateCount)
{
/* Quit, since the user held the button longer than
the terminate count. */
break;
}
}
else if (!currentData.m_buttonState && prevData.m_buttonState)
{
/* Reset the button hold count, since the user stopped holding
down the stylus button. */
buttonHoldCount = 0;
}
/* Check if an error occurred. */
if (HD_DEVICE_ERROR(currentData.m_error))
{
hduPrintError(stderr, ¤tData.m_error, "Device error detected");
if (hduIsSchedulerError(¤tData.m_error))
{
/* Quit, since communication with the device was disrupted. */
fprintf(stderr, "\nPress any key to quit.\n");
getch();
break;
}
}
/* Store off the current data for the next loop. */
memcpy(&prevData, ¤tData, sizeof(DeviceData));
}
}
