//============================================================================= #include "stdafx.h" #include "Mv_Proto.h" #include "SsiStatus.h" #include "Logger.h" #include "math.h" #define MY_CONFIG 1 #define MAX_DEVPATH_LENGTH 256 #define ENDPOINT_TIMEOUT 500 //***** Static Data ***** struct_ep_buff CMv_Proto::ep_buff[lEPSIZE]; //================================================================ int CMv_Proto::g_hEP81_Thread_State=THREAD_PAUSED; HANDLE CMv_Proto::g_hEP81_Thread_Id=NULL; //================================================================ int CMv_Proto::g_hEP82_Thread_State=THREAD_PAUSED; HANDLE CMv_Proto::g_hEP82_Thread_Id=NULL; //================================================================ int CMv_Proto::g_hEP01_Thread_State=THREAD_PAUSED; HANDLE CMv_Proto::g_hEP01_Thread_Id=NULL; HANDLE CMv_Proto::g_hEP01_Serial_Mutex; //================================================================ int CMv_Proto::g_hEP02_Thread_State=THREAD_PAUSED; HANDLE CMv_Proto::g_hEP02_Thread_Id=NULL; HANDLE CMv_Proto::g_hEP02_Serial_Mutex; //================================================================ struct_machine CMv_Proto::g_machine; usb_dev_handle *CMv_Proto::g_dev=NULL; CLogger *CMv_Proto::g_pLogger; HANDLE CMv_Proto::g_hHomedEvent = NULL; //=========================================================================== // Worker Thread to serialize EP_01 commands. //=========================================================================== unsigned __stdcall CMv_Proto::g_EP01_Thread(LPVOID pThis) { CMv_Proto* _This = (CMv_Proto*)pThis; for (;;) { TRACE0("g_hSerialUsbThread in loop set.\n"); if (g_hEP01_Thread_State == THREAD_EXIT) ExitThread(0); WaitForSingleObject(ep_buff[EP_01_CMD_IDX]._event,INFINITE); TRACE0("g_hSerialUsbThread obtained event.\n"); switch (g_hEP01_Thread_State) { case THREAD_EXIT: { ExitThread(0); } case THREAD_PAUSED: break; case THREAD_RUNNING: { TRACE0("g_hSerialUsbThread processing _send_usb_data();\n"); _This->_send_usb_data(EP_01_CMD_IDX); TRACE0("g_hSerialUsbThread return from _send_usb_data();\n"); break; } default: ExitThread(0); } }; g_hEP01_Thread_State = THREAD_EXIT; ExitThread(0); }; //============================================== ============================= // Worker Thread to serialize EP_02 commands. //=========================================================================== unsigned __stdcall CMv_Proto::g_EP02_Thread(LPVOID pThis) { CMv_Proto* _This = (CMv_Proto*)pThis; for (;;) { TRACE0("g_hEP02_Thread in loop set.\n"); if (g_hEP02_Thread_State == THREAD_EXIT) ExitThread(0); WaitForSingleObject(ep_buff[EP_02_CMD_IDX]._event,INFINITE); TRACE0("g_hEP02_Thread obtained event.\n"); switch (g_hEP02_Thread_State) { case THREAD_EXIT: { ExitThread(0); } case THREAD_PAUSED: break; case THREAD_RUNNING: { TRACE0("g_hEP02_Thread calling _send_usb_data. EP_02; %x\n"); _This->_send_usb_data(EP_02_CMD_IDX); TRACE0("g_hEP02_Thread return _send_usb_data. EP_02; %x\n"); break; } default: ExitThread(0); } }; g_hEP02_Thread_State = THREAD_EXIT; ExitThread(0); }; //****************************************************************************** // This is the worker thread to process USBD_TRANSFER_DIRECTION_IN //****************************************************************************** unsigned __stdcall CMv_Proto::g_EP81_Thread(LPVOID pThis) { CMv_Proto* _This = (CMv_Proto*)pThis; for (;;) { WaitForSingleObject(ep_buff[EP_81_DATA_IDX]._event, INFINITE); switch (g_hEP81_Thread_State) { case THREAD_EXIT: { ExitThread(0); } case THREAD_PAUSED: break; case THREAD_RUNNING: { _This->_reap_async_8x(EP_81_DATA_IDX); break; } default: ExitThread(0); } } } //****************************************************************************** // This is the worker thread to process USBD_TRANSFER_DIRECTION_IN //****************************************************************************** unsigned __stdcall CMv_Proto::g_EP82_Thread(LPVOID pThis) { CMv_Proto* _This = (CMv_Proto*)pThis; for (;;) { WaitForSingleObject(ep_buff[EP_82_DATA_IDX]._event, INFINITE); switch (g_hEP82_Thread_State) { case THREAD_EXIT: { ExitThread(0); } case THREAD_PAUSED: break; case THREAD_RUNNING: { _This->_reap_async_8x(EP_82_DATA_IDX); break; } default: ExitThread(0); } } } //=========================================================================== double CMv_Proto::ScaleToMM(long lCount, double dResolution) { double dMM = 0.0; dMM = lCount * dResolution; return dMM; } //=========================================================================== long CMv_Proto::MMtoScale(double lDistanceMM, double dResolution) { long lCounts = 0; if (dResolution) lCounts = (long) (lDistanceMM / dResolution); else ASSERT(0); return lCounts; } //=========================================================================== void CMv_Proto::Trace_EP_Buff(long lIndex) { UCHAR tmp[256]; CString csTmp; memcpy(tmp, ep_buff[lIndex]._buffer, ep_buff[lIndex]._size); csTmp = _T("Trace_EP_Buff _59 "); for (int ii= 0 ; ii < ep_buff[lIndex]._size ; ++ii) { CString csTmpHex; csTmpHex.Format(_T("%2X"), tmp[ii] ); csTmp += csTmpHex; } TRACE1("_process_MV_CMD_GET_INDEX_4E() Trace_EP_Buff %s \n", csTmp); } //===================================================================================== long CMv_Proto::_4char2long(unsigned char *cBuff) { union { long l_value; char c_array[5]; }; memset (c_array, 0, 5); c_array[0] = cBuff[3]; c_array[1] = cBuff[2]; c_array[2] = cBuff[1]; c_array[3] = cBuff[0]; return(l_value); } //======================================================================== void CMv_Proto::_reverse_dword(DWORD *dWord) { BYTE cBuff[4]; BYTE *dwBuff = (BYTE *)dWord; for ( int ii = 0 ; ii < 4 ; ++ii ) cBuff[ii]= dwBuff[ii]; dwBuff[0] = cBuff[3]; dwBuff[1] = cBuff[2]; dwBuff[2] = cBuff[1]; dwBuff[3] = cBuff[0]; } //======================================================================== void CMv_Proto::mv_set_full_ringlight_data(long lIntensity) { if (g_machine.s_machine_config._vector_light_model == 3) { for (int ii=0 ; iig_machine.s_machine_config._vector_light_model == 2) { for (int ii=0 ; ii>8; Val = MSB|LSB; } //****************************************************************************** double CMv_Proto::TimeInSecs(void) { double Secs; LARGE_INTEGER HPCounterTicksPerSecond; BOOL HasHPCounter = QueryPerformanceFrequency(&HPCounterTicksPerSecond); if (HasHPCounter == TRUE) { // Use high resolution clock. double HPCounterTicksPersec = (DWORD)((double) HPCounterTicksPerSecond.QuadPart); LARGE_INTEGER HPTicks; QueryPerformanceCounter(&HPTicks); Secs = ((double)HPTicks.QuadPart / HPCounterTicksPersec); } else { // Use clock with less resolution. Secs = GetTickCount(); Secs /= 1000.0; } return Secs; } //****************************************************************************** CMv_Proto::CMv_Proto() { ep_buff[EP_01_CMD_IDX]._ep = EP_01; ep_buff[EP_81_DATA_IDX]._ep = EP_81; ep_buff[EP_02_CMD_IDX]._ep = EP_02; ep_buff[EP_82_DATA_IDX]._ep = EP_82; for (int i=0;i 2)) { token = strtok( szLine, seps ); // Get the command token if (!_stricmp(token, "DEBUG")) // Hex Mask { token = strtok( NULL, seps); if (token) { g_pLogger->m_lLogMask=atoi(token); // 0 or 1 to indicate ON/Off } } else if (!_stricmp(token, "STR_2B01")) // STR_2B_01 { token = strtok( NULL, seps); if (token) { strcpy(g_machine.s_machine_config._str_2b, token); _char2bin((unsigned char *)g_machine.s_machine_config._str_2b, g_machine.s_machine_config._str_bin_2b, (int) strlen(g_machine.s_machine_config._str_2b)); } } else if (!_stricmp(token, "STR_7000")) // STR_7000 { token = strtok( NULL, seps); if (token) { strcpy(g_machine.s_machine_config._str_7000_signature, token); _char2bin((unsigned char *)g_machine.s_machine_config._str_7000_signature, g_machine.s_machine_config._str_bin_7000, (int) strlen(g_machine.s_machine_config._str_7000_signature)); } } else if (!_stricmp(token, "STR_6f00")) // STR_6f00 { token = strtok( NULL, seps); if (token) { strcpy(g_machine.s_machine_config._str_6f00_signature, token); _char2bin((unsigned char *)g_machine.s_machine_config._str_6f00_signature, g_machine.s_machine_config._str_bin_6f00, (int) strlen(g_machine.s_machine_config._str_6f00_signature)); } } else if (!_stricmp(token, "USB_COMMAND_WAIT")) // Pause between Usb Receive Commands { token = strtok( NULL, seps); if (token) { g_machine.s_machine_config._usb_command_wait=atoi(token); // 0 or 1 to indicate ON/Off } } else if (!_stricmp(token, "USB_COMMAND_TIMEOUT")) // How long should we wait before we call it timeout { token = strtok( NULL, seps); if (token) { g_machine.s_machine_config._usb_command_timeout=atoi(token); // 0 or 1 to indicate ON/Off } } } } fclose(hConfigFile); } else { return SSI_STATUS_MV_CONFIG_FILE_NOT_FOUND; }; return SSI_STATUS_NORMAL; }; //****************************************************************************** usb_dev_handle* CMv_Proto::_open_usb_dev(void) { struct usb_bus *bus = NULL; struct usb_device *dev = NULL; for (bus = usb_get_busses(); bus; bus = bus->next) { for (dev = bus->devices; dev; dev = dev->next) { if (dev->descriptor.idVendor == MICROVU_VID && dev->descriptor.idProduct == MICROVU_PID) { return usb_open(dev); } } } return NULL; } //****************************************************************************** // Send is direct and async. // The receive thread will receive data and interpret it. //****************************************************************************** SSI_STATUS CMv_Proto::Init_MvUsb() { // Set initial state of the machine g_machine.s_status._machine_running = false; g_machine.s_status._poll_58_active = false; g_machine.s_status._poll_59_active = false; g_machine.s_status._poll_2b_active = false; SSI_STATUS Status=SSI_STATUS_NORMAL; UNREFERENCED_PARAMETER(Status); if (g_pLogger->m_lLogMask & LOGACTIONS) g_pLogger->SendAndFlushPerMode(_T("Enter Initialize Mv Usb\n")); int usb_status = NULL; usb_init(); // initialize the library usb_status = usb_find_busses(); // find all busses usb_status = usb_find_devices(); // find all connected devices g_dev = _open_usb_dev(); if (!g_dev) { MessageBox(NULL, _T("Unable to open device"), _T("Message"), MB_OK); g_pLogger->SendAndFlushPerMode(_T("Unable to open device %s"), usb_strerror()); return SSI_STATUS_DATALINK_ERROR; } if (usb_set_configuration(g_dev, MY_CONFIG) < 0) { MessageBox(NULL, _T("Unable to SET CONFIGURATION"), _T("Message"), MB_OK); return SSI_STATUS_DATALINK_ERROR; } if (usb_claim_interface(g_dev, 0) < 0) { usb_close(g_dev); MessageBox(NULL, _T("Unable to CLAIM DEVICE"), _T("Message"), MB_OK); return SSI_STATUS_DATALINK_ERROR; } // ******************************************************************** // This event is used to kick the Serial Usb Command process. This threading model // is important because the underlying library is not thread-safe. // ep_buff[EP_01_CMD_IDX]._event = CreateEvent(NULL, // default security attributes FALSE, // manual reset event object NULL, // signaled NULL); // unamed object g_hEP01_Thread_Id = CreateThread( (LPSECURITY_ATTRIBUTES) NULL, 0, (LPTHREAD_START_ROUTINE) g_EP01_Thread, (LPVOID) this, 0, NULL); g_hEP01_Thread_State = THREAD_RUNNING; // ******************************************************************** // This event is used to kick the Serial Usb Command process. This threading model // is important because the underlying library is not thread-safe. // ep_buff[EP_02_CMD_IDX]._event = CreateEvent(NULL, // default security attributes FALSE, // manual reset event object NULL, // signaled NULL); // unamed object g_hEP02_Thread_Id = CreateThread( (LPSECURITY_ATTRIBUTES) NULL, 0, (LPTHREAD_START_ROUTINE) g_EP02_Thread, (LPVOID) this, 0, NULL); g_hEP02_Thread_State = THREAD_RUNNING; // ******************************************************************** // Prepare and start EP_81 Thread - Use async commit. // ep_buff[EP_81_DATA_IDX]._event = CreateEvent(NULL, // default security attributes FALSE, // manual reset event object NULL, // signaled NULL); // unamed object g_hEP81_Thread_State = THREAD_PAUSED; g_hEP81_Thread_Id = CreateThread( (LPSECURITY_ATTRIBUTES) NULL, 0, (LPTHREAD_START_ROUTINE) g_EP81_Thread, (LPVOID) this, 0, NULL); g_hEP01_Serial_Mutex = CreateMutex(NULL, // default security attributes FALSE, // initial owner NULL); // name // ******************************************************************** // Prepare and start EP_82 Thread - Use async commit. // ep_buff[EP_82_DATA_IDX]._event = CreateEvent(NULL, // default security attributes FALSE, // manual reset event object NULL, // signaled NULL); // unamed object g_hEP82_Thread_State = THREAD_PAUSED; g_hEP82_Thread_Id = CreateThread( (LPSECURITY_ATTRIBUTES) NULL, 0, (LPTHREAD_START_ROUTINE) g_EP82_Thread, (LPVOID) this, 0, NULL); g_hEP02_Serial_Mutex = CreateMutex(NULL, // default security attributes FALSE, // initial owner NULL); // name // ********************************************************************* g_hHomedEvent = CreateEvent(NULL, // default security attributes TRUE, // manual reset event object FALSE, // initial state is signaled NULL); // unamed object if (g_pLogger->m_lLogMask & LOGACTIONS) g_pLogger->SendAndFlushPerMode(_T("Exit Initialize Usb\n")); return SSI_STATUS_NORMAL; } //****************************************************************************** SSI_STATUS CMv_Proto::Exit_MvUsb() { SSI_STATUS Status=SSI_STATUS_NORMAL; if (g_pLogger->m_lLogMask & LOGACTIONS) g_pLogger->SendAndFlushPerMode(_T("Enter Exit_MvUsb\n")); g_hEP81_Thread_State = THREAD_EXIT; g_hEP82_Thread_State = THREAD_EXIT; g_hEP01_Thread_State = THREAD_EXIT; g_hEP02_Thread_State = THREAD_EXIT; SetEvent(ep_buff[EP_81_DATA_IDX]._event); if (g_hEP81_Thread_Id) { DWORD dwCode = STILL_ACTIVE; while (dwCode == STILL_ACTIVE) { GetExitCodeThread(g_hEP81_Thread_Id,&dwCode); Sleep(1); } } SetEvent(ep_buff[EP_82_DATA_IDX]._event); if (g_hEP82_Thread_Id) { DWORD dwCode = STILL_ACTIVE; while (dwCode == STILL_ACTIVE) { GetExitCodeThread(g_hEP82_Thread_Id,&dwCode); Sleep(1); } } SetEvent(ep_buff[EP_01_CMD_IDX]._event); if (g_hEP01_Thread_Id) { DWORD dwCode = STILL_ACTIVE; while (dwCode == STILL_ACTIVE) { GetExitCodeThread(g_hEP01_Thread_Id,&dwCode); Sleep(1); } } SetEvent(ep_buff[EP_02_CMD_IDX]._event); if (g_hEP02_Thread_Id) { DWORD dwCode = STILL_ACTIVE; while (dwCode == STILL_ACTIVE) { GetExitCodeThread(g_hEP02_Thread_Id,&dwCode); Sleep(1); } } if (g_dev) { usb_release_interface(g_dev,0); usb_close(g_dev); g_dev = NULL; } SetEvent(ep_buff[EP_81_DATA_IDX]._event); CloseHandle(ep_buff[EP_81_DATA_IDX]._event); SetEvent(ep_buff[EP_82_DATA_IDX]._event); CloseHandle(ep_buff[EP_82_DATA_IDX]._event); g_hEP01_Thread_State = THREAD_EXIT; SetEvent(ep_buff[EP_01_CMD_IDX]._event); CloseHandle(ep_buff[EP_01_CMD_IDX]._event); g_hEP02_Thread_State = THREAD_EXIT; SetEvent(ep_buff[EP_02_CMD_IDX]._event); CloseHandle(ep_buff[EP_02_CMD_IDX]._event); ReleaseMutex(g_hEP01_Serial_Mutex); CloseHandle(g_hEP01_Serial_Mutex); ReleaseMutex(g_hEP02_Serial_Mutex); CloseHandle(g_hEP02_Serial_Mutex); if (g_pLogger->m_lLogMask & LOGACTIONS) g_pLogger->SendAndFlushPerMode(_T("Exit Exit_MvUsb\n")); return Status; } //****************************************************************************** SSI_STATUS CMv_Proto::ExtractAppPath(CString &Path) { CString tmpPath = Path; tmpPath.TrimRight(); tmpPath.TrimLeft(); int nLastSlash = tmpPath.ReverseFind('\\'); if (nLastSlash > -1) { // complete path tmpPath = Path.Left(nLastSlash); Path = tmpPath; } else { // not a complete path Path=""; }; return SSI_STATUS_NORMAL; }; //****************************************************************************** SSI_STATUS CMv_Proto::GetAppPath(CString &Path) { Path=_T(""); // Speed optimization - noticed slow in GlowCode if (Path.IsEmpty()) { CString tmpPath; GetModuleFileName(NULL,tmpPath.GetBuffer(255),255); tmpPath.ReleaseBuffer(); tmpPath.TrimRight(); int nLastSlash = tmpPath.ReverseFind('\\'); if (nLastSlash >= 0) tmpPath = tmpPath.Left(nLastSlash); else tmpPath.Empty(); Path=tmpPath; } return SSI_STATUS_NORMAL; }; //****************************************************************************** // Replay the capture file. // There are three special commands: // 7000 // 6f00 // 4c02 //****************************************************************************** SSI_STATUS CMv_Proto::_replay_capture(CString s_replay_file) { char *_0x4e00_cmd = "4e00"; FILE* pInFile; _wfopen_s(&pInFile, s_replay_file, _T("r")); if (pInFile == NULL) return SSI_STATUS_REPLAY_FILE_ERROR; char *cData = (char *)malloc(MAX_BUFF_SIZE); char *inBuff = (char *)malloc(MAX_BUFF_SIZE); fgets((char *)inBuff, MAX_BUFF_SIZE, pInFile ); // pick up the first line while (!feof(pInFile)) { if (*inBuff == '>') { if (strstr(inBuff, "0x00000001")) { if (!(_strnicmp(inBuff+35, _0x4e00_cmd, 4))) break; _process_replay_capture_commands(inBuff, pInFile); } }; fgets((char *)inBuff, MAX_BUFF_SIZE, pInFile ); // pick up the first line }; fclose(pInFile); free(cData); free(inBuff); return SSI_STATUS_NORMAL; } //****************************************************************************** // Do not send out DCC Home. We can send everything else. //****************************************************************************** SSI_STATUS CMv_Proto::_process_replay_capture_commands(char *inBuff, FILE* pInFile) { char x[3]; char cSize[9]; int iSendSize; int iRcvSize; if ( ((*(inBuff+35) == '4')) && (*(inBuff+36) == 'f')) { fgets((char *)inBuff, MAX_BUFF_SIZE, pInFile ); // pick up the first line return SSI_STATUS_NORMAL; }; memset(cSize, 0 , 9); memcpy(cSize, inBuff+24, 8); sscanf_s(cSize, "%8x", &iSendSize); // get the length of the transmission memset(ep_buff[EP_01_CMD_IDX]._buffer, 0, MAX_BUFF_SIZE); for (int j=0;j 0) { _process_rcv_transfer_data(iEP_Base); usb_free_async(&(ep_buff[iEP_Base]._async_context)); ep_buff[iEP_Base]._hProtoPending = false; } else { usb_free_async(&(ep_buff[iEP_Base]._async_context)); ASSERT(0); return SSI_STATUS_MV_COMMAND_TIMEOUT; } return SSI_STATUS_NORMAL; } //=============================================================================== // _send_usb_data(iEP) sends data to the corresponding iEP channel. // iEP = EP_01 / EP_02 EP_01 = 0x01; EP_02 = 0x02; //=============================================================================== SSI_STATUS CMv_Proto::_send_usb_data(int iEP_Base) { int _ret; ep_buff[iEP_Base]._save_send_cmd = ep_buff[iEP_Base]._buffer[0]; ep_buff[iEP_Base]._save_send_cmd0 = ep_buff[iEP_Base]._buffer[1]; ep_buff[iEP_Base]._save_send_cmd1 = ep_buff[iEP_Base]._buffer[2]; TRACE3("_send_usb_data() iEP : %X - ep_buff[iEP]._save_send_cmd : %X ._buffer[0] : %X\r\n", iEP_Base, ep_buff[EP_01_CMD_IDX]._save_send_cmd, ep_buff[EP_81_DATA_IDX]._buffer[0]); _ret = usb_bulk_setup_async(g_dev, &(ep_buff[iEP_Base]._async_context), (unsigned char)ep_buff[iEP_Base]._ep); if (_ret < 0) { return SSI_STATUS_SETUP_ASYNC_CONTEXT_ERROR; } _ret = usb_submit_async(ep_buff[iEP_Base]._async_context, ep_buff[iEP_Base]._buffer, ep_buff[iEP_Base]._size); if (_ret < 0) { printf("error usb_submit_async_ep_xx:\n%s\n", usb_strerror()); return SSI_STATUS_SETUP_ASYNC_CONTEXT_ERROR; } TRACE1("_submit_async_8x(EP:%X)\r\n", iEP_Base); // ep_buff[iEP_Base+1]._save_send_cmd = ep_buff[iEP_Base+1]._buffer[0]; ep_buff[iEP_Base+1]._save_send_cmd0 = ep_buff[iEP_Base+1]._buffer[1]; ep_buff[iEP_Base+1]._save_send_cmd1 = ep_buff[iEP_Base+1]._buffer[2]; // _submit_async_8x(iEP_Base+1); // Send EP_81/EP_82 // Get ready to receive on EP_81 or EP_82 SetEvent(ep_buff[iEP_Base+1]._event); // Get ready to receive on EP_01 or EP_02 _ret = usb_reap_async(ep_buff[iEP_Base]._async_context, 10000); if (_ret > 0) { _process_rcv_transfer_data(iEP_Base); usb_free_async(&(ep_buff[iEP_Base]._async_context)); ep_buff[iEP_Base]._hProtoPending = false; } else { usb_free_async(&(ep_buff[iEP_Base]._async_context)); ASSERT(0); return SSI_STATUS_MV_COMMAND_TIMEOUT; } return SSI_STATUS_NORMAL; } //======================================================================== // Poll Machine 58 //======================================================================== SSI_STATUS CMv_Proto::_poll_machine_58() { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); *(ep_buff[EP_01_CMD_IDX]._buffer) = MV_CMD_GET_SCALE_58; ep_buff[EP_01_CMD_IDX]._size = 0x01; // send 0x01 bytes over ep_buff[EP_81_DATA_IDX]._size = 0x11; // expects 45 bytes back _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //======================================================================== // Poll Machine // submit poll command, update the position readout //======================================================================== SSI_STATUS CMv_Proto::_poll_machine_59() { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); *(ep_buff[EP_01_CMD_IDX]._buffer) = MV_CMD_GET_POLL_59; *(ep_buff[EP_01_CMD_IDX]._buffer+1) = 0x01; *(ep_buff[EP_01_CMD_IDX]._buffer+2) = 0x01; ep_buff[EP_01_CMD_IDX]._size = 0x10; // send 0x01 bytes over ep_buff[EP_81_DATA_IDX]._size = 0x3e; // expects 45 bytes back _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //======================================================================== // _poll_machine_2b //======================================================================== SSI_STATUS CMv_Proto::_poll_machine_2b() { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); *ep_buff[EP_01_CMD_IDX]._buffer = MV_CMD_SERVO_CMD_2B; ep_buff[EP_01_CMD_IDX]._size = 0x01; // send 0x01 bytes over ep_buff[EP_81_DATA_IDX]._size = 0x45; // expects 45 bytes back _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //======================================================================== // //======================================================================== SSI_STATUS CMv_Proto::mv_motion_is_supported() { return SSI_STATUS_NORMAL; } //======================================================================== // call from Hsi //======================================================================== SSI_STATUS CMv_Proto::mv_motion_startup(double dScaleResolutionX, double dScaleResolutionY, double dScaleResolutionZ) { g_machine.s_machine_config.x_axis._scale_resolution = dScaleResolutionX; g_machine.s_machine_config.y_axis._scale_resolution = dScaleResolutionY; g_machine.s_machine_config.z_axis._scale_resolution = dScaleResolutionZ; _start_machine(); mv_motion_Dcc_Home(); _replay_capture(_T("Replay_Capture")); return SSI_STATUS_NORMAL; }; //======================================================================== // //======================================================================== SSI_STATUS CMv_Proto::mv_motion_get_axis_count(int &iCount) { iCount = 4; return SSI_STATUS_NORMAL; } //======================================================================== // This speed setting will be carried out in the next DCC move // Full Speed -> dPercentSpeed = 100% // Slow Speed -> dPercentSpeed = 20% //======================================================================== SSI_STATUS CMv_Proto::mv_motion_set_speed_xyz(double dPercentSpeed) { g_machine.s_machine_config._dXYZSpeed = dPercentSpeed; return SSI_STATUS_NORMAL; } //======================================================================== SSI_STATUS CMv_Proto::mv_motion_get_speed_xyz(double &dPercentSpeed) { dPercentSpeed = g_machine.s_machine_config._dXYZSpeed; return SSI_STATUS_NORMAL; } //======================================================================== // //======================================================================== SSI_STATUS CMv_Proto::mv_motion_set_speed_r(long z_speed) { g_machine.z._speed = z_speed; return SSI_STATUS_NORMAL; }; //======================================================================== // //======================================================================== SSI_STATUS CMv_Proto::mv_motion_get_settle_time() { return SSI_STATUS_NORMAL; }; //======================================================================== // Not implemented //======================================================================== SSI_STATUS CMv_Proto::mv_motion_get_deadband() { return SSI_STATUS_NORMAL; }; //======================================================================== // Not implemented //======================================================================== SSI_STATUS CMv_Proto::mv_motion_get_refresh_deadband() { return SSI_STATUS_NORMAL; }; //======================================================================== // Just get the values from g_machine. //======================================================================== SSI_STATUS CMv_Proto::mv_motion_get_position_xyz(double & dX, double & dY, double & dZ) { long lX=0, lY=0, lZ=0, lZm=0; _send_cmd_MV_CMD_GET_POLL_59(lX, lY, lZ, lZm); lX -= g_machine.s_machine_config.x_axis._neg_working_limit; lY -= g_machine.s_machine_config.y_axis._neg_working_limit; lZ -= g_machine.s_machine_config.z_axis._neg_working_limit; dX = ScaleToMM(lX, g_machine.s_machine_config.x_axis._scale_resolution); dY = ScaleToMM(lY, g_machine.s_machine_config.y_axis._scale_resolution); dZ = ScaleToMM(lZ, g_machine.s_machine_config.z_axis._scale_resolution); return SSI_STATUS_NORMAL; }; //======================================================================== SSI_STATUS CMv_Proto::_set_position_xyz(AXISMOVE &X, AXISMOVE &Y, AXISMOVE &Z) { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_MOVE_TO_53; pMV_CMD_01->S_MV_CMD_0x53.uSubCmdByte = 0; pMV_CMD_01->S_MV_CMD_0x53.statusByte[0] = 1; pMV_CMD_01->S_MV_CMD_0x53.statusByte[1] = 1; pMV_CMD_01->S_MV_CMD_0x53.statusByte[2] = 1; pMV_CMD_01->S_MV_CMD_0x53.statusByte[3] = 0; pMV_CMD_01->S_MV_CMD_0x53.S_POS.x = X.to; pMV_CMD_01->S_MV_CMD_0x53.S_POS.y = Y.to; pMV_CMD_01->S_MV_CMD_0x53.S_POS.z = Z.to; pMV_CMD_01->S_MV_CMD_0x53.S_SPEED.x = X.speed; pMV_CMD_01->S_MV_CMD_0x53.S_SPEED.y = Y.speed; pMV_CMD_01->S_MV_CMD_0x53.S_SPEED.z = Z.speed; #pragma message("set accel and decel") pMV_CMD_01->S_MV_CMD_0x53.S_ACC.x = X.acc; pMV_CMD_01->S_MV_CMD_0x53.S_ACC.y = Y.acc; pMV_CMD_01->S_MV_CMD_0x53.S_ACC.z = Z.acc; pMV_CMD_01->S_MV_CMD_0x53.S_DEC.x = X.dec; pMV_CMD_01->S_MV_CMD_0x53.S_DEC.y = Y.dec; pMV_CMD_01->S_MV_CMD_0x53.S_DEC.z = Z.dec; // swap the bytes for buffer sent to controller _reverse_dword((DWORD *)&(pMV_CMD_01->S_MV_CMD_0x53.S_POS.x)); _reverse_dword((DWORD *)&(pMV_CMD_01->S_MV_CMD_0x53.S_POS.y)); _reverse_dword((DWORD *)&(pMV_CMD_01->S_MV_CMD_0x53.S_POS.z)); _reverse_dword((DWORD *)&(pMV_CMD_01->S_MV_CMD_0x53.S_SPEED.x)); _reverse_dword((DWORD *)&(pMV_CMD_01->S_MV_CMD_0x53.S_SPEED.y)); _reverse_dword((DWORD *)&(pMV_CMD_01->S_MV_CMD_0x53.S_SPEED.z)); _reverse_dword((DWORD *)&(pMV_CMD_01->S_MV_CMD_0x53.S_ACC.x)); _reverse_dword((DWORD *)&(pMV_CMD_01->S_MV_CMD_0x53.S_ACC.y)); _reverse_dword((DWORD *)&(pMV_CMD_01->S_MV_CMD_0x53.S_ACC.z)); _reverse_dword((DWORD *)&(pMV_CMD_01->S_MV_CMD_0x53.S_DEC.x)); _reverse_dword((DWORD *)&(pMV_CMD_01->S_MV_CMD_0x53.S_DEC.y)); _reverse_dword((DWORD *)&(pMV_CMD_01->S_MV_CMD_0x53.S_DEC.z)); ep_buff[EP_01_CMD_IDX]._size = 0x60; // send 0x60 bytes over ep_buff[EP_82_DATA_IDX]._size = 0x2; // expects 2 bytes back // look at the start of the buffer if (0) { long lMax = 60; char * pch = ep_buff[EP_01_CMD_IDX]._buffer; CString tmp; for (long ii = 0 ; ii < lMax ; ++ii) { tmp.Format(_T("%X"), (unsigned char ) *(pch+ii)); TRACE1("Set Position %s", tmp); } TRACE0("\n"); } _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; } //==================================================================================================================== // MV uses a proportional max accel and decel for all moves sample 10, 10, 1 move //> [endpoint 0x00000001] 00000060 : 530001010100ffff2be6fffeea72ffffcd8e000000000003d08f0003ce20000061a100000000000f423b000f387d0001868200000000003d08ee003ce1f500061a07000000000000000000000000000000000000000000000000000000000000 // [6015605 ms] *Command : 53 Move To -- # X Y Z ZM Scale // *Pos: -0054298 -0071054 -0012914 00000000 // *Vel: 00249999 00249376 00024993 00000000 // *Acc: 00999995 00997501 00099970 00000000 // *Dec: 03999982 03990005 00399879 00000000 //==================================================================================================================== SSI_STATUS CMv_Proto::_calculate_straightline_motion(AXISMOVE &X, AXISMOVE &Y, AXISMOVE &Z, double dSpeedMM) { double dTime=0.0; double dXTime = 0.0, dYTime = 0.0, dZTime = 0.0; double lSpeedXMM = ScaleToMM(g_machine.s_machine_config.x_axis._max_speed, g_machine.s_machine_config.x_axis._scale_resolution); double lSpeedYMM = ScaleToMM(g_machine.s_machine_config.y_axis._max_speed, g_machine.s_machine_config.y_axis._scale_resolution); double lSpeedZMM = ScaleToMM(g_machine.s_machine_config.z_axis._max_speed, g_machine.s_machine_config.z_axis._scale_resolution); #pragma message("Add code to apply the speed") if (lSpeedXMM) dXTime = fabs((double)(X.dToMM-X.dFromMM)/lSpeedXMM); if (lSpeedYMM) dYTime = fabs((double)(Y.dToMM-Y.dFromMM)/lSpeedYMM); if (lSpeedZMM) dZTime = fabs((double)(Z.dToMM-Z.dFromMM)/lSpeedZMM); dTime = (dXTime>dYTime)?dXTime:dYTime; dTime = (dZTime>dTime)?dZTime:dTime; double dDistance = sqrt( pow(X.dToMM-X.dFromMM, 2) + pow(Y.dToMM-Y.dFromMM, 2) + pow(Z.dToMM-Z.dFromMM, 2) ); double dMaxSpeed = dDistance/dTime; double dSpeedFactor = dSpeedMM / dMaxSpeed; dSpeedFactor = (dSpeedFactor > 1.0) ? 1.0 : dSpeedFactor; double dXFactor = dSpeedFactor * dXTime/dTime; double dYFactor = dSpeedFactor * dYTime/dTime; double dZFactor = dSpeedFactor * dZTime/dTime; // MV uses a proportional max accel and decel for all moves sample 10, 10, 1 move //> [endpoint 0x00000001] 00000060 : 530001010100ffff2be6fffeea72ffffcd8e000000000003d08f0003ce20000061a100000000000f423b000f387d0001868200000000003d08ee003ce1f500061a07000000000000000000000000000000000000000000000000000000000000 // [6015605 ms] *Command : 53 Move To -- # X Y Z ZM Scale // *Pos: -0054298 -0071054 -0012914 00000000 // *Vel: 00249999 00249376 00024993 00000000 // *Acc: 00999995 00997501 00099970 00000000 // *Dec: 03999982 03990005 00399879 00000000 X.speed = (long)(dXFactor * g_machine.s_machine_config.x_axis._max_speed); X.acc = (long)(dXFactor * g_machine.s_machine_config.x_axis._max_accel); X.dec = (long)(dXFactor * g_machine.s_machine_config.x_axis._max_decel); Y.speed = (long)(dYFactor * g_machine.s_machine_config.y_axis._max_speed); Y.acc = (long)(dYFactor * g_machine.s_machine_config.y_axis._max_accel); Y.dec = (long)(dYFactor * g_machine.s_machine_config.y_axis._max_decel); Z.speed = (long)(dZFactor * g_machine.s_machine_config.z_axis._max_speed); Z.acc = (long)(dZFactor * g_machine.s_machine_config.z_axis._max_accel); Z.dec = (long)(dZFactor * g_machine.s_machine_config.z_axis._max_decel); return SSI_STATUS_NORMAL; } //======================================================================== // To do a move. //======================================================================== SSI_STATUS CMv_Proto::mv_motion_set_position_xyz(double dX, double dY, double dZ, bool bWait) { _poll_machine_58(); _poll_machine_59(); // get the current position double dXStart, dYStart, dZStart; mv_motion_get_position_xyz(dXStart, dYStart, dZStart); AXISMOVE X; AXISMOVE Y; AXISMOVE Z; X.dFromMM = dXStart; X.dToMM = dX; Y.dFromMM = dYStart; Y.dToMM = dY; Z.dFromMM = dZStart; Z.dToMM = dZ; X.from = MMtoScale(dXStart, g_machine.s_machine_config.x_axis._scale_resolution); X.to = MMtoScale(dX, g_machine.s_machine_config.x_axis._scale_resolution); Y.from = MMtoScale(dYStart, g_machine.s_machine_config.y_axis._scale_resolution); Y.to = MMtoScale(dY, g_machine.s_machine_config.y_axis._scale_resolution); Z.from = MMtoScale(dZStart, g_machine.s_machine_config.z_axis._scale_resolution); Z.to = MMtoScale(dZ, g_machine.s_machine_config.z_axis._scale_resolution); // move the position to make the -X, -Y, -Z position 0,0,0 X.to += g_machine.s_machine_config.x_axis._neg_working_limit; Y.to += g_machine.s_machine_config.y_axis._neg_working_limit; Z.to += g_machine.s_machine_config.z_axis._neg_working_limit; X.from += g_machine.s_machine_config.x_axis._neg_working_limit; Y.from += g_machine.s_machine_config.y_axis._neg_working_limit; Z.from += g_machine.s_machine_config.z_axis._neg_working_limit; _calculate_straightline_motion(X, Y, Z, g_machine.s_machine_config._dXYZSpeed); _set_position_xyz(X, Y, Z); #pragma message("Test settle wait comparing the status bit to the scale monitor") if (bWait) { const long lSleep = 20; const long lMaxLoopCnt = 20000/lSleep; // use max homing time of 20 seconds long lLoopCnt = 0; Sleep(lSleep); _poll_machine_58(); _poll_machine_59(); while (!g_machine.s_status._bXYZZMIdle && lLoopCnt < lMaxLoopCnt) { _poll_machine_58(); _poll_machine_59(); Sleep(lSleep); ++lLoopCnt; } TRACE1("Presettle Time: %lf\n", TimeInSecs()); WaitForSettleXYZZM(); TRACE1("Postsettle Time: %lf\n", TimeInSecs()); } return SSI_STATUS_NORMAL; }; //======================================================================== // 43 10 //======================================================================== SSI_STATUS CMv_Proto::mv_motion_get_position_r(double &dPositionR) { dPositionR = g_machine.r1._deg; return SSI_STATUS_NORMAL; }; //======================================================================== // //======================================================================== SSI_STATUS CMv_Proto::mv_motion_set_position_r(double dPositionR) { _send_cmd_MV_CMD_GET_X_INDEX_41(0x10); // 41 10 _send_cmd_MV_CMD_GET_Y_INDEX_42(0x10); // 42 10 _send_cmd_PRE_ROTARY(); _send_cmd_MV_CMD_GET_MAG_43(0x10); // 43 10 // Converts dPositionR to scale count. long lCount = (long) (g_machine.s_machine_config.r1_axis._scale_resolution*dPositionR); _send_cmd_MV_CMD_MOVE_AXIS_4C10(lCount); Sleep(200); // while ( g_machine.s_status._bRMoving) // wait for move to complete // { // Sleep(50); // }; // once complete, we can update the true position. g_machine.r1._deg = dPositionR; return SSI_STATUS_NORMAL; }; //======================================================================== // todo: //======================================================================== SSI_STATUS CMv_Proto::mv_motion_store_position_xyz() { return SSI_STATUS_NORMAL; }; //======================================================================== // todo: //======================================================================== SSI_STATUS CMv_Proto::mv_motion_get_stored_positions_xyz() { return SSI_STATUS_NORMAL; }; //======================================================================== // todo: Check 0x59 commands. //======================================================================== SSI_STATUS CMv_Proto::mv_motion_is_axis_moving() { return SSI_STATUS_NORMAL; } //======================================================================== // home position is the position of the magnetic mark calculated // from the power off position. // When the machine is powered off, it has a position. //======================================================================== bool CMv_Proto::mv_motion_is_homed() { return(g_machine.s_machine_config.x_axis._pos_working_limit == 0 && g_machine.s_machine_config.x_axis._neg_working_limit == 0)?false:true; } //======================================================================== //======================================================================== SSI_STATUS CMv_Proto::mv_motion_set_stage_limits() { return SSI_STATUS_NORMAL; } //======================================================================== // Stage Limit is .5" from the edge. so, it is // Positive Limit = homing mark + (150 - (.5"25.4)) // Negative Limit = homing mark - (150 - (.5"25.4)) //======================================================================== SSI_STATUS CMv_Proto::mv_motion_get_stage_limits() { return SSI_STATUS_NORMAL; } //======================================================================== // Do not implement. //======================================================================== SSI_STATUS CMv_Proto::mv_motion_dcc_scan_start() { return SSI_STATUS_NORMAL; } //======================================================================== // Do not implement. //======================================================================== SSI_STATUS CMv_Proto::mv_motion_dcc_scan_get_data_size() { return SSI_STATUS_NORMAL; } //======================================================================== // Not implemented //======================================================================== SSI_STATUS CMv_Proto::mv_motion_dcc_scan_get_data() { return SSI_STATUS_NORMAL; } //======================================================================== // Move the stage left/right until the index location is non-zero //======================================================================== SSI_STATUS CMv_Proto::mv_motion_Dcc_Home() { _get_config(); //======================================== // Get the current position _poll_machine_59(); _send_cmd_MV_CMD_GET_MAG_43(0x03); //======================================== // Read the index status from the controller // if the returned 5E working_limit == 0, we need to home the machine _send_cmd_MV_CMD_GET_WORKING_LIMITS_5E(); _get_xyz_index(); _send_cmd_MV_CMD_GET_CONFIG_69(0x01); if (!(g_machine.s_machine_config.x_axis._pos_working_limit == 0 && g_machine.s_machine_config.x_axis._neg_working_limit == 0 )) { SetEvent(g_hHomedEvent); return SSI_STATUS_NORMAL; } m_bHomingActive = true; // Tell the world we need to home the stage // Seek Zm - Index Position _send_cmd_MV_CMD_SEEK_INDEX_4F(0x03, 0x00); // Don't know what this is _send_cmd_MV_CMD_GET_CONFIG_69(0x01); // Seek Z - Index Position _send_cmd_MV_CMD_SEEK_INDEX_4F(0x02, 0x00); Sleep(20); _poll_machine_59(); _poll_machine_58(); TRACE0(" - waiting for Z to stop moving\n"); //======================================== // Wait until Z stopped moving while (g_machine.s_status._bZMoving) { Sleep(50); _poll_machine_59(); } //===================================== // get stage working limits // move the Z to the top to clear the working area. _send_cmd_MV_CMD_GET_WORKING_LIMITS_5E(); _send_cmd_MV_CMD_MOVE_AXIS_4C(0x02, g_machine.s_machine_config.z_axis._pos_working_limit, 1); _poll_machine_59(); //===================================== // Seek X- Index Positions _send_cmd_MV_CMD_SEEK_INDEX_4F(0x00, 0x00); // Seek Y- Index Positions _send_cmd_MV_CMD_SEEK_INDEX_4F(0x01, 0x00); _poll_machine_59(); _poll_machine_58(); TRACE0(" - waiting for X,Y,Zm to stop moving\n"); //======================================== // Wait until X-Y-Zm stopped moving while (g_machine.s_status._bYMoving || g_machine.s_status._bXMoving || g_machine.s_status._bZMMoving) { Sleep(50); _poll_machine_59(); } //======================================== // 5e Get stage working limits after homing _send_cmd_MV_CMD_GET_WORKING_LIMITS_5E(); // 5e Get state working limits to update to the real limits data _send_cmd_MV_CMD_GET_WORKING_LIMITS_5E(); _get_xyz_index(); m_bHomingActive = false; SetEvent(g_hHomedEvent); return SSI_STATUS_NORMAL; } //======================================================================== // //======================================================================== SSI_STATUS CMv_Proto::mv_motion_shut_down() { return SSI_STATUS_NORMAL; }; //======================================================================== // Not implemented //======================================================================== SSI_STATUS CMv_Proto::mv_motion_fly_mode_cancel() { return SSI_STATUS_NORMAL; }; //======================================================================== // From g_machine //======================================================================== SSI_STATUS CMv_Proto::mv_motion_get_axes_max_speed(double &dMaxSpeedX, double &dMaxSpeedY, double &dMaxSpeedZ) { dMaxSpeedX = g_machine.s_machine_config.x_axis._max_speed; dMaxSpeedY = g_machine.s_machine_config.y_axis._max_speed; dMaxSpeedZ = g_machine.s_machine_config.z_axis._max_speed; return SSI_STATUS_NORMAL; } //======================================================================== SSI_STATUS CMv_Proto::mv_motion_get_3D_max_speed(double &dMaxSpeed) { double dMaxSpeedX = g_machine.s_machine_config.x_axis._max_speed; double dMaxSpeedY = g_machine.s_machine_config.y_axis._max_speed; double dMaxSpeedZ = g_machine.s_machine_config.z_axis._max_speed; dMaxSpeed = sqrt(dMaxSpeedX*dMaxSpeedX + dMaxSpeedY*dMaxSpeedY + dMaxSpeedZ*dMaxSpeedZ); return SSI_STATUS_NORMAL; } //======================================================================== // //======================================================================== SSI_STATUS CMv_Proto::mv_motion_get_position_meas_point() { return SSI_STATUS_NORMAL; }; //======================================================================== // //======================================================================== SSI_STATUS CMv_Proto::mv_optics_is_supported() { return SSI_STATUS_NORMAL; }; //======================================================================== // //======================================================================== SSI_STATUS CMv_Proto::mv_optics_start_up() { WaitForSingleObject(g_hHomedEvent, INFINITE); // machine start and homing is done return SSI_STATUS_NORMAL; }; //======================================================================== // 11 steps //======================================================================== SSI_STATUS CMv_Proto::mv_optics_get_magnification_steps(int &iSteps) { WaitForSingleObject(g_hHomedEvent, INFINITE); // machine start and homing is done iSteps = 11; TRACE1("Optics Get Mag Steps: %X \r\n",iSteps); return SSI_STATUS_NORMAL; }; //======================================================================== // Not implemented //======================================================================== SSI_STATUS CMv_Proto::mv_optics_get_numeric_aperture() { WaitForSingleObject(g_hHomedEvent, INFINITE); // machine start and homing is done return SSI_STATUS_NORMAL; }; //======================================================================== // //======================================================================== SSI_STATUS CMv_Proto::mv_optics_get_magnification(int& iStep) { WaitForSingleObject(g_hHomedEvent, INFINITE); // machine start and homing is done iStep = g_machine.s_machine_config.zm_axis._mag_step; TRACE1("Optics Get Mag : %X \r\n",g_machine.s_machine_config.zm_axis._mag_step); return SSI_STATUS_NORMAL; }; //======================================================================== /* *Command : 4C 03 00 Set Mag 00002633 00009484 00024487 00097948 *Command : 4C 03 01 Set Mag -0000287 00009484 00024487 00097948 *Command : 4C 03 00 Set Mag 00000013 00009484 00024487 00097948 *Command : 4C 03 00 Set Mag 00005253 00009484 00024487 00097948 *Command : 4C 03 01 Set Mag -0000287 00009484 00024487 00097948 *Command : 4C 03 00 Set Mag 00000013 00009484 00024487 00097948 *Command : 4C 03 00 Set Mag 00007873 00009484 00024487 00097948 */ //======================================================================== SSI_STATUS CMv_Proto::mv_optics_set_magnification(int iStep) { WaitForSingleObject(g_hHomedEvent, INFINITE); // machine start and homing is done g_machine.s_machine_config.zm_axis._mag_step = iStep; TRACE1("Optics Set Mag : %X \r\n",g_machine.s_machine_config.zm_axis._mag_step); return SSI_STATUS_NORMAL; }; //======================================================================== // //======================================================================== SSI_STATUS CMv_Proto::mv_optics_get_deadband() { WaitForSingleObject(g_hHomedEvent, INFINITE); // machine start and homing is done return SSI_STATUS_NORMAL; }; //======================================================================== // //======================================================================== SSI_STATUS CMv_Proto::mv_optics_get_scale_range(long &neg_scale_range, long &pos_scale_range) { WaitForSingleObject(g_hHomedEvent, INFINITE); // machine start and homing is done neg_scale_range = g_machine.s_machine_config.zm_axis._neg_working_limit; pos_scale_range = g_machine.s_machine_config.zm_axis._pos_working_limit; neg_scale_range -= g_machine.s_machine_config.zm_axis._neg_working_limit; pos_scale_range -= g_machine.s_machine_config.zm_axis._neg_working_limit; TRACE2("Get Scale Range : %X, %X \r\n",neg_scale_range, pos_scale_range); TRACE2("Get Scale Range : %X, %X \r\n",neg_scale_range, pos_scale_range); return SSI_STATUS_NORMAL; }; //======================================================================== // //======================================================================== SSI_STATUS CMv_Proto::mv_optics_get_scale_position(long &lScale) { WaitForSingleObject(g_hHomedEvent, INFINITE); // machine start and homing is done _poll_machine_59(); lScale = g_machine.zm._pos_59; lScale -= g_machine.s_machine_config.zm_axis._neg_working_limit; TRACE1("get scale position : g_machine.s_machine_config.zm_axis._neg_working_limit = %X \r\n",g_machine.s_machine_config.zm_axis._neg_working_limit); TRACE1(" scale position : %X \r\n",lScale); return SSI_STATUS_NORMAL; }; //======================================================================== // get scale first. // if the target is backwards, // set destination with 01 status // set destination with 00 status // //======================================================================== SSI_STATUS CMv_Proto::mv_optics_set_scale_position(long lScale) { // const long lSleep = 20; // const long lMaxLoopCnt = 20000/lSleep; // use max homing time of 20 seconds // long lLoopCnt = 0; WaitForSingleObject(g_hHomedEvent, INFINITE); // machine start and homing is done TRACE1("caller set scale position = %X \r\n", lScale); lScale += g_machine.s_machine_config.zm_axis._neg_working_limit; _poll_machine_59(); long lBacklashMove = g_machine.s_machine_config.zm_axis._slack; long lLast = g_machine.zm._pos_59; // if a negative move, overshoot and come back for backlash removal if (lScale < g_machine.zm._pos_59) { long lDestination = lScale - lBacklashMove; lDestination = (lDestinationg_machine.s_machine_config.zm_axis._pos_working_limit) ? g_machine.s_machine_config.zm_axis._pos_working_limit : lDestination; lDestination = (lDestinationg_machine.s_machine_config._vector_light_model == 2) { g_machine.s_lights_200._axial_light = (long)(dTopPercent/100.0 * (mvMAXLIGHTVALUE - 1)); g_machine.s_lights_200._bottom_light = (long)(dBottomPercent/100.0 * (mvMAXLIGHTVALUE - 1)); TRACE2("mv_light_set_lamp_state bottom: %ld top: %ld\n", g_machine.s_lights_300._bottom_light, g_machine.s_lights_200._axial_light); }; return SSI_STATUS_NORMAL; } //======================================================================== // Do nothing //======================================================================== SSI_STATUS CMv_Proto::mv_light_set_light_off() { WaitForSingleObject(g_hHomedEvent, INFINITE); // machine start and homing is done if (g_machine.s_machine_config._vector_light_model == 3) { g_machine.s_lights_300._axial_light = 0; g_machine.s_lights_300._bottom_light = 0; } else // if (g_machine.s_machine_config._vector_light_model == 2) { g_machine.s_lights_200._axial_light = 0; g_machine.s_lights_200._bottom_light = 0; } mv_set_full_ringlight_data(0); mv_light_set_light(); return SSI_STATUS_NORMAL; } //======================================================================== //======================================================================== SSI_STATUS CMv_Proto::mv_light_set_light() { WaitForSingleObject(g_hHomedEvent, INFINITE); // machine start and homing is done _send_cmd_MV_CMD_SET_LIGHT_6E(); return SSI_STATUS_NORMAL; } //======================================================================== //======================================================================== SSI_STATUS CMv_Proto::mv_pendant_enable(BOOL bTrue) { UNREFERENCED_PARAMETER(bTrue); WaitForSingleObject(g_hHomedEvent, INFINITE); // machine start and homing is done _send_cmd_MV_CMD_SET_JOYSTICK_MODE_47(bTrue); return SSI_STATUS_NORMAL; }; //======================================================================== // read the configuration from the controller and populate the g_machine // data structure // 1. Extract machine signature : 6f00 // 2. Get Limits //======================================================================== SSI_STATUS CMv_Proto::_get_config() { _send_cmd_MV_CMD_START_MACHINE_2C(0x01); // 50 Get Max Speed - axis set one _send_cmd_MV_CMD_MAX_SPEED_50(0x00, 0x01, 0x11); // 51 Get Max Acceleration - axis set one _send_cmd_MV_CMD_MAX_ACCEL_51(0x00, 0x01, 0x11); // 76 Get Max Deceleration - axis set one _send_cmd_MV_CMD_MAX_DECEL_76(0x00, 1, 0x11); // 5001 Get Max Speed - axis set two _send_cmd_MV_CMD_MAX_SPEED_50(0x01, 0x02, 0x11); // 5101 Get Max Acceleration - axis set two _send_cmd_MV_CMD_MAX_ACCEL_51(0x01, 0x02, 0x11); // 7601 Get Max Deceleration - axis set two _send_cmd_MV_CMD_MAX_DECEL_76(0x01, 0x02, 0x11); // 4103 _send_cmd_MV_CMD_GET_X_INDEX_41(0x03); // 4203 _send_cmd_MV_CMD_GET_Y_INDEX_42(0x03); // 4503 _send_cmd_MV_CMD_GET_ZOOM_DECEL_45(0x03); _send_cmd_MV_CMD_GET_SCALE_SLACK_7C(0x03); // 4303 - Get Mag Settings _send_cmd_MV_CMD_GET_MAG_43(0x03); _send_cmd_MV_CMD_GET_CONFIG_60(); // 5e Get state working limits _send_cmd_MV_CMD_GET_WORKING_LIMITS_5E(); return SSI_STATUS_NORMAL; } //======================================================================== // read the configuration from the controller and populate the g_machine // data structure //======================================================================== SSI_STATUS CMv_Proto::_get_xyz_index() { _send_cmd_MV_CMD_GET_INDEX_4E(0x00, 0x02, 0x07); // Get X - Index _send_cmd_MV_CMD_GET_INDEX_4E(0x01, 0x02, 0x07); // Get Y - Index _send_cmd_MV_CMD_GET_INDEX_4E(0x02, 0x02, 0x07); // Get Z - Index return SSI_STATUS_NORMAL; } //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_GET_POS_XYZ_00_02() { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_GET_POS_XYZ_00_02; ep_buff[EP_01_CMD_IDX]._size = 0x01; ep_buff[EP_81_DATA_IDX]._size = 0x45; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_SET_POS_XYZ_00_03() { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_SET_POS_XYZ_00_03; ep_buff[EP_01_CMD_IDX]._size = 0x01; ep_buff[EP_81_DATA_IDX]._size = 0x45; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_GET_POS_R_00_04() { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_GET_POS_R_00_04; ep_buff[EP_01_CMD_IDX]._size = 0x01; ep_buff[EP_81_DATA_IDX]._size = 0x45; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_SET_POS_R_00_05() { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_SET_POS_R_00_05; ep_buff[EP_01_CMD_IDX]._size = 0x01; ep_buff[EP_81_DATA_IDX]._size = 0x45; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_SET_ZOOM_00_06() { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_SET_ZOOM_00_06; ep_buff[EP_01_CMD_IDX]._size = 0x01; ep_buff[EP_81_DATA_IDX]._size = 0x45; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_GET_ZOOM_00_07() { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_GET_ZOOM_00_07; ep_buff[EP_01_CMD_IDX]._size = 0x01; ep_buff[EP_81_DATA_IDX]._size = 0x45; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_GET_STARUP_CONFIG_00() { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_GET_STARUP_CONFIG_00; ep_buff[EP_01_CMD_IDX]._size = 0x01; ep_buff[EP_81_DATA_IDX]._size = 0x45; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_GET_SUB_CONFIG_28() { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_GET_SUB_CONFIG_28; ep_buff[EP_01_CMD_IDX]._size = 0x20; ep_buff[EP_81_DATA_IDX]._size = 0x12; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //====================================================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_GET_SUB_CONFIG_29(BYTE cmd0, DWORD addr, BYTE cmd1) { unsigned char x[4]; WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); x[0] = *(((unsigned char *)&addr + 3)); x[1] = *(((unsigned char *)&addr + 2)); x[2] = *(((unsigned char *)&addr + 1)); x[3] = *(((unsigned char *)&addr)); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_GET_SUB_CONFIG_29; pMV_CMD_01->S_MV_CMD_0x29.cmd0 = cmd0; // take the DWORD addr, create an array of bytes to force a little endian format. memcpy((void *)(&pMV_CMD_01->S_MV_CMD_0x29.dAddr), x, 4); pMV_CMD_01->S_MV_CMD_0x29.cmd1 = cmd1; switch (cmd0) { case 0x00: case 0x02: ep_buff[EP_01_CMD_IDX]._size = 0x20; ep_buff[EP_81_DATA_IDX]._size = 0x60; break; default: ep_buff[EP_01_CMD_IDX]._size = 0x20; ep_buff[EP_81_DATA_IDX]._size = 0x60; break; }; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_SERVO_CMD_2B() { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_SERVO_CMD_2B; ep_buff[EP_01_CMD_IDX]._size = 0x01; ep_buff[EP_81_DATA_IDX]._size = 0x45; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; } //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_START_MACHINE_2C(BYTE cmd0) { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_START_MACHINE_2C; ep_buff[EP_01_CMD_IDX]._buffer[1] = cmd0; ep_buff[EP_01_CMD_IDX]._size = 0x10; ep_buff[EP_81_DATA_IDX]._size = 0x20; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_GET_UNK_31() { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_GET_UNK_31; ep_buff[EP_01_CMD_IDX]._size = 0x01; ep_buff[EP_81_DATA_IDX]._size = 0x20; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_GET_UNK_32() { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_GET_UNK_32; ep_buff[EP_01_CMD_IDX]._size = 0x01; ep_buff[EP_81_DATA_IDX]._size = 0x45; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_GET_UNK_34() { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_GET_UNK_34; ep_buff[EP_01_CMD_IDX]._size = 0x01; ep_buff[EP_81_DATA_IDX]._size = 0x45; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_GET_UNK_35() { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_GET_UNK_35; ep_buff[EP_01_CMD_IDX]._size = 0x01; ep_buff[EP_81_DATA_IDX]._size = 0x45; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_GET_UNK_3C() { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_GET_UNK_3C; ep_buff[EP_01_CMD_IDX]._size = 0x01; ep_buff[EP_81_DATA_IDX]._size = 0x45; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_GET_X_INDEX_41(BYTE cmd0) { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_GET_X_INDEX_41; pMV_CMD_01->S_MV_CMD_0x41.cmd0 = cmd0; ep_buff[EP_01_CMD_IDX]._size = 0x02; ep_buff[EP_81_DATA_IDX]._size = 0x06; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_GET_Y_INDEX_42(BYTE cmd0) { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_GET_Y_INDEX_42; pMV_CMD_01->S_MV_CMD_0x42.cmd0 = cmd0; ep_buff[EP_01_CMD_IDX]._size = 0x02; ep_buff[EP_81_DATA_IDX]._size = 0x06; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_GET_MAG_43(BYTE cmd0) { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_GET_MAG_43; pMV_CMD_01->S_MV_CMD_0x43.cmd0 = cmd0; ep_buff[EP_01_CMD_IDX]._size = 0x02; ep_buff[EP_81_DATA_IDX]._size = 0x06; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_GET_ZOOM_DECEL_45(BYTE cmd0) { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_GET_ZOOM_DECEL_45; pMV_CMD_01->S_MV_CMD_0x45.cmd0 = cmd0; ep_buff[EP_01_CMD_IDX]._size = 0x02; ep_buff[EP_81_DATA_IDX]._size = 0x06; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_SET_JOYSTICK_MODE_47(BOOL bEnable) { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_SET_JOYSTICK_MODE_47; ep_buff[EP_01_CMD_IDX]._size = 0x10; ep_buff[EP_81_DATA_IDX]._size = 0x02; if (!bEnable) { ep_buff[EP_01_CMD_IDX]._buffer[1] = 0x01; ep_buff[EP_01_CMD_IDX]._buffer[2] = 0x01; ep_buff[EP_01_CMD_IDX]._buffer[3] = 0x01; ep_buff[EP_01_CMD_IDX]._buffer[4] = 0x01; }; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_MOVE_AXIS_4C10(long lCount) { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_MOVE_AXIS_4C; pMV_CMD_01->S_MV_CMD_0x4C10.uAxis = 0x10; pMV_CMD_01->S_MV_CMD_0x4C10.lR1_Pos = lCount; pMV_CMD_01->S_MV_CMD_0x4C10.lX = g_machine.s_machine_config.x_axis._index_10; pMV_CMD_01->S_MV_CMD_0x4C10.lY = g_machine.s_machine_config.y_axis._index_10; _reverse_dword((DWORD *)&(pMV_CMD_01->S_MV_CMD_0x4C10.lR1_Pos)); _reverse_dword((DWORD *)&(pMV_CMD_01->S_MV_CMD_0x4C10.lX)); _reverse_dword((DWORD *)&(pMV_CMD_01->S_MV_CMD_0x4C10.lY)); ep_buff[EP_01_CMD_IDX]._size = 0x1f; ep_buff[EP_81_DATA_IDX]._size = 0x02; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_MOVE_AXIS_4C(BYTE Axis, long lDestination, BYTE lStatusByte) { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_MOVE_AXIS_4C; pMV_CMD_01->S_MV_CMD_0x4C.uAxis = Axis; #pragma message("what exactly does the message byte do") pMV_CMD_01->S_MV_CMD_0x4C.statusByte = lStatusByte; pMV_CMD_01->S_MV_CMD_0x4C.lPosition = lDestination; switch (Axis) { case 0: pMV_CMD_01->S_MV_CMD_0x4C.lSpeed = g_machine.s_machine_config.x_axis._max_speed; pMV_CMD_01->S_MV_CMD_0x4C.lAcc = g_machine.s_machine_config.x_axis._max_accel; pMV_CMD_01->S_MV_CMD_0x4C.lDecel = g_machine.s_machine_config.x_axis._max_decel; break; case 1: pMV_CMD_01->S_MV_CMD_0x4C.lSpeed = g_machine.s_machine_config.y_axis._max_speed; pMV_CMD_01->S_MV_CMD_0x4C.lAcc = g_machine.s_machine_config.y_axis._max_accel; pMV_CMD_01->S_MV_CMD_0x4C.lDecel = g_machine.s_machine_config.y_axis._max_decel; break; case 2: pMV_CMD_01->S_MV_CMD_0x4C.lSpeed = g_machine.s_machine_config.z_axis._max_speed; pMV_CMD_01->S_MV_CMD_0x4C.lAcc = g_machine.s_machine_config.z_axis._max_accel; pMV_CMD_01->S_MV_CMD_0x4C.lDecel = g_machine.s_machine_config.z_axis._max_decel; break; case 3: pMV_CMD_01->S_MV_CMD_0x4C.lSpeed = g_machine.s_machine_config.zm_axis._max_speed; pMV_CMD_01->S_MV_CMD_0x4C.lAcc = g_machine.s_machine_config.zm_axis._max_accel; pMV_CMD_01->S_MV_CMD_0x4C.lDecel = g_machine.s_machine_config.zm_axis._max_decel; break; default: break; } CString msg; msg.Format(_T("_send_cmd_MV_CMD_MOVE_AXIS_4C Zoom move %ld %ld %ld %ld\n"), pMV_CMD_01->S_MV_CMD_0x4C.lPosition, pMV_CMD_01->S_MV_CMD_0x4C.lSpeed, pMV_CMD_01->S_MV_CMD_0x4C.lAcc, pMV_CMD_01->S_MV_CMD_0x4C.lDecel); TRACE1("%s", msg); if (0) { long lPos = 500; pMV_CMD_01->S_MV_CMD_0x4C.lPosition = lPos; pMV_CMD_01->S_MV_CMD_0x4C.lSpeed = 9000; pMV_CMD_01->S_MV_CMD_0x4C.lAcc = 24000; pMV_CMD_01->S_MV_CMD_0x4C.lDecel = 97000; } // swap the bytes for buffer sent to controller _reverse_dword((DWORD *)&(pMV_CMD_01->S_MV_CMD_0x4C.lPosition)); _reverse_dword((DWORD *)&(pMV_CMD_01->S_MV_CMD_0x4C.lSpeed)); _reverse_dword((DWORD *)&(pMV_CMD_01->S_MV_CMD_0x4C.lAcc)); _reverse_dword((DWORD *)&(pMV_CMD_01->S_MV_CMD_0x4C.lDecel)); ep_buff[EP_01_CMD_IDX]._size = 0x21; ep_buff[EP_81_DATA_IDX]._size = 0x02; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_GET_INDEX_4E(BYTE cmd0, BYTE send_len, BYTE recv_len) { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_GET_INDEX_4E; pMV_CMD_01->S_MV_CMD_0x4E.cmd0 = cmd0; ep_buff[EP_01_CMD_IDX]._size = send_len; ep_buff[EP_81_DATA_IDX]._size = recv_len; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //====================================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_SEEK_INDEX_4F(BYTE cmd0, BYTE cmd1) { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_SEEK_INDEX_4F; pMV_CMD_01->S_MV_CMD_0x4F.cmd0 = cmd0; pMV_CMD_01->S_MV_CMD_0x4F.cmd1 = cmd1; ep_buff[EP_01_CMD_IDX]._size = 0x03; ep_buff[EP_81_DATA_IDX]._size = 0x02; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_MAX_SPEED_50(BYTE cmd0, BYTE send_len, BYTE recv_len) { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_MAX_SPEED_50; ep_buff[EP_01_CMD_IDX]._buffer[1] = cmd0; ep_buff[EP_01_CMD_IDX]._size = send_len; ep_buff[EP_81_DATA_IDX]._size = recv_len; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_MAX_ACCEL_51(BYTE cmd0, BYTE send_len, BYTE recv_len) { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_MAX_ACCEL_51; pMV_CMD_01->S_MV_CMD_0x50.cmd0 = cmd0; ep_buff[EP_01_CMD_IDX]._size = send_len; ep_buff[EP_81_DATA_IDX]._size = recv_len; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_MOVE_TO_53() { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_MOVE_TO_53; ep_buff[EP_01_CMD_IDX]._size = 0x01; ep_buff[EP_81_DATA_IDX]._size = 0x45; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_GET_SCALE_58(long &lX, long &lY, long &lZ) { _poll_machine_58(); lX = g_machine.x._pos_58; lY = g_machine.y._pos_58; lZ = g_machine.z._pos_58; return SSI_STATUS_NORMAL; }; //======================================================================= SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_GET_POLL_59(long &lX, long &lY, long &lZ, long &lZM) { _poll_machine_59(); lX = g_machine.x._pos_59; lY = g_machine.y._pos_59; lZ = g_machine.z._pos_59; lZM = g_machine.zm._pos_59; return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_GET_WORKING_LIMITS_5E() { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_GET_WORKING_LIMITS_5E; ep_buff[EP_01_CMD_IDX]._size = 0x01; ep_buff[EP_81_DATA_IDX]._size = 0x78; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_GET_CONFIG_60() { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_GET_CONFIG_60; ep_buff[EP_01_CMD_IDX]._size = 0x01; ep_buff[EP_81_DATA_IDX]._size = 0x7e; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_GET_RAW_SCALE_63() { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_GET_RAW_SCALE_63; ep_buff[EP_01_CMD_IDX]._size = 0x01; ep_buff[EP_81_DATA_IDX]._size = 0x45; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_GET_CONFIG_69(BYTE cmd0) { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_GET_CONFIG_69; pMV_CMD_01->S_MV_CMD_0x69.cmd0 = cmd0; ep_buff[EP_01_CMD_IDX]._size = 0x10; ep_buff[EP_81_DATA_IDX]._size = 0x02; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_GET_UNK_6A() { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_GET_UNK_6A; ep_buff[EP_01_CMD_IDX]._size = 0x01; ep_buff[EP_81_DATA_IDX]._size = 0x45; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_SET_LIGHT_6E() { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); if (g_machine.s_machine_config._vector_light_model == 3) { // moved here form mv_light_set_light where it clearly does not belong as it is outside the USB mutex pMV_CMD_01->uCmdByte = MV_CMD_SET_LIGHT_6E; pMV_CMD_01->S_MV_CMD_300_0x6e.uSubCmdByte = 0; memcpy(pMV_CMD_01->S_MV_CMD_300_0x6e.segment, g_machine.s_lights_300._segment, sizeof(g_machine.s_lights_300._segment[FIVE_RINGS][EIGHT_SEGS])); pMV_CMD_01->S_MV_CMD_300_0x6e.bottom_light = g_machine.s_lights_300._bottom_light; pMV_CMD_01->S_MV_CMD_300_0x6e.axial_light = g_machine.s_lights_300._axial_light; memcpy(ep_buff[EP_01_CMD_IDX]._buffer+2, &(g_machine.s_lights_300), 0xfe); unsigned char x[4]; for (int i=0;i<42;i++) // MAXSEGS_5*MAXRINGS_8 + 2 = 5*8 + 2 { memcpy(x, (unsigned char *)(ep_buff[EP_01_CMD_IDX]._buffer+2+i*4), 4); *(ep_buff[EP_01_CMD_IDX]._buffer+2+i*4+0) = x[3]; *(ep_buff[EP_01_CMD_IDX]._buffer+2+i*4+1) = x[2]; *(ep_buff[EP_01_CMD_IDX]._buffer+2+i*4+2) = x[1]; *(ep_buff[EP_01_CMD_IDX]._buffer+2+i*4+3) = x[0]; }; } else // if (g_machine.s_machine_config._vector_light_model == 3) { pMV_CMD_01->uCmdByte = MV_CMD_SET_LIGHT_6E; pMV_CMD_01->S_MV_CMD_200_0x6e.uSubCmdByte = 0; memcpy(pMV_CMD_01->S_MV_CMD_200_0x6e.segment, g_machine.s_lights_200._segment, sizeof(g_machine.s_lights_200._segment[FIVE_RINGS][EIGHT_SEGS])); pMV_CMD_01->S_MV_CMD_200_0x6e.bottom_light = g_machine.s_lights_200._bottom_light; pMV_CMD_01->S_MV_CMD_200_0x6e.axial_light = g_machine.s_lights_200._axial_light; memcpy(ep_buff[EP_01_CMD_IDX]._buffer+2, &(g_machine.s_lights_200), 0xfe); unsigned char x[4]; for (int i=0;i<18;i++) // MAXSEGS_2*MAXRINGS_8 + 2 = 5*8 + 2 = 2*8 + 2 { memcpy(x, (unsigned char *)(ep_buff[EP_01_CMD_IDX]._buffer+2+i*4), 4); *(ep_buff[EP_01_CMD_IDX]._buffer+2+i*4+0) = x[3]; *(ep_buff[EP_01_CMD_IDX]._buffer+2+i*4+1) = x[2]; *(ep_buff[EP_01_CMD_IDX]._buffer+2+i*4+2) = x[1]; *(ep_buff[EP_01_CMD_IDX]._buffer+2+i*4+3) = x[0]; }; }; pMV_CMD_01->uCmdByte = MV_CMD_SET_LIGHT_6E; ep_buff[EP_01_CMD_IDX]._size = 0xfe; ep_buff[EP_81_DATA_IDX]._size = 0x02; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); Sleep(10); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_GET_MACHINE_SIGNATURE_6F(BYTE cmd0) { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_GET_MACHINE_SIGNATURE_6F; pMV_CMD_01->S_MV_CMD_0x6F.cmd0 = cmd0; ep_buff[EP_01_CMD_IDX]._size = 0x10; ep_buff[EP_81_DATA_IDX]._size = 0xfe; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_GET_LIGHT_CONFIG_70() { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); unsigned char unk1[] = {0x00, 0x01, 0x00, 0x20, 0x00, 0x00, 0x00, 0xe4, 0xfb, 0x12, 0x00, 0x12, 0x00, 0x00, 0x00}; memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_GET_LIGHT_CONFIG_70; memcpy((void *)(&(ep_buff[EP_01_CMD_IDX]._buffer[1])), unk1, 15); ep_buff[EP_01_CMD_IDX]._size = 0x10; ep_buff[EP_81_DATA_IDX]._size = 0xfe; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_GET_UNK_71() { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_GET_UNK_71; ep_buff[EP_01_CMD_IDX]._size = 0x01; ep_buff[EP_81_DATA_IDX]._size = 0x45; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_GET_UNK_74() { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_GET_UNK_74; ep_buff[EP_01_CMD_IDX]._size = 0x01; ep_buff[EP_81_DATA_IDX]._size = 0x45; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_GET_UNK_75() { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_GET_UNK_75; ep_buff[EP_01_CMD_IDX]._size = 0x01; ep_buff[EP_81_DATA_IDX]._size = 0x45; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_MAX_DECEL_76(BYTE cmd0, BYTE send_len, BYTE recv_len) { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_MAX_DECEL_76; pMV_CMD_01->S_MV_CMD_0x76.cmd0 = cmd0; ep_buff[EP_01_CMD_IDX]._size = send_len; ep_buff[EP_81_DATA_IDX]._size = recv_len; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_GET_SCALE_SLACK_7C(BYTE cmd0) { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_01_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); pMV_CMD_01->uCmdByte = MV_CMD_GET_SCALE_SLACK_7C; pMV_CMD_01->S_MV_CMD_0x7C.cmd0 = cmd0; ep_buff[EP_01_CMD_IDX]._size = 0x02; ep_buff[EP_81_DATA_IDX]._size = 0x04; _do_single_threaded_usb_comm(EP_01_CMD_IDX); ReleaseMutex(g_hEP01_Serial_Mutex); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_send_cmd_MV_CMD_GET_FINE_SCALE(long &lX, long &lY, long &lZ, long &lZm) { WaitForSingleObject(g_hEP02_Serial_Mutex, INFINITE); memset(ep_buff[EP_02_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); ep_buff[EP_02_CMD_IDX]._buffer[0] = 0x00; ep_buff[EP_02_CMD_IDX]._buffer[1] = 0x01; ep_buff[EP_02_CMD_IDX]._buffer[2] = 0x01; ep_buff[EP_02_CMD_IDX]._buffer[3] = 0x01; ep_buff[EP_02_CMD_IDX]._buffer[4] = 0x01; ep_buff[EP_02_CMD_IDX]._buffer[5] = 0x01; ep_buff[EP_02_CMD_IDX]._size = 0x06; ep_buff[EP_82_DATA_IDX]._size = 0x10; _do_single_threaded_usb_comm(EP_02_CMD_IDX); ReleaseMutex(g_hEP02_Serial_Mutex); lX = g_machine.x._scale_pos; lY = g_machine.y._scale_pos; lZ = g_machine.z._scale_pos; lZm = g_machine.zm._scale_pos; return SSI_STATUS_NORMAL; }; //============================================================= // Send 0x6 bytes to the device // 00 01 01 01 01 01 // Get 0x10 bytes from the device // FF FF FF FF FF FF FF FD 00 00 06 0E FF FF FE 6B // This is actually getting index value. // Probably not useful. //============================================================== SSI_STATUS CMv_Proto::_send_cmd_PRE_ROTARY() { WaitForSingleObject(g_hEP01_Serial_Mutex, INFINITE); memset(ep_buff[EP_02_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE); ep_buff[EP_02_CMD_IDX]._buffer[0] = 0; ep_buff[EP_02_CMD_IDX]._buffer[1] = 1; ep_buff[EP_02_CMD_IDX]._buffer[2] = 1; ep_buff[EP_02_CMD_IDX]._buffer[3] = 1; ep_buff[EP_02_CMD_IDX]._buffer[4] = 1; ep_buff[EP_02_CMD_IDX]._buffer[5] = 1; ep_buff[EP_02_CMD_IDX]._size = 0x6; ep_buff[EP_82_DATA_IDX]._size = 0x10; _do_single_threaded_usb_comm(EP_02_CMD_IDX); ReleaseMutex(g_hEP02_Serial_Mutex); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_process_MV_CMD_GET_SUB_CONFIG_28() { return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_process_MV_CMD_SERVO_CMD_2B() { memcpy((char *)g_machine.s_machine_config._str_servo_Id, ep_buff[EP_81_DATA_IDX]._buffer+1, 32); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_process_MV_CMD_2C() { return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_process_MV_CMD_GET_X_INDEX_41() { if (*(ep_buff[EP_81_DATA_IDX]._buffer+1) == 0x03) { g_machine.s_machine_config.x_axis._index_03 = _4char2long((unsigned char*)(ep_buff[EP_81_DATA_IDX]._buffer+2)); TRACE1("_process_MV_CMD_GET_X_INDEX_41()_index_03 : %ld\n", g_machine.s_machine_config.x_axis._index_03); } else if (*(ep_buff[EP_81_DATA_IDX]._buffer+1) == 0x10) { g_machine.s_machine_config.x_axis._index_10 = _4char2long((unsigned char*)(ep_buff[EP_81_DATA_IDX]._buffer+2)); TRACE1("_process_MV_CMD_GET_X_INDEX_41()_index_10 : %ld\n", g_machine.s_machine_config.x_axis._index_10); }; return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_process_MV_CMD_GET_Y_INDEX_42() { if (*(ep_buff[EP_81_DATA_IDX]._buffer+1) == 0x03) { g_machine.s_machine_config.y_axis._index_03 = _4char2long((unsigned char*)(ep_buff[EP_81_DATA_IDX]._buffer+2)); TRACE1("_process_MV_CMD_GET_Y_INDEX_42()_index_03 : %ld\n", g_machine.s_machine_config.y_axis._index_03); } else if (*(ep_buff[EP_81_DATA_IDX]._buffer+1) == 0x10) { g_machine.s_machine_config.y_axis._index_10 = _4char2long((unsigned char*)(ep_buff[EP_81_DATA_IDX]._buffer+2)); TRACE1("_process_MV_CMD_GET_Y_INDEX_42()_index_10 : %ld\n", g_machine.s_machine_config.y_axis._index_10); }; return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_process_MV_CMD_GET_MAG_43() { if (*(ep_buff[EP_81_DATA_IDX]._buffer+2) == 0x03) { g_machine.s_machine_config.zm_axis._index_03 = _4char2long((unsigned char*)(ep_buff[EP_81_DATA_IDX]._buffer+2)); TRACE1("_process_MV_CMD_GET_MAG_43() : %ld\n", g_machine.s_machine_config.zm_axis._index_03); } else if (*(ep_buff[EP_81_DATA_IDX]._buffer+2) == 0x10) { g_machine.s_machine_config.zm_axis._index_10 = _4char2long((unsigned char*)(ep_buff[EP_81_DATA_IDX]._buffer+2)); TRACE1("_process_MV_CMD_GET_MAG_43() : %ld\n", g_machine.s_machine_config.zm_axis._index_10); }; return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_process_MV_CMD_GET_ZOOM_DECEL_45() { g_machine.s_machine_config.zm_axis._index_03 = _4char2long((unsigned char*)(ep_buff[EP_81_DATA_IDX]._buffer+2)); TRACE1("_process_MV_CMD_GET_ZOOM_DECEL_45() : %ld\n", g_machine.s_machine_config.zm_axis._index_03); return SSI_STATUS_NORMAL; } //============================================================== SSI_STATUS CMv_Proto::_process_MV_CMD_GET_INDEX_4E() { long lIdx = _4char2long((unsigned char*)(ep_buff[EP_81_DATA_IDX]._buffer+3)); switch (*(ep_buff[EP_01_CMD_IDX]._buffer+1)) { case 0 : // X-Index g_machine.s_machine_config.x_axis._index_03 = lIdx; break; case 1 : // Y-Index g_machine.s_machine_config.y_axis._index_03 = lIdx; break; case 2 : // Z - Index g_machine.s_machine_config.z_axis._index_03 = lIdx; break; default: return SSI_STATUS_DATALINK_ERROR; }; return SSI_STATUS_NORMAL; } //============================================================== SSI_STATUS CMv_Proto::_process_MV_CMD_MAX_SPEED_50() { g_machine.s_machine_config.x_axis._max_speed = _4char2long((unsigned char*)(ep_buff[EP_81_DATA_IDX]._buffer+1)); g_machine.s_machine_config.y_axis._max_speed = _4char2long((unsigned char*)(ep_buff[EP_81_DATA_IDX]._buffer+5)); g_machine.s_machine_config.z_axis._max_speed = _4char2long((unsigned char*)(ep_buff[EP_81_DATA_IDX]._buffer+9)); g_machine.s_machine_config.zm_axis._max_speed = _4char2long((unsigned char*)(ep_buff[EP_81_DATA_IDX]._buffer+13)); TRACE3("_process_MV_CMD_MAX_SPEED_50() x,y,z: %ld %ld %ld\n", g_machine.s_machine_config.x_axis._max_speed, g_machine.s_machine_config.y_axis._max_speed, g_machine.s_machine_config.z_axis._max_speed); TRACE1("_process_MV_CMD_MAX_SPEED_50() Zm : %ld\n", g_machine.s_machine_config.zm_axis._max_speed); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_process_MV_CMD_MAX_ACCEL_51() { g_machine.s_machine_config.x_axis._max_accel = _4char2long((unsigned char*)(ep_buff[EP_81_DATA_IDX]._buffer+1)); g_machine.s_machine_config.y_axis._max_accel = _4char2long((unsigned char*)(ep_buff[EP_81_DATA_IDX]._buffer+5)); g_machine.s_machine_config.z_axis._max_accel = _4char2long((unsigned char*)(ep_buff[EP_81_DATA_IDX]._buffer+9)); g_machine.s_machine_config.zm_axis._max_accel = _4char2long((unsigned char*)(ep_buff[EP_81_DATA_IDX]._buffer+13)); TRACE3("_process_MV_CMD_MAX_ACCEL_51() x,y,z: %ld %ld %ld\n", g_machine.s_machine_config.x_axis._max_accel, g_machine.s_machine_config.y_axis._max_accel, g_machine.s_machine_config.z_axis._max_accel); TRACE1("_process_MV_CMD_MAX_ACCEL_51() Zm : %ld\n", g_machine.s_machine_config.zm_axis._max_accel); return SSI_STATUS_NORMAL; }; //============================================================== // SSI_STATUS CMv_Proto::_process_MV_CMD_GET_UNK_56() { return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_process_MV_CMD_GET_SCALE_58() { memcpy(&(g_machine.x._pos_58), ep_buff[EP_81_DATA_IDX]._buffer+1, 4); _reverse_dword((DWORD *)&(g_machine.x._pos_58)); memcpy(&(g_machine.y._pos_58), ep_buff[EP_81_DATA_IDX]._buffer+1+4, 4); _reverse_dword((DWORD *)&(g_machine.y._pos_58)); memcpy(&(g_machine.z._pos_58), ep_buff[EP_81_DATA_IDX]._buffer+1+4*2, 4); _reverse_dword((DWORD *)&(g_machine.z._pos_58)); memcpy(&(g_machine.zm._pos_58), ep_buff[EP_81_DATA_IDX]._buffer+1+4*3, 4); _reverse_dword((DWORD *)&(g_machine.zm._pos_58)); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_process_MV_CMD_GET_POLL_59() { CString Tmp; ASSERT(ep_buff[EP_81_DATA_IDX]._buffer[0]); memcpy(&(g_machine.x._pos_59), ep_buff[EP_81_DATA_IDX]._buffer+1+45, 4); _reverse_dword((DWORD *)&(g_machine.x._pos_59)); memcpy(&(g_machine.y._pos_59), ep_buff[EP_81_DATA_IDX]._buffer+1+49, 4); _reverse_dword((DWORD *)&(g_machine.y._pos_59)); memcpy(&(g_machine.z._pos_59), ep_buff[EP_81_DATA_IDX]._buffer+1+53, 4); _reverse_dword((DWORD *)&(g_machine.z._pos_59)); memcpy(&(g_machine.zm._pos_59), ep_buff[EP_81_DATA_IDX]._buffer+1+57, 4); _reverse_dword((DWORD *)&(g_machine.zm._pos_59)); long lXIdle = *(ep_buff[EP_81_DATA_IDX]._buffer+16); long lYIdle = *(ep_buff[EP_81_DATA_IDX]._buffer+17); long lZIdle = *(ep_buff[EP_81_DATA_IDX]._buffer+18); long lZMIdle = *(ep_buff[EP_81_DATA_IDX]._buffer+19); g_machine.s_status._bXMoving = lXIdle == 0 ? true : false; g_machine.s_status._bYMoving = lYIdle == 0 ? true : false; g_machine.s_status._bZMoving = lZIdle == 0 ? true : false; g_machine.s_status._bZMMoving = lZMIdle == 0 ? true : false; g_machine.s_status._bXYZZMIdle = (lXIdle + lYIdle + lZIdle + lZMIdle == 4) ? true : false; TRACE1("_bXYZZMIdle %ld\n", g_machine.s_status._bXYZZMIdle); Tmp.Format(_T("_process_MV_CMD_GET_POLL_59() %lf: _save_cmd %X ep_buff[81] %X x %ld y %ld z %ld zm %ld x_pos %ld y_pos %ld z_pos %ld zm-pos %ld \r\n"), TimeInSecs(), ep_buff[EP_01_CMD_IDX]._save_send_cmd, ep_buff[EP_81_DATA_IDX]._buffer[0], lXIdle, lYIdle, lZIdle, lZMIdle, g_machine.x._pos_59, g_machine.y._pos_59, g_machine.z._pos_59, g_machine.zm._pos_59); TRACE1("%s", Tmp); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_process_MV_CMD_GET_WORKING_LIMITS_5E() { memcpy(&(g_machine.s_machine_config.x_axis._neg_working_limit), ep_buff[EP_81_DATA_IDX]._buffer+5, 4); _reverse_dword((DWORD *)&(g_machine.s_machine_config.x_axis._neg_working_limit)); memcpy(&(g_machine.s_machine_config.y_axis._neg_working_limit), ep_buff[EP_81_DATA_IDX]._buffer+5+4*1, 4); _reverse_dword((DWORD *)&(g_machine.s_machine_config.y_axis._neg_working_limit)); memcpy(&(g_machine.s_machine_config.z_axis._neg_working_limit), ep_buff[EP_81_DATA_IDX]._buffer+5+4*2, 4); _reverse_dword((DWORD *)&(g_machine.s_machine_config.z_axis._neg_working_limit)); memcpy(&(g_machine.s_machine_config.zm_axis._neg_working_limit), ep_buff[EP_81_DATA_IDX]._buffer+5+4*3, 4); _reverse_dword((DWORD *)&(g_machine.s_machine_config.zm_axis._neg_working_limit)); memcpy(&(g_machine.s_machine_config.x_axis._pos_working_limit), ep_buff[EP_81_DATA_IDX]._buffer+5+4*4, 4); _reverse_dword((DWORD *)&(g_machine.s_machine_config.x_axis._pos_working_limit)); memcpy(&(g_machine.s_machine_config.y_axis._pos_working_limit), ep_buff[EP_81_DATA_IDX]._buffer+5+4*5, 4); _reverse_dword((DWORD *)&(g_machine.s_machine_config.y_axis._pos_working_limit)); memcpy(&(g_machine.s_machine_config.z_axis._pos_working_limit), ep_buff[EP_81_DATA_IDX]._buffer+5+4*6, 4); _reverse_dword((DWORD *)&(g_machine.s_machine_config.z_axis._pos_working_limit)); memcpy(&(g_machine.s_machine_config.zm_axis._pos_working_limit), ep_buff[EP_81_DATA_IDX]._buffer+5+4*7, 4); _reverse_dword((DWORD *)&(g_machine.s_machine_config.zm_axis._pos_working_limit)); CString Tmp; Tmp.Format(_T("_process_MV_CMD_GET_WORKING_LIMITS_5E() +:- xL-%ld yL-%ld zL-%ld zmL-%ld xH-%ld yH-%ld zH-%ld zmH-%ld\n"), g_machine.s_machine_config.x_axis._pos_working_limit, g_machine.s_machine_config.x_axis._neg_working_limit, g_machine.s_machine_config.y_axis._pos_working_limit, g_machine.s_machine_config.y_axis._neg_working_limit, g_machine.s_machine_config.z_axis._pos_working_limit, g_machine.s_machine_config.z_axis._neg_working_limit, g_machine.s_machine_config.zm_axis._pos_working_limit, g_machine.s_machine_config.zm_axis._neg_working_limit ); TRACE1("%s", Tmp); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_process_MV_CMD_GET_CONFIG_60() { memcpy((char *)g_machine.s_machine_config._str_serial_no, ep_buff[EP_81_DATA_IDX]._buffer+1, 32); memcpy((char *)g_machine.s_machine_config._str_model, ep_buff[EP_81_DATA_IDX]._buffer+1+32, 32); memcpy((char *)g_machine.s_machine_config._str_model_nbr, ep_buff[EP_81_DATA_IDX]._buffer+1+32+32, 32); if (wcsstr(g_machine.s_machine_config._str_model_nbr, L"220")) g_machine.s_machine_config._vector_light_model = 2; else if (wcsstr(g_machine.s_machine_config._str_model_nbr, L"310")) g_machine.s_machine_config._vector_light_model = 3; else g_machine.s_machine_config._vector_light_model = 3; return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_process_MV_CMD_GET_CONFIG_69() { return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_process_MV_CMD_SET_LIGHT_6E() { return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_process_MV_CMD_GET_LIGHT_CONFIG_70() { if (g_machine.s_machine_config._vector_light_model == 3) memcpy((char *)g_machine.s_lights_300._segment, ep_buff[EP_81_DATA_IDX]._buffer+1, EIGHT_SEGS*FIVE_RINGS*4+8); else // if (g_machine.s_machine_config._vector_light_model == 3) memcpy((char *)g_machine.s_lights_200._segment, ep_buff[EP_81_DATA_IDX]._buffer+1, EIGHT_SEGS*TWO_RINGS*4+8); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_process_MV_CMD_MAX_DECEL_76() { g_machine.s_machine_config.x_axis._max_decel = _4char2long((unsigned char*)(ep_buff[EP_81_DATA_IDX]._buffer+1)); g_machine.s_machine_config.y_axis._max_decel = _4char2long((unsigned char*)(ep_buff[EP_81_DATA_IDX]._buffer+5)); g_machine.s_machine_config.z_axis._max_decel = _4char2long((unsigned char*)(ep_buff[EP_81_DATA_IDX]._buffer+9)); g_machine.s_machine_config.zm_axis._max_decel = _4char2long((unsigned char*)(ep_buff[EP_81_DATA_IDX]._buffer+13)); TRACE3("_process_MV_CMD_MAX_DECEL_76() x,y,z: %ld %ld %ld\n", g_machine.s_machine_config.x_axis._max_decel, g_machine.s_machine_config.y_axis._max_decel, g_machine.s_machine_config.z_axis._max_decel); TRACE1("_process_MV_CMD_MAX_DECEL_76() Zm : %ld\n", g_machine.s_machine_config.zm_axis._max_decel); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_process_MV_CMD_GET_SCALE_SLACK_7C() { union { int iValue; char c_array[4]; }; memset (c_array, 0, 4); c_array[0] = *(ep_buff[EP_81_DATA_IDX]._buffer+3); c_array[1] = *(ep_buff[EP_81_DATA_IDX]._buffer+2); g_machine.s_machine_config.zm_axis._slack = iValue; TRACE1("_process_MV_CMD_GET_SCALE_SLACK_7C() Zm : %ld\n", iValue); return SSI_STATUS_NORMAL; }; //============================================================== SSI_STATUS CMv_Proto::_process_MV_CMD_GET_FINE_SCALE() { memcpy(&(g_machine.x._scale_pos), ep_buff[EP_82_DATA_IDX]._buffer, 4); _reverse_dword((DWORD *)&(g_machine.x._scale_pos)); memcpy(&(g_machine.y._scale_pos), ep_buff[EP_82_DATA_IDX]._buffer+4*1, 4); _reverse_dword((DWORD *)&(g_machine.y._scale_pos)); memcpy(&(g_machine.z._scale_pos), ep_buff[EP_82_DATA_IDX]._buffer+4*2, 4); _reverse_dword((DWORD *)&(g_machine.z._scale_pos)); memcpy(&(g_machine.zm._scale_pos), ep_buff[EP_82_DATA_IDX]._buffer+4*3, 4); _reverse_dword((DWORD *)&(g_machine.zm._scale_pos)); CString Tmp; Tmp.Format(_T("_process_MV_CMD_GET_FINE_SCALE() +:- x-%ld y-%ld z-%ld zm-%ld\n"), g_machine.x._scale_pos, g_machine.y._scale_pos, g_machine.z._scale_pos, g_machine.zm._scale_pos ); TRACE1("%s", Tmp); return SSI_STATUS_NORMAL; }; #pragma message("Add error reporting and dbug logging in the HSI for when the move settle fails") //=============================================================================== // not sure what To Do if the settle testing fails. Should this genreate errors // to indicate a creeping stage and increased settle parameters? // retry count is the number of extra times To Do the stage moving test // bandwidth is the maximum motion in a single watch time // timeout is the maximum time to wait for a sub bandwidth move //=============================================================================== SSI_STATUS CMv_Proto::WaitForSettleXYZZM() { // if TP active in DCC or rach mode then dont wait for settle MV_TRACE(_T("Enter CSsi::WaitForSettleXYZZM\n")); static bool bTest=true; // if homing is active then we dont need to wait for this settle if ( m_bHomingActive ) { MV_TRACE(_T("Enter CSsi::WaitForSettleXYZZM Homing active\n")); return SSI_STATUS_NORMAL; } long lSettledMask = 0; long XSettled = 0x00000001; long YSettled = 0x00000002; long ZSettled = 0x00000004; long ZMSettled = 0x00000008; long AllSettled = 0x000000F; long XTimedOut = 0x00010000; long YTimedOut = 0x00020000; long ZTimedOut = 0x00040000; long ZMTimedOut = 0x00080000; long AllTimeouts =0x000F0000; long lStartX = 0; long lStartY = 0; long lStartZ = 0; long lStartZM = 0; // long lStartR = 0; // long Reserved; for ( int ii = 0 ; ii < 0 ; ++ii ) { double dStartTime = TimeInSecs(); _send_cmd_MV_CMD_GET_POLL_59(lStartX, lStartY, lStartZ, lStartZM); CString msg; msg.Format(_T("\nSettlePositionTrace: Sec: %lf XYZZM: %ld %ld %ld %ld"), dStartTime, lStartX, lStartY, lStartZ, lStartZM); TRACE1("%s", msg); Sleep(100); } CSettleStageParams cParams; _send_cmd_MV_CMD_GET_POLL_59(lStartX, lStartY, lStartZ, lStartZM); double dStartTime = TimeInSecs(); long lXDeadband = (long)(cParams.EDGE_RETRY_BANDWIDTH_X * 1000.0 * g_machine.s_machine_config.x_axis._scale_resolution); long lYDeadband = (long)(cParams.EDGE_RETRY_BANDWIDTH_Y * 1000.0 * g_machine.s_machine_config.y_axis._scale_resolution); long lZDeadband = (long)(cParams.EDGE_RETRY_BANDWIDTH_Z * 1000.0 * g_machine.s_machine_config.z_axis._scale_resolution); long lZMDeadband = (long)(cParams.EDGE_RETRY_BANDWIDTH_MAG/2000000.0); double dScaleTestIncr = 0.15; long lX=0,lY=0,lZ=0, lZM=0; //long lR=0, lReserved=0; TRACE1("\nSettle start time %lf\n\n", dStartTime); double dMaxSettleTimeSec=0.0; dMaxSettleTimeSec = (cParams.EDGE_RETRY_TIMEOUT_X/1000.0 < dMaxSettleTimeSec)?dMaxSettleTimeSec:cParams.EDGE_RETRY_TIMEOUT_X/1000.0; dMaxSettleTimeSec = (cParams.EDGE_RETRY_TIMEOUT_Y/1000.0 < dMaxSettleTimeSec)?dMaxSettleTimeSec:cParams.EDGE_RETRY_TIMEOUT_Y/1000.0; dMaxSettleTimeSec = (cParams.EDGE_RETRY_TIMEOUT_Z/1000.0 < dMaxSettleTimeSec)?dMaxSettleTimeSec:cParams.EDGE_RETRY_TIMEOUT_Z/1000.0; dMaxSettleTimeSec = (cParams.EDGE_RETRY_TIMEOUT_MAG/1000.0 < dMaxSettleTimeSec)?dMaxSettleTimeSec:cParams.EDGE_RETRY_TIMEOUT_MAG/1000.0; CSettleStage cXAxis(1, dStartTime, lStartX, lXDeadband, dScaleTestIncr, 5*dMaxSettleTimeSec); CSettleStage cYAxis(2, dStartTime, lStartY, lYDeadband, dScaleTestIncr, 5*dMaxSettleTimeSec); CSettleStage cZAxis(3, dStartTime, lStartZ, lZDeadband, dScaleTestIncr, 5*dMaxSettleTimeSec); CSettleStage cZMAxis(4, dStartTime, lStartZM, lZMDeadband, dScaleTestIncr, 5*dMaxSettleTimeSec); // if the axis time increment has passed and the stage has not moved more than the // bandwidth then it has settled. If the max settle time widow has expired // then it failed to settle // loop until or one has timed out or all settled while ( !(AllTimeouts&lSettledMask) && ((AllSettled&lSettledMask) != AllSettled) ) { Sleep(10); double dCurTime = TimeInSecs(); enum SETTLE_STATUS SettleStatus = NOTSETTLED; _send_cmd_MV_CMD_GET_POLL_59(lX, lY, lZ, lZM); TRACE1("time: %lf\n", dCurTime); SettleStatus = cXAxis.Test(dCurTime, lX); TRACE1("X Test Status %ld\n", SettleStatus); #ifdef _DEBUG if ( SettleStatus == TIMEDOUT ) TRACE0("X settle timeout\n"); if ( SettleStatus == SETTLED ) TRACE0("X settled\n"); #endif lSettledMask = (SettleStatus == SETTLED)?(lSettledMask |XSettled):lSettledMask; lSettledMask = (SettleStatus == TIMEDOUT)?(lSettledMask |XTimedOut):lSettledMask; SettleStatus = cYAxis.Test(dCurTime, lY); TRACE1("Y Test Status %ld\n", SettleStatus); #ifdef _DEBUG if ( SettleStatus == TIMEDOUT ) TRACE0("Y settle timeout\n"); if ( SettleStatus == SETTLED ) TRACE0("Y settled\n"); #endif lSettledMask = (SettleStatus == SETTLED)?lSettledMask |= YSettled:lSettledMask; lSettledMask = (SettleStatus == TIMEDOUT)?lSettledMask |= YTimedOut:lSettledMask; SettleStatus = cZAxis.Test(dCurTime, lZ); TRACE1("Z Test Status %ld\n", SettleStatus); #ifdef _DEBUG if ( SettleStatus == TIMEDOUT ) TRACE0("Z settle timeout\n"); if ( SettleStatus == SETTLED ) TRACE0("Z settled\n"); #endif lSettledMask = (SettleStatus == SETTLED)?lSettledMask |= ZSettled:lSettledMask; lSettledMask = (SettleStatus == TIMEDOUT)?lSettledMask |= ZTimedOut:lSettledMask; SettleStatus = cZMAxis.Test(dCurTime, lZM); TRACE1("ZM Test Status %ld\n", SettleStatus); #ifdef _DEBUG if ( SettleStatus == TIMEDOUT ) TRACE0("ZM settle timeout\n"); if ( SettleStatus == SETTLED ) TRACE0("ZM settled\n"); #endif lSettledMask = (SettleStatus == SETTLED)?lSettledMask |= ZMSettled:lSettledMask; lSettledMask = (SettleStatus == TIMEDOUT)?lSettledMask |= ZMTimedOut:lSettledMask; } TRACE1("CSsi::WaitForSettle exit status %X", lSettledMask); TRACE2("\nSettle finish time %lf Elapsed: %lf\n\n", TimeInSecs(), TimeInSecs()-dStartTime); if ( (AllSettled&lSettledMask) == AllSettled ) { TRACE0(" - no timeout\n"); MV_TRACE(_T("Exit CSsi::WaitForSettleXYZZM normal\n")); return SSI_STATUS_NORMAL; } else { TRACE0(" - timeout\n"); MV_TRACE(_T("Exit CSsi::WaitForSettleXYZZM timeout\n")); return SSI_STATUS_ERROR; } } CSettleStage::CSettleStage(long lAxis, double dStartTime, long lStart, long lDeadband, double dTestIncrSec, double dMaxTimeSec) { m_dStartTime = dStartTime; m_lStartPos = lStart; m_lDeadband = lDeadband; m_dTestIncrSec = dTestIncrSec; m_dMaxTimeSec = dMaxTimeSec; m_dIncrStartTimeSec = m_dStartTime; m_lAxis = lAxis; } enum SETTLE_STATUS CSettleStage::Test(double dCurTime, long lCurPos) { if ( dCurTime - m_dStartTime > m_dMaxTimeSec ) { TRACE1("CSettleStage::Test Axis: %ld Final TIMEDOUT\n", m_lAxis); ASSERT(0); return TIMEDOUT; } if ( dCurTime - m_dIncrStartTimeSec > m_dTestIncrSec ) { if ( abs(lCurPos - m_lStartPos) <= m_lDeadband ) { TRACE2("settle test Axis: %ld %lf Settled\n", m_lAxis, dCurTime); return SETTLED; } else { CString msg; msg.Format(_T("Settle test Axis: %ld Incr timeout cur time: %lf, inc start time: %lf cur pos: %ld start pos %ld\n"), m_lAxis, dCurTime, m_dIncrStartTimeSec, lCurPos, m_lStartPos); TRACE1("%s", msg); m_lStartPos = lCurPos; m_dIncrStartTimeSec = dCurTime; } } return NOTSETTLED; }