/******************************************************************************* 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"); }