/*******************************************************************************
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.");
}
}
HDCallbackCode HDCALLBACK CybPhantom::UpdateCalibrationCallback(void *pUserData)
{
CalibrationState *pState = (CalibrationState *) pUserData;
if( hdCheckCalibration() == HD_CALIBRATION_NEEDS_UPDATE)
{
hdUpdateCalibration(pState->calibrationType);
}
return HD_CALLBACK_DONE;
}
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;
}
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");
}
