TAO Cheng
3295 lines
124 KiB
C++
3295 lines
124 KiB
C++
//=============================================================================
|
|
#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 ; ii<FIVE_RINGS; ii++ )
|
|
{
|
|
for (int jj=0 ; jj<EIGHT_SEGS ; jj++)
|
|
{
|
|
g_machine.s_lights_300._segment[ii][jj] = lIntensity;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
// if (m_pMv_Proto->g_machine.s_machine_config._vector_light_model == 2)
|
|
{
|
|
for (int ii=0 ; ii<TWO_RINGS; ii++ )
|
|
{
|
|
for (int jj=0 ; jj<EIGHT_SEGS ; jj++)
|
|
{
|
|
g_machine.s_lights_200._segment[ii][jj] = lIntensity;
|
|
}
|
|
}
|
|
};
|
|
}
|
|
|
|
//******************************************************************************
|
|
void CMv_Proto::mv_set_ringlight_data(long lMaxSize, double *pSegments)
|
|
{
|
|
if (pSegments)
|
|
{
|
|
if (g_machine.s_machine_config._vector_light_model == 3)
|
|
{
|
|
for (int ii=0 ; ii<FIVE_RINGS; ii++ )
|
|
{
|
|
for (int jj=0 ; jj<EIGHT_SEGS ; jj++)
|
|
{
|
|
if ((ii+1) * (jj+1) < lMaxSize)
|
|
g_machine.s_lights_300._segment[ii][jj] = (long)((pSegments[ii * EIGHT_SEGS + jj])/100.0 * (mvMAXLIGHTVALUE-1));
|
|
}
|
|
}
|
|
}
|
|
else
|
|
// if (g_machine.s_machine_config._vector_light_model == 2)
|
|
{
|
|
for (int ii=0 ; ii<TWO_RINGS; ii++ )
|
|
{
|
|
for (int jj=0 ; jj<EIGHT_SEGS ; jj++)
|
|
{
|
|
if ((ii+1) * (jj+1) < lMaxSize)
|
|
g_machine.s_lights_200._segment[ii][jj] = (long)((pSegments[ii * EIGHT_SEGS + jj])/100.0 * (mvMAXLIGHTVALUE-1));
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
//******************************************************************************
|
|
// Parse the data received based on the index (which EP are we processing).
|
|
// The CCmdObj should only have one CMD_STATUS_PROCESSING.
|
|
// Update CCmdObj _ep_01_status and _ep_81_status.
|
|
// Update _status to CMD_STATUS_COMPLETE when both _ep_01_status and _ep_81_status
|
|
// are CMD_STATUS_COMPLETE.
|
|
//
|
|
//******************************************************************************
|
|
void CMv_Proto::_process_rcv_transfer_data(int iEP)
|
|
{
|
|
static long lRcvCnt = 0;
|
|
static long lRcvCnt2 = 0;
|
|
switch (iEP)
|
|
{
|
|
case EP_01_CMD_IDX :
|
|
TRACE0("_process_rcv_transfer_data() : Update EP_01_CMD_IDX.\r\n");
|
|
++lRcvCnt;
|
|
break;
|
|
case EP_81_DATA_IDX : //
|
|
++lRcvCnt;
|
|
switch (ep_buff[EP_01_CMD_IDX]._save_send_cmd)
|
|
{
|
|
case MV_CMD_GET_SUB_CONFIG_28:
|
|
_process_MV_CMD_GET_SUB_CONFIG_28();
|
|
break;
|
|
case MV_CMD_SERVO_CMD_2B:
|
|
_process_MV_CMD_SERVO_CMD_2B();
|
|
break;
|
|
case MV_CMD_GET_X_INDEX_41:
|
|
_process_MV_CMD_GET_X_INDEX_41();
|
|
break;
|
|
case MV_CMD_GET_Y_INDEX_42:
|
|
_process_MV_CMD_GET_Y_INDEX_42();
|
|
break;
|
|
case MV_CMD_GET_MAG_43:
|
|
_process_MV_CMD_GET_MAG_43();
|
|
break;
|
|
case MV_CMD_GET_ZOOM_DECEL_45:
|
|
_process_MV_CMD_GET_ZOOM_DECEL_45();
|
|
break;
|
|
case MV_CMD_GET_INDEX_4E:
|
|
_process_MV_CMD_GET_INDEX_4E();
|
|
break;
|
|
case MV_CMD_MAX_ACCEL_51:
|
|
_process_MV_CMD_MAX_ACCEL_51();
|
|
break;
|
|
case MV_CMD_MAX_SPEED_50:
|
|
_process_MV_CMD_MAX_SPEED_50();
|
|
break;
|
|
case MV_CMD_GET_SCALE_58:
|
|
_process_MV_CMD_GET_SCALE_58();
|
|
break;
|
|
case MV_CMD_GET_POLL_59:
|
|
_process_MV_CMD_GET_POLL_59();
|
|
break;
|
|
case MV_CMD_GET_CONFIG_60:
|
|
_process_MV_CMD_GET_CONFIG_60();
|
|
break;
|
|
case MV_CMD_GET_CONFIG_69:
|
|
_process_MV_CMD_GET_CONFIG_69();
|
|
break;
|
|
case MV_CMD_SET_LIGHT_6E:
|
|
_process_MV_CMD_SET_LIGHT_6E();
|
|
if (ep_buff[EP_81_DATA_IDX]._buffer[0] != 0x6E)
|
|
{
|
|
ASSERT(0);
|
|
};
|
|
break;
|
|
case MV_CMD_GET_LIGHT_CONFIG_70:
|
|
_process_MV_CMD_GET_LIGHT_CONFIG_70();
|
|
break;
|
|
case MV_CMD_MAX_DECEL_76:
|
|
_process_MV_CMD_MAX_DECEL_76();
|
|
break;
|
|
case MV_CMD_GET_SCALE_SLACK_7C:
|
|
_process_MV_CMD_GET_SCALE_SLACK_7C();
|
|
break;
|
|
case MV_CMD_GET_WORKING_LIMITS_5E:
|
|
_process_MV_CMD_GET_WORKING_LIMITS_5E();
|
|
break;
|
|
case 0x2C :
|
|
_process_MV_CMD_2C();
|
|
break;
|
|
default:
|
|
TRACE1("_process_rcv_transfer_data() : Unknown ep_buff[EP_01_CMD_IDX]._save_send_cmd : %X \r\n", ep_buff[EP_01_CMD_IDX]._save_send_cmd);
|
|
TRACE1("_process_rcv_transfer_data() : Unknown ep_buff[EP_81_DATA_IDX]._buffer[0] : %X \r\n", ep_buff[EP_81_DATA_IDX]._buffer[0]);
|
|
break;
|
|
};
|
|
TRACE0("_process_rcv_transfer_data() : Update EP_81_DATA_IDX status.\r\n");
|
|
break;
|
|
case EP_02_CMD_IDX :
|
|
TRACE0("_process_rcv_transfer_data() : Update EP_02_CMD_IDX.\r\n");
|
|
++lRcvCnt2;
|
|
break;
|
|
case EP_82_DATA_IDX :
|
|
// parse result and put into its proper place.
|
|
_process_MV_CMD_GET_FINE_SCALE();
|
|
++lRcvCnt2;
|
|
break;
|
|
default:
|
|
break;
|
|
};
|
|
lRcvCnt = (lRcvCnt == 2) ? 0:lRcvCnt;
|
|
lRcvCnt2 = (lRcvCnt2 == 2) ? 0:lRcvCnt2;
|
|
};
|
|
|
|
//******************************************************************************
|
|
void CMv_Proto::_scale2inch(unsigned long scale, double &inch)
|
|
{
|
|
UNREFERENCED_PARAMETER(scale);
|
|
UNREFERENCED_PARAMETER(inch);
|
|
}
|
|
|
|
//******************************************************************************
|
|
void CMv_Proto::_inch2scale(unsigned long &scale, double inch)
|
|
{
|
|
UNREFERENCED_PARAMETER(scale);
|
|
UNREFERENCED_PARAMETER(inch);
|
|
}
|
|
|
|
//******************************************************************************
|
|
// convert a string of characters into its binary form
|
|
void CMv_Proto::_char2bin(unsigned char *cBuff, BYTE *cBytes, int iLen)
|
|
{
|
|
memset(cBytes, 0, MAX_BUFF_SIZE);
|
|
for (int i=0;i<iLen;i++)
|
|
{
|
|
sscanf_s((const char *)(cBuff+i*2), "%2x", (cBytes+i));
|
|
};
|
|
return;
|
|
}
|
|
|
|
//******************************************************************************
|
|
void CMv_Proto::_swap_byte(unsigned short &Val)
|
|
{
|
|
unsigned short MSB = Val<<8;
|
|
BYTE LSB = Val>>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<lEPSIZE;i++)
|
|
{
|
|
ep_buff[i]._size = 0;
|
|
ep_buff[i]._save_send_cmd = 99;
|
|
ep_buff[i]._async_context = NULL;
|
|
ep_buff[i]._buffer = (char *)malloc(MAX_BUFF_SIZE);
|
|
ep_buff[i]._hProtoPending = false;
|
|
ep_buff[i]._event = NULL;
|
|
};
|
|
g_machine.x._pos_58 = -7777;
|
|
g_machine.y._pos_58 = -8888;
|
|
g_machine.z._pos_58 = -9999;
|
|
g_machine.zm._pos_58 = -6666;
|
|
g_machine.x._pos_59 = 333;
|
|
g_machine.y._pos_59 = 444;
|
|
g_machine.z._pos_59 = 555;
|
|
g_machine.zm._pos_59 = 222;
|
|
g_machine.x._scale_pos = -1111;
|
|
g_machine.y._scale_pos = -2222;
|
|
g_machine.z._scale_pos = -3333;
|
|
g_machine.zm._scale_pos = -4444;
|
|
|
|
g_machine.s_machine_config.x_axis._scale_resolution = 0.001;
|
|
g_machine.s_machine_config.y_axis._scale_resolution = 0.001;
|
|
g_machine.s_machine_config.z_axis._scale_resolution = 0.001;
|
|
|
|
g_machine.s_machine_config.x_axis._max_speed = 50000;
|
|
g_machine.s_machine_config.y_axis._max_speed = 50000;
|
|
g_machine.s_machine_config.z_axis._max_speed = 50000;
|
|
g_machine.s_machine_config.zm_axis._max_speed = 5000;
|
|
|
|
g_machine.s_machine_config.x_axis._max_accel = 1000000;
|
|
g_machine.s_machine_config.x_axis._max_decel = 4000000;
|
|
|
|
g_machine.s_machine_config.y_axis._max_accel = 1000000;
|
|
g_machine.s_machine_config.y_axis._max_decel = 4000000;
|
|
|
|
g_machine.s_machine_config.z_axis._max_accel = 1000000;
|
|
g_machine.s_machine_config.z_axis._max_decel = 4000000;
|
|
|
|
g_machine.s_machine_config.zm_axis._slack = 299;
|
|
|
|
g_machine.s_machine_config._nbr_lamps = 3;
|
|
|
|
g_machine.s_status._bXYZZMIdle = false;
|
|
g_machine.s_machine_config._dXYZSpeed = 50;
|
|
m_bHomingActive = false;
|
|
|
|
g_pLogger = new CLogger();
|
|
};
|
|
|
|
//******************************************************************************
|
|
CMv_Proto::~CMv_Proto()
|
|
{
|
|
for (int i=0;i<lEPSIZE;i++)
|
|
{
|
|
free(ep_buff[i]._buffer);
|
|
};
|
|
delete g_pLogger;
|
|
g_pLogger = NULL;
|
|
}
|
|
|
|
#pragma warning(disable:4996)
|
|
//******************************************************************************
|
|
// Log file shows the machine configuration data
|
|
//
|
|
SSI_STATUS CMv_Proto::Load_MicroVu_Config()
|
|
{
|
|
FILE *hConfigFile = NULL;
|
|
char szLine[MAX_BUFF_SIZE];
|
|
char *token = NULL;
|
|
char seps[] = " =,\t\n";
|
|
|
|
CString csAppPath;
|
|
GetAppPath(csAppPath);
|
|
CString csMvConfigFile = csAppPath + _T("\\mv_config.cfg");
|
|
_wfopen_s(&hConfigFile, csMvConfigFile, _T("rt"));
|
|
if (hConfigFile)
|
|
{
|
|
while (!feof(hConfigFile))
|
|
{
|
|
fgets(szLine,MAX_BUFF_SIZE,hConfigFile); //read a line
|
|
if ((szLine[0] != ';') && (strlen(szLine) > 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<iSendSize;j++)
|
|
{
|
|
memset(x, 0, 3);
|
|
memcpy(x, inBuff+35+(j*2), 2);
|
|
sscanf_s(x, "%2x", ep_buff[EP_01_CMD_IDX]._buffer+j);
|
|
};
|
|
ep_buff[EP_01_CMD_IDX]._size = iSendSize;
|
|
fgets((char *)inBuff, MAX_BUFF_SIZE, pInFile ); // pick up the first line
|
|
if (feof(pInFile)) SSI_STATUS_NORMAL; // if error, exit
|
|
memset(cSize, 0 , 9);
|
|
memcpy(cSize, inBuff+24, 8);
|
|
sscanf_s(cSize, "%8x", &iRcvSize); // get the length of the transmission
|
|
ep_buff[EP_81_DATA_IDX]._size = iRcvSize;
|
|
#pragma message("no usb comm??")
|
|
// _do_single_threaded_usb_comm(EP_01_CMD_IDX);
|
|
return SSI_STATUS_NORMAL;
|
|
};
|
|
|
|
//******************************************************************************
|
|
// Kick the g_hEP01_Thread_Event to get the g_EP01_Thread going.
|
|
// iEP = EP_01 or EP_02 = 0x01 or 0x02
|
|
//******************************************************************************
|
|
SSI_STATUS CMv_Proto::_do_single_threaded_usb_comm(int iEP_Base)
|
|
{
|
|
TRACE1("=====_do_single_threaded_usb_comm(iEP) g_hEP01_Thread_Event. %x\n", iEP_Base);
|
|
while ((ep_buff[iEP_Base]._hProtoPending == TRUE) || (ep_buff[iEP_Base+1]._hProtoPending == TRUE))
|
|
{
|
|
ASSERT(0);
|
|
Sleep(3);
|
|
}
|
|
ep_buff[iEP_Base]._hProtoPending = ep_buff[iEP_Base+1]._hProtoPending = TRUE;
|
|
TRACE1("=====_do_single_threaded_usb_comm(iEP_Base) SetEvent(g_hEP01_Thread_Event): %X \r\n", ep_buff[iEP_Base]._save_send_cmd);
|
|
SetEvent(ep_buff[iEP_Base]._event);
|
|
while ((ep_buff[iEP_Base]._hProtoPending == TRUE) || (ep_buff[iEP_Base+1]._hProtoPending == TRUE))
|
|
{
|
|
Sleep(3);
|
|
}
|
|
TRACE1("=====_do_single_threaded_usb_comm(iEP) g_hProtoDoneEvents. %x\n", iEP_Base);
|
|
return SSI_STATUS_NORMAL;
|
|
}
|
|
|
|
//******************************************************************************
|
|
// This startup just kicks off the EP_81 worker thread.
|
|
//******************************************************************************
|
|
SSI_STATUS CMv_Proto::_start_machine()
|
|
{
|
|
g_hEP81_Thread_State = THREAD_RUNNING;
|
|
g_hEP82_Thread_State = THREAD_RUNNING;
|
|
g_machine.s_status._machine_running = true;
|
|
return SSI_STATUS_NORMAL;
|
|
};
|
|
|
|
//===============================================================================
|
|
SSI_STATUS CMv_Proto::_shutdown_machine()
|
|
{
|
|
g_machine.s_status._poll_58_active = false;
|
|
g_machine.s_status._poll_59_active = false;
|
|
g_machine.s_status._machine_running = false;
|
|
return SSI_STATUS_NORMAL;
|
|
};
|
|
|
|
//===============================================================================
|
|
// iEP = EP_01 or EP_02
|
|
//===============================================================================
|
|
SSI_STATUS CMv_Proto::_submit_async_8x(int iEP_Base)
|
|
{
|
|
int _ret;
|
|
_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;
|
|
}
|
|
return SSI_STATUS_NORMAL;
|
|
}
|
|
|
|
//===============================================================================
|
|
// iEP_Base : EP_81_DATA_IDX/EP_82_DATA_IDX
|
|
//
|
|
//===============================================================================
|
|
SSI_STATUS CMv_Proto::_reap_async_8x(int iEP_Base)
|
|
{
|
|
int _ret;
|
|
_ret = usb_reap_async(ep_buff[iEP_Base]._async_context, 50000);
|
|
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;
|
|
}
|
|
|
|
//===============================================================================
|
|
// _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 = (lDestination<g_machine.s_machine_config.zm_axis._neg_working_limit) ? g_machine.s_machine_config.zm_axis._neg_working_limit : lDestination;
|
|
_send_cmd_MV_CMD_MOVE_AXIS_4C(0x03, lDestination, 1);
|
|
TRACE1("set actual negative scale position = %X \r\n", lDestination);
|
|
g_machine.s_status._bZMMoving = true;
|
|
Sleep(50);
|
|
#pragma message("test status flags for move done")
|
|
{
|
|
while (g_machine.s_status._bZMMoving)
|
|
{
|
|
_poll_machine_59();
|
|
Sleep(50); // wait until it begins to move
|
|
TRACE0("Zm negative moving \r\n");
|
|
}
|
|
}
|
|
_poll_machine_59();
|
|
_poll_machine_58();
|
|
}
|
|
|
|
lLast = g_machine.zm._pos_59;
|
|
long lDestination = lScale;
|
|
lDestination = (lDestination>g_machine.s_machine_config.zm_axis._pos_working_limit) ? g_machine.s_machine_config.zm_axis._pos_working_limit : lDestination;
|
|
lDestination = (lDestination<g_machine.s_machine_config.zm_axis._neg_working_limit) ? g_machine.s_machine_config.zm_axis._neg_working_limit : lDestination;
|
|
|
|
TRACE1("set actual positive scale position = %X \r\n", lDestination);
|
|
_send_cmd_MV_CMD_MOVE_AXIS_4C(0x03, lDestination, 1);
|
|
_send_cmd_MV_CMD_MOVE_AXIS_4C(0x03, lDestination, 0);
|
|
#pragma message("test status flags to eliminate the sleep")
|
|
g_machine.s_status._bZMMoving = true;
|
|
Sleep(50);
|
|
while (g_machine.s_status._bZMMoving)
|
|
{
|
|
_poll_machine_59();
|
|
Sleep(50); // wait until it starts moving
|
|
TRACE0("Zm positive moving\r\n");
|
|
}
|
|
return SSI_STATUS_NORMAL;
|
|
}
|
|
|
|
//========================================================================
|
|
//
|
|
//========================================================================
|
|
SSI_STATUS CMv_Proto::mv_optics_refresh_lens()
|
|
{
|
|
WaitForSingleObject(g_hHomedEvent, INFINITE); // machine start and homing is done
|
|
return SSI_STATUS_NORMAL;
|
|
};
|
|
|
|
//========================================================================
|
|
// Not implemented
|
|
//========================================================================
|
|
SSI_STATUS CMv_Proto::mv_optics_enable_grid()
|
|
{
|
|
return SSI_STATUS_NORMAL;
|
|
};
|
|
|
|
//========================================================================
|
|
// Not implemented
|
|
//========================================================================
|
|
SSI_STATUS CMv_Proto::mv_optics_is_grid_enabled()
|
|
{
|
|
return SSI_STATUS_NORMAL;
|
|
};
|
|
|
|
//========================================================================
|
|
// Not implemented
|
|
//========================================================================
|
|
SSI_STATUS CMv_Proto::mv_optics_enable_laser()
|
|
{
|
|
return SSI_STATUS_NORMAL;
|
|
};
|
|
|
|
//========================================================================
|
|
// Not implemented
|
|
//========================================================================
|
|
SSI_STATUS CMv_Proto::mv_optics_is_laser_enabled()
|
|
{
|
|
return SSI_STATUS_NORMAL;
|
|
};
|
|
|
|
//========================================================================
|
|
// Nothing to do
|
|
//========================================================================
|
|
SSI_STATUS CMv_Proto::mv_optics_shut_down()
|
|
{
|
|
return SSI_STATUS_NORMAL;
|
|
};
|
|
|
|
//========================================================================
|
|
// return true
|
|
//========================================================================
|
|
SSI_STATUS CMv_Proto::mv_light_is_supported(BOOL &bTrue)
|
|
{
|
|
bTrue = TRUE;
|
|
return SSI_STATUS_NORMAL;
|
|
};
|
|
|
|
//========================================================================
|
|
// Nothing to do. Just make sure g_machine is there.
|
|
//========================================================================
|
|
SSI_STATUS CMv_Proto::mv_light_start_up()
|
|
{
|
|
return SSI_STATUS_NORMAL;
|
|
};
|
|
|
|
//========================================================================
|
|
SSI_STATUS CMv_Proto::mv_light_get_lamp_count(int &iCount)
|
|
{
|
|
WaitForSingleObject(g_hHomedEvent, INFINITE); // machine start and homing is done
|
|
iCount = g_machine.s_machine_config._nbr_lamps;
|
|
return SSI_STATUS_NORMAL;
|
|
};
|
|
|
|
//========================================================================
|
|
// Do nothing. g_machine gives you everything.
|
|
//========================================================================
|
|
SSI_STATUS CMv_Proto::mv_light_get_lamp_info()
|
|
{
|
|
WaitForSingleObject(g_hHomedEvent, INFINITE); // machine start and homing is done
|
|
return SSI_STATUS_NORMAL;
|
|
};
|
|
|
|
////========================================================================
|
|
//// Do nothing. g_machine gives you everything.
|
|
////========================================================================
|
|
//SSI_STATUS CMv_Proto::mv_light_get_lamp_state()
|
|
//{
|
|
// return SSI_STATUS_NORMAL;
|
|
//};
|
|
|
|
//========================================================================
|
|
// ON/OFF?
|
|
//========================================================================
|
|
SSI_STATUS CMv_Proto::mv_light_set_lamp_state(double dBottomPercent, double dTopPercent)
|
|
{
|
|
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 = (long)(dTopPercent/100.0 * (mvMAXLIGHTVALUE - 1));
|
|
g_machine.s_lights_300._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_300._axial_light);
|
|
}
|
|
else
|
|
// if (m_pMv_Proto->g_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 ( bTest )
|
|
// if ( m_lTPEnable && (m_lTPMode==0 || m_lTPMode==3) )
|
|
// {
|
|
// MV_TRACE(_T("Exit CSsi::WaitForSettleXYZZM m_lTPEnable && (m_lTPMode==0 || m_lTPMode==3\n"));
|
|
// return SSI_STATUS_NORMAL;
|
|
// }
|
|
|
|
// 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;
|
|
}
|