// This routine will reset the 3DMicroscribe, zeroing the Origin position, // mode. int vrpn_3DMicroscribe::reset(void) { #ifdef VRPN_USE_MICROSCRIBE int iResult; // ARM_FULL: ArmDll32 calculates and updates the Cartesian position and orientation of the stylus tip. iResult = ArmSetUpdate(ARM_FULL); if(iResult != ARM_SUCCESS) { //error setting the update type, disconnect and end the thread VRPN_MSG_ERROR( "Unable to set the update type for the MicroScribe." ); return -1; } //use mm instead of inches ArmSetLengthUnits(ARM_MM); //use radians instead of degrees //ArmSetAngleUnits(ARM_RADIANS); //get the position of the tip length_3D tipPosition; angle_3D tipVector; iResult = ArmGetTipPosition(&tipPosition); //retrieves the current stylus tip position in Cartesian coordinates iResult = ArmGetTipOrientationUnitVector(&tipVector); //retrieves the current stylus tip's unit vector orientation if(iResult == ARM_NOT_CONNECTED) { //error connecting VRPN_MSG_ERROR( "MicroScribe connection lost!" ); return -1; } #endif // We're now waiting for a response from the box status = STATUS_SYNCING; vrpn_gettimeofday(×tamp, NULL); // Set watchdog now return 0; }
int vrpn_3DMicroscribe::get_report(void) { #ifdef VRPN_USE_MICROSCRIBE length_3D tipPosition; angle_3D tipOri; DWORD buts; int iResult = ArmGetTipPosition(&tipPosition); //retrieves the current stylus tip position in Cartesian coordinates iResult = ArmGetTipOrientation(&tipOri); //retrieves the current stylus tip's unit vector orientation iResult = ArmGetButtonsState(&buts); if(iResult == ARM_NOT_CONNECTED) { //error connecting VRPN_MSG_ERROR( "MicroScribe connection lost!" ); return 0; } //set the position, considering the scale, offset and origin matrix pos[0] = (tipPosition.y * m_Scale + m_OffSet[0])* MM_TO_METERS ; pos[1] = (tipPosition.z * m_Scale + m_OffSet[1])* MM_TO_METERS; pos[2] = (tipPosition.x * m_Scale + m_OffSet[2])* MM_TO_METERS; //vPosition = m_Matrix * vPosition + m_vPlaneOffset; extending the microscribe onto a plane //set the orientation, considering the origin matrix float ori[3]={tipOri.y, tipOri.z, tipOri.x}; ConvertOriToQuat(ori); status = STATUS_READING; // ready to process event packet vrpn_gettimeofday(×tamp, NULL); // set timestamp of this event buttons[0] = ((buts & 0x02) != 0); // button 1 buttons[1] = ((buts & 0x01) != 0); // button 2 #endif report_changes(); // Report updates to VRPN return 0; }
void mtsMicroScribeDigitizer::Run(void) { // timestamp of last reconnect trial in case of disconnection static double lastReconnectTryTime = 0; ProcessQueuedCommands(); // If not connected, try reconnect at every 1 sec if (!DeviceConnected) { if (lastReconnectTryTime == 0) { lastReconnectTryTime = osaGetTime(); } else if (lastReconnectTryTime + 1.0 * cmn_s < osaGetTime()) { CMN_LOG_CLASS_INIT_VERBOSE << "Run: Try reconnecting to digitizer..." << std::endl; if (ARM_SUCCESS == ArmReconnect()) { OnDeviceConnection(); } else { CMN_LOG_CLASS_INIT_VERBOSE << "Run: Failed to reconnect to digitizer." << std::endl; } } return; } // read digitizer tip position if (ARM_NOT_CONNECTED == ArmGetTipPosition(&TipPositionVendor)) { // Disconnection detected OnDeviceDisconnection(); return; } // read digitizer tip orientation if (ARM_NOT_CONNECTED == ArmGetTipOrientation(&TipOrientationVendor)) { // Disconnection detected OnDeviceDisconnection(); return; } // read digitizer tip orientation unit vector if (ARM_NOT_CONNECTED == ArmGetTipOrientationUnitVector(&TipOrientationUnitVectorVendor)) { // Disconnection detected OnDeviceDisconnection(); return; } // read digitizer button states if (ARM_NOT_CONNECTED == ArmGetButtonsState(&ButtonStateVendor)) { // Disconnection detected OnDeviceDisconnection(); return; } // read digitizer joint readings if (ARM_NOT_CONNECTED == ArmGetJointAngles(ARM_DEGREES, // or ARM_RADIANS JointReadingsVendor)) { // Disconnection detected OnDeviceDisconnection(); return; } // Convert vendor-defined container to cisst container // position TipPosition(X) = TipPositionVendor.x; TipPosition(Y) = TipPositionVendor.y; TipPosition(Z) = TipPositionVendor.z; // orientation TipOrientation(ROLL) = TipOrientationVendor.x; TipOrientation(PITCH) = TipOrientationVendor.y; TipOrientation(YAW) = TipOrientationVendor.z; // orientation unit vector TipOrientationUnitVector(ROLL) = TipOrientationUnitVectorVendor.x; TipOrientationUnitVector(PITCH) = TipOrientationUnitVectorVendor.y; TipOrientationUnitVector(YAW) = TipOrientationUnitVectorVendor.z; // button state static DWORD lastButtonStateVendor = 0; bool buttonStateChange = false; if (!(lastButtonStateVendor & ARM_BUTTON_1) && (ButtonStateVendor & ARM_BUTTON_1)) { EventButton1Down(); ButtonState(BUTTON_1) = DOWN; buttonStateChange = true; } else if ((lastButtonStateVendor & ARM_BUTTON_1) && !(ButtonStateVendor & ARM_BUTTON_1)) { EventButton1Up(); ButtonState(BUTTON_1) = UP; buttonStateChange = true; } if (!(lastButtonStateVendor & ARM_BUTTON_2) && (ButtonStateVendor & ARM_BUTTON_2)) { EventButton2Down(); ButtonState(BUTTON_2) = DOWN; buttonStateChange = true; } else if ((lastButtonStateVendor & ARM_BUTTON_2) && !(ButtonStateVendor & ARM_BUTTON_2)) { EventButton2Up(); ButtonState(BUTTON_2) = UP; buttonStateChange = true; } if (buttonStateChange) { lastButtonStateVendor = ButtonStateVendor; } // Joint readings for (size_t i = 0; i < NUM_DOF; ++i) { JointReadings(i) = JointReadingsVendor[i]; } }