// HSI_Motion.cpp : 定义 DLL 的初始化例程。
#include "stdafx.h"
#include "HSI.h"
#include "HSI_Sevenocean_EF3.h"
#include "HSI_Motion.h"
#include "logger.h"
#include "SevenOcean\CMMIO_SERIAL.h"
#include "version.h"
#include <math.h>
#include <fstream>
#include <iomanip>
using namespace std;

#ifdef _DEBUG
#define new DEBUG_NEW
#endif
//===========================================================================
HSI_Motion *g_pHSI_Motion = nullptr;
CLogger  extern  *g_pLogger = nullptr;
HANDLE hCom;//串口句柄

const int WSA_MAJOR_VERSION = 2;
const int WSA_MINOR_VERSION = 2;
#define WSA_VERSION MAKEWORD(WSA_MAJOR_VERSION, WSA_MINOR_VERSION)
#define SOCKADDR_LEN sizeof(SOCKADDR_IN)

//===========================================================================
HANDLE HSI_Motion::m_Thread_Id = NULL;
HANDLE HSI_Motion::m_Thread_Mutex = NULL;
HANDLE HSI_Motion::g_RW_Data_Mutex = NULL;
HANDLE HSI_Motion::g_WR_ToMove_Mutex = NULL;
HANDLE HSI_Motion::g_Lock_JogAndTrigger = NULL;
HANDLE HSI_Motion::m_hTriggerEvent;
HANDLE HSI_Motion::m_Thread_IdIO = NULL;
HANDLE HSI_Motion::m_Thread_MutexIO = NULL;
HANDLE HSI_Motion::m_hTriggerEventIO;
HANDLE HSI_Motion::m_Thread_IdData = NULL;
HANDLE HSI_Motion::m_Thread_MutexData = NULL;
HANDLE HSI_Motion::m_hTriggerEventData;
HANDLE HSI_Motion::m_Thread_IdProbe = NULL;
HANDLE HSI_Motion::m_Thread_MutexProbe = NULL;
HANDLE HSI_Motion::m_hTriggerEventProbe;

HANDLE HSI_Motion::m_Thread_IdJOGStop = NULL;
HANDLE HSI_Motion::m_Thread_MutexJOGStop = NULL;
HANDLE HSI_Motion::m_hTriggerEventJOGStop;
int    HSI_Motion::m_Thread_StateJOGStop = THREAD_PAUSED;

int    HSI_Motion::m_Thread_State = THREAD_PAUSED;
int    HSI_Motion::m_Thread_StateIO = THREAD_PAUSED;
int    HSI_Motion::m_Thread_StateData = THREAD_PAUSED;
int    HSI_Motion::m_Thread_StateProbe = THREAD_PAUSED;
int    HSI_Motion::bRunGlueDispenser = THREAD_PAUSED;
//===========================================================================
HANDLE HSI_Motion::m_ThreadTCP_Id = NULL;
int    HSI_Motion::m_ThreadTCP_State = TCPIP_THREAD_PAUSED;
SOCKET m_socket[4] = { 0 };

//===========================================================================
HSI_Motion::HSI_Motion()
{
	TRACE0("HSI_Motion Constructor!\n");
	sEvenProp.Init();
	sEvenProp.EventCallbackID = 0;
	sEvenProp.EventResponse = HSI_EVENT_FUNCTION_OK;
	CurrentMotionState = E_SO7_MOTION_NONE;
	CurrentReadDataType = E_DATA_TYPE_NONE;
	GluerunCount = 0;
	m_IsExMotion = 2;
	g_IsClose = false;
	setLightFlag = false;
	m_bISUseMoreLights = 0;
	m_Jog_Auto_Focus = 1;
	m_StartInputPort = 1;
	iaxisNum = 0;
	iScanMotionType = 0;
	iTriggleNum = 0;
	iMotionDirection = 1;
	m_InputStatus = 0;
	m_ForStatus = 0;
	m_setPositionDelay = 0;
	m_setPositionPrecision = 0;
	m_IsUse_HSICompensation = 0;
	m_Compensation_Pluse = 20;
	m_IsHardLimit = 7;
	m_IsEnableAxis = 7;
	m_IsHavePattern = 15;
	m_IsUseExternalTrigger = 1;
	m_IsUseSixRingEightArea = 0;
	m_IsUseTwentySixLight = 0;
	m_IsUseEF3 = 0;
	m_IsUseRocker = 0;
	m_IsCloseRocker = 0;
	m_DeviceType = 0;
	m_iJoyStick = 0;
	m_IsProbe = 0;
	m_ProbeAllAxis = 3;
	m_ProbeReturnPos = 10.0;
	m_ProbeReturnSpeed = 40;
	m_IsHomeEncPos = 0;
	m_IsHomePrfPos = 1;
	m_IsIOFuntion = 0;
	m_IsStartInput = 0;
	m_IsUsePPS = false;
	m_MSTRunFlag = false;
	m_SendDataLength = 64;
	m_LightType = 1;
	m_IsUseFourthSpeed = 0;
	m_ETIPort = 1;
	m_EF3LightType = 1;
	m_IbinCount = 0;
	m_IsUseJerk = 0;
	t_start = 0;
	t_end = 0;
	m_isOKGlint = 0;
	m_selectedIndex = 0;
	m_setPositionNum = 5;
	m_axisStatus = 0;
	m_axisAlarmStatus = 0;
	m_EF3COMPort = 2;
	m_AxisHomeDirection = 15;
	m_PositionA = 0.0;
	m_ForSoft = 0;
	m_SaveAxisNum = 0;
	m_SaveAxisSpeed = 0;
	bSaveSpeedFlag = false;
	m_IsUseManualRunin = 0;
	fourthAxisFlag = false;
	bCircleRun = false;//圆弧插补
	m_UseAxisNum = 1;
	jogAxisNum = 0;
	jogDirFlag = false;
	jogMoving = false;
	jogspeed = 0;
	set_start = 0;
	set_end = 0;
	m_iSpeedType = 0;
	m_axisDirX = 0;//探针运动时X轴的运动方向
	m_axisDirY = 0;//探针运动时Y轴的运动方向
	m_axisDirZ = 0;//探针运动时Z轴的运动方向
	m_probeSeekSpeed = 0;

	bUseGlueDispenser = false;
	m_iGlueStartSpeed = 1;
	m_iGlueDriveSpeed = 1;
	m_iGlueAccSpeed = 1;
	GlueDispenserindexNum = 0;

	m_isUseAport = 0;//A串口
	m_portAnum = 0;
	m_isUseBport = 0;//B串口
	m_portBnum = 0;
  m_bEmergencyState = false;
	SpCompleteTStart = 0;
	SpCompleteTEnd = 0;
	SpTimeCount = 0;
	SetPotionRunEnd = false;
	PntToPntDistance = 0.0;
  m_IsLightDebug = 0;
	for (int i = 0; i < 4; i++)
	{
		m_IsOpenTCPIP[i] = "";
		m_tcpCntFlag[i] = false;
		m_Led8MotionFlag[i] = false;
	}

	for (int i = 0; i < 4; i++)
	{
		m_rockerHStartSpeed[i] = 5;
		m_rockerHDriveSpeed[i] = 20;
		m_rockerLStartSpeed[i] = 2;
		m_rockerLDriveSpeed[i] = 10;
		m_rockerDSpeed[i] = 100;
		m_rockerASpeed[i] = 100;
		begin_position[i] = 0;
	}

	for (size_t i = 0; i < 8; i++)
	{
		m_SixEightSubArea[i] = 0;
	}

	CTime tm = CTime::GetCurrentTime();
	CString csTime = tm.Format("%Y-%m-%d");//显示年月日
	CString dir = L"\\Log\\" + csTime += L".SO7_EF3.Log";
	g_pLogger = new CLogger(dir);
	g_pLogger2 = new CLogger(L"\\Log\\EF3_SumTime.Log");
	for (int i = 0; i < 5; i++)
	{
		for (int j = 0; j < 5; j++)
		{
			m_JogDriveSpeed[j][i] = 10; //表示5个档位
			m_JogStartSpeed[j][i] = 10;
			m_JogAccLine[j][i] = 5;
			m_JogAccCurve[j][i] = 0;
			m_JogDecLine[j][i] = 5;
			m_JogDecCurve[j][i] = 0;
		}
		m_Home_Machine_Axis[i] = 1;
		m_Home_Pos_Axis[i] = 0;
	}
	m_Home_Machine_Axis[4] = 0;
	for (int i = 0; i < 5; i++)
	{
		m_N_Work_Limit[i] = -40;
		m_P_Work_Limit[i] = 160;
		m_Resolution[i] = 0.0004;
		m_Home_AddJogGears[i] = 4;
		m_Home_DecJogGears[i] = 3;
		m_SetPotion_StartSpeed[i] = 20;
		m_SetPotion_DriveSpeed[i] = 20;
		m_SetPotion_Line[i] = 150;
		m_SetPotion_Buffer[i] = 40;
		m_SetPotion_AccCurve[i] = 0;
		m_SetPotion_DecCurve[i] = 0;
		m_SpeedAdjustPeriod[i] = 1;
		m_SpeedMax[i] = 150;
		m_precisionCount[i] = 5;
		m_precisionTime[i] = 350;
		m_Home_Time[i] = 1500;
		m_SetPotion_Count[i] = 200;
		m_PosThread[i] = 0;
		m_PosNow[i] = 0;
		m_LogIsOpen[i] = 0;
		m_StopJogMode[i] = 0;
		m_LockPos[i] = 0.0;
		m_EncPos[i] = 0;
		m_PrfPos[i] = 0;
		m_PosForAllAxis[i] = 0.0;
		targetpos_n[i] = 0;
		targetpos_l[i] = 0;
		m_ProbeCapturePos[i] = 0;
		iCircleRunPnt[i] = 0;
		m_ijk[i] = 0;
	}
	GetAppPath(m_AppPath);
	m_LogIsOpen[0] = GetPrivateProfileInt(L"LOG", L"LOG_IS_OPEN_0", 0, m_AppPath + _T("\\Config\\EF3_Motion.ini"));
	g_pLogger->IsEnabledLog = m_LogIsOpen[0] == 1 ? true : false;
	m_Set_XYZA_Reserve = 0;
	m_motorType = 0;
	m_AxisHex[0] = 0;
	m_direct_pos = 0;
	m_AxisHex[1] = AXIS_X;
	m_AxisHex[2] = AXIS_Y;
	m_AxisHex[3] = AXIS_Z;
	m_AxisHex[4] = AXIS_U;

	for (int i = 0; i < 64; i++)
	{
		m_cSendData[i] = 0;
	}
	m_cSendData[0] = 2;
	m_cSendData[1] = 2;
	m_cSendData[2] = 2;
	m_cSendData[3] = 48;
	m_cSendData[4] = 4;
	m_cSendData[6] = 4;
	m_bConnected = false;

	first = true;
	LightSend = 0;
	OrderSend = 0;
	IOSend = 0;
	for (int i = 0; i < 64; i++)
	{
		lightdata[i] = 0;
		IOdata[i] = 0;
		Orderdata[i] = 0;
		IOCheck[i] = 0;
		TempData[i] = 0;
	}
	IOCheck[0] = 0x01;
	IOCheck[1] = 0x04;
	memset(tReciveData, 0x00, TCPIP_MAX_DAT_SIZE);

	g_RW_Data_Mutex = CreateMutex(NULL, FALSE, NULL);
	g_WR_ToMove_Mutex = CreateMutex(NULL, FALSE, NULL);
	g_Lock_JogAndTrigger = CreateMutex(NULL, FALSE, NULL);

}
//===========================================================================
HSI_Motion::~HSI_Motion()
{
	TRACE0("HSI_Motion Destructor!\n");
}
//===========================================================================
bool HSI_Motion::PortInit(int iSerialComPort, int iBuadRate)
{
	if (hCom == NULL)
	{
		char  buf[10];
		sprintf_s(buf, "COM%d", iSerialComPort);
		CString comName(buf);
		hCom = CreateFile(comName,
			GENERIC_READ | GENERIC_WRITE, //允许读和写
			0, //独占方式,串口必须为0
			NULL,
			OPEN_EXISTING, //打开而不是创建，串口必须为打开
			0, //同步方式,同步执行时，函数直到操作完成后才返回
			NULL);//串口必须为NULL
		if (hCom != (HANDLE)-1)
		{
			PurgeComm(hCom, PURGE_TXCLEAR | PURGE_RXCLEAR);
		}
		SetupComm(hCom, 1024, 1024);

		DCB dcb;
		FillMemory(&dcb, sizeof(dcb), 0);
		dcb.DCBlength = sizeof(dcb);
		BuildCommDCB(L"2400,n,8,1", &dcb);

		if (!SetCommState(hCom, &dcb))
		{
			return false;//Setting Error!!!!
		}

		//设置读写超时时间
		COMMTIMEOUTS to;
		memset(&to, 0, sizeof(to));
		to.ReadIntervalTimeout = 100;
		to.ReadTotalTimeoutMultiplier = 10;
		to.ReadTotalTimeoutConstant = 10;
		SetCommTimeouts(hCom, &to);
		PurgeComm(hCom, PURGE_TXCLEAR | PURGE_RXCLEAR);
	}
	return true;
}
//===========================================================================
HSI_STATUS HSI_Motion::IsSupported(UINT &Types)
{
	auto rStatus = HSI_STATUS_NORMAL;
	Types = 1;
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::Startup(HWND _hWnd, bool _bOfflineOnly)
{
	auto rStatus = HSI_STATUS_NORMAL;
	hWnd = _hWnd;
	bOfflineOnly = _bOfflineOnly;

	if (!bOfflineOnly)
	{
		CString GoogolMotionConfigFile(_T(""));
		if (!g_pHSI_Motion)
		{
			g_pHSI_Motion = new HSI_Motion();
		}
		if (CurrentHomeMachineState == E_EF3_HOME_ING)
		{
			g_pLogger->SendAndFlushWithTime(L"[Startup] Going Home\n");
			return HSI_STATUS_NORMAL;
		}
		g_pLogger->SendAndFlushWithTime(L"[Startup] In\n");
		g_pLogger->SendAndFlushWithTime(L"[Startup] EF3 HSI.dll version = %s, date = %s\n", HSI_VERSION_CSTRING, HSI_FILE_CSDESCRIPTION);

		GoogolMotionConfigFile = m_AppPath + _T("\\Config\\EF3_Config.ini");
		Load_EF3_Config_Inifile(GoogolMotionConfigFile);
		GoogolMotionConfigFile = m_AppPath + _T("\\Config\\EF3_Motion.ini");
		Load_EF3_Motion_Inifile(GoogolMotionConfigFile);
		if (m_IsUseEF3 == 1)
		{
			if (!m_bConnected)
			{
				m_SO7_Serial.SetPort(m_EF3COMPort, 115200, 0, 8, 1, 0);
				if (!m_SO7_Serial.Open())
				{
					g_pLogger->SendAndFlushWithTime(L"[Startup] m_SO7_Serial.Open is false,connected failed\n");
					sEvenProp.Init();
					sEvenProp.EventType = HSI_EVENT_ERROR;
					sEvenProp.EventID = HSI_EVENT_MOTION;
					sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK;
					strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "EF3控制器打开失败,串口");
					EventCallback(sEvenProp);
					return HSI_STATUS_FAILED;
				}
				m_SO7_Serial.SetTimeouts(1000, 1000);
				m_bConnected = true;
			}
			else
			{
				g_pLogger->SendAndFlushWithTime(L"[Startup] Serial is opened\n");
			}
		}
		//AbortMotion();
		m_cSendData[0] = CT_ORDER;
		m_cSendData[1] = CT_STOP;
		m_cSendData[2] = AXIS_XYZU;
		m_StopJogMode[1] == 0 ? m_cSendData[3] = 0X4A : m_cSendData[3] = 0X05;
		m_StopJogMode[2] == 0 ? m_cSendData[3] = 0X4A : m_cSendData[3] = 0X05;
		m_StopJogMode[3] == 0 ? m_cSendData[3] = 0X4A : m_cSendData[3] = 0X05;
		m_StopJogMode[4] == 0 ? m_cSendData[3] = 0X4A : m_cSendData[3] = 0X05;
		m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
		Sleep(5);

		if (m_DeviceType == 3)
		{
			m_cSendData[0] = CT_TURNTABLE;
			m_cSendData[1] = CT_RTSTOP;
			m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
			Sleep(5);
			m_Thread_StateData = HSI_THREAD_RUNNING;
			SetEvent(m_hTriggerEventData);
		}
		if (bUseGlueDispenser)
		{
			bUseGlueDispenser = false;
			m_cSendData[0] = CT_MOTOR;
			m_cSendData[1] = CT_GLUEDISPENSER_STOP;
			m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
			Sleep(5);
		}

		g_pLogger->SendAndFlushWithTime(L"[Startup] Connected scuuess\n");
		if (HSI_STATUS_FAILED == DriverAlarmStatus())
		{
			g_pLogger->SendAndFlushWithTime(L"[Startup] DriverAlarmStatus HSI_STATUS_FAILED\n");
			//return HSI_STATUS_FAILED;
		}
		//无效软限位
		m_cSendData[0] = CT_MOTOR;
		m_cSendData[1] = CT_MOTOR_SET;
		m_cSendData[2] = AXIS_XYZU;
		m_cSendData[3] = SOFT_LIMIT_POS_NEG;
		m_cSendData[4] = 0;
		m_cSendData[5] = 0;
		m_cSendData[6] = 0;
		m_cSendData[7] = 0;
		m_cSendData[8] = 0;
		m_cSendData[9] = 0;
		m_cSendData[10] = 0;
		m_cSendData[11] = 0;
		m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);//初始化防止第一次无效
		Sleep(5);
		m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
		g_pLogger->SendAndFlushWithTime(L"[Startup] Limit no Enable\n");

		//设置方向4个轴的方向，按位
		m_cSendData[0] = CT_MOTOR;
		m_cSendData[1] = CT_MOTOR_SET;
		m_cSendData[2] = AXIS_X;
		m_cSendData[3] = ENC_POS_DIR;
		m_cSendData[4] = m_Set_XYZA_Reserve;
		m_cSendData[5] = m_setPositionDelay;
		m_cSendData[6] = m_setPositionPrecision;
		m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
		Sleep(10);
		m_cSendData[0] = CT_MOTOR;
		m_cSendData[1] = CT_MDATA_INIT;
		m_cSendData[2] = m_motorType & 0xff; //电机类型（步进电机或伺服电机）
		m_cSendData[3] = m_IsUseExternalTrigger;					 //是否启用外触发功能
		m_cSendData[4] = m_IsUseSixRingEightArea;					 //是否启用六环八区灯功能
		m_cSendData[5] = m_IsHardLimit;
		m_cSendData[6] = m_IsEnableAxis;
		m_cSendData[7] = m_IsProbe;								   //是否启用探针
		m_cSendData[8] = m_EF3LightType;						   //5V高频灯光配置
		m_cSendData[9] = m_IsUseRocker;							   //是否启用摇杆
		m_cSendData[10] = m_IsHavePattern;						   //光栅
		m_cSendData[11] = m_AxisHomeDirection;					   //轴回家方向
		m_cSendData[12] = m_IsCollectPos;							//是否从串口打印位置
		m_cSendData[16] = m_IsLightDebug;							//是否不回家也能调试灯光

		if (m_IsStartInput == 1 && m_IsUseRocker == 2)
		{
			m_cSendData[14] = m_StartInputPort >> 8 & 0xff;				//外部启动端口号  H
			m_cSendData[15] = m_StartInputPort & 0xff;					//外部启动端口号  L 
		}
		m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
		Sleep(10);

		////步进电机补偿值设定
		//if (m_motorType == 0)
		//{
		//	m_cSendData[0] = CT_MOTOR;
		//	m_cSendData[1] = CT_MDATA_COM;
		//	for (int i = 1, j = 2; i < 5; i++)
		//	{
		//		m_cSendData[j++] = (m_SpeedMax[i] & 0xff);
		//		m_cSendData[j++] = ((m_SpeedMax[i] >> 8) & 0xff);
		//		m_cSendData[j++] = ((m_SpeedMax[i] >> 16) & 0xff);
		//		m_cSendData[j++] = ((m_SpeedMax[i] >> 24) & 0xff);
		//	}
		//	m_WriteByte = Send_Command(0,(const char*)m_cSendData, m_SendDataLength);
		//	g_pLogger->SendAndFlushWithTime(L"[Startup] Set Encoder Dir\n");
		//}

		//多光源板
		if (m_bISUseMoreLights > 0)
		{
			for (int i = 0; i < m_bISUseMoreLights; i++)
			{
				if (m_tcpCntFlag[i])
				{
					continue;
				}
				USES_CONVERSION;
				char* ip = T2A(m_IsOpenTCPIP[i]);
				u_short port = 8088;
				TCPIP_RETURN_CODE rCode = TCPConnect(i, ip, port);
				if (rCode != TCPIP_CONNECT_OK)
				{
					g_pLogger->SendAndFlushWithTime(L"[Startup] m_SO7_Serial.Open is false,connected failed\n");
					sEvenProp.Init();
					sEvenProp.EventType = HSI_EVENT_ERROR;
					sEvenProp.EventID = HSI_EVENT_MOTION;
					sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK;
					strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "光源板打开网口失败");
					EventCallback(sEvenProp);
					return HSI_STATUS_FAILED;
				}
				m_tcpCntFlag[i] = true;
			}
			Create_Thread();
			for (int i = 0; i < m_bISUseMoreLights; i++)
			{
				if (m_Led8MotionFlag[i])
				{
					m_selectedIndex = i;
					Orderdata[0] = 0x01;
					Orderdata[1] = 0x05;
					Orderdata[2] = m_LightType;
					OrderSend++;
					Orderdata[0] = 0x01;
					Orderdata[1] = 0x06;
					Orderdata[2] = 0x00;
					OrderSend++;
				}
			}
		}

		//摇杆速度设置
		if (m_IsUseRocker == 1)
		{
			m_cSendData[0] = CT_MOTOR;
			m_cSendData[1] = CT_ROCKER;
			int j = 2;
			for (int i = 0; i < 3; i++)
			{
				m_cSendData[j++] = (m_rockerHStartSpeed[i] & 0xff);
				m_cSendData[j++] = ((m_rockerHStartSpeed[i] >> 8) & 0xff);
				m_cSendData[j++] = (m_rockerHDriveSpeed[i] & 0xff);
				m_cSendData[j++] = ((m_rockerHDriveSpeed[i] >> 8) & 0xff);
				m_cSendData[j++] = (m_rockerASpeed[i] & 0xff);
				m_cSendData[j++] = ((m_rockerASpeed[i] >> 8) & 0xff);
			}
			for (int i = 0; i < 3; i++)
			{
				m_cSendData[j++] = (m_rockerLStartSpeed[i] & 0xff);
				m_cSendData[j++] = ((m_rockerLStartSpeed[i] >> 8) & 0xff);
				m_cSendData[j++] = (m_rockerLDriveSpeed[i] & 0xff);
				m_cSendData[j++] = ((m_rockerLDriveSpeed[i] >> 8) & 0xff);
				m_cSendData[j++] = (m_rockerDSpeed[i] & 0xff);
				m_cSendData[j++] = ((m_rockerDSpeed[i] >> 8) & 0xff);
			}
			m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
			Sleep(5);
			g_pLogger->SendAndFlushWithTime(L"[Startup] Set Rocker Success\n");
		}
		//摇杆2下载档位
		if (m_IsUseRocker == 2)
		{
			SetAllGears();
			Sleep(5);
			g_pLogger->SendAndFlushWithTime(L"[Startup] Set Gears Success\n");
		}
		CreateThread();
		SetEvent(m_hTriggerEvent);
		m_Thread_State = HSI_THREAD_PAUSED;
		g_pLogger->SendAndFlushWithTime(L"[Startup] SetpositionXyz Enable\n");
		CreateThreadData();
		SetEvent(m_hTriggerEventData);
		m_Thread_StateData = HSI_THREAD_RUNNING;
		g_pLogger->SendAndFlushWithTime(L"[Startup] Read EF3 Status Run\n");

		if (m_DeviceType != 3)
		{
			CreateThreadJOGStop();
			SetEvent(m_hTriggerEventJOGStop);
			m_Thread_StateJOGStop = HSI_THREAD_PAUSED;
		}
		if (m_IsIOFuntion == 1)
		{
			m_Thread_StateIO = HSI_THREAD_RUNNING;
			CreateThreadIO();
			SetEvent(m_hTriggerEventIO);
			g_pLogger->SendAndFlushWithTime(L"[Startup] m_IsIOFuntion Enable\n");
			SetDIO(HSI_MOTION_OUTPUT_CH1, 0xfffff);
		}
		if (m_IsProbe == 1)
		{
			CreateThreadProbe();
			m_Thread_StateProbe = HSI_THREAD_RUNNING;
			SetEvent(m_hTriggerEventProbe);
			g_pLogger->SendAndFlushWithTime(L"[Startup] m_IsProbe Enable\n");
		}
		g_pLogger->SendAndFlushWithTime(L"[Startup] Out\n");
	}
	else
	{
		g_pLogger->SendAndFlushWithTime(L"[Startup] No enable controller\n");
		rStatus = HSI_STATUS_FAILED;
	}
	return rStatus;
}
//=============================获取EF3固件版本===============================
HSI_STATUS HSI_Motion::GetFirmwareVersion(byte *version)
{
	m_Thread_StateData = HSI_THREAD_PAUSED;
	Sleep(3);
	int waite_count = 0;
	unsigned char senddata[64] = { 0 };
	senddata[0] = 0x04;
	senddata[1] = 0x03;
	m_SO7_Serial.m_RecvData[0] = 0;
	m_WriteByte = Send_Command(0, (const char*)senddata, m_SendDataLength);
	Sleep(30);
	m_SO7_Serial.m_RecvData[0] = 0;
	m_WriteByte = Send_Command(0, (const char*)senddata, m_SendDataLength);
	Sleep(30);
	m_SO7_Serial.m_RecvData[0] = 0;
	m_WriteByte = Send_Command(0, (const char*)senddata, m_SendDataLength);
	Sleep(30);
	while ((m_SO7_Serial.m_RecvData[0] != 3) && (m_SO7_Serial.m_RecvData[0] != 2))
	{
		waite_count++;
		if (waite_count > 100)
			break;
		Sleep(1);
	}
	if (waite_count > 100)
	{
		waite_count = 0;
		m_WriteByte = Send_Command(0, (const char*)senddata, m_SendDataLength);
		while ((m_SO7_Serial.m_RecvData[0] != 3) && (m_SO7_Serial.m_RecvData[0] != 2))
		{
			waite_count++;
			if (waite_count > 100)
				break;
			Sleep(1);
		}
	}
	if (waite_count > 100)
	{
		version[0] = 't';
		version[1] = 'i';
		version[2] = 'm';
		version[3] = 'e';
		version[4] = 'o';
		version[5] = 'u';
		version[6] = 't';
	}
	else
	{
		for (int i = 0; i < 20; i++)
		{
			version[i] = m_SO7_Serial.m_RecvData[i];
		}
	}
	m_Thread_StateData = HSI_THREAD_RUNNING;
	SetEvent(m_hTriggerEventData); //触发事件，其中hEvent表示句柄，返回值：如果操作成功，则返回非零值，否则为0。
	return HSI_STATUS_NORMAL;
}
//================================回家=======================================
HSI_STATUS HSI_Motion::HomeMachine(bool bHomed)
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (!bHomed)
	{
		g_pLogger->SendAndFlushWithTime(L"[HomeMachine] bHomed No Need Reture\n");
		return HSI_STATUS_NORMAL;
	}
	if (m_IsUseEF3 == 0)
	{
		return HSI_STATUS_NORMAL;
	}
	if (g_pHSI_Motion)
	{
		g_pLogger->SendAndFlushWithTime(L"[HomeMachine] In\n");
		//判断是否需要回家
		bool home(false);
		IsHomed(home);
		if (home == true)
		{
			g_pLogger->SendAndFlushWithTime(L"[HomeMachine] IsHomed No Need Go Home\n");
			return HSI_STATUS_NORMAL;
		}
		sEvenProp.Init();
		sEvenProp.EventType = HSI_EVENT_FUNCTION;
		sEvenProp.EventID = HSI_EVENT_MOTION_DCC_HOME;
		sEvenProp.EventResponse = HSI_EVENT_RESPONSE_CANCEL;
		EventCallback(sEvenProp);
		if (sEvenProp.EventCallbackID == HSI_EVENT_RESPONSE_CANCEL)
		{
			g_pLogger->SendAndFlushWithTime(L"[HomeMachine] Cancel\n");
			return HSI_STATUS_NORMAL;
		}
    if (m_bEmergencyState)
    {
      AfxMessageBox(_T("急停或安全门或安全光幕触发！"));
      return HSI_STATUS_FAILED;
    }
		CurrentHomeMachineState = E_EF3_HOME_ING;
		int GetHomePos[5] = { 0 };
		if (m_SO7_Serial.m_RecvData[0] == 2)
		{
			if (m_IsHavePattern & 0x01 == 0x01)
				GetHomePos[1] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial.m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]);
			else
				GetHomePos[1] = (m_SO7_Serial.m_RecvData[17] << 24 | m_SO7_Serial.m_RecvData[18] << 16 | m_SO7_Serial.m_RecvData[19] << 8 | m_SO7_Serial.m_RecvData[20]);
			if (m_IsHavePattern & 0x02)
				GetHomePos[2] = (m_SO7_Serial.m_RecvData[5] << 24 | m_SO7_Serial.m_RecvData[6] << 16 | m_SO7_Serial.m_RecvData[7] << 8 | m_SO7_Serial.m_RecvData[8]);
			else
				GetHomePos[2] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial.m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]);
			if (m_IsHavePattern & 0x04)
				GetHomePos[3] = (m_SO7_Serial.m_RecvData[9] << 24 | m_SO7_Serial.m_RecvData[10] << 16 | m_SO7_Serial.m_RecvData[11] << 8 | m_SO7_Serial.m_RecvData[12]);
			else
				GetHomePos[3] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial.m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]);
			if (m_IsHavePattern & 0x08)
				GetHomePos[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | m_SO7_Serial.m_RecvData[15] << 8 | m_SO7_Serial.m_RecvData[16]);
			else
				GetHomePos[4] = (m_SO7_Serial.m_RecvData[29] << 24 | m_SO7_Serial.m_RecvData[30] << 16 | m_SO7_Serial.m_RecvData[31] << 8 | m_SO7_Serial.m_RecvData[32]);
			//GetHomePos[1] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial.m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]);
			//GetHomePos[2] = (m_SO7_Serial.m_RecvData[5] << 24 | m_SO7_Serial.m_RecvData[6] << 16 | m_SO7_Serial.m_RecvData[7] << 8 | m_SO7_Serial.m_RecvData[8]);
			//GetHomePos[3] = (m_SO7_Serial.m_RecvData[9] << 24 | m_SO7_Serial.m_RecvData[10] << 16 | m_SO7_Serial.m_RecvData[11] << 8 | m_SO7_Serial.m_RecvData[12]);
			//GetHomePos[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | m_SO7_Serial.m_RecvData[15] << 8 | m_SO7_Serial.m_RecvData[16]);
		}
		if (HSI_STATUS_NORMAL != HomeFindIndex())
		{
			return HSI_STATUS_FAILED;
		}
		int SavePos[5] = { 0 };
		if (m_SO7_Serial.m_RecvData[0] == 2)
		{
			SavePos[1] = (m_SO7_Serial.m_RecvData[41] << 24 | m_SO7_Serial.m_RecvData[42] << 16 | m_SO7_Serial.m_RecvData[43] << 8 | m_SO7_Serial.m_RecvData[44]);
			SavePos[2] = (m_SO7_Serial.m_RecvData[45] << 24 | m_SO7_Serial.m_RecvData[46] << 16 | m_SO7_Serial.m_RecvData[47] << 8 | m_SO7_Serial.m_RecvData[48]);
			SavePos[3] = (m_SO7_Serial.m_RecvData[49] << 24 | m_SO7_Serial.m_RecvData[50] << 16 | m_SO7_Serial.m_RecvData[51] << 8 | m_SO7_Serial.m_RecvData[52]);
			SavePos[4] = (m_SO7_Serial.m_RecvData[53] << 24 | m_SO7_Serial.m_RecvData[54] << 16 | m_SO7_Serial.m_RecvData[55] << 8 | m_SO7_Serial.m_RecvData[56]);
		}
		double PrinfPos[5] = { 0 };
		PrinfPos[1] = SavePos[1] * m_Resolution[1];
		PrinfPos[2] = SavePos[2] * m_Resolution[2];
		PrinfPos[3] = SavePos[3] * m_Resolution[3];
		PrinfPos[4] = SavePos[4] * m_Resolution[4];

		g_pLogger->SendAndFlushWithTime(L"[HomeMachine] SavePos[1] = %d,SavePos[2] = %d,SavePos[3] = %d,SavePos[4] = %d\n", SavePos[1], SavePos[2], SavePos[3], SavePos[4]);
		g_pLogger->SendAndFlushWithTime(L"[HomeMachine] PrinfPos[1] = %.4f,PrinfPos[2] = %.4f,PrinfPos[3] = %.4f,PrinfPos[4] = %.4f\n", PrinfPos[1], PrinfPos[2], PrinfPos[3], PrinfPos[4]);

		int LastPos[5] = { 0 };
		if (m_SO7_Serial.m_RecvData[0] == 2)
		{
			LastPos[1] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial.m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]);
			LastPos[2] = (m_SO7_Serial.m_RecvData[5] << 24 | m_SO7_Serial.m_RecvData[6] << 16 | m_SO7_Serial.m_RecvData[7] << 8 | m_SO7_Serial.m_RecvData[8]);
			LastPos[3] = (m_SO7_Serial.m_RecvData[9] << 24 | m_SO7_Serial.m_RecvData[10] << 16 | m_SO7_Serial.m_RecvData[11] << 8 | m_SO7_Serial.m_RecvData[12]);
			LastPos[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | m_SO7_Serial.m_RecvData[15] << 8 | m_SO7_Serial.m_RecvData[16]);
		}
		m_Thread_State = HSI_THREAD_RUNNING;
		g_pLogger->SendAndFlushWithTime(L"[HomeMachine] SetPositionXyz\n");
		double PrinfMovePos[5] = { 0 };
		PrinfMovePos[1] = (GetHomePos[1] - SavePos[1])*m_Resolution[1];
		PrinfMovePos[2] = (GetHomePos[2] - SavePos[2])*m_Resolution[2];
		PrinfMovePos[3] = (GetHomePos[3] - SavePos[3])*m_Resolution[3];
		PrinfMovePos[4] = (GetHomePos[4] - SavePos[4])*m_Resolution[4];
		g_pLogger->SendAndFlushWithTime(L"[HomeMachine] SetPos[1] = %.4fmm,SetPos[2] = %.4fmm,SetPos[3] = %.4fmm,SetPos[4] = %.4fmm\n", PrinfMovePos[1], PrinfMovePos[2], PrinfMovePos[3], PrinfMovePos[4]);
		if (m_DeviceType == 0 || m_DeviceType == 1)
		{
			for (short i = 1; i < 5; i++)
			{
				if (m_Home_Machine_Axis[i] == 0)
				{
					PrinfMovePos[i] = GetHomePos[i] * m_Resolution[i];
				}
			}
			Sleep(20);
			SetPositionXyza(0, PrinfMovePos[1], PrinfMovePos[2], PrinfMovePos[3], PrinfMovePos[4], HSI_MOTION_MOVE_WAIT, 0);
		}
		CurrentHomeMachineState = E_EF3_HOME_FINISHED;
		for (int AxisTypes = 1; AxisTypes <= 8;)
		{
			if (CurrentHomeMachineState == E_EF3_HOME_FINISHED)
			{
				byte AxisNumber = (byte)AxisConvertIndex(AxisTypes);
				m_cSendData[0] = CT_MOTOR;
				m_cSendData[1] = CT_MOTOR_SET;
				m_cSendData[2] = AxisTypes;
				m_cSendData[3] = SOFT_LIMIT_POS_NEG;
				m_cSendData[4] = (int)(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) & 0xff;
				m_cSendData[5] = ((int)(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 8) & 0xff;
				m_cSendData[6] = ((int)(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 16) & 0xff;
				m_cSendData[7] = ((int)(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 24) & 0xff;

				m_cSendData[8] = (int)(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) & 0xff;
				m_cSendData[9] = ((int)(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 8) & 0xff;
				m_cSendData[10] = ((int)(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 16) & 0xff;
				m_cSendData[11] = ((int)(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 24) & 0xff;

				m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
				Sleep(10);
				AxisTypes = 2 * AxisTypes;
				g_pLogger->SendAndFlushWithTime(L"[HomeMachine] Limit Enable, m_P_Work_Limit[AxisNumber] = %f,m_N_Work_Limit[AxisNumber] = %f\n", m_P_Work_Limit[AxisNumber], m_N_Work_Limit[AxisNumber]);
			}
		}
		//SetPositionXyz(0, -10,-10,-10, HSI_MOTION_MOVE_NOWAIT, 0);
		//Sleep(100);
		//SetPositionXyz(0, PrinfMovePos[1], PrinfMovePos[2], PrinfMovePos[3], HSI_MOTION_MOVE_NOWAIT, 0);

		g_pLogger->SendAndFlushWithTime(L"[HomeMachine] Out\n");
	}
	return rStatus;

}
//===========================================================================
HSI_STATUS HSI_Motion::HomeJog(short AxisNumber, short Dir, bool Wait)
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		g_pLogger->SendAndFlushWithTime(L"[HomeJog] In\n");

		g_pLogger->SendAndFlushWithTime(L"[HomeJog] Out\n");
	}
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::HomeFindIndex()
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		g_pLogger->SendAndFlushWithTime(L"[HomeFindIndex] In\n");

		int AxisTypes = 1;
		int AxisAll = 0;
		g_IsClose = false;
		g_pLogger->SendAndFlushWithTime(L"[HomeFindIndex] Limit no Enable\n");
		ZeroPos(true);
		for (short i = 1; i < 5; i++)
		{
			if (m_Home_Machine_Axis[i] == 1)
			{
				AxisTypes = IndexConvertAxis(i);
				//无效软限位
				m_cSendData[0] = CT_MOTOR;
				m_cSendData[1] = CT_MOTOR_SET;
				m_cSendData[2] = AxisTypes;
				m_cSendData[3] = SOFT_LIMIT_POS_NEG;
				m_cSendData[4] = 0;
				m_cSendData[5] = 0;
				m_cSendData[6] = 0;
				m_cSendData[7] = 0;
				m_cSendData[8] = 0;
				m_cSendData[9] = 0;
				m_cSendData[10] = 0;
				m_cSendData[11] = 0;
				m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
				Sleep(10);
				int DriveSpeed(1);
				int StartSpeed(1);
				int AccLine(1);
				int DecLine(1);
				int AccCurve(1);
				int DecCurve(1);
				m_cSendData[0] = CT_MOTOR;
				m_cSendData[1] = CT_MOTOR_SET;
				m_cSendData[2] = AxisTypes;
				m_cSendData[3] = HOME_LIMIT_SPEED_ACC_DEC;
				HomeJogGearsChoice(i, m_Home_AddJogGears[i], DriveSpeed, StartSpeed, AccLine, DecLine, AccCurve, DecCurve);

				m_cSendData[4] = (StartSpeed & 0xff);
				m_cSendData[5] = ((StartSpeed >> 8) & 0xff);
				m_cSendData[6] = ((StartSpeed >> 16) & 0xff);
				m_cSendData[7] = ((StartSpeed >> 24) & 0xff);

				m_cSendData[8] = (DriveSpeed & 0xff);
				m_cSendData[9] = ((DriveSpeed >> 8) & 0xff);
				m_cSendData[10] = ((DriveSpeed >> 16) & 0xff);
				m_cSendData[11] = ((DriveSpeed >> 24) & 0xff);

				m_cSendData[12] = (AccCurve & 0xff);
				m_cSendData[13] = ((AccCurve >> 8) & 0xff);
				m_cSendData[14] = ((AccCurve >> 16) & 0xff);
				m_cSendData[15] = ((AccCurve >> 24) & 0xff);

				m_cSendData[16] = (AccLine & 0xff);
				m_cSendData[17] = ((AccLine >> 8) & 0xff);
				m_cSendData[18] = ((AccLine >> 16) & 0xff);
				m_cSendData[19] = ((AccLine >> 24) & 0xff);

				m_cSendData[20] = (DecCurve & 0xff);
				m_cSendData[21] = ((DecCurve >> 8) & 0xff);
				m_cSendData[22] = ((DecCurve >> 16) & 0xff);
				m_cSendData[23] = ((DecCurve >> 24) & 0xff);

				m_cSendData[24] = (DecLine & 0xff);
				m_cSendData[25] = ((DecLine >> 8) & 0xff);
				m_cSendData[26] = ((DecLine >> 16) & 0xff);
				m_cSendData[27] = ((DecLine >> 24) & 0xff);
				m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
				g_pLogger->SendAndFlushWithTime(L"[HomeFindIndex] m_Home_Speed_High Go Limit\n");
				Sleep(10);
				m_cSendData[0] = CT_MOTOR;
				m_cSendData[1] = CT_MOTOR_SET;
				m_cSendData[2] = AxisTypes;
				m_cSendData[3] = HOME_ORG_SPEED_ACC_DEC;
				HomeJogGearsChoice(i, m_Home_DecJogGears[i], DriveSpeed, StartSpeed, AccLine, DecLine, AccCurve, DecCurve);
				m_cSendData[4] = (StartSpeed & 0xff);
				m_cSendData[5] = ((StartSpeed >> 8) & 0xff);
				m_cSendData[6] = ((StartSpeed >> 16) & 0xff);
				m_cSendData[7] = ((StartSpeed >> 24) & 0xff);

				m_cSendData[8] = (DriveSpeed & 0xff);
				m_cSendData[9] = ((DriveSpeed >> 8) & 0xff);
				m_cSendData[10] = ((DriveSpeed >> 16) & 0xff);
				m_cSendData[11] = ((DriveSpeed >> 24) & 0xff);

				m_cSendData[12] = (AccCurve & 0xff);
				m_cSendData[13] = ((AccCurve >> 8) & 0xff);
				m_cSendData[14] = ((AccCurve >> 16) & 0xff);
				m_cSendData[15] = ((AccCurve >> 24) & 0xff);

				m_cSendData[16] = (AccLine & 0xff);
				m_cSendData[17] = ((AccLine >> 8) & 0xff);
				m_cSendData[18] = ((AccLine >> 16) & 0xff);
				m_cSendData[19] = ((AccLine >> 24) & 0xff);

				m_cSendData[20] = (DecCurve & 0xff);
				m_cSendData[21] = ((DecCurve >> 8) & 0xff);
				m_cSendData[22] = ((DecCurve >> 16) & 0xff);
				m_cSendData[23] = ((DecCurve >> 24) & 0xff);

				m_cSendData[24] = (DecLine & 0xff);
				m_cSendData[25] = ((DecLine >> 8) & 0xff);
				m_cSendData[26] = ((DecLine >> 16) & 0xff);
				m_cSendData[27] = ((DecLine >> 24) & 0xff);

				m_cSendData[28] = (m_Home_Time[i] & 0xff);
				m_cSendData[29] = ((m_Home_Time[i] >> 8) & 0xff);
				m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
				g_pLogger->SendAndFlushWithTime(L"[HomeFindIndex] m_Home_Speed_High Go Limit and find index signal\n");
				Sleep(10);
				AxisAll += (1 << (i - 1));
			}
		}
		m_cSendData[0] = CT_MOTOR;
		m_cSendData[1] = CT_GOHOME;
		m_cSendData[2] = AxisAll;
		m_cSendData[3] = 0x53;
		m_cSendData[4] = m_motorType & 0xff;
		m_cSendData[5] = 0x0f;
		m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
		g_pLogger->SendAndFlushWithTime(L"[HomeFindIndex] Starting, AxisAll = %d\n", AxisAll);
		Sleep(1000);

		int Count = 0;
		do
		{
			DoEvents();
			if (g_IsClose)
			{
				g_IsClose = false;
				return HSI_STATUS_FAILED;
			}
			Sleep(1);
			if (m_SO7_Serial.m_RecvData[0] == 2)
			{
				if (m_SO7_Serial.m_RecvData[38] & 16)
				{
					g_pLogger->SendAndFlushWithTime(L"[HomeFindIndex] Go Home Motion Done\n");
					break;
				}
			}
      if (m_bEmergencyState)
      {
        AfxMessageBox(_T("急停或安全门或安全光幕触发！"));
        return HSI_STATUS_FAILED;
      }
			if (Count > 25000)
			{
				g_pLogger->SendAndFlushWithTime(L"[HomeFindIndex] Go Home Timeout HSI_STATUS_FAILED\n");
				AbortMotion();
				sEvenProp.EventType = HSI_EVENT_ERROR;
				sEvenProp.EventID = HSI_EVENT_MOTION;
				sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK;
				strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "回家超时");
				EventCallback(sEvenProp);
				return HSI_STATUS_FAILED;
			}
			Count++;
		} while (true);

		char MessageHome[100] = "";
		bool bHomed = true;
		if ((m_SO7_Serial.m_RecvData[38] & 0x01) == 0 && m_Home_Machine_Axis[1] == 1)
		{
			strcat_s(MessageHome, 30, "1、");
			bHomed = false;
		}
		if ((m_SO7_Serial.m_RecvData[38] & 0x02) == 0 && m_Home_Machine_Axis[2] == 1)
		{
			strcat_s(MessageHome, 30, "2、");
			bHomed = false;
		}
		if ((m_SO7_Serial.m_RecvData[38] & 0x04) == 0 && m_Home_Machine_Axis[3] == 1)
		{
			strcat_s(MessageHome, 30, "3、");
			bHomed = false;
		}
		if ((m_SO7_Serial.m_RecvData[38] & 0x08) == 0 && m_Home_Machine_Axis[4] == 1)
		{
			strcat_s(MessageHome, 30, "4、");
			bHomed = false;
		}
		if (!bHomed)
		{
			sEvenProp.Init();
			sEvenProp.EventType = HSI_EVENT_ERROR;
			sEvenProp.EventID = HSI_EVENT_MOTION;
			sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK;
			strcat_s(MessageHome, 100, "轴回家失败!");
			strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, MessageHome);
			EventCallback(sEvenProp);
			return HSI_STATUS_FAILED;
		}
		g_pLogger->SendAndFlushWithTime(L"[HomeFindIndex] Go Home Success\n");

		g_pLogger->SendAndFlushWithTime(L"[HomeFindIndex] Out\n");
	}
	return rStatus;
}
//===========================================================================
void HSI_Motion::HomeJogGearsChoice(int AxisTypes, int JogGears, int &DriveSpeed, int &StartSpeed, int &AccLine, int &DecLine, int &AccCurve, int &DecCurve)
{
	DriveSpeed = m_JogDriveSpeed[AxisTypes][4 - JogGears];
	StartSpeed = m_JogStartSpeed[AxisTypes][4 - JogGears];
	AccLine = m_JogAccLine[AxisTypes][4 - JogGears];
	DecLine = m_JogDecLine[AxisTypes][4 - JogGears];
	AccCurve = m_JogAccCurve[AxisTypes][4 - JogGears];
	DecCurve = m_JogDecCurve[AxisTypes][4 - JogGears];
}
//===========================================================================
HSI_STATUS HSI_Motion::GetAppPath(CString &Path)
{
	auto rStatus = HSI_STATUS_NORMAL;
	Path = _T("");
	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 rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::IsHomed(bool &bHomed)
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		g_pLogger->SendAndFlushWithTime(L"[IsHomed] In\n");
		short isHomed[5] = { 1, 1, 1, 1, 1 };
		//int Count = 0;
		//if (bHomed == true)//定位是增大判断精度
		//{
		//  Count = 1000;
		//}
		//所有轴都不需要回家
		if (m_Home_Machine_Axis[1] == 0 && m_Home_Machine_Axis[2] == 0 && m_Home_Machine_Axis[3] == 0 && m_Home_Machine_Axis[4] == 0)
		{
			g_pLogger->SendAndFlushWithTime(L"[IsHomed] No Axis Go Home E_GTS_HOME_FINISHED\n");
			CurrentHomeMachineState = E_EF3_HOME_FINISHED;
			bHomed = true;
			return HSI_STATUS_NORMAL;
		}

		//判断是否需要回家
		int Delay = 0;
		while (m_SO7_Serial.m_RecvData[0] != 2)
		{
			DoEvents();
			Sleep(1);
			if (m_SO7_Serial.m_RecvData[0] == 2)
			{
				g_pLogger->SendAndFlushWithTime(L"[IsHomed] normal break Delay = %d\n", Delay);
				break;
			}
			if (Delay > 300)
			{
				g_pLogger->SendAndFlushWithTime(L"[IsHomed] timeout break Delay = %d\n", Delay);
				break;
			}
			Delay++;
		}
		for (short i = 1; i < 5; i++)
		{
			if (m_Home_Machine_Axis[i] == 1)
			{
				if (m_SO7_Serial.m_RecvData[0] == 2)
				{
					isHomed[i] = (m_SO7_Serial.m_RecvData[38] & m_AxisHex[i] ? 1 : 0);
				}
				else
				{
					isHomed[i] = 0;
				}
			}
		}
		if (isHomed[1] == 1 && isHomed[2] == 1 && isHomed[3] == 1 && isHomed[4] == 1)
		{
			g_pLogger->SendAndFlushWithTime(L"[IsHomed] E_GTS_HOME_FINISHED\n");
			CurrentHomeMachineState = E_EF3_HOME_FINISHED;
			bHomed = true;
		}
		else
		{
			g_pLogger->SendAndFlushWithTime(L"[IsHomed] Is No Go Home E_GTS_HOME_NONE\n");
			CurrentHomeMachineState = E_EF3_HOME_NONE;
			bHomed = false;
		}
		g_pLogger->SendAndFlushWithTime(L"[IsHomed] Out\n");
	}
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::ZeroPos(bool bZeroPos)
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		g_pLogger->SendAndFlushWithTime(L"[ZeroPos] In\n");
		if (bZeroPos == true)
		{
			m_cSendData[0] = CT_MOTOR;
			m_cSendData[1] = CT_MOTOR_SET;
			m_cSendData[2] = AXIS_X;
			m_cSendData[3] = POS_CLEAR;
			Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
			Sleep(8);
			m_cSendData[0] = CT_MOTOR;
			m_cSendData[1] = CT_MOTOR_SET;
			m_cSendData[2] = AXIS_Y;
			m_cSendData[3] = POS_CLEAR;
			Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
			Sleep(8);
			m_cSendData[0] = CT_MOTOR;
			m_cSendData[1] = CT_MOTOR_SET;
			m_cSendData[2] = AXIS_Z;
			m_cSendData[3] = POS_CLEAR;
			Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
			Sleep(8);
			m_cSendData[0] = CT_MOTOR;
			m_cSendData[1] = CT_MOTOR_SET;
			m_cSendData[2] = AXIS_U;
			m_cSendData[3] = POS_CLEAR;
			Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
			Sleep(8);
			g_pLogger->SendAndFlushWithTime(L"[ZeroPos] All Pos ZeroPos\n");
		}
		m_cSendData[0] = CT_MOTOR;
		m_cSendData[1] = CT_MOTOR_SET;
		m_cSendData[2] = AXIS_X;
		m_cSendData[3] = HOME_CLEAR;
		Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
		g_pLogger->SendAndFlushWithTime(L"[ZeroPos] All HomeFlag ZeroPos\n");
		g_pLogger->SendAndFlushWithTime(L"[ZeroPos] Out\n");
	}
	return rStatus;
}

//===============================JOG模式============================================
HSI_STATUS HSI_Motion::Jog(UINT AxisTypes, double Speed)
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		if (m_DeviceType != 3)
		{
			m_Thread_StateJOGStop = HSI_THREAD_RUNNING;
			SetEvent(m_hTriggerEventJOGStop);
		}
		int DriveSpeed(1);
		int StartSpeed(1);
		int AccLine(1);
		int DecLine(1);
		int AccCurve(1);
		int DecCurve(1);
		int JogSpeed(1);
		bool bJOGDir = Speed > 0 ? true : false;
		byte AxisNumber = (byte)AxisConvertIndex(AxisTypes);
		jogAxisNum = AxisNumber;
		jogDirFlag = bJOGDir;
		m_Thread_State = HSI_THREAD_PAUSED;
		if (CurrentHomeMachineState == E_EF3_HOME_FINISHED)
		{
			m_cSendData[0] = CT_MOTOR;
			m_cSendData[1] = CT_MOTOR_SET;
			m_cSendData[2] = AxisTypes;
			m_cSendData[3] = SOFT_LIMIT_POS_NEG;
			m_cSendData[4] = (int)(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) & 0xff;//正限位
			m_cSendData[5] = ((int)(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 8) & 0xff;
			m_cSendData[6] = ((int)(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 16) & 0xff;
			m_cSendData[7] = ((int)(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 24) & 0xff;

			m_cSendData[8] = (int)(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) & 0xff; //负限位
			m_cSendData[9] = ((int)(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 8) & 0xff;
			m_cSendData[10] = ((int)(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 16) & 0xff;
			m_cSendData[11] = ((int)(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 24) & 0xff;

			m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
			g_pLogger->SendAndFlushWithTime(L"[Jog] Limit Enable, m_P_Work_Limit[AxisNumber] = %f,m_N_Work_Limit[AxisNumber] = %f\n", m_P_Work_Limit[AxisNumber], m_N_Work_Limit[AxisNumber]);
		}
		else
		{
			//无效软限位
			m_cSendData[0] = CT_MOTOR;
			m_cSendData[1] = CT_MOTOR_SET;
			m_cSendData[2] = AxisTypes;
			m_cSendData[3] = SOFT_LIMIT_POS_NEG;
			m_cSendData[4] = 0;
			m_cSendData[5] = 0;
			m_cSendData[6] = 0;
			m_cSendData[7] = 0;
			m_cSendData[8] = 0;
			m_cSendData[9] = 0;
			m_cSendData[10] = 0;
			m_cSendData[11] = 0;
			m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
			g_pLogger->SendAndFlushWithTime(L"[Jog] Limit No Enable\n");
		}
		if (m_Home_Machine_Axis[AxisNumber] == 0)
		{
			return rStatus;
		}
		double now_pos[5] = { 0 };
		double Prf_pos[5] = { 0 };
		double limitpos[4] = { 0 };
		int RemainPul = 0;
		int limitSDPul = 0;
		double time;
		/*GetPositionXyz(1, now_pos[0], now_pos[1], now_pos[2], time);*/
		GetPositionEncPrfMulti(1, now_pos, Prf_pos, 1);
		JogSpeed = abs(SpeedPercent(AxisNumber, Speed, DriveSpeed, StartSpeed, AccLine, DecLine, AccCurve, DecCurve));
		if (m_DeviceType != 3)
		{
			if (AxisTypes == AXIS_X && m_motorType & 0x01)
			{
				if (!bJOGDir)//负方向
				{
					RemainPul = (int)(now_pos[1] / m_Resolution[1]) - (int)(m_N_Work_Limit[1] / m_Resolution[1]);
					limitSDPul = (DriveSpeed - StartSpeed) * 13;
					if ((RemainPul < limitSDPul * 2) && (RemainPul>0))
					{
						float SpeedRatio = limitSDPul * 2 / RemainPul;
						DriveSpeed = DriveSpeed / SpeedRatio;
						if (DriveSpeed < StartSpeed)
						{
							DriveSpeed = StartSpeed;
						}
					}
				}
				else
				{
					RemainPul = (int)(m_P_Work_Limit[1] / m_Resolution[1]) - (int)(now_pos[1] / m_Resolution[1]);
					limitSDPul = (DriveSpeed - StartSpeed) * 13;
					if ((RemainPul < limitSDPul * 2) && (RemainPul>0))
					{
						float SpeedRatio = limitSDPul * 2 / RemainPul;
						DriveSpeed = DriveSpeed / SpeedRatio;
						if (DriveSpeed < StartSpeed)
						{
							DriveSpeed = StartSpeed;
						}
					}
				}
			}
			else if (AxisTypes == AXIS_Y && m_motorType & 0x02)
			{
				if (!bJOGDir)//负方向
				{
					RemainPul = (int)(now_pos[2] / m_Resolution[2]) - (int)(m_N_Work_Limit[2] / m_Resolution[2]);
					limitSDPul = (DriveSpeed - StartSpeed) * 13;
					if ((RemainPul < limitSDPul * 2) && (RemainPul>0))
					{
						float SpeedRatio = 1 + limitSDPul * 2 / RemainPul;
						DriveSpeed = DriveSpeed / SpeedRatio;
						if (DriveSpeed < StartSpeed)
						{
							DriveSpeed = StartSpeed;
						}
					}
				}
				else
				{
					RemainPul = (int)(m_P_Work_Limit[2] / m_Resolution[2]) - (int)(now_pos[2] / m_Resolution[2]);
					limitSDPul = (DriveSpeed - StartSpeed) * 13;
					if ((RemainPul < limitSDPul * 2) && (RemainPul>0))
					{
						float SpeedRatio = limitSDPul * 2 / RemainPul;
						DriveSpeed = DriveSpeed / SpeedRatio;
						if (DriveSpeed < StartSpeed)
						{
							DriveSpeed = StartSpeed;
						}
					}
				}
			}
			if (AxisTypes == AXIS_Z && m_motorType & 0x04)
			{
				if (!bJOGDir)//负方向
				{
					RemainPul = (int)(now_pos[3] / m_Resolution[3]) - (int)(m_N_Work_Limit[3] / m_Resolution[3]);
					limitSDPul = (DriveSpeed - StartSpeed) * 13;
					if ((RemainPul < limitSDPul * 2) && (RemainPul>0))
					{
						float SpeedRatio = limitSDPul * 2 / RemainPul;
						DriveSpeed = DriveSpeed / SpeedRatio;
						if (DriveSpeed < StartSpeed)
						{
							DriveSpeed = StartSpeed;
						}
					}
				}
				else
				{
					RemainPul = (int)(m_P_Work_Limit[3] / m_Resolution[3]) - (int)(now_pos[3] / m_Resolution[3]);
					limitSDPul = (DriveSpeed - StartSpeed) * 13;
					if ((RemainPul < limitSDPul * 2) && (RemainPul>0))
					{
						float SpeedRatio = limitSDPul * 2 / RemainPul;
						DriveSpeed = DriveSpeed / SpeedRatio;
						if (DriveSpeed < StartSpeed)
						{
							DriveSpeed = StartSpeed;
						}
					}
				}
			}
			if (AxisTypes == AXIS_U && m_motorType & 0x08)
			{
				if (!bJOGDir)//负方向
				{
					RemainPul = (int)(now_pos[4] / m_Resolution[4]) - (int)(m_N_Work_Limit[4] / m_Resolution[4]);
					limitSDPul = (DriveSpeed - StartSpeed) * 13;
					if ((RemainPul < limitSDPul * 2) && (RemainPul>0))
					{
						float SpeedRatio = limitSDPul * 2 / RemainPul;
						DriveSpeed = DriveSpeed / SpeedRatio;
						if (DriveSpeed < StartSpeed)
						{
							DriveSpeed = StartSpeed;
						}
					}
				}
				else
				{
					RemainPul = (int)(m_P_Work_Limit[4] / m_Resolution[4]) - (int)(now_pos[4] / m_Resolution[4]);
					limitSDPul = (DriveSpeed - StartSpeed) * 13;
					if ((RemainPul < limitSDPul * 2) && (RemainPul>0))
					{
						float SpeedRatio = limitSDPul * 2 / RemainPul;
						DriveSpeed = DriveSpeed / SpeedRatio;
						if (DriveSpeed < StartSpeed)
						{
							DriveSpeed = StartSpeed;
						}
					}
				}
			}
		}
		m_cSendData[0] = CT_MOTOR;
		m_cSendData[1] = CT_MOTOR_SET;
		m_cSendData[2] = AxisTypes;
		m_cSendData[3] = JOG_SPEED_ACC_DEC;
		m_cSendData[4] = (StartSpeed & 0xff);
		m_cSendData[5] = ((StartSpeed >> 8) & 0xff);
		m_cSendData[6] = ((StartSpeed >> 16) & 0xff);
		m_cSendData[7] = ((StartSpeed >> 24) & 0xff);

		m_cSendData[8] = (DriveSpeed & 0xff);
		m_cSendData[9] = ((DriveSpeed >> 8) & 0xff);
		m_cSendData[10] = ((DriveSpeed >> 16) & 0xff);
		m_cSendData[11] = ((DriveSpeed >> 24) & 0xff);

		m_cSendData[12] = (AccCurve & 0xff);
		m_cSendData[13] = ((AccCurve >> 8) & 0xff);
		m_cSendData[14] = ((AccCurve >> 16) & 0xff);
		m_cSendData[15] = ((AccCurve >> 24) & 0xff);

		m_cSendData[16] = (AccLine & 0xff);
		m_cSendData[17] = ((AccLine >> 8) & 0xff);
		m_cSendData[18] = ((AccLine >> 16) & 0xff);
		m_cSendData[19] = ((AccLine >> 24) & 0xff);

		m_cSendData[20] = (DecCurve & 0xff);
		m_cSendData[21] = ((DecCurve >> 8) & 0xff);
		m_cSendData[22] = ((DecCurve >> 16) & 0xff);
		m_cSendData[23] = ((DecCurve >> 24) & 0xff);

		m_cSendData[24] = (DecLine & 0xff);
		m_cSendData[25] = ((DecLine >> 8) & 0xff);
		m_cSendData[26] = ((DecLine >> 16) & 0xff);
		m_cSendData[27] = ((DecLine >> 24) & 0xff);

		jogspeed = DriveSpeed;

		if ((StartSpeed < 250) && (DriveSpeed < 6))
		{
			m_IsUseJerk = 1;
		}
		else
		{
			m_IsUseJerk = 0;
		}
		m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
		Sleep(5);
		m_cSendData[0] = CT_MOTOR;
		if (AxisNumber == 1 || AxisNumber == 2)
		{
			if (!bJOGDir)//方向
			{
				m_cSendData[1] = CT_START_JOG_NEG;
			}
			else
			{
				m_cSendData[1] = CT_START_JOG_POS;
			}
		}
		else
		{
			if (bJOGDir)//方向
			{
				m_cSendData[1] = CT_START_JOG_POS;
			}
			else
			{
				m_cSendData[1] = CT_START_JOG_NEG;
			}
		}
		m_cSendData[2] = AxisTypes;
		m_cSendData[3] = 0x53;
		t_start = GetTickCount();
		m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
		jogMoving = true;
		g_pLogger->SendAndFlushWithTime(L"[Jog] Out,bJOGDir = %d\n", bJOGDir);
	}
	return rStatus;
}
//JOG模式
//===========================================================================
HSI_STATUS HSI_Motion::JoyStick(UINT AxisTypes, long Speed)
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		if (m_DeviceType != 3)
		{
			m_Thread_StateJOGStop = HSI_THREAD_RUNNING;
			SetEvent(m_hTriggerEventJOGStop);
		}
		int DriveSpeed(1);
		int StartSpeed(1);
		int AccLine(1);
		int DecLine(1);
		int AccCurve(1);
		int DecCurve(1);
		int JogSpeed(1);
		bool bJOGDir = Speed > 0 ? true : false;
		byte AxisNumber = (byte)AxisConvertIndex(AxisTypes);
		jogAxisNum = AxisNumber;
		jogDirFlag = bJOGDir;
		m_Thread_State = HSI_THREAD_PAUSED;
		if (CurrentHomeMachineState == E_EF3_HOME_FINISHED)
		{
			m_cSendData[0] = CT_MOTOR;
			m_cSendData[1] = CT_MOTOR_SET;
			m_cSendData[2] = AxisTypes;
			m_cSendData[3] = SOFT_LIMIT_POS_NEG;
			m_cSendData[4] = (int)(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) & 0xff;
			m_cSendData[5] = ((int)(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 8) & 0xff;
			m_cSendData[6] = ((int)(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 16) & 0xff;
			m_cSendData[7] = ((int)(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 24) & 0xff;

			m_cSendData[8] = (int)(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) & 0xff;
			m_cSendData[9] = ((int)(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 8) & 0xff;
			m_cSendData[10] = ((int)(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 16) & 0xff;
			m_cSendData[11] = ((int)(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 24) & 0xff;

			m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
			g_pLogger->SendAndFlushWithTime(L"[Jog] Limit Enable, m_P_Work_Limit[AxisNumber] = %f,m_N_Work_Limit[AxisNumber] = %f\n", m_P_Work_Limit[AxisNumber], m_N_Work_Limit[AxisNumber]);
		}
		else
		{
			//无效软限位
			m_cSendData[0] = CT_MOTOR;
			m_cSendData[1] = CT_MOTOR_SET;
			m_cSendData[2] = AxisTypes;
			m_cSendData[3] = SOFT_LIMIT_POS_NEG;
			m_cSendData[4] = 0;
			m_cSendData[5] = 0;
			m_cSendData[6] = 0;
			m_cSendData[7] = 0;
			m_cSendData[8] = 0;
			m_cSendData[9] = 0;
			m_cSendData[10] = 0;
			m_cSendData[11] = 0;
			m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
			g_pLogger->SendAndFlushWithTime(L"[Jog] Limit No Enable\n");
		}
		if (m_Home_Machine_Axis[AxisNumber] == 0)
		{
			return rStatus;
		}
		double now_pos[5] = { 0 };
		double Prf_pos[5] = { 0 };
		double limitpos[4] = { 0 };
		int RemainPul = 0;
		int limitSDPul = 0;
		double time;
		/*GetPositionXyz(1, now_pos[0], now_pos[1], now_pos[2], time);*/
		GetPositionEncPrfMulti(1, now_pos, Prf_pos, 1);
		long lSpeed = abs(Speed);
		if (!abs(SpeedPercentJoyStick(AxisNumber, lSpeed, DriveSpeed, StartSpeed, AccLine, DecLine, AccCurve, DecCurve)))
		{
			return rStatus;
		}
		if (m_DeviceType != 3)
		{
			if (AxisTypes == AXIS_X && m_motorType & 0x01)
			{
				if (!bJOGDir)//负方向
				{
					RemainPul = (int)(now_pos[1] / m_Resolution[1]) - (int)(m_N_Work_Limit[1] / m_Resolution[1]);
					limitSDPul = (DriveSpeed - StartSpeed) * 13;
					if ((RemainPul < limitSDPul * 2) && (RemainPul>0))
					{
						float SpeedRatio = limitSDPul * 2 / RemainPul;
						DriveSpeed = DriveSpeed / SpeedRatio;
						if (DriveSpeed < StartSpeed)
						{
							DriveSpeed = StartSpeed;
						}
					}
				}
				else
				{
					RemainPul = (int)(m_P_Work_Limit[1] / m_Resolution[1]) - (int)(now_pos[1] / m_Resolution[1]);
					limitSDPul = (DriveSpeed - StartSpeed) * 13;
					if ((RemainPul < limitSDPul * 2) && (RemainPul>0))
					{
						float SpeedRatio = limitSDPul * 2 / RemainPul;
						DriveSpeed = DriveSpeed / SpeedRatio;
						if (DriveSpeed < StartSpeed)
						{
							DriveSpeed = StartSpeed;
						}
					}
				}
			}
			else if (AxisTypes == AXIS_Y && m_motorType & 0x02)
			{
				if (!bJOGDir)//负方向
				{
					RemainPul = (int)(now_pos[2] / m_Resolution[2]) - (int)(m_N_Work_Limit[2] / m_Resolution[2]);
					limitSDPul = (DriveSpeed - StartSpeed) * 13;
					if ((RemainPul < limitSDPul * 2) && (RemainPul>0))
					{
						float SpeedRatio = 1 + limitSDPul * 2 / RemainPul;
						DriveSpeed = DriveSpeed / SpeedRatio;
						if (DriveSpeed < StartSpeed)
						{
							DriveSpeed = StartSpeed;
						}
					}
				}
				else
				{
					RemainPul = (int)(m_P_Work_Limit[2] / m_Resolution[2]) - (int)(now_pos[2] / m_Resolution[2]);
					limitSDPul = (DriveSpeed - StartSpeed) * 13;
					if ((RemainPul < limitSDPul * 2) && (RemainPul>0))
					{
						float SpeedRatio = limitSDPul * 2 / RemainPul;
						DriveSpeed = DriveSpeed / SpeedRatio;
						if (DriveSpeed < StartSpeed)
						{
							DriveSpeed = StartSpeed;
						}
					}
				}
			}
			if (AxisTypes == AXIS_Z && m_motorType & 0x04)
			{
				if (!bJOGDir)//负方向
				{
					RemainPul = (int)(now_pos[3] / m_Resolution[3]) - (int)(m_N_Work_Limit[3] / m_Resolution[3]);
					limitSDPul = (DriveSpeed - StartSpeed) * 13;
					if ((RemainPul < limitSDPul * 2) && (RemainPul>0))
					{
						float SpeedRatio = limitSDPul * 2 / RemainPul;
						DriveSpeed = DriveSpeed / SpeedRatio;
						if (DriveSpeed < StartSpeed)
						{
							DriveSpeed = StartSpeed;
						}
					}
				}
				else
				{
					RemainPul = (int)(m_P_Work_Limit[3] / m_Resolution[3]) - (int)(now_pos[3] / m_Resolution[3]);
					limitSDPul = (DriveSpeed - StartSpeed) * 13;
					if ((RemainPul < limitSDPul * 2) && (RemainPul>0))
					{
						float SpeedRatio = limitSDPul * 2 / RemainPul;
						DriveSpeed = DriveSpeed / SpeedRatio;
						if (DriveSpeed < StartSpeed)
						{
							DriveSpeed = StartSpeed;
						}
					}
				}
			}
			if (AxisTypes == AXIS_U && m_motorType & 0x08)
			{
				if (!bJOGDir)//负方向
				{
					RemainPul = (int)(now_pos[4] / m_Resolution[4]) - (int)(m_N_Work_Limit[4] / m_Resolution[4]);
					limitSDPul = (DriveSpeed - StartSpeed) * 13;
					if ((RemainPul < limitSDPul * 2) && (RemainPul>0))
					{
						float SpeedRatio = limitSDPul * 2 / RemainPul;
						DriveSpeed = DriveSpeed / SpeedRatio;
						if (DriveSpeed < StartSpeed)
						{
							DriveSpeed = StartSpeed;
						}
					}
				}
				else
				{
					RemainPul = (int)(m_P_Work_Limit[4] / m_Resolution[4]) - (int)(now_pos[4] / m_Resolution[4]);
					limitSDPul = (DriveSpeed - StartSpeed) * 13;
					if ((RemainPul < limitSDPul * 2) && (RemainPul>0))
					{
						float SpeedRatio = limitSDPul * 2 / RemainPul;
						DriveSpeed = DriveSpeed / SpeedRatio;
						if (DriveSpeed < StartSpeed)
						{
							DriveSpeed = StartSpeed;
						}
					}
				}
			}
		}
		m_cSendData[0] = CT_MOTOR;
		m_cSendData[1] = CT_MOTOR_SET;
		m_cSendData[2] = AxisTypes;
		m_cSendData[3] = JOG_SPEED_ACC_DEC;
		m_cSendData[4] = (StartSpeed & 0xff);
		m_cSendData[5] = ((StartSpeed >> 8) & 0xff);
		m_cSendData[6] = ((StartSpeed >> 16) & 0xff);
		m_cSendData[7] = ((StartSpeed >> 24) & 0xff);

		m_cSendData[8] = (DriveSpeed & 0xff);
		m_cSendData[9] = ((DriveSpeed >> 8) & 0xff);
		m_cSendData[10] = ((DriveSpeed >> 16) & 0xff);
		m_cSendData[11] = ((DriveSpeed >> 24) & 0xff);

		m_cSendData[12] = (AccCurve & 0xff);
		m_cSendData[13] = ((AccCurve >> 8) & 0xff);
		m_cSendData[14] = ((AccCurve >> 16) & 0xff);
		m_cSendData[15] = ((AccCurve >> 24) & 0xff);

		m_cSendData[16] = (AccLine & 0xff);
		m_cSendData[17] = ((AccLine >> 8) & 0xff);
		m_cSendData[18] = ((AccLine >> 16) & 0xff);
		m_cSendData[19] = ((AccLine >> 24) & 0xff);

		m_cSendData[20] = (DecCurve & 0xff);
		m_cSendData[21] = ((DecCurve >> 8) & 0xff);
		m_cSendData[22] = ((DecCurve >> 16) & 0xff);
		m_cSendData[23] = ((DecCurve >> 24) & 0xff);

		m_cSendData[24] = (DecLine & 0xff);
		m_cSendData[25] = ((DecLine >> 8) & 0xff);
		m_cSendData[26] = ((DecLine >> 16) & 0xff);
		m_cSendData[27] = ((DecLine >> 24) & 0xff);

		jogspeed = DriveSpeed;

		if ((StartSpeed < 250) && (DriveSpeed < 6))
		{
			m_IsUseJerk = 1;
		}
		else
		{
			m_IsUseJerk = 0;
		}
		m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
		Sleep(5);
		m_cSendData[0] = CT_MOTOR;
		if (AxisNumber == 1 || AxisNumber == 2)
		{
			if (!bJOGDir)//方向
			{
				m_cSendData[1] = CT_START_JOG_NEG;
			}
			else
			{
				m_cSendData[1] = CT_START_JOG_POS;
			}
		}
		else
		{
			if (bJOGDir)//方向
			{
				m_cSendData[1] = CT_START_JOG_POS;
			}
			else
			{
				m_cSendData[1] = CT_START_JOG_NEG;
			}
		}
		m_cSendData[2] = AxisTypes;
		m_cSendData[3] = 0x53;
		t_start = GetTickCount();
		m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
		jogMoving = true;
		g_pLogger->SendAndFlushWithTime(L"[Jog] Out,bJOGDir = %d\n", bJOGDir);
	}
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::StopJog()
{
	WaitForSingleObject(g_Lock_JogAndTrigger, INFINITE);
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		g_pLogger->SendAndFlushWithTime(L"[StopJog] In\n");
		t_end = GetTickCount();
		g_pLogger->SendAndFlushWithTime(L"[StopJog] t_start = %d, t_end = %d\n", t_start, t_end);
		jogMoving = false;
		if (t_end - t_start < 100)
		{
			DWORD t_use = 100 - (t_end - t_start);
			g_pLogger->SendAndFlushWithTime(L"[StopJog] PushButtonTime = %d\n", t_use);
			Sleep(t_use);
		}
		unsigned char m_SendJogData[64] = { 0 };
		if (m_IsUseJerk == 0)
		{
			m_SendJogData[0] = CT_MOTOR;
			m_SendJogData[1] = CT_STOP;
			m_SendJogData[2] = AXIS_XYZU;
			m_StopJogMode[1] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49;
			m_StopJogMode[2] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49;
			m_StopJogMode[3] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49;
			m_StopJogMode[4] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49;
			m_WriteByte = Send_Command(0, (const char*)m_SendJogData, m_SendDataLength);
		}
		else
		{
			m_SendJogData[0] = CT_MOTOR;
			m_SendJogData[1] = CT_STOP;
			m_SendJogData[2] = AXIS_XYZU;
			m_StopJogMode[1] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49;
			m_StopJogMode[2] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49;
			m_StopJogMode[3] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49;
			m_StopJogMode[4] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49;
			m_WriteByte = Send_Command(0, (const char*)m_SendJogData, m_SendDataLength);
		}
		m_Thread_StateJOGStop = HSI_THREAD_PAUSED;
		g_pLogger->SendAndFlushWithTime(L"[StopJog] Out\n");
		ReleaseMutex(g_Lock_JogAndTrigger);
	}
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::StopJogEx(UINT AxisTypes)
{
	WaitForSingleObject(g_Lock_JogAndTrigger, INFINITE);
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		byte AxisNumber = (byte)IndexConvertAxis(AxisTypes);
		g_pLogger->SendAndFlushWithTime(L"[StopJog] In\n");
		t_end = GetTickCount();
		g_pLogger->SendAndFlushWithTime(L"[StopJog] t_start = %d, t_end = %d\n", t_start, t_end);
		jogMoving = false;
		if (t_end - t_start < 100)
		{
			DWORD t_use = 100 - (t_end - t_start);
			g_pLogger->SendAndFlushWithTime(L"[StopJog] PushButtonTime = %d\n", t_use);
			Sleep(t_use);
		}
		unsigned char m_SendJogData[64] = { 0 };
		if (m_IsUseJerk == 0)
		{
			m_SendJogData[0] = CT_MOTOR;
			m_SendJogData[1] = CT_STOP;
			m_SendJogData[2] = AxisNumber;
			m_StopJogMode[1] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49;
			m_StopJogMode[2] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49;
			m_StopJogMode[3] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49;
			m_StopJogMode[4] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49;
			for (int i = 0; i < 5; i++)
			{
				m_WriteByte = Send_Command(0, (const char*)m_SendJogData, m_SendDataLength);
				Sleep(5);
			}
		}
		else
		{
			m_SendJogData[0] = CT_MOTOR;
			m_SendJogData[1] = CT_STOP;
			m_SendJogData[2] = AxisNumber;
			m_StopJogMode[1] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49;
			m_StopJogMode[2] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49;
			m_StopJogMode[3] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49;
			m_StopJogMode[4] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49;
			for (int i = 0; i < 5; i++)
			{
				m_WriteByte = Send_Command(0, (const char*)m_SendJogData, m_SendDataLength);
				Sleep(5);
			}
		}
		m_Thread_StateJOGStop = HSI_THREAD_PAUSED;
		g_pLogger->SendAndFlushWithTime(L"[StopJog] Out\n");
		ReleaseMutex(g_Lock_JogAndTrigger);
	}
	return rStatus;
}
//===========================================================================
int HSI_Motion::P2P(short AxisNumber, long Pos, double Speed, double Acc)
{
	if (g_pHSI_Motion)
	{

	}
	return 0;
}
//===========================================================================
//运动控制部分
//===========================================================================
HSI_STATUS HSI_Motion::GetPositionEncPrfMulti(UINT AxisTypes, double *EncPos, double *PrfPos, int Count)
{
	auto rStatus = HSI_STATUS_NORMAL;
	UNREFERENCED_PARAMETER(AxisTypes);
	CString tempStr;
	if (g_pHSI_Motion)
	{
		//g_pLogger->SendAndFlushWithTime(L"[GetPositionEncPrfMulti] In\n"); 
		if (m_SO7_Serial.m_RecvData[0] == 2)
		{
			EncPos[1] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial.m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]) * m_Resolution[1];
			EncPos[2] = (m_SO7_Serial.m_RecvData[5] << 24 | m_SO7_Serial.m_RecvData[6] << 16 | m_SO7_Serial.m_RecvData[7] << 8 | m_SO7_Serial.m_RecvData[8]) * m_Resolution[2];
			EncPos[3] = (m_SO7_Serial.m_RecvData[9] << 24 | m_SO7_Serial.m_RecvData[10] << 16 | m_SO7_Serial.m_RecvData[11] << 8 | m_SO7_Serial.m_RecvData[12]) * m_Resolution[3];
			EncPos[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | m_SO7_Serial.m_RecvData[15] << 8 | m_SO7_Serial.m_RecvData[16]) * m_Resolution[4];

			PrfPos[1] = (m_SO7_Serial.m_RecvData[17] << 24 | m_SO7_Serial.m_RecvData[18] << 16 | m_SO7_Serial.m_RecvData[19] << 8 | m_SO7_Serial.m_RecvData[20]) * m_Resolution[1];
			PrfPos[2] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial.m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]) * m_Resolution[2];
			PrfPos[3] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial.m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]) * m_Resolution[3];
			PrfPos[4] = (m_SO7_Serial.m_RecvData[29] << 24 | m_SO7_Serial.m_RecvData[30] << 16 | m_SO7_Serial.m_RecvData[31] << 8 | m_SO7_Serial.m_RecvData[32]) * m_Resolution[4];

			if (m_IsHavePattern & 0x01)
				m_EncPos[1] = EncPos[1];
			else
				m_EncPos[1] = PrfPos[1];
			if (m_IsHavePattern & 0x02)
				m_EncPos[2] = EncPos[2];
			else
				m_EncPos[2] = PrfPos[2];
			if (m_IsHavePattern & 0x04)
				m_EncPos[3] = EncPos[3];
			else
				m_EncPos[3] = PrfPos[3];
			if (m_IsHavePattern & 0x08)
				m_EncPos[4] = EncPos[4];
			else
				m_EncPos[4] = PrfPos[4];

			m_PrfPos[1] = PrfPos[1];
			m_PrfPos[2] = PrfPos[2];
			m_PrfPos[3] = PrfPos[3];
			m_PrfPos[4] = PrfPos[4];

			//begin_position[1] = EncPos[1] / m_Resolution[1];
			//begin_position[2] = EncPos[2] / m_Resolution[1];
			//begin_position[3] = EncPos[3] / m_Resolution[1];
			//begin_position[4] = EncPos[4] / m_Resolution[1];
		}
		else
		{
			EncPos[1] = m_EncPos[1];
			EncPos[2] = m_EncPos[2];
			EncPos[2] = m_EncPos[3];
			EncPos[4] = m_EncPos[4];
			PrfPos[1] = m_PrfPos[1];
			PrfPos[2] = m_PrfPos[2];
			PrfPos[2] = m_PrfPos[3];
			PrfPos[4] = m_PrfPos[4];
			g_pLogger->SendAndFlushWithTime(L"[GetPositionEncPrfMulti] failed\n");
		}
		//g_pLogger->SendAndFlushWithTime(L"[GetPositionEncPrfMulti] Out\n");
	}
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::GetPositionXyz(UINT AxisTypes, double &PositionX, double &PositionY, double &PositionZ, double &Time)
{
	auto rStatus = HSI_STATUS_NORMAL;
	UNREFERENCED_PARAMETER(AxisTypes);
	//读取3个轴的位置
	CString tempStr;
	if (g_pHSI_Motion)
	{
		if (m_SO7_Serial.m_RecvData[0] == 2)
		{
			if (m_DeviceType != 1)
			{
				if (m_IsHavePattern & 0x01)
					PositionX = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial.m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]) * m_Resolution[1];
				else
					PositionX = (m_SO7_Serial.m_RecvData[17] << 24 | m_SO7_Serial.m_RecvData[18] << 16 | m_SO7_Serial.m_RecvData[19] << 8 | m_SO7_Serial.m_RecvData[20]) * m_Resolution[1];
				if (m_IsHavePattern & 0x02)
					PositionY = (m_SO7_Serial.m_RecvData[5] << 24 | m_SO7_Serial.m_RecvData[6] << 16 | m_SO7_Serial.m_RecvData[7] << 8 | m_SO7_Serial.m_RecvData[8]) * m_Resolution[2];
				else
					PositionY = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial.m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]) * m_Resolution[2];
				if (m_IsHavePattern & 0x04)
					PositionZ = (m_SO7_Serial.m_RecvData[9] << 24 | m_SO7_Serial.m_RecvData[10] << 16 | m_SO7_Serial.m_RecvData[11] << 8 | m_SO7_Serial.m_RecvData[12]) * m_Resolution[3];
				else
					PositionZ = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial.m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]) * m_Resolution[3];
				if (m_IsHavePattern & 0x08)
					m_PosForAllAxis[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | m_SO7_Serial.m_RecvData[15] << 8 | m_SO7_Serial.m_RecvData[16]) * m_Resolution[4];
				else
					m_PosForAllAxis[4] = (m_SO7_Serial.m_RecvData[29] << 24 | m_SO7_Serial.m_RecvData[30] << 16 | m_SO7_Serial.m_RecvData[31] << 8 | m_SO7_Serial.m_RecvData[32]) * m_Resolution[4];
			}
			else
			{
				PositionX = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial.m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]) * m_Resolution[1];
				PositionY = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial.m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]) * m_Resolution[2];
				PositionZ = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial.m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]) * m_Resolution[3];
			}
			m_EncPos[1] = PositionX;
			m_EncPos[2] = PositionY;
			m_EncPos[3] = PositionZ;
			m_EncPos[4] = m_PosForAllAxis[4];
			m_PosForAllAxis[1] = PositionX;
			m_PosForAllAxis[2] = PositionY;
			m_PosForAllAxis[3] = PositionZ;
		}
		else
		{
			PositionX = m_EncPos[1];
			PositionY = m_EncPos[2];
			PositionZ = m_EncPos[3];
			m_PosForAllAxis[1] = m_EncPos[1];
			m_PosForAllAxis[2] = m_EncPos[2];
			m_PosForAllAxis[3] = m_EncPos[3];
			m_PosForAllAxis[4] = m_EncPos[4];
			g_pLogger->SendAndFlushWithTime(L"[GetPositionEncPrfMulti] failed\n");
		}
		//LARGE_INTEGER tima;
		//QueryPerformanceCounter(&tima);
		//Time = static_cast<double>(tima.QuadPart);
		Time = set_end - set_start;
	}
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::GetEncoderXyz(long *lEncoderVal)
{
	auto rStatus = HSI_STATUS_NORMAL;
	//读取3个轴的编码器值
	if (g_pHSI_Motion)
	{
		if (m_SO7_Serial.m_RecvData[0] == 2)
		{
			if (m_DeviceType != 1)
			{
				if (m_IsHavePattern & 0x01)
					lEncoderVal[0] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial.m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]);
				else
					lEncoderVal[0] = (m_SO7_Serial.m_RecvData[17] << 24 | m_SO7_Serial.m_RecvData[18] << 16 | m_SO7_Serial.m_RecvData[19] << 8 | m_SO7_Serial.m_RecvData[20]);
				if (m_IsHavePattern & 0x02)
					lEncoderVal[1] = (m_SO7_Serial.m_RecvData[5] << 24 | m_SO7_Serial.m_RecvData[6] << 16 | m_SO7_Serial.m_RecvData[7] << 8 | m_SO7_Serial.m_RecvData[8]);
				else
					lEncoderVal[1] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial.m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]);
				if (m_IsHavePattern & 0x04)
					lEncoderVal[2] = (m_SO7_Serial.m_RecvData[9] << 24 | m_SO7_Serial.m_RecvData[10] << 16 | m_SO7_Serial.m_RecvData[11] << 8 | m_SO7_Serial.m_RecvData[12]);
				else
					lEncoderVal[2] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial.m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]);
			}
			else
			{
				lEncoderVal[0] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial.m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]);
				lEncoderVal[1] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial.m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]);
				lEncoderVal[2] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial.m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]);
			}
		}
		else
		{
			auto rStatus = HSI_STATUS_FAILED;
			g_pLogger->SendAndFlushWithTime(L"[GetEncoderXyz] failed\n");
		}
	}
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::GetPositionXyzaProbe(UINT AxisTypes, double &PositionX, double &PositionY, double &PositionZ, double &PositionA)
{
	auto rStatus = HSI_STATUS_NORMAL;
	UNREFERENCED_PARAMETER(AxisTypes);
	if (g_pHSI_Motion)
	{
		PositionX = m_LockPos[1];
		PositionY = m_LockPos[2];
		PositionZ = m_LockPos[3];
		PositionA = m_LockPos[4];
	}
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::SetPositionXyz(UINT AxisTypes, double  PositionX, double  PositionY, double  PositionZ, HSI_MOTION_MOVE_TYPE eType, double dFlyRadius)
{
	WaitForSingleObject(g_WR_ToMove_Mutex, INFINITE);
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] In\n");
		g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Pos[1] = %.4f,Pos[2] = %.4f,Pos[3] = %.4f\n", PositionX, PositionY, PositionZ);
		unsigned char send_pos_data[64] = { 0 };
		axis_start = 0;
		unsigned char direct_pos = 0;
		unsigned char xyzAxis = 0;
		if (CurrentMotionState != E_SO7_MOTION_MOVETO)
		{
			CurrentMotionState = E_SO7_MOTION_MOVETO;
			LimitOver(HSI_MOTION_AXIS_X, PositionX);
			LimitOver(HSI_MOTION_AXIS_Y, PositionY);
			LimitOver(HSI_MOTION_AXIS_Z, PositionZ);
			LimitOver(HSI_MOTION_AXIS_R, m_PositionA);

			m_PosThread[1] = PositionX;
			m_PosThread[2] = PositionY;
			m_PosThread[3] = PositionZ;
			m_PosThread[4] = m_PositionA;

			targetpos_n[1] = PositionX;
			targetpos_n[2] = PositionY;
			targetpos_n[3] = PositionZ;
			targetpos_n[4] = m_PositionA;

			int Pos_t[5] = { 0 };
			int Pos_n[5] = { 0 };
			int Pos[5] = { 0 };
			int NowPos[5] = { 0 };
			int target_pos[5] = { 0 };

			if (m_SO7_Serial.m_RecvData[0] == 2)
			{
				if (m_DeviceType != 1)
				{
					if (m_IsHavePattern & 0x01 == 0x01)
						NowPos[1] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial.m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]);
					else
						NowPos[1] = (m_SO7_Serial.m_RecvData[17] << 24 | m_SO7_Serial.m_RecvData[18] << 16 | m_SO7_Serial.m_RecvData[19] << 8 | m_SO7_Serial.m_RecvData[20]);
					if (m_IsHavePattern & 0x02)
						NowPos[2] = (m_SO7_Serial.m_RecvData[5] << 24 | m_SO7_Serial.m_RecvData[6] << 16 | m_SO7_Serial.m_RecvData[7] << 8 | m_SO7_Serial.m_RecvData[8]);
					else
						NowPos[2] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial.m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]);
					if (m_IsHavePattern & 0x04)
						NowPos[3] = (m_SO7_Serial.m_RecvData[9] << 24 | m_SO7_Serial.m_RecvData[10] << 16 | m_SO7_Serial.m_RecvData[11] << 8 | m_SO7_Serial.m_RecvData[12]);
					else
						NowPos[3] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial.m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]);
					if (m_IsHavePattern & 0x08)
						NowPos[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | m_SO7_Serial.m_RecvData[15] << 8 | m_SO7_Serial.m_RecvData[16]);
					else
						NowPos[4] = (m_SO7_Serial.m_RecvData[29] << 24 | m_SO7_Serial.m_RecvData[30] << 16 | m_SO7_Serial.m_RecvData[31] << 8 | m_SO7_Serial.m_RecvData[32]);
				}
				else
				{
					NowPos[1] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial.m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]);
					NowPos[2] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial.m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]);
					NowPos[3] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial.m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]);
					NowPos[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | m_SO7_Serial.m_RecvData[15] << 8 | m_SO7_Serial.m_RecvData[16]);
				}
				//NowPos[1] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial.m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]);
				//NowPos[2] = (m_SO7_Serial.m_RecvData[5] << 24 | m_SO7_Serial.m_RecvData[6] << 16 | m_SO7_Serial.m_RecvData[7] << 8 | m_SO7_Serial.m_RecvData[8]);
				//NowPos[3] = (m_SO7_Serial.m_RecvData[9] << 24 | m_SO7_Serial.m_RecvData[10] << 16 | m_SO7_Serial.m_RecvData[11] << 8 | m_SO7_Serial.m_RecvData[12]);
				//NowPos[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | m_SO7_Serial.m_RecvData[15] << 8 | m_SO7_Serial.m_RecvData[16]);

				Pos_t[1] = (m_SO7_Serial.m_RecvData[17] << 24 | m_SO7_Serial.m_RecvData[18] << 16 | m_SO7_Serial.m_RecvData[19] << 8 | m_SO7_Serial.m_RecvData[20]);
				Pos_t[2] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial.m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]);
				Pos_t[3] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial.m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]);
				Pos_t[4] = (m_SO7_Serial.m_RecvData[29] << 24 | m_SO7_Serial.m_RecvData[30] << 16 | m_SO7_Serial.m_RecvData[31] << 8 | m_SO7_Serial.m_RecvData[32]);
			}
			else
			{
				Pos_t[1] = NowPos[1] = (int)(m_EncPos[1] / m_Resolution[1]);
				Pos_t[2] = NowPos[2] = (int)(m_EncPos[2] / m_Resolution[2]);
				Pos_t[3] = NowPos[3] = (int)(m_EncPos[3] / m_Resolution[3]);
				Pos_t[4] = NowPos[4] = (int)(m_EncPos[4] / m_Resolution[4]);
			}

			if (m_motorType & 0x01) //步进电机
				Pos[1] = (int)(PositionX / m_Resolution[1]) - NowPos[1];
			else
				Pos[1] = (int)(PositionX / m_Resolution[1]) - Pos_t[1];
			if (m_motorType & 0x02) //步进电机
				Pos[2] = (int)(PositionX / m_Resolution[2]) - NowPos[2];
			else
				Pos[2] = (int)(PositionY / m_Resolution[2]) - Pos_t[2];
			if (m_motorType & 0x04) //步进电机
				Pos[3] = (int)(PositionX / m_Resolution[3]) - NowPos[3];
			else
				Pos[3] = (int)(PositionZ / m_Resolution[3]) - Pos_t[3];
			if (m_motorType & 0x08) //步进电机
				Pos[4] = (int)(PositionX / m_Resolution[4]) - NowPos[4];
			else
				Pos[4] = (int)(m_PositionA / m_Resolution[4]) - Pos_t[4];

			/*if (m_motorType==1)
			{
			if (m_IsUse_HSICompensation)
			{

			}
			else
			{
			Pos[1] = (int)(PositionX / m_Resolution[1]) - NowPos[1];
			Pos[2] = (int)(PositionY / m_Resolution[2]) - NowPos[2];
			Pos[3] = (int)(PositionZ / m_Resolution[3]) - NowPos[3];
			Pos[4] = (int)(m_PositionA / m_Resolution[4]) - NowPos[4];
			}
			}
			else
			{
			Pos_n[1] = (int)(targetpos_n[1] / m_Resolution[1]) - NowPos[1];
			Pos_n[2] = (int)(targetpos_n[2] / m_Resolution[2]) - NowPos[2];
			Pos_n[3] = (int)(targetpos_n[3] / m_Resolution[3]) - NowPos[3];

			Pos_t[1] = (int)(targetpos_n[1] / m_Resolution[1]) - (int)(targetpos_l[1] / m_Resolution[1]);
			Pos_t[2] = (int)(targetpos_n[2] / m_Resolution[2]) - (int)(targetpos_l[2] / m_Resolution[1]);
			Pos_t[3] = (int)(targetpos_n[3] / m_Resolution[3]) - (int)(targetpos_l[3] / m_Resolution[1]);

			if (m_IsUse_HSICompensation)
			{
			for (int k = 1; k < 4; k++)
			{
			if (abs(Pos_n[k] - Pos_t[k]) > m_Compensation_Pluse)
			{
			Pos[k] = Pos_n[k];
			}
			else
			{
			Pos[k] = Pos_t[k];
			}
			if (abs(Pos[k]) < m_Compensation_Pluse)
			{
			Pos[k] = 0;
			}
			}
			}
			else
			{
			Pos[1] = (int)(PositionX / m_Resolution[1]) - NowPos[1];
			Pos[2] = (int)(PositionY / m_Resolution[2]) - NowPos[2];
			Pos[3] = (int)(PositionZ / m_Resolution[3]) - NowPos[3];
			Pos[4] = (int)(m_PositionA / m_Resolution[4]) - NowPos[4];
			}
			}*/
			target_pos[1] = (int)(PositionX / m_Resolution[1]);
			target_pos[2] = (int)(PositionY / m_Resolution[2]);
			target_pos[3] = (int)(PositionZ / m_Resolution[3]);
			target_pos[4] = (int)(m_PositionA / m_Resolution[4]);

			begin_position[1] = target_pos[1];
			begin_position[2] = target_pos[2];
			begin_position[3] = target_pos[3];
			begin_position[4] = target_pos[4];

			float scale[4] = { 0 };
			int Stepdriverspeed[5] = { 0 };
			int StepStartspeed[5] = { 0 };
			int StepAcc[5] = { 0 };
			//if (Pos[1] > 0)  direct_pos |= 0x01;
			//if (Pos[2] > 0)  direct_pos |= 0x02;
			//if (Pos[3] > 0)  direct_pos |= 0x04;
			if (abs(Pos[1]) > 2) axis_start |= 0x01;
			if (abs(Pos[2]) > 2) axis_start |= 0x02;
			if (abs(Pos[3]) > 2) axis_start |= 0x04;
			int MaxPos = abs(Pos[1]);
			for (int i = 2; i < 5; i++)
			{
				if (MaxPos < abs(Pos[i]))
				{
					MaxPos = abs(Pos[i]);
				}
			}
			//MaxPos = abs(MaxPos);
			scale[0] = abs(Pos[1]) / (float)MaxPos;
			scale[1] = abs(Pos[2]) / (float)MaxPos;
			scale[2] = abs(Pos[3]) / (float)MaxPos;
			scale[3] = abs(Pos[4]) / (float)MaxPos;

			g_pLogger->SendAndFlushWithTime(L"[SetPositionXyzNowPos] Pos[1] = %.4f,Pos[2] = %.4f,Pos[3] = %.4f,Pos[4] = %.4f\n", NowPos[1] * m_Resolution[1], NowPos[2] * m_Resolution[2], NowPos[3] * m_Resolution[3], NowPos[4] * m_Resolution[4]);
			g_pLogger->SendAndFlushWithTime(L"[SetPositionXyzTagPos] Pos[1] = %.4f,Pos[2] = %.4f,Pos[3] = %.4f,Pos[4] = %.4f\n", PositionX, PositionY, PositionZ, m_PositionA);


			int stepinterpolation = 0x03;
			//if (!m_motorType)
			//{
			//	/*if ((stepinterpolation & 0x01) && m_IsUseManualRunin)
			//	{
			//		Stepdriverspeed[1] = m_SetPotion_DriveSpeed[1] * scale[0];
			//		if (Stepdriverspeed[1] < m_SetPotion_StartSpeed[1])
			//		{
			//			Stepdriverspeed[1] = m_SetPotion_StartSpeed[1];
			//		}
			//	}
			//	else
			//	{
			//		Stepdriverspeed[1] = m_SetPotion_DriveSpeed[1];
			//	}
			//	if ((stepinterpolation & 0x02) && m_IsUseManualRunin)
			//	{
			//		Stepdriverspeed[2] = m_SetPotion_DriveSpeed[2] * scale[1];
			//		if (Stepdriverspeed[2] < m_SetPotion_StartSpeed[2])
			//		{
			//			Stepdriverspeed[2] = m_SetPotion_StartSpeed[2];
			//		}
			//	}
			//	else
			//	{
			//		Stepdriverspeed[2] = m_SetPotion_DriveSpeed[2];
			//	}
			//	if ((stepinterpolation & 0x04) && m_IsUseManualRunin)
			//	{
			//		Stepdriverspeed[3] = m_SetPotion_DriveSpeed[3] * scale[2];
			//		if (Stepdriverspeed[3] < m_SetPotion_StartSpeed[3])
			//		{
			//			Stepdriverspeed[3] = m_SetPotion_DriveSpeed[3];
			//		}
			//	}
			//	else
			//	{
			//		Stepdriverspeed[3] = m_SetPotion_DriveSpeed[3];
			//	}
			//	if ((stepinterpolation & 0x08) && m_IsUseManualRunin)
			//	{
			//		Stepdriverspeed[4] = m_SetPotion_DriveSpeed[4] * scale[3];
			//		if (Stepdriverspeed[4] < m_SetPotion_StartSpeed[4])
			//		{
			//			Stepdriverspeed[4] = m_SetPotion_DriveSpeed[4];
			//		}
			//	}
			//	else
			//	{
			//		Stepdriverspeed[4] = m_SetPotion_DriveSpeed[4];
			//	}*/
			//	for (int i = 1; i < 3; i++)
			//	{
			//		if (abs(Pos[i]) < m_stepPosition_Load[0])
			//		{
			//			StepStartspeed[i] = m_stepPosition_L_speed[0];
			//			Stepdriverspeed[i] = m_stepPosition_H_speed[0];
			//			StepAcc[i] = m_stepPosition_acc[0];
			//		}
			//		else if (abs(Pos[i]) < m_stepPosition_Load[1])
			//		{
			//			StepStartspeed[i] = m_stepPosition_L_speed[1];
			//			Stepdriverspeed[i] = m_stepPosition_H_speed[1];
			//			StepAcc[i] = m_stepPosition_acc[1];
			//		}
			//		else if (abs(Pos[i]) < m_stepPosition_Load[2])
			//		{
			//			StepStartspeed[i] = m_stepPosition_L_speed[2];
			//			Stepdriverspeed[i] = m_stepPosition_H_speed[2];
			//			StepAcc[i] = m_stepPosition_acc[2];
			//		}
			//		else if (abs(Pos[i]) < m_stepPosition_Load[3])
			//		{
			//			StepStartspeed[i] = m_stepPosition_L_speed[3];
			//			Stepdriverspeed[i] = m_stepPosition_H_speed[3];
			//			StepAcc[i] = m_stepPosition_acc[3];
			//		}
			//		else if (abs(Pos[i]) < m_stepPosition_Load[4])
			//		{
			//			StepStartspeed[i] = m_stepPosition_L_speed[4];
			//			Stepdriverspeed[i] = m_stepPosition_H_speed[4];
			//			StepAcc[i] = m_stepPosition_acc[4];
			//		}
			//		else if (abs(Pos[i]) < m_stepPosition_Load[5])
			//		{
			//			StepStartspeed[i] = m_stepPosition_L_speed[5];
			//			Stepdriverspeed[i] = m_stepPosition_H_speed[5];
			//			StepAcc[i] = m_stepPosition_acc[5];
			//		}
			//		else if (abs(Pos[i]) < m_stepPosition_Load[6])
			//		{
			//			StepStartspeed[i] = m_stepPosition_L_speed[6];
			//			Stepdriverspeed[i] = m_stepPosition_H_speed[6];
			//			StepAcc[i] = m_stepPosition_acc[6];
			//		}
			//		else if (abs(Pos[i]) < m_stepPosition_Load[7])
			//		{
			//			StepStartspeed[i] = m_stepPosition_L_speed[7];
			//			Stepdriverspeed[i] = m_stepPosition_H_speed[7];
			//			StepAcc[i] = m_stepPosition_acc[7];
			//		}
			//		else if (abs(Pos[i]) < m_stepPosition_Load[8])
			//		{
			//			StepStartspeed[i] = m_stepPosition_L_speed[8];
			//			Stepdriverspeed[i] = m_stepPosition_H_speed[8];
			//			StepAcc[i] = m_stepPosition_acc[8];
			//		}
			//		else if (abs(Pos[i]) < m_stepPosition_Load[9])
			//		{
			//			StepStartspeed[i] = m_stepPosition_L_speed[9];
			//			Stepdriverspeed[i] = m_stepPosition_H_speed[9];
			//			StepAcc[i] = m_stepPosition_acc[9];
			//		}
			//		else
			//		{
			//			StepStartspeed[i] = m_SetPotion_StartSpeed[i];
			//			Stepdriverspeed[i] = m_SetPotion_DriveSpeed[i]; 
			//			StepAcc[i] = m_SetPotion_Line[i];
			//		}
			//	}
			//	StepStartspeed[3] = m_SetPotion_StartSpeed[3];
			//	StepAcc[3] = m_SetPotion_Line[3];
			//	Stepdriverspeed[3] = m_SetPotion_DriveSpeed[3];
			//}

			int axisCount = 4;
			if (fourthAxisFlag)
			{
				fourthAxisFlag = false;
				if (Pos[4] > 0)  direct_pos |= 0x08;
				if (abs(Pos[4]) > 20) axis_start |= 0x08;
				axisCount = 5;
				xyzAxis = AXIS_XYZU;
			}
			else
				xyzAxis = AXIS_XYZ;
			for (int i = 1; i < axisCount; i++)
			{
				int time_out_send = 0;

				send_pos_data[0] = CT_MOTOR;
				send_pos_data[1] = CT_MOTOR_SET;
				send_pos_data[2] = 1 << (i - 1);
				send_pos_data[3] = POSITION_SPEED_ACC_DEC_POS;
				/*if (m_motorType == 1)
				{*/
				send_pos_data[4] = (m_SetPotion_StartSpeed[i] & 0xff);
				send_pos_data[5] = ((m_SetPotion_StartSpeed[i] >> 8) & 0xff);
				send_pos_data[6] = ((m_SetPotion_StartSpeed[i] >> 16) & 0xff);
				send_pos_data[7] = ((m_SetPotion_StartSpeed[i] >> 24) & 0xff);
				if ((xyzAxis == AXIS_XYZU) && (m_IsUseFourthSpeed == 1))
				{
					send_pos_data[8] = (m_SetPotion_DriveSpeed[4] & 0xff);
					send_pos_data[9] = ((m_SetPotion_DriveSpeed[4] >> 8) & 0xff);
					send_pos_data[10] = ((m_SetPotion_DriveSpeed[4] >> 16) & 0xff);
					send_pos_data[11] = ((m_SetPotion_DriveSpeed[4] >> 24) & 0xff);
				}
				else
				{
					send_pos_data[8] = (m_SetPotion_DriveSpeed[i] & 0xff);
					send_pos_data[9] = ((m_SetPotion_DriveSpeed[i] >> 8) & 0xff);
					send_pos_data[10] = ((m_SetPotion_DriveSpeed[i] >> 16) & 0xff);
					send_pos_data[11] = ((m_SetPotion_DriveSpeed[i] >> 24) & 0xff);
				}
				send_pos_data[12] = (m_SetPotion_Line[i] & 0xff);
				send_pos_data[13] = ((m_SetPotion_Line[i] >> 8) & 0xff);
				send_pos_data[14] = ((m_SetPotion_Line[i] >> 16) & 0xff);
				send_pos_data[15] = ((m_SetPotion_Line[i] >> 24) & 0xff);

				send_pos_data[16] = (m_SetPotion_Buffer[i] & 0xff);
				send_pos_data[17] = ((m_SetPotion_Buffer[i] >> 8) & 0xff);
				send_pos_data[18] = ((m_SetPotion_Buffer[i] >> 16) & 0xff);
				send_pos_data[19] = ((m_SetPotion_Buffer[i] >> 24) & 0xff);

				send_pos_data[28] = (target_pos[i] & 0xff);
				send_pos_data[29] = ((target_pos[i] >> 8) & 0xff);
				send_pos_data[30] = ((target_pos[i] >> 16) & 0xff);
				send_pos_data[31] = ((target_pos[i] >> 24) & 0xff);

				send_pos_data[32] = (Pos[i] & 0xff);
				send_pos_data[33] = ((Pos[i] >> 8) & 0xff);
				send_pos_data[34] = ((Pos[i] >> 16) & 0xff);
				send_pos_data[35] = ((Pos[i] >> 24) & 0xff);

				g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] m_SetPotion_StartSpeed[%d] = %d,m_SetPotion_DriveSpeed[%d] = %d\n", i, m_SetPotion_StartSpeed[i], i, m_SetPotion_DriveSpeed[i]);
				//send_pos_data[32] = 0x21;
				/*}*/

				/*else
				{
				send_pos_data[4] = (StepStartspeed[i] & 0xff);
				send_pos_data[5] = ((StepStartspeed[i] >> 8) & 0xff);
				send_pos_data[6] = ((StepStartspeed[i] >> 16) & 0xff);
				send_pos_data[7] = ((StepStartspeed[i] >> 24) & 0xff);


				send_pos_data[8] = (Stepdriverspeed[i] & 0xff);
				send_pos_data[9] = ((Stepdriverspeed[i] >> 8) & 0xff);
				send_pos_data[10] = ((Stepdriverspeed[i] >> 16) & 0xff);
				send_pos_data[11] = ((Stepdriverspeed[i] >> 24) & 0xff);

				send_pos_data[12] = (StepAcc[i] & 0xff);
				send_pos_data[13] = ((StepAcc[i] >> 8) & 0xff);
				send_pos_data[14] = ((StepAcc[i] >> 16) & 0xff);
				send_pos_data[15] = ((StepAcc[i] >> 24) & 0xff);

				send_pos_data[16] = (m_SetPotion_Buffer[i] & 0xff);
				send_pos_data[17] = ((m_SetPotion_Buffer[i] >> 8) & 0xff);
				send_pos_data[18] = ((m_SetPotion_Buffer[i] >> 16) & 0xff);
				send_pos_data[19] = ((m_SetPotion_Buffer[i] >> 24) & 0xff);

				send_pos_data[28] = (target_pos[i] & 0xff);
				send_pos_data[29] = ((target_pos[i] >> 8) & 0xff);
				send_pos_data[30] = ((target_pos[i] >> 16) & 0xff);
				send_pos_data[31] = ((target_pos[i] >> 24) & 0xff);

				send_pos_data[32] = (Pos[i] & 0xff);
				send_pos_data[33] = ((Pos[i] >> 8) & 0xff);
				send_pos_data[34] = ((Pos[i] >> 16) & 0xff);
				send_pos_data[35] = ((Pos[i] >> 24) & 0xff);

				g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] m_SetPotion_StartSpeed[%d] = %d,m_SetPotion_DriveSpeed[%d] = %d\n", i, m_SetPotion_StartSpeed[i], i, m_SetPotion_DriveSpeed[i]);

				}*/
				m_WriteByte = Send_Command(0, (const char*)send_pos_data, m_SendDataLength);
				Sleep(6);
			}

			while (m_SO7_Serial.m_RecvData[39])
			{
				send_pos_data[0] = CT_ORDER;
				send_pos_data[1] = CT_POSFLAG_CLEAR;
				m_WriteByte = Send_Command(0, (const char*)send_pos_data, m_SendDataLength);
				Sleep(10);
			}

			Sleep(10);
			//启动插补和定位功能
			/*	if (m_motorType == 1)
				{
				send_pos_data[0] = CT_MOTOR;
				send_pos_data[1] = CT_START_POSITION;
				send_pos_data[2] = xyzAxis;
				send_pos_data[3] = 0x53;
				send_pos_data[4] = INTERPOLATION;
				}
				else
				{*/
			send_pos_data[0] = CT_MOTOR;
			send_pos_data[1] = CT_START_POSITION;
			send_pos_data[2] = axis_start;
			send_pos_data[3] = 0x53;
			send_pos_data[4] = m_motorType & 0xff;
			/*	}*/
			for (int j = 1; j < axisCount; j++)
			{
				send_pos_data[9 + 4 * (j - 1)] = (target_pos[j] & 0xff);
				send_pos_data[10 + 4 * (j - 1)] = ((target_pos[j] >> 8) & 0xff);
				send_pos_data[11 + 4 * (j - 1)] = ((target_pos[j] >> 16) & 0xff);
				send_pos_data[12 + 4 * (j - 1)] = ((target_pos[j] >> 24) & 0xff);
			}
			//send_pos_data[25] = direct_pos;
			int stepmotoracc = 0;
			stepmotoracc = CaculateStepMotorACC(Pos[1], m_SetPotion_Line[1], 10);
			send_pos_data[25] = stepmotoracc;
			stepmotoracc = CaculateStepMotorACC(Pos[2], m_SetPotion_Line[2], 10);
			send_pos_data[26] = stepmotoracc;
			stepmotoracc = CaculateStepMotorACC(Pos[3], m_SetPotion_Line[3], 10);
			send_pos_data[27] = stepmotoracc;
			stepmotoracc = CaculateStepMotorACC(Pos[4], m_SetPotion_Line[4], 10);
			/*send_pos_data[25] = m_SetPotion_Line[1];
			send_pos_data[26] = m_SetPotion_Line[2];
			send_pos_data[27] = m_SetPotion_Line[3];
			send_pos_data[28] = m_SetPotion_Line[4];*/

			send_pos_data[29] = m_SpeedAdjustPeriod[1];
			send_pos_data[30] = m_SpeedAdjustPeriod[2];
			send_pos_data[31] = m_SpeedAdjustPeriod[3];
			send_pos_data[32] = m_SpeedAdjustPeriod[4];

			for (size_t i = 1; i < 5; i++)
			{
				Stepdriverspeed[i] = m_SetPotion_DriveSpeed[i] * m_Resolution[i] * 50;
			}

			send_pos_data[33] = (Stepdriverspeed[1] >> 8) & 0xff;
			send_pos_data[34] = Stepdriverspeed[1] & 0xff;
			send_pos_data[35] = (Stepdriverspeed[2] >> 8) & 0xff;
			send_pos_data[36] = Stepdriverspeed[2] & 0xff;
			send_pos_data[37] = (Stepdriverspeed[3] >> 8) & 0xff;
			send_pos_data[38] = Stepdriverspeed[3] & 0xff;
			send_pos_data[39] = (Stepdriverspeed[4] >> 8) & 0xff;
			send_pos_data[40] = Stepdriverspeed[4] & 0xff;


			if (bCircleRun)
			{
				bCircleRun = false;
				send_pos_data[1] = CT_CIRCLERUN_POSITION;
				send_pos_data[2] = 0x03;
				send_pos_data[4] = CIRCLER;
				for (size_t i = 1; i < 3; i++)
				{
					iCircleRunPnt[i] = iCircleRunPnt[i] - NowPos[i];
					send_pos_data[26 + 4 * (i - 1)] = (iCircleRunPnt[i] & 0xff);
					send_pos_data[27 + 4 * (i - 1)] = ((iCircleRunPnt[i] >> 8) & 0xff);
					send_pos_data[28 + 4 * (i - 1)] = ((iCircleRunPnt[i] >> 16) & 0xff);
					send_pos_data[29 + 4 * (i - 1)] = ((iCircleRunPnt[i] >> 24) & 0xff);
				}
			}
			g_IsClose = false;
			m_WriteByte = Send_Command(0, (const char*)send_pos_data, m_SendDataLength);
			set_start = GetTickCount();
			Sleep(3);//
			if (eType == HSI_MOTION_MOVE_NOWAIT)
			{
				g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Nowait SetEvent\n");
				g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Nowait move!\n");
				g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Out Success Nowait\n");
				m_IsExMotion = 0;
				bRunGlueDispenser = HSI_THREAD_PAUSED;
				m_Thread_State = HSI_THREAD_RUNNING;
				SetEvent(m_hTriggerEvent);
			}
			if (eType == HSI_MOTION_MOVE_WAIT)
			{
				g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Out Success Wait Mode\n");
				m_Thread_State = HSI_THREAD_RUNNING;
				m_IsExMotion = 0;
				UpdateMotionState();
				m_Thread_State = HSI_THREAD_PAUSED;
			}
		}
		else
		{
			g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] HSI_STATUS_MOTION_MOVING\n");
			g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Out\n");
			rStatus = HSI_STATUS_MOTION_MOVING;
		}
		g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Out\n");
		targetpos_l[1] = PositionX;
		targetpos_l[2] = PositionY;
		targetpos_l[3] = PositionZ;
	}
	ReleaseMutex(g_WR_ToMove_Mutex);
	return rStatus;
}
//===========================================================================
int HSI_Motion::CaculateStepMotorACC(int pos, int maxacc, int minacc)
{
	double mpos = (double)abs(pos) / 1000;
	int acc = (mpos * mpos) / 20 + mpos / 2 + 8;
	if (acc < minacc)
		acc = minacc;
	if (acc > maxacc)
		acc = maxacc;
	return acc;
}
//===========================================================================
HSI_STATUS HSI_Motion::SetPositionXyza(UINT AxisTypes, double  PositionX, double  PositionY, double  PositionZ, double  PositionA, HSI_MOTION_MOVE_TYPE eType, double dFlyRadius)
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		g_pLogger->SendAndFlushWithTime(L"[SetPositionXyza] SetPositionXyza\n");
		fourthAxisFlag = true;
		m_PositionA = PositionA;
		rStatus = SetPositionXyz(AxisTypes, PositionX, PositionY, PositionZ, eType, dFlyRadius);
	}
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::SetPositionXyzCache(UINT AxisTypes, HSI_MOTION_MOVE_TYPE eType, int DataCount, Point *CacheData)
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		g_pLogger->SendAndFlushWithTime(L"SetPositionXyzCache : start\n");

		g_pLogger->SendAndFlushWithTime(L"SetPositionXyzCache : end\n");
	}
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::SetCircleInterpolate(double  PositionX, double  PositionY, double  PositionZ)
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		g_pLogger->SendAndFlushWithTime(L"SetCircleInterpolate : start\n");
		bCircleRun = true;
		iCircleRunPnt[1] = (int)(PositionX / m_Resolution[1]);
		iCircleRunPnt[2] = (int)(PositionY / m_Resolution[2]);
		iCircleRunPnt[3] = (int)(PositionZ / m_Resolution[3]);
		g_pLogger->SendAndFlushWithTime(L"SetCircleInterpolate : end\n");
	}
	return rStatus;
}
//===========================探针接口================================================
void  HSI_Motion::ProbeRetractManDist(int RetractManDist)
{
	if (g_pHSI_Motion)
	{
		m_cSendData[0] = 0x01;
		m_cSendData[1] = 23;
		m_cSendData[2] = (RetractManDist >> 24 & 0xff);
		m_cSendData[3] = ((RetractManDist >> 16) & 0xff);
		m_cSendData[4] = ((RetractManDist >> 8) & 0xff);
		m_cSendData[5] = ((RetractManDist)& 0xff);
		m_cSendData[6] = (m_ProbeReturnSpeed >> 8) & 0xff;
		m_cSendData[7] = m_ProbeReturnSpeed & 0xff;
		m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
		Sleep(10);
	}
}

HSI_STATUS HSI_Motion::JogProbe(UINT AxisTypes, double Speed)
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		while (m_SO7_Serial.m_RecvData[57])
		{
			m_cSendData[0] = CT_ORDER;
			m_cSendData[1] = CT_POSFLAG_CLEAR;
			m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
			Sleep(10);
		}
		unsigned char send_probe_data[64] = { 0 };
		unsigned char motionAxis = 0;
		unsigned char motionDir = 0;
		for (size_t i = 0; i < 3; i++)
		{
			if ((i == 0) && (m_axisDirX != 0) || (i == 1) && (m_axisDirY != 0) || (i == 2) && (m_axisDirZ != 0))
			{
				if ((m_axisDirX == 1) && (i == 0))
				{
					motionAxis |= 0x01;
					motionDir |= 0x01;
				}
				else if ((i == 0) && (m_axisDirX == 2))
				{
					motionAxis |= 0x01;
				}
				if ((m_axisDirY == 1) && (i == 1))
				{
					motionAxis |= 0x02;
					motionDir |= 0x02;
				}
				else if ((i == 1) && (m_axisDirY == 2))
				{
					motionAxis |= 0x02;
				}
				if ((m_axisDirZ == 1) && (i == 2))
				{
					motionAxis |= 0x04;
					motionDir |= 0x04;
				}
				else if ((i == 2) && (m_axisDirZ == 2))
				{
					motionAxis |= 0x04;
				}
				int DriveSpeed(1);
				int StartSpeed(1);

				int AccLine(1);
				int DecLine(1);
				int AccCurve(1);
				int DecCurve(1);
				int JogSpeed(1);
				byte AxisNumber = (byte)IndexConvertAxis(i + 1);
				m_Thread_State = HSI_THREAD_PAUSED;

				JogSpeed = abs(SpeedPercent(AxisNumber, Speed, DriveSpeed, StartSpeed, AccLine, DecLine, AccCurve, DecCurve));
				send_probe_data[0] = CT_MOTOR;
				send_probe_data[1] = CT_MOTOR_SET;
				send_probe_data[2] = AxisNumber;
				send_probe_data[3] = JOG_SPEED_ACC_DEC;
				send_probe_data[4] = (StartSpeed & 0xff);
				send_probe_data[5] = ((StartSpeed >> 8) & 0xff);
				send_probe_data[6] = ((StartSpeed >> 16) & 0xff);
				send_probe_data[7] = ((StartSpeed >> 24) & 0xff);

				send_probe_data[8] = (m_probeSeekSpeed & 0xff);
				send_probe_data[9] = ((m_probeSeekSpeed >> 8) & 0xff);
				send_probe_data[10] = ((m_probeSeekSpeed >> 16) & 0xff);
				send_probe_data[11] = ((m_probeSeekSpeed >> 24) & 0xff);

				send_probe_data[12] = (AccCurve & 0xff);
				send_probe_data[13] = ((AccCurve >> 8) & 0xff);
				send_probe_data[14] = ((AccCurve >> 16) & 0xff);
				send_probe_data[15] = ((AccCurve >> 24) & 0xff);

				send_probe_data[16] = (AccLine & 0xff);
				send_probe_data[17] = ((AccLine >> 8) & 0xff);
				send_probe_data[18] = ((AccLine >> 16) & 0xff);
				send_probe_data[19] = ((AccLine >> 24) & 0xff);

				send_probe_data[20] = (DecCurve & 0xff);
				send_probe_data[21] = ((DecCurve >> 8) & 0xff);
				send_probe_data[22] = ((DecCurve >> 16) & 0xff);
				send_probe_data[23] = ((DecCurve >> 24) & 0xff);

				send_probe_data[24] = (DecLine & 0xff);
				send_probe_data[25] = ((DecLine >> 8) & 0xff);
				send_probe_data[26] = ((DecLine >> 16) & 0xff);
				send_probe_data[27] = ((DecLine >> 24) & 0xff);

				m_WriteByte = Send_Command(0, (const char*)send_probe_data, m_SendDataLength);
				Sleep(5);
			}
		}

		send_probe_data[0] = CT_MOTOR;
		send_probe_data[1] = CT_START_LATCH;
		send_probe_data[2] = motionAxis;
		send_probe_data[3] = motionDir;
		int returnPos = m_ProbeReturnPos / m_Resolution[AxisTypes];
		send_probe_data[4] = (returnPos >> 8) & 0xff;
		send_probe_data[5] = returnPos & 0xff;
		send_probe_data[6] = (m_ProbeReturnSpeed >> 8) & 0xff;
		send_probe_data[7] = m_ProbeReturnSpeed & 0xff;

		m_WriteByte = Send_Command(0, (const char*)send_probe_data, m_SendDataLength);
		Sleep(3);//

	}
	return rStatus;
}
//=============================读取配置==============================================
HSI_STATUS HSI_Motion::Load_EF3_Motion_Inifile(CString GoogolIniFile)
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		CString csAppPath = GoogolIniFile;
		USES_CONVERSION;
		CString temp = L"";
		CString strGear[5] = { L"GEAR0_", L"GEAR1_", L"GEAR2_", L"GEAR3_", L"GEAR4_" };
		CString axisNum[5] = { L"0", L"1", L"2", L"3", L"4" };
		//判断Log目录是否存在，不存在就创建
		if (CreateDirectory(m_AppPath + L"\\Log", NULL))
		{
			g_pLogger->SendAndFlushWithTime(L"[Load_EF3_Motion_Inifile] Create Log Directory\n");
			CreateDirectory(m_AppPath + L"\\Log", NULL);
		}
		if (GetFileAttributes(GoogolIniFile) == -1)
		{
			g_pLogger->SendAndFlushWithTime(L"[Load_EF3_Motion_Inifile] EF3_Motion.ini file no find\n");
			sEvenProp.Init();
			sEvenProp.EventType = HSI_EVENT_ERROR;
			sEvenProp.EventID = HSI_EVENT_MOTION;
			sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK;
			strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "EF3_Motion.ini文件不存在！");
			EventCallback(sEvenProp);
			return HSI_STATUS_FAILED;
		}
		m_motorType = GetPrivateProfileInt(L"MOTORTYPE", L"SERVOMOTOR", 1, csAppPath);
		for (int i = 1; i < 5; i++)
		{
			m_precisionCount[i] = GetPrivateProfileInt(L"PRECISION", L"PRECISION_COUNT_" + axisNum[i], 14000, csAppPath);
			m_precisionTime[i] = (long)GetPrivateProfileInt(L"PRECISION", L"PRECISION_TIME_" + axisNum[i], 14000, csAppPath);
			GetPrivateProfileString(L"RESOLUTION", L"SCALE_RESOLUTION_" + axisNum[i], L"0.0004", temp.GetBufferSetLength(50), 50, csAppPath);
			m_Resolution[i] = (atof(T2A(temp)));

			GetPrivateProfileString(L"LIMIT", L"NEG_WORKING_LIMIT_" + axisNum[i], L"-40", temp.GetBufferSetLength(50), 50, csAppPath);
			m_N_Work_Limit[i] = (atof(T2A(temp)));
			GetPrivateProfileString(L"LIMIT", L"POS_WORKING_LIMIT_" + axisNum[i], L"160", temp.GetBufferSetLength(50), 50, csAppPath);
			m_P_Work_Limit[i] = (atof(T2A(temp)));
			m_Home_Time[i] = (long)GetPrivateProfileInt(L"HOME", L"HOME_TIME_" + axisNum[i], 1500, csAppPath);
			m_Home_AddJogGears[i] = GetPrivateProfileInt(L"HOME", L"HOME_ADD_JOGCHOICE_" + axisNum[i], 100, csAppPath);
			m_Home_DecJogGears[i] = GetPrivateProfileInt(L"HOME", L"HOME_DEC_JOGCHOICE_" + axisNum[i], 100, csAppPath);
			m_SetPotion_Count[i] = GetPrivateProfileInt(L"SET_POSITION_SPEED", L"SET_POSITION_COUNT_" + axisNum[i], 10, csAppPath);
			m_SetPotion_DriveSpeed[i] = GetPrivateProfileInt(L"SET_POSITION_SPEED", L"SET_POTION_DRIVESPEED_" + axisNum[i], 100, csAppPath);
			m_SetPotion_DriveSpeed[i] = m_SetPotion_DriveSpeed[i] / (m_Resolution[i] * 50);
			m_SetPotion_Line[i] = GetPrivateProfileInt(L"SET_POSITION_SPEED", L"SET_POTION_LINE_" + axisNum[i], 100, csAppPath);
			m_SetPotion_StartSpeed[i] = GetPrivateProfileInt(L"SET_POSITION_SPEED", L"SET_POTION_STARTSPEED_" + axisNum[i], 100, csAppPath);
			m_SetPotion_StartSpeed[i] = m_SetPotion_StartSpeed[i] / (m_Resolution[i] * 500);
			m_SetPotion_Buffer[i] = GetPrivateProfileInt(L"SET_POSITION_SPEED", L"SET_POTION_BUFFER_" + axisNum[i], 105, csAppPath);
			m_LogIsOpen[i] = GetPrivateProfileInt(L"LOG", L"LOG_IS_OPEN_" + axisNum[i], 0, csAppPath);
			m_Home_Machine_Axis[i] = GetPrivateProfileInt(L"HOME", L"HOME_MACHINE_AXIS_" + axisNum[i], 0, csAppPath);
			m_Home_Pos_Axis[i] = GetPrivateProfileInt(L"HOME", L"HOME_POS_AXIS_" + axisNum[i], 0, csAppPath);
			m_StopJogMode[i] = GetPrivateProfileInt(L"JOG_SPEED", L"JOG_STOP_MODE_" + axisNum[i], 0, csAppPath);
			m_SpeedAdjustPeriod[i] = GetPrivateProfileInt(L"SET_POSITION_SPEED", L"SPEED_ADJUSTMENT_" + axisNum[i], 1, csAppPath);
			m_SpeedMax[i] = GetPrivateProfileInt(L"SET_POSITION_SPEED", L"SPEED_RUNMAX_" + axisNum[i], 150, csAppPath);
		}
		for (int i = 0; i < 5; i++)
		{
			for (int j = 1; j < 5; j++)
			{
				GetPrivateProfileString(L"JOG_SPEED", L"JOG_DRIVESPEED_" + strGear[i] + axisNum[j], L"10", temp.GetBufferSetLength(50), 10, csAppPath);
				float speed = (atof(T2A(temp)));
				//m_JogDriveSpeed[j][i] = GetPrivateProfileInt(L"JOG_SPEED", L"JOG_DRIVESPEED_" , 10, csAppPath);
				m_JogDriveSpeed[j][i] = speed / (m_Resolution[j] * 50);
				GetPrivateProfileString(L"JOG_SPEED", L"JOG_STARTSPEED_" + strGear[i] + axisNum[j], L"10", temp.GetBufferSetLength(50), 10, csAppPath);
				speed = (atof(T2A(temp)));
				//m_JogStartSpeed[j][i] = GetPrivateProfileInt(L"JOG_SPEED", L"JOG_STARTSPEED_" + strGear[i] + axisNum[j], 10, csAppPath);
				m_JogStartSpeed[j][i] = speed / (m_Resolution[j] * 50);
				m_JogAccLine[j][i] = GetPrivateProfileInt(L"JOG_SPEED", L"JOG_LINE_" + strGear[i] + axisNum[j], 10, csAppPath);
				m_JogDecLine[j][i] = GetPrivateProfileInt(L"JOG_SPEED", L"JOG_LINE_" + strGear[i] + axisNum[j], 10, csAppPath);
			}
		}
		m_Jog_Auto_Focus = m_JogDriveSpeed[3][4];
		m_Home_Machine_Axis[1] = GetPrivateProfileInt(L"HOME", L"HOME_MACHINE_AXIS_1", 1, csAppPath);
		m_Home_Machine_Axis[2] = GetPrivateProfileInt(L"HOME", L"HOME_MACHINE_AXIS_2", 1, csAppPath);
		m_Home_Machine_Axis[3] = GetPrivateProfileInt(L"HOME", L"HOME_MACHINE_AXIS_3", 1, csAppPath);
		m_Home_Machine_Axis[4] = GetPrivateProfileInt(L"HOME", L"HOME_MACHINE_AXIS_4", 1, csAppPath);
		m_IsHomeEncPos = GetPrivateProfileInt(L"HOME", L"IS_HOME_ENC_POS", 0, csAppPath);
		m_IsHomePrfPos = GetPrivateProfileInt(L"HOME", L"IS_HOME_PRF_POS", 1, csAppPath);
		m_Set_XYZA_Reserve = GetPrivateProfileInt(L"SET_XYZ", L"SET_XYZ_RESERVE", 2, csAppPath);
		m_setPositionDelay = GetPrivateProfileInt(L"SETPOSITION", L"SETPOSITION_DELAY", 1, csAppPath);
		m_setPositionPrecision = GetPrivateProfileInt(L"SETPOSITION", L"SETPPSITION_PRECISION", 1, csAppPath);
		m_setPositionNum = GetPrivateProfileInt(L"SETPOSITION", L"SETPOSITION_NUMBER", 1, csAppPath);

		//两块四路光源板
		m_isUseAport = GetPrivateProfileInt(L"COMPORT", L"IS_COM_PORT_A", 0, csAppPath);
		m_portAnum = GetPrivateProfileInt(L"COMPORT", L"COM_PORT_A", 0, csAppPath);
		m_isUseBport = GetPrivateProfileInt(L"COMPORT", L"IS_COM_PORT_B", 0, csAppPath);
		m_portBnum = GetPrivateProfileInt(L"COMPORT", L"COM_PORT_B", 0, csAppPath);

		m_iGlueStartSpeed = GetPrivateProfileInt(L"GLUEDISPENSER", L"DISPENSER_STARTSPEED", 0, csAppPath);
		m_iGlueStartSpeed = m_iGlueStartSpeed / (m_Resolution[1] * 500);
		m_iGlueDriveSpeed = GetPrivateProfileInt(L"GLUEDISPENSER", L"DISPENSER_DRIVESPEED", 0, csAppPath);
		m_iGlueDriveSpeed = m_iGlueDriveSpeed / (m_Resolution[1] * 50);
		m_iGlueAccSpeed = GetPrivateProfileInt(L"GLUEDISPENSER", L"DISPENSER_ACCSPEED", 0, csAppPath);

		m_LogIsOpen[0] = GetPrivateProfileInt(L"LOG", L"LOG_IS_OPEN_0", 0, csAppPath);
		g_pLogger->IsEnabledLog = m_LogIsOpen[0] == 1 ? true : false;

		m_iSpeedType = GetPrivateProfileInt(L"SET_SPEED", L"SPEEDTYPE", 0, csAppPath);

		CString comNum[5] = { L"0", L"1", L"2", L"3" };
		for (int i = 0; i < 4; i++)
		{
			GetPrivateProfileString(L"ROCKER_SPEED", L"ROCKER_HSTARTSPEED_" + comNum[i], L"5", temp.GetBufferSetLength(50), 10, csAppPath);
			float speed = (atof(T2A(temp)));
			m_rockerHStartSpeed[i] = speed / (m_Resolution[i] * 50);
			GetPrivateProfileString(L"ROCKER_SPEED", L"ROCKER_HDRIVESPEED_" + comNum[i], L"20", temp.GetBufferSetLength(50), 10, csAppPath);
			speed = (atof(T2A(temp)));
			m_rockerHDriveSpeed[i] = speed / (m_Resolution[i] * 50);
			GetPrivateProfileString(L"ROCKER_SPEED", L"ROCKER_LSTARTSPEED_" + comNum[i], L"2", temp.GetBufferSetLength(50), 10, csAppPath);
			speed = (atof(T2A(temp)));
			m_rockerLStartSpeed[i] = speed / (m_Resolution[i] * 50);
			GetPrivateProfileString(L"ROCKER_SPEED", L"ROCKER_LDRIVESPEED_" + comNum[i], L"10", temp.GetBufferSetLength(50), 10, csAppPath);
			speed = (atof(T2A(temp)));
			m_rockerLDriveSpeed[i] = speed / (m_Resolution[i] * 50);
			GetPrivateProfileString(L"ROCKER_SPEED", L"ROCKER_ASPEED_" + comNum[i], L"100", temp.GetBufferSetLength(50), 10, csAppPath);
			m_rockerASpeed[i] = (atoi(T2A(temp)));
			GetPrivateProfileString(L"ROCKER_SPEED", L"ROCKER_DSPEED_" + comNum[i], L"100", temp.GetBufferSetLength(50), 10, csAppPath);
			m_rockerDSpeed[i] = (atoi(T2A(temp)));
		}
		CString strAxis[10] = { L"0", L"1", L"2", L"3", L"4", L"5", L"6", L"7", L"8", L"9" };
		for (int i = 0; i < 10; i++)
		{
			m_stepPosition_L_speed[i] = GetPrivateProfileInt(L"STEPMOTORPOSITIONINIT", L"StepPositionLSpeed_" + strAxis[i], 1, csAppPath);
			m_stepPosition_L_speed[i] = m_stepPosition_L_speed[i] / (m_Resolution[1] * 50);
			m_stepPosition_H_speed[i] = GetPrivateProfileInt(L"STEPMOTORPOSITIONINIT", L"StepPositionHSpeed_" + strAxis[i], 1, csAppPath);
			m_stepPosition_H_speed[i] = m_stepPosition_H_speed[i] / (m_Resolution[1] * 50);
			m_stepPosition_Load[i] = GetPrivateProfileInt(L"STEPMOTORPOSITIONINIT", L"StepPositionLoad_" + strAxis[i], 1, csAppPath);
			m_stepPosition_Load[i] = m_stepPosition_Load[i] / (m_Resolution[1]);
			m_stepPosition_acc[i] = GetPrivateProfileInt(L"STEPMOTORPOSITIONINIT", L"StepPositionAcc_" + strAxis[i], 1, csAppPath);
		}
		CString SubArea[8] = { L"0", L"1", L"2", L"3", L"4", L"5", L"6", L"7" };
		for (int i = 0; i < 8; i++)
		{
			m_SixEightSubArea[i] = GetPrivateProfileInt(L"SIXEIGHTSUBAREA", L"SIXEIGHT_SUBARE_" + SubArea[i], 0, csAppPath);
		}

	}
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::Load_EF3_Config_Inifile(CString GoogolIniFile)
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		CString csAppPath = GoogolIniFile;
		USES_CONVERSION;
		CString temp;
		if (GetFileAttributes(GoogolIniFile) == -1)
		{
			g_pLogger->SendAndFlushWithTime(L"[Load_EF3_Config_Inifile] EF3_Config.ini file no find\n");
			sEvenProp.Init();
			sEvenProp.EventType = HSI_EVENT_ERROR;
			sEvenProp.EventID = HSI_EVENT_MOTION;
			sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK;
			strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "EF3_Config.ini文件不存在！");
			EventCallback(sEvenProp);
			return HSI_STATUS_FAILED;
		}
		m_IsUseEF3 = GetPrivateProfileInt(L"EF3", L"IS_USEEF3", 0, csAppPath);

		m_ForSoft = GetPrivateProfileInt(L"SOFTWARE", L"USE_SOFTWARE", 0, csAppPath);

		m_IsUse_HSICompensation = GetPrivateProfileInt(L"ASIX", L"IS_USE_HSICOMPENSATION", 0, csAppPath);
		m_Compensation_Pluse = GetPrivateProfileInt(L"ASIX", L"COMPENSATE_PLUSE", 20, csAppPath);
		m_IsHardLimit = GetPrivateProfileInt(L"ASIX", L"IS_HARD_LIMIT", 7, csAppPath);
		m_IsEnableAxis = GetPrivateProfileInt(L"ASIX", L"ISENABLE_AXIS", 7, csAppPath);
		m_IsHavePattern = GetPrivateProfileInt(L"ASIX", L"ISHAVE_AXISPATTERN", 15, csAppPath);
		m_AxisHomeDirection = GetPrivateProfileInt(L"ASIX", L"AXIS_HOMEDIRECTION", 15, csAppPath);
		m_IsUseManualRunin = GetPrivateProfileInt(L"ASIX", L"IS_USEMANUALRUNIN", 0, csAppPath);

		m_IsUseExternalTrigger = GetPrivateProfileInt(L"FUNCTION", L"IS_USEEXTERNALTRIGGER", 1, csAppPath);
		m_IsUseSixRingEightArea = GetPrivateProfileInt(L"FUNCTION", L"IS_USESIXRINGEIGHTAREA", 0, csAppPath);
		m_IsUseTwentySixLight = GetPrivateProfileInt(L"FUNCTION", L"IS_USETWENTYSIXLIGHT", 0, csAppPath);
		m_EF3LightType = GetPrivateProfileInt(L"FUNCTION", L"IS_LIGHTTYPE", 1, csAppPath);
		m_IsCollectPos = GetPrivateProfileInt(L"FUNCTION", L"IS_COLLECT_POS", 0, csAppPath);
    m_IsLightDebug = GetPrivateProfileInt(L"FUNCTION", L"IS_LIGHT_DEBUG", 0, csAppPath);

		m_IsIOFuntion = GetPrivateProfileInt(L"IO", L"IS_IO_FUNTION", 0, csAppPath);
		m_IsStartInput = GetPrivateProfileInt(L"IO", L"IS_START_INPUT", 0, csAppPath);
		m_StartInputPort = GetPrivateProfileInt(L"IO", L"START_INPUT_PORT", 1, csAppPath);

		m_IsProbe = GetPrivateProfileInt(L"PROBE", L"IS_PROBE", 0, csAppPath);
		m_ProbeAllAxis = GetPrivateProfileInt(L"PROBE", L"PROBE_All_AXIS", 3, csAppPath);
		m_ProbeReturnSpeed = GetPrivateProfileInt(L"PROBE", L"PROBE_RETURN_SPEED", 40, csAppPath);
		m_ProbeReturnSpeed = m_ProbeReturnSpeed * 50;
		GetPrivateProfileString(L"PROBE", L"PROBE_RETURN_POS", L"10.0", temp.GetBufferSetLength(50), 50, csAppPath);
		m_ProbeReturnPos = atof(T2A(temp));

		m_IsUseRocker = GetPrivateProfileInt(L"ROCKER", L"IS_USEROCKER", 0, csAppPath);
		m_IsCloseRocker = GetPrivateProfileInt(L"ROCKER", L"IS_CLOSEROCKER", 0, csAppPath);

		m_bISUseMoreLights = GetPrivateProfileInt(L"IPADDRESS", L"IS_USENUM", 0, csAppPath);

		m_LightType = GetPrivateProfileInt(L"LIGHTCONTROL", L"IS_LIGHTTYPE", 1, csAppPath);
		m_IsUseFourthSpeed = GetPrivateProfileInt(L"FOURTHSPEED", L"IS_USEFOURTHSPEED", 1, csAppPath);
		m_isOKGlint = GetPrivateProfileInt(L"LIGHTCONTROL", L"IS_OKGLINT", 0, csAppPath);
		m_ETIPort = GetPrivateProfileInt(L"LIGHTCONTROL", L"IS_ETIPORT", 0, csAppPath);

		m_DeviceType = GetPrivateProfileInt(L"DEVICE", L"DEVICE_TYPE", 1, csAppPath);
		m_UseAxisNum = GetPrivateProfileInt(L"DEVICE", L"USE_AXISNUM", 0, csAppPath);
		m_iJoyStick = GetPrivateProfileInt(L"JOYSTICK", L"JOY_STICK_TYPE", 0, csAppPath);

		CString comNum[5] = { L"0", L"1", L"2", L"3" };
		for (int i = 0; i < 4; i++)
		{
			GetPrivateProfileString(L"IPADDRESS", L"IP_ADDRESS_" + comNum[i], L"192.168.0.1", temp.GetBufferSetLength(50), 50, csAppPath);
			m_IsOpenTCPIP[i] = temp;
			temp = temp.Trim();
			int iNext = temp.Find(':');
			CString ss = temp.Mid(0, iNext);
			if (ss == "8")
			{
				m_Led8MotionFlag[i] = true;
			}
			m_IsOpenTCPIP[i] = temp.Mid(iNext + 1, temp.GetLength() - iNext);
		}

		m_EF3COMPort = GetPrivateProfileInt(L"EF3COM", L"COMPORT", 0, csAppPath);
	}
	return rStatus;
}
//===============================读取/设置光栅尺精度============================================
HSI_STATUS HSI_Motion::GetScaleResolution(double &_ScaleX, double &_ScaleY, double &_ScaleZ)
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		_ScaleX = m_Resolution[1];
		_ScaleY = m_Resolution[2];
		_ScaleZ = m_Resolution[3];
	}
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::SetScaleResolution(double _ScaleX, double _ScaleY, double _ScaleZ)
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		m_Resolution[1] = (int)(_ScaleX * 10000);
		m_Resolution[2] = (int)(_ScaleY * 10000);
		m_Resolution[3] = (int)(_ScaleZ * 10000);
	}
	return rStatus;
}
//============================回调定位完成===============================================
void HSI_Motion::SendMsgMotionFinished()
{
	sEvenProp.Init();
	sEvenProp.EventType = HSI_EVENT_FUNCTION;
	sEvenProp.EventID = HSI_EVENT_MOVE_POINT;
	sEvenProp.EventResponse = HSI_EVENT_FUNCTION_OK;
	EventCallback(sEvenProp);
}

//=============================回调探针运行==============================================
void HSI_Motion::SendMsgProbeFinished()
{
	sEvenProp.Init();
	sEvenProp.EventType = HSI_EVENT_FUNCTION;
	sEvenProp.EventID = HSI_EVENT_MOTION_PROBE;
	sEvenProp.EventResponse = HSI_EVENT_FUNCTION_OK;
	EventCallback(sEvenProp);
}
//===========================================================================
void HSI_Motion::UpdateMotionState()
{
	if (g_pHSI_Motion && m_IsExMotion == 0)
	{
		g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] In\n");
		while (m_Thread_State == HSI_THREAD_RUNNING)
		{
			g_IsClose = false;
			bool interpolationflag = false;
			bool timeoutflag = false;
			int timeStart = GetTickCount();
			//	bool singleaxisflag_x = false;
			//	bool singleaxisflag_y = false;
			//	bool singleaxisflag_z = false;
			//	bool singleaxisflag_a = false;
			do
			{
				if (g_IsClose)
				{
					g_IsClose = false;
					break;
				}
				if (m_SO7_Serial.m_RecvData[0] == 2)
				{
					if ((m_SO7_Serial.m_RecvData[39] & 0x10))
					{
						m_SO7_Serial.m_RecvData[39] = 0;
						SpCompleteTStart = GetTickCount();
						interpolationflag = true;
						break;
					}
					//else if ((m_SO7_Serial.m_RecvData[39] == 0x07))//axis_start
					else if ((m_SO7_Serial.m_RecvData[39] == axis_start)/* &&(m_motorType == 0)*/)
					{
						m_SO7_Serial.m_RecvData[39] = 0;
						SpCompleteTStart = GetTickCount();
						interpolationflag = true;
						break;
					}
					else if (m_SO7_Serial.m_RecvData[39] & 0x20)
					{
						m_SO7_Serial.m_RecvData[39] = 0;
						timeoutflag = true;
						break;
					}
				}
				Sleep(1);
				int timeEnd = GetTickCount();
				if (timeStart - timeEnd > 10 * 1000)
				{
					timeoutflag = true;
					break;
				}
			} while (true);
			g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Nowait Run End\n");
			unsigned int tempPrecision[5] = { 0, m_precisionCount[1], m_precisionCount[2], m_precisionCount[3], m_precisionCount[4] };
			tempPrecision[1] = m_Home_Machine_Axis[1] == 0 ? 10000000 : m_precisionCount[1];
			tempPrecision[2] = m_Home_Machine_Axis[2] == 0 ? 10000000 : m_precisionCount[2];
			tempPrecision[3] = m_Home_Machine_Axis[3] == 0 ? 10000000 : m_precisionCount[3];
			tempPrecision[4] = m_Home_Machine_Axis[4] == 0 ? 10000000 : m_precisionCount[4];
			int i = 0, j = 0;
			unsigned long Count = 0;
			double prfpos[5] = { 0 };

			//GetPositionEncPrfMulti(1, m_EncPos, m_PrfPos, 4);
			//double EncPos[5] = { 0.0 };
			// double ProPulse[5] = { 0.0 };
			if (interpolationflag&&m_motorType)
			{
				while (Count < m_SetPotion_Count[1])//到位次数判断
				{
					Sleep(2);
					GetPositionXyz(HSI_MOTION_AXIS_ALL, prfpos[1], prfpos[2], prfpos[3], prfpos[0]);
					if ((fabs(m_PosThread[1] - prfpos[1]) <= tempPrecision[1] * m_Resolution[1]) && (fabs(m_PosThread[2] - prfpos[2]) <= tempPrecision[2] * m_Resolution[2]) && (fabs(m_PosThread[3] - prfpos[3]) <= tempPrecision[3] * m_Resolution[3]) && (fabs(m_PosThread[4] - m_PosForAllAxis[4]) <= tempPrecision[4] * m_Resolution[4]))
					{
						i++;
						if (m_SetPotion_Count[1] > m_setPositionNum)
						{
							j = m_setPositionNum;
						}
						else
						{
							j = m_SetPotion_Count[1];
						}
						if (i == j)
						{
							set_end = GetTickCount();
							break;
						}
					}
					else
					{
					}
					Count++;
					g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] m_SetPotion_Count = %d\n", Count);
				}
				//if (Count == m_SetPotion_Count[1]) //超时退出
				//{
				//	if (g_IsClose == false)
				//	{
				//		g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Nowait Timeout\n");
				//		sEvenProp.Init();
				//		sEvenProp.EventType = HSI_EVENT_ERROR;
				//		sEvenProp.EventID = HSI_EVENT_MOTION;
				//		sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK;
				//		strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "Nowait_HSI定位超时!");
				//		EventCallback(sEvenProp);
				//	}
				//	else
				//	{
				//		g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Nowait Timeout Is AbortMotion\n");
				//	}
				//}
			}
			if (interpolationflag)
			{
				SpCompleteTEnd = GetTickCount();
				SpTimeCount = SpCompleteTEnd - SpCompleteTStart;
				double dis = pow(m_PrfPos[1] - m_EncPos[1], 2.0) + pow(m_PrfPos[2] - m_EncPos[2], 2.0) + pow(m_PrfPos[3] - m_EncPos[3], 2.0);
				if (dis > 0)
				{
					PntToPntDistance = sqrt(dis);
				}
				else
				{
					PntToPntDistance = 0.0;
				}
				SetPotionRunEnd = true;
			}
			g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] m_PosThread[1] = %.4f,m_PosThread[2] = %.4f,m_PosThread[3] = %.4f\n", m_PosThread[1], m_PosThread[2], m_PosThread[3]);
			g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Pos[1] = %.4f,Pos[2] = %.4f,Pos[3] = %.4f\n", prfpos[1], prfpos[2], prfpos[3]);
			if (timeoutflag)
			{
				g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Nowait Timeout\n");
				sEvenProp.Init();
				sEvenProp.EventType = HSI_EVENT_ERROR;
				sEvenProp.EventID = HSI_EVENT_MOTION;
				sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK;
				strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "Nowait_EF3定位超时!");
				EventCallback(sEvenProp);
			}
			switch (CurrentMotionState)
			{
				case E_SO7_MOTION_MOVETO:
				{
					g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Nowait CurrentMotionState E_SO7_MOTION_MOVETO\n");
					m_Thread_State = HSI_THREAD_PAUSED;
					CurrentMotionState = E_SO7_MOTION_NONE;
					m_IsExMotion = 2;
					SendMsgMotionFinished();
					break;
				}
				default:
				{
					g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Nowait CurrentMotionState default\n");
					break;
				}
			}
		}
		g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Out\n");
	}
}
//===========================================================================
void HSI_Motion::UpdateMotionStateEx()
{
	if (g_pHSI_Motion && m_IsExMotion == 1)
	{
		g_pLogger->SendAndFlushWithTime(L"[UpdateMotionStateEx] In\n");

		g_pLogger->SendAndFlushWithTime(L"[UpdateMotionStateEx] Out\n");
	}
}
//===========================================================================
void HSI_Motion::UpdateMotionStateIO()
{
	if (g_pHSI_Motion)
	{
		int StartFlag = 0;
		UINT recvData = 0;
		while (m_Thread_StateIO == HSI_THREAD_RUNNING)
		{
			//1个脚踏开关
			Sleep(3);
			GetDIO(HSI_MOTION_INPUT_CH1, m_InputStatus);
			if (m_IsStartInput == 1)
			{
				if ((m_InputStatus & m_StartInputPort) == 0)
				{
					StartFlag = 1;
				}
				else if (StartFlag == 1 && (m_InputStatus & m_StartInputPort) == m_StartInputPort)
				{
					if (m_isOKGlint == 1)
					{
						memset(m_cSendData, 0x00, 63);
						m_cSendData[0] = 0x02;
						m_cSendData[1] = 0x02;
						m_cSendData[2] = 0x01;
						m_cSendData[6] = 0x04;
						m_cSendData[7] = 0xff;
						m_cSendData[8] = 0xff;
						m_cSendData[9] = 0xff;
						m_cSendData[10] = 0xff;
						m_cSendData[14] = 0xff;
						m_WriteByte = Send_Command(1, (const char*)m_cSendData, m_SendDataLength);
						Sleep(5);
					}
					g_pLogger->SendAndFlushWithTime(L"[UpdateMotionStateIO] One Foot Switch Trigger\n");
					StartFlag = 0;
					sEvenProp.Init();
					sEvenProp.EventType = HSI_EVENT_NOTIFY;
					sEvenProp.EventID = HSI_NOTIFY_PROGRAM_EXECUTION_START;
					sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK;
					EventCallback(sEvenProp);
				}
			}
			if ((m_axisStatus & 0xAA) == 0xAA)
			{
				if (!m_bEmergencyState)
				{
					Sleep(100);
					GetDIO(HSI_MOTION_INPUT_CH1, m_InputStatus);
					if ((m_axisStatus & 0xAA) == 0xAA)
					{
						sEvenProp.Init();
						sEvenProp.EventType = HSI_EVENT_NOTIFY;
						sEvenProp.EventID = HSI_NOTIFY_EMERGENCY_STATE;
						sEvenProp.EventData[0] = 1;
						m_bEmergencyState = true;
						EventCallback(sEvenProp);
					}
				}
			}
			else
			{
				if (m_bEmergencyState)
				{
					Sleep(100);
					GetDIO(HSI_MOTION_INPUT_CH1, m_InputStatus);
					if ((m_axisStatus & 0xAA) != 0xAA)
					{
						sEvenProp.Init();
						sEvenProp.EventType = HSI_EVENT_NOTIFY;
						sEvenProp.EventID = HSI_NOTIFY_EMERGENCY_STATE;
						sEvenProp.EventData[0] = 0;
						m_bEmergencyState = false;
						EventCallback(sEvenProp);
					}
				}
			}
		}
	}
}
//===========================================================================
void HSI_Motion::UpdateMotionStateData()
{
	while (m_Thread_StateData == HSI_THREAD_RUNNING)
	{
		if (m_bConnected)
		{
			Sleep(3);
			m_cSendData[0] = CT_ORDER;
			m_cSendData[1] = MOTORPOSITION;
			Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
		}
		else
		{

		}

	}
}
//===========================================================================
void HSI_Motion::UpdateMotionStateProbe()
{
	if (g_pHSI_Motion)
	{
		bool hitFlag = false;
		while (m_Thread_StateProbe == HSI_THREAD_RUNNING)
		{
			Sleep(3);
			if (g_IsClose)
			{
				g_IsClose = false;
				break;
			}
			if (m_SO7_Serial.m_RecvData[57] == 0)
			{
				hitFlag = true;
			}
			else if (m_SO7_Serial.m_RecvData[57] == 1)
			{
				//AbortMotion();
				//Beep(300,1000);
				//printf("\a");
				//	m_SO7_Serial.m_RecvData[57] = 0;
				m_LockPos[1] = (m_SO7_Serial.m_RecvData[41] << 24 | m_SO7_Serial.m_RecvData[42] << 16 | m_SO7_Serial.m_RecvData[43] << 8 | m_SO7_Serial.m_RecvData[44])*m_Resolution[1];
				m_LockPos[2] = (m_SO7_Serial.m_RecvData[45] << 24 | m_SO7_Serial.m_RecvData[46] << 16 | m_SO7_Serial.m_RecvData[47] << 8 | m_SO7_Serial.m_RecvData[48])*m_Resolution[2];
				m_LockPos[3] = (m_SO7_Serial.m_RecvData[49] << 24 | m_SO7_Serial.m_RecvData[50] << 16 | m_SO7_Serial.m_RecvData[51] << 8 | m_SO7_Serial.m_RecvData[52])*m_Resolution[3];
				m_LockPos[4] = (m_SO7_Serial.m_RecvData[53] << 24 | m_SO7_Serial.m_RecvData[54] << 16 | m_SO7_Serial.m_RecvData[55] << 8 | m_SO7_Serial.m_RecvData[56])*m_Resolution[4];
				continue;
			}
			else if (m_SO7_Serial.m_RecvData[57] == 2)
			{
				Sleep(400);
				m_LockPos[1] = 0.0;
				m_LockPos[2] = 0.0;
				m_LockPos[3] = 0.0;
				m_LockPos[4] = 0.0;
				//m_SO7_Serial.m_RecvData[57] == 0;
				continue;
			}
			else if (m_SO7_Serial.m_RecvData[57] == 3 && hitFlag)
			{
				Sleep(200);
				hitFlag = false;
				double ProbeHitCompletePos[4] = { 0 };
				GetPositionXyz(0, ProbeHitCompletePos[0], ProbeHitCompletePos[1], ProbeHitCompletePos[2], ProbeHitCompletePos[3]);
				m_ijk[0] = ProbeHitCompletePos[0];
				m_ijk[1] = ProbeHitCompletePos[1];
				m_ijk[2] = ProbeHitCompletePos[2];
				g_pLogger->SendAndFlushWithTime(L"[UpdateMotionStateProbe] Probe hit\n");
				SendMsgProbeFinished();
			}
		}
	}
}
//===========================================================================
void HSI_Motion::DoEvents()
{
	MSG msg;
	while (PeekMessage(&msg, NULL, 0, 0, PM_REMOVE))
	{
		__try
		{
			DispatchMessage(&msg);
			TranslateMessage(&msg);
		}
		__except (EXCEPTION_EXECUTE_HANDLER)
		{
			g_pLogger->SendAndFlushWithTime(msg.message + L"----------DoEvents---------- : Exception");
			sEvenProp.Init();
			sEvenProp.EventType = HSI_EVENT_ERROR;
			sEvenProp.EventID = HSI_EVENT_MOTION;
			sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK;
			strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "DoEvents_异常");
			EventCallback(sEvenProp);
		}
		if (g_IsClose)
		{
			break;
		}
	}
}
//===========================================================================
HSI_STATUS HSI_Motion::DriverAlarmStatus()
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		g_pLogger->SendAndFlushWithTime(L"[DriverAlarmStatus] In\n");

		g_pLogger->SendAndFlushWithTime(L"[DriverAlarmStatus] Out\n");
	}
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::FirstHome()
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		g_pLogger->SendAndFlushWithTime(L"[FirstHome] In\n");

		g_pLogger->SendAndFlushWithTime(L"[FirstHome] Out\n");
	}
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::SpecialMotorHome(short AxisNum)
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		g_pLogger->SendAndFlushWithTime(L"[SpecialMotorHome] In\n");

		g_pLogger->SendAndFlushWithTime(L"[SpecialMotorHome] Out\n");
	}
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::SpecialMotorMove(short AxisNumber, double Position)
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		g_pLogger->SendAndFlushWithTime(L"[SpecialMotorMove] In\n");
		g_pLogger->SendAndFlushWithTime(L"[SpecialMotorMove] Out\n");
	}
	return rStatus;
}

//========================IO===================================================
HSI_STATUS HSI_Motion::GetDIO(UINT IOChannel, UINT& _Status)
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		if (m_SO7_Serial.m_RecvData[0] == 2)
		{
			if (IOChannel == HSI_MOTION_INPUT_LIMIT_SWITCH)
			{
				_Status = m_SO7_Serial.m_RecvData[33];
			}
			if (IOChannel == HSI_MOTION_INPUT_ALARM)
			{
				_Status = m_SO7_Serial.m_RecvData[40];
			}
			if (IOChannel == HSI_MOTION_INPUT_CH1) //获取通用输入
			{
				_Status = m_SO7_Serial.m_RecvData[34];
				_Status = (m_SO7_Serial.m_RecvData[35] | (_Status << 8)) & 0xffff;
			}
			if (IOChannel == HSI_MOTION_OUTPUT_CH1) //获取通用输出
			{
				_Status = m_SO7_Serial.m_RecvData[36];
				_Status = (m_SO7_Serial.m_RecvData[37] | (_Status << 8)) & 0xffff;
			}
			m_axisStatus = m_SO7_Serial.m_RecvData[58];
			m_axisAlarmStatus = m_SO7_Serial.m_RecvData[59];
		}
		else if (m_bISUseMoreLights > 0)
		{
			if (m_Led8MotionFlag[m_selectedIndex])
			{
				if (tReciveData[0] == 0x05)
				{
					_Status = tReciveData[2];
				}
			}
			else
			{
				if (tReciveData[0] == 0x05)
				{
					_Status = tReciveData[1] << 24 | tReciveData[1] << 16 | tReciveData[3] << 8 | tReciveData[4];
				}
			}
		}
		else
		{
			g_pLogger->SendAndFlushWithTime(L"[GetDIO] failed\n");
		}
	}
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::SetDIO(UINT IOChannel, UINT _Status)
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		if (m_bISUseMoreLights > 0 && (m_ForStatus != _Status))
		{
			if (m_Led8MotionFlag[m_selectedIndex])
			{
				m_ForStatus = _Status;
				IOdata[0] = 0x02;
				IOdata[1] = 0x02;
				IOdata[2] = 0x01;
				IOdata[6] = 0x04;
				IOdata[7] = 0xff;
				IOdata[8] = 0xff;
				IOdata[9] = 0xff;
				IOdata[10] = 0xff;
				IOdata[14] = _Status & 0xff;
				IOSend++;
			}
			else
			{
				m_ForStatus = _Status;
				IOdata[0] = 0x02;
				IOdata[1] = 0x02;
				IOdata[2] = 0x01;
				IOdata[3] = 0x36;
				IOdata[4] = 0x04;
				IOdata[7] = 0xff;
				IOdata[8] = 0xff;
				IOdata[9] = 0xff;
				IOdata[10] = 0xff;
				IOdata[11] = (_Status >> 24) & 0xff;
				IOdata[12] = (_Status >> 16) & 0xff;
				IOdata[13] = (_Status >> 8) & 0xff;
				IOdata[14] = _Status & 0xff;
				IOSend++;
			}
		}
		if ((IOChannel == HSI_MOTION_OUTPUT_CH1)/* && (m_ForStatus != _Status)*/)
		{
			m_ForStatus = _Status;
			m_cSendData[0] = CT_PORT;
			m_cSendData[1] = 0x02;
			m_cSendData[2] = (_Status >> 8) & 0xff;
			m_cSendData[3] = _Status & 0xff;
			m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
			Sleep(5);
		}
	}
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::GetAxisStatus(int* _Status)
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		_Status[0] = m_axisStatus;
		_Status[1] = m_axisAlarmStatus;
	}
	return rStatus;
}

//=============================暂停和关闭==============================================
HSI_STATUS HSI_Motion::AbortMotion()
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		g_IsClose = true;
		g_pLogger->SendAndFlushWithTime(L"[AbortMotion] In\n");
		m_cSendData[0] = CT_ORDER;
		m_cSendData[1] = CT_STOP;
		m_cSendData[2] = AXIS_XYZU;
		m_StopJogMode[1] == 0 ? m_cSendData[3] = 0X4A : m_cSendData[3] = 0X05;
		m_StopJogMode[2] == 0 ? m_cSendData[3] = 0X4A : m_cSendData[3] = 0X05;
		m_StopJogMode[3] == 0 ? m_cSendData[3] = 0X4A : m_cSendData[3] = 0X05;
		m_StopJogMode[4] == 0 ? m_cSendData[3] = 0X4A : m_cSendData[3] = 0X05;
		m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
		Sleep(5);

		if (m_DeviceType == 3)
		{
			m_cSendData[0] = CT_TURNTABLE;
			m_cSendData[1] = CT_RTSTOP;
			m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
			Sleep(5);
			m_Thread_StateData = HSI_THREAD_RUNNING;
			SetEvent(m_hTriggerEventData);
		}
		if (bUseGlueDispenser)
		{
			bUseGlueDispenser = false;
			m_cSendData[0] = CT_MOTOR;
			m_cSendData[1] = CT_GLUEDISPENSER_STOP;
			m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
			Sleep(5);
		}
		g_pLogger->SendAndFlushWithTime(L"[AbortMotion] Out\n");
	}
	return rStatus;
}
//===============================关闭============================================
HSI_STATUS HSI_Motion::Shutdown()
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		g_pLogger->SendAndFlushWithTime(L"[Shutdown] In\n");
		g_IsClose = true;
		SetDIO(HSI_MOTION_OUTPUT_CH1, 0xffff);
		if (m_bConnected)
		{
			CloseHandle(hCom);
			hCom = nullptr;
			//m_SO7_Serial.Close();
		}
		if (m_IsCloseRocker == 0)
		{
			memset(m_cSendData, 0x00, 64);
			m_cSendData[0] = CT_SOFTSTOP;
			m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
			Sleep(5);
		}

		if (m_bISUseMoreLights > 0)
		{
			for (size_t i = 0; i < m_bISUseMoreLights; i++)
			{
				if (m_Led8MotionFlag[i])
				{
					m_selectedIndex = i;
					Orderdata[0] = 0x01;
					Orderdata[1] = 0x07;
					Orderdata[2] = 0x00;
					OrderSend++;
				}
				Sleep(20);
			}
			DisConnect();
		}

		AbortMotion();
		m_bConnected = false;
		CurrentMotionState = E_SO7_MOTION_NONE;
		CurrentHomeMachineState = E_EF3_HOME_NONE;
		m_Thread_StateData = HSI_THREAD_EXIT;
		m_Thread_StateData = HSI_THREAD_EXIT;
		m_Thread_StateIO = HSI_THREAD_EXIT;
		m_Thread_State = HSI_THREAD_EXIT;
		m_Thread_StateJOGStop = HSI_THREAD_EXIT;
		SendMsgMotionFinished();
		CloseThread();
		CloseThreadIO();
		CloseThreadData();
		CloseThreadProbe();
		CloseThreadJOGStop();
		ReleaseMutex(g_RW_Data_Mutex);
		CloseHandle(g_RW_Data_Mutex);
		ReleaseMutex(g_WR_ToMove_Mutex);
		CloseHandle(g_WR_ToMove_Mutex);
		g_pLogger->SendAndFlushWithTime(L"[Shutdown] Out\n");
	}
	return rStatus;
}

//==============================触发灯光=============================================
HSI_STATUS HSI_Motion::SetTriggerLight(int triggleNum, int delayLighting, int delayLightBefor, int triggleMode, double* Intensities)
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		setLightFlag = true;
		unsigned char m_SetTriggerLightData[64] = { 0 };
		m_SetTriggerLightData[0] = CT_LIGHT;
		m_SetTriggerLightData[1] = 0x03;
		m_SetTriggerLightData[2] = triggleNum;
		int num = triggleNum / 3;
		int i = 0;
		for (i = 0; i < num; i++)
		{
			m_SetTriggerLightData[3] = i;
			int z = 4;
			for (int j = 24 * i; j < 24 * (i + 1); ++j)
			{
				m_SetTriggerLightData[z++] = ((int(Intensities[j] * 11.2)) >> 8) & 0xff;
				m_SetTriggerLightData[z++] = (int(Intensities[j] * 11.2)) & 0xff;
			}
			m_SetTriggerLightData[z++] = delayLighting & 0xff;
			m_SetTriggerLightData[z++] = triggleMode & 0xff;
			m_SetTriggerLightData[z] = delayLightBefor & 0xff;
			m_WriteByte = Send_Command(0, (const char*)m_SetTriggerLightData, m_SendDataLength);
			Sleep(1);
		}
		if ((num > 0) && (triggleNum % 3 != 0))
		{
			m_SetTriggerLightData[3] = i;
			int z = 4;
			for (int j = 24 * i; j < 8 * triggleNum; ++j)
			{
				m_SetTriggerLightData[z++] = ((int(Intensities[j] * 11.2)) >> 8) & 0xff;
				m_SetTriggerLightData[z++] = (int(Intensities[j] * 11.2)) & 0xff;
			}
			m_SetTriggerLightData[52] = delayLighting & 0xff;
			m_SetTriggerLightData[53] = triggleMode & 0xff;
			m_SetTriggerLightData[54] = delayLightBefor & 0xff;
			m_WriteByte = Send_Command(0, (const char*)m_SetTriggerLightData, m_SendDataLength);
			Sleep(1);
		}
		else if (num == 0)
		{
			m_SetTriggerLightData[3] = 0;
			int z = 4;
			for (int j = 0; j < 8 * triggleNum; ++j)
			{
				m_SetTriggerLightData[z++] = ((int(Intensities[j] * 11.2)) >> 8) & 0xff;
				m_SetTriggerLightData[z++] = (int(Intensities[j] * 11.2)) & 0xff;
			}
			m_SetTriggerLightData[52] = delayLighting & 0xff;
			m_SetTriggerLightData[53] = triggleMode & 0xff;
			m_SetTriggerLightData[54] = delayLightBefor & 0xff;
			m_WriteByte = Send_Command(0, (const char*)m_SetTriggerLightData, m_SendDataLength);
			Sleep(1);
		}
	}
	return rStatus;
}

//=============================硬件触发拍照==============================================
HSI_STATUS HSI_Motion::DCCPPStartPoint(double *startPoint)
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		m_IsUsePPS = true;
		begin_position[1] = (int)(startPoint[1] / m_Resolution[1]);
		begin_position[2] = (int)(startPoint[2] / m_Resolution[2]);
		begin_position[3] = (int)(startPoint[3] / m_Resolution[3]);
	}
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::DCCScanSetData(UINT AxisTypes, HSI_SCAN_MOTION_TYPE eType, UINT lTrigNumber, double* dTrigDis)
{
	WaitForSingleObject(g_WR_ToMove_Mutex, INFINITE);
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		//触发的位置为相对位置,用法一般是移动到起点位置，再开始设置触发位置(相对位置)，最终设置终点位置
		g_pLogger->SendAndFlushWithTime(L"[DCCScanSetData] In\n");
		int axisNum;
		unsigned char m_SendDCCData[64] = { 0 };
		m_SendDCCData[0] = CT_MOTOR;
		m_SendDCCData[1] = CT_TRIGGER_DATA;
		m_SendDCCData[2] = 0x01;
		iTriggleNum = lTrigNumber;
		double limit = 0.0;
		//memset(m_cSendData,0,64);
		if (AxisTypes & HSI_MOTION_AXIS_X)
		{
			iaxisNum = AXIS_X;
			axisNum = 1;
		}
		else if (AxisTypes & HSI_MOTION_AXIS_Y)
		{
			iaxisNum = AXIS_Y;
			axisNum = 2;
		}
		else if (AxisTypes & HSI_MOTION_AXIS_Z)
		{
			iaxisNum = AXIS_Z;
			axisNum = 3;
		}
		else
		{
			iaxisNum = AXIS_X;
			axisNum = 1;
		}
		switch (eType)
		{
		case HSI_SCAN_MOTION_SPEC_LOCA:
		{
			if (dTrigDis[0] > 0.0001)
			{
				iMotionDirection = 0;
			}
			else
			{
				iMotionDirection = 1;
			}

			switch (iaxisNum)
			{
			case AXIS_X:
			{
				if (!m_IsUsePPS)
				{
					begin_position[1] = begin_position[1] + dTrigDis[0] / m_Resolution[axisNum];
				}
				else
				{
					if (iMotionDirection == 0)
					{
						begin_position[1] = begin_position[1] - 20;
					}
					else
					{
						begin_position[1] = begin_position[1] + 20;
					}
				}
				if (iMotionDirection == 0)
				{
					limit = 1000.0;
				}
				else
				{
					limit = -1000.0;
				}
				break;
			}
			case AXIS_Y:
			{
				if (!m_IsUsePPS)
				{
					begin_position[2] = begin_position[2] + dTrigDis[0] / m_Resolution[axisNum];
				}
				else
				{
					if (iMotionDirection == 0)
					{
						begin_position[2] = begin_position[2] - 20;
					}
					else
					{
						begin_position[2] = begin_position[2] + 20;
					}
				}
				if (iMotionDirection == 0)
				{
					limit = 1000.0;
				}
				else
				{
					limit = -1000.0;
				}
				break;
			}
			case AXIS_Z:
			{
				if (!m_IsUsePPS)
				{
					begin_position[3] = begin_position[3] + dTrigDis[0] / m_Resolution[axisNum];
				}
				else
				{
					if (iMotionDirection == 0)
					{
						begin_position[3] = begin_position[3] - 20;
					}
					else
					{
						begin_position[3] = begin_position[3] + 20;
					}
				}
				if (iMotionDirection == 0)
				{
					limit = 1000.0;
				}
				else
				{
					limit = -1000.0;
				}
				break;
			}
			default:
				break;
			}

			iScanMotionType = HSI_SCAN_MOTION_SPEC_LOCA;
			if (lTrigNumber > 100)
			{
				return HSI_STATUS_FAILED;
			}
			else
			{
				int num = lTrigNumber / 14;
				int i = 0;
				for (i = 0; i < num; i++)
				{
					m_SendDCCData[3] = i;
					for (size_t j = 14 * i, z = 4; j < 14 * (i + 1); ++j)
					{
						m_SendDCCData[z++] = ((long)(dTrigDis[j] / m_Resolution[axisNum])) & 0xff;
						m_SendDCCData[z++] = (((long)(dTrigDis[j] / m_Resolution[axisNum])) >> 8) & 0xff;
						m_SendDCCData[z++] = (((long)(dTrigDis[j] / m_Resolution[axisNum])) >> 16) & 0xff;
						m_SendDCCData[z++] = ((long)(dTrigDis[j] / m_Resolution[axisNum])) >> 24;
					}
					m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
					Sleep(1);
				}
				if ((num > 0) && (lTrigNumber % 14 != 0))
				{
					m_SendDCCData[3] = i;
					int j = 0, z = 0;
					for (j = 14 * i, z = 4; j < lTrigNumber; ++j)
					{
						m_SendDCCData[z++] = ((long)(dTrigDis[j] / m_Resolution[axisNum])) & 0xff;
						m_SendDCCData[z++] = (((long)(dTrigDis[j] / m_Resolution[axisNum])) >> 8) & 0xff;
						m_SendDCCData[z++] = (((long)(dTrigDis[j] / m_Resolution[axisNum])) >> 16) & 0xff;
						m_SendDCCData[z++] = ((long)(dTrigDis[j] / m_Resolution[axisNum])) >> 24;
					}
					m_SendDCCData[z++] = ((long)(limit / m_Resolution[axisNum])) & 0xff;
					m_SendDCCData[z++] = (((long)(limit / m_Resolution[axisNum])) >> 8) & 0xff;
					m_SendDCCData[z++] = (((long)(limit / m_Resolution[axisNum])) >> 16) & 0xff;
					m_SendDCCData[z++] = ((long)(limit / m_Resolution[axisNum])) >> 24;
					m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
					Sleep(10);
				}
				else if (num == 0)
				{
					m_SendDCCData[3] = 0;
					int j = 0, z = 0;
					for (j = 0, z = 4; j < lTrigNumber; ++j)
					{
						m_SendDCCData[z++] = ((long)(dTrigDis[j] / m_Resolution[axisNum])) & 0xff;
						m_SendDCCData[z++] = (((long)(dTrigDis[j] / m_Resolution[axisNum])) >> 8) & 0xff;
						m_SendDCCData[z++] = (((long)(dTrigDis[j] / m_Resolution[axisNum])) >> 16) & 0xff;
						m_SendDCCData[z++] = ((long)(dTrigDis[j] / m_Resolution[axisNum])) >> 24;
					}
					m_SendDCCData[z++] = ((long)(limit / m_Resolution[axisNum])) & 0xff;
					m_SendDCCData[z++] = (((long)(limit / m_Resolution[axisNum])) >> 8) & 0xff;
					m_SendDCCData[z++] = (((long)(limit / m_Resolution[axisNum])) >> 16) & 0xff;
					m_SendDCCData[z++] = ((long)(limit / m_Resolution[axisNum])) >> 24;
					m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
					Sleep(10);
				}

			}
			break;
		}
		case HSI_SCAN_MOTION_EQ_DIS:
		{
			if (dTrigDis[0] > 0.0001)
			{
				iMotionDirection = 0;
			}
			else
			{
				iMotionDirection = 1;
			}
			switch (iaxisNum)
			{
			case AXIS_X:
			{
				if (!m_IsUsePPS)
				{
					begin_position[1] = begin_position[1] + dTrigDis[0] / m_Resolution[axisNum];
				}
				else
				{
					if (iMotionDirection == 0)
					{
						begin_position[1] = begin_position[1] - 20;
					}
					else
					{
						begin_position[1] = begin_position[1] + 20;
					}
				}
				if (iMotionDirection == 0)
				{
					limit = 1000;
				}
				else
				{
					limit = -1000;
				}
				break;
			}
			case AXIS_Y:
			{
				if (!m_IsUsePPS)
				{
					begin_position[2] = begin_position[2] + dTrigDis[0] / m_Resolution[axisNum];
				}
				else
				{
					if (iMotionDirection == 0)
					{
						begin_position[2] = begin_position[2] - 20;
					}
					else
					{
						begin_position[2] = begin_position[2] + 20;
					}
				}
				if (iMotionDirection == 0)
				{
					limit = 1000;
				}
				else
				{
					limit = -1000;
				}
				break;
			}
			case AXIS_Z:
			{
				if (!m_IsUsePPS)
				{
					begin_position[3] = begin_position[3] + dTrigDis[0] / m_Resolution[axisNum];
				}
				else
				{
					if (iMotionDirection == 0)
					{
						begin_position[3] = begin_position[3] - 20;
					}
					else
					{
						begin_position[3] = begin_position[3] + 20;
					}

				}
				if (iMotionDirection == 0)
				{
					limit = 1000;
				}
				else
				{
					limit = -1000;
				}
				break;
			}
			default:
				break;
			}
			iScanMotionType = HSI_SCAN_MOTION_EQ_DIS;
			m_SendDCCData[3] = 0x00;
			m_SendDCCData[4] = ((long)(dTrigDis[0] / m_Resolution[axisNum])) & 0xff;
			m_SendDCCData[5] = (((long)(dTrigDis[0] / m_Resolution[axisNum])) >> 8) & 0xff;
			m_SendDCCData[6] = (((long)(dTrigDis[0] / m_Resolution[axisNum])) >> 16) & 0xff;
			m_SendDCCData[7] = ((long)(dTrigDis[0] / m_Resolution[axisNum])) >> 24;
			m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
			Sleep(10);
			break;
		}
		case HSI_SCAN_MOTION_EQ_DIS_II:
		{
			if (dTrigDis[1] > 0.0001)
			{
				iMotionDirection = 0;
			}
			else
			{
				iMotionDirection = 1;
			}

			switch (iaxisNum)
			{
			case AXIS_X:
			{
				if (!m_IsUsePPS)
				{
					begin_position[1] = begin_position[1] + dTrigDis[0] / m_Resolution[axisNum];
				}
				else
				{
					if (iMotionDirection == 0)
					{
						begin_position[1] = begin_position[1] - 20;
					}
					else
					{
						begin_position[1] = begin_position[1] + 20;
					}
				}
				if (iMotionDirection == 0)
				{
					limit = 1000;
				}
				else
				{
					limit = -1000;
				}
				break;
			}
			case AXIS_Y:
			{
				if (!m_IsUsePPS)
				{
					begin_position[2] = begin_position[2] + dTrigDis[0] / m_Resolution[axisNum];
				}
				else
				{
					if (iMotionDirection == 0)
					{
						begin_position[2] = begin_position[2] - 20;
					}
					else
					{
						begin_position[2] = begin_position[2] + 20;
					}
				}
				if (iMotionDirection == 0)
				{
					limit = 1000;
				}
				else
				{
					limit = -1000;
				}
				break;
			}
			case AXIS_Z:
			{
				if (!m_IsUsePPS)
				{
					begin_position[3] = begin_position[3] + dTrigDis[0] / m_Resolution[axisNum];
				}
				else
				{
					if (iMotionDirection == 0)
					{
						begin_position[3] = begin_position[3] - 20;
					}
					else
					{
						begin_position[3] = begin_position[3] + 20;
					}
				}
				if (iMotionDirection == 0)
				{
					limit = 1000;
				}
				else
				{
					limit = -1000;
				}
				break;
			}
			default:
				break;
			}

			iScanMotionType = HSI_SCAN_MOTION_EQ_DIS_II;
			m_SendDCCData[3] = 0x00;
			int i = 0, j = 0;
			for (i = 0, j = 4; i < 2; ++i)
			{
				m_SendDCCData[j++] = ((long)(dTrigDis[i] / m_Resolution[axisNum])) & 0xff;
				m_SendDCCData[j++] = (((long)(dTrigDis[i] / m_Resolution[axisNum])) >> 8) & 0xff;
				m_SendDCCData[j++] = (((long)(dTrigDis[i] / m_Resolution[axisNum])) >> 16) & 0xff;
				m_SendDCCData[j++] = ((long)(dTrigDis[i] / m_Resolution[axisNum])) >> 24;
			}
			m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
			Sleep(10);
			break;
		}
		default:
			break;
		}
		g_pLogger->SendAndFlushWithTime(L"[DCCScanSetData] Out\n");
	}
	ReleaseMutex(g_WR_ToMove_Mutex);
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::DCCScanStart()
{
	WaitForSingleObject(g_WR_ToMove_Mutex, INFINITE);
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		if (!setLightFlag)
		{
			unsigned char m_SetTriggerLightData[64] = { 0 };
			m_SetTriggerLightData[0] = CT_LIGHT;
			m_SetTriggerLightData[1] = 0x03;
			m_SetTriggerLightData[53] = 0;
			m_WriteByte = Send_Command(0, (const char*)m_SetTriggerLightData, m_SendDataLength);
			Sleep(1);
		}
		setLightFlag = false;
		g_pLogger->SendAndFlushWithTime(L"[DCCScanStart] In\n");
		unsigned char m_SendDCCData[64] = { 0 };
		m_SendDCCData[0] = CT_MOTOR;
		m_SendDCCData[1] = CT_TRIGGER_DATA;
		m_SendDCCData[2] = 0x00;
		m_SendDCCData[3] = iaxisNum;
		m_SendDCCData[4] = iScanMotionType;
		m_SendDCCData[5] = ((iTriggleNum)& 0xff);
		m_SendDCCData[6] = (iTriggleNum >> 8) & 0xff;
		m_SendDCCData[7] = iMotionDirection;
		switch (iaxisNum)
		{
		case AXIS_X: m_SendDCCData[8] = begin_position[1] & 0xff;
			m_SendDCCData[9] = (begin_position[1] >> 8) & 0xff;
			m_SendDCCData[10] = (begin_position[1] >> 16) & 0xff;
			m_SendDCCData[11] = (begin_position[1] >> 24) & 0xff;
			m_SendDCCData[12] = m_SetPotion_DriveSpeed[1] & 0xff;
			m_SendDCCData[13] = (m_SetPotion_DriveSpeed[1] >> 8) & 0xff;
			break;
		case AXIS_Y:  m_SendDCCData[8] = begin_position[2] & 0xff;
			m_SendDCCData[9] = (begin_position[2] >> 8) & 0xff;
			m_SendDCCData[10] = (begin_position[2] >> 16) & 0xff;
			m_SendDCCData[11] = (begin_position[2] >> 24) & 0xff;
			m_SendDCCData[12] = m_SetPotion_DriveSpeed[2] & 0xff;
			m_SendDCCData[13] = (m_SetPotion_DriveSpeed[2] >> 8) & 0xff;
			break;
		case AXIS_Z:
			m_SendDCCData[8] = begin_position[3] & 0xff;
			m_SendDCCData[9] = (begin_position[3] >> 8) & 0xff;
			m_SendDCCData[10] = (begin_position[3] >> 16) & 0xff;
			m_SendDCCData[11] = (begin_position[3] >> 24) & 0xff;
			m_SendDCCData[12] = m_SetPotion_DriveSpeed[3] & 0xff;
			m_SendDCCData[13] = (m_SetPotion_DriveSpeed[3] >> 8) & 0xff;
			break;
		default:
			break;
		}
		m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
		Sleep(2);
		g_pLogger->SendAndFlushWithTime(L"[DCCScanStart] Out\n");
		m_IsUsePPS = false;
	}
	ReleaseMutex(g_WR_ToMove_Mutex);
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::DCCScanStop()
{
	WaitForSingleObject(g_WR_ToMove_Mutex, INFINITE);
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		g_pLogger->SendAndFlushWithTime(L"[DCCScanStop] In\n");
		unsigned char m_SendDCCData[64] = { 0 };
		m_SendDCCData[0] = CT_MOTOR;
		m_SendDCCData[1] = CT_TRIGGER_DATA;
		m_SendDCCData[2] = 0x02;
		m_SendDCCData[3] = iaxisNum;
		m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
		g_pLogger->SendAndFlushWithTime(L"[DCCScanStop] Out\n");
	}
	ReleaseMutex(g_WR_ToMove_Mutex);
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::DCCForLightPlate()
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		g_pLogger->SendAndFlushWithTime(L"[DCCForLightPlate] In\n");
		memset(m_cSendData, 0x00, 63);
		m_cSendData[0] = 0x02;
		m_cSendData[1] = 0x02;
		m_cSendData[2] = 0x01;
		m_cSendData[6] = 0x04;
		m_cSendData[7] = 0xff;
		m_cSendData[8] = 0xff;
		m_cSendData[9] = 0xff;
		m_cSendData[10] = 0xff;
		m_cSendData[14] = 0x00;
		m_WriteByte = Send_Command(1, (const char*)m_cSendData, m_SendDataLength);
		Sleep(5);
		memset(m_cSendData, 0x00, 63);
		m_cSendData[0] = 0x02;
		m_cSendData[1] = 0x02;
		m_cSendData[2] = 0x01;
		m_cSendData[6] = 0x04;
		m_cSendData[7] = 0xff;
		m_cSendData[8] = 0xff;
		m_cSendData[9] = 0xff;
		m_cSendData[10] = 0xff;
		m_cSendData[14] = m_ETIPort & 0xff;
		m_WriteByte = Send_Command(1, (const char*)m_cSendData, m_SendDataLength);
		Sleep(5);
		g_pLogger->SendAndFlushWithTime(L"[DCCForLightPlate] Out\n");
	}
	return rStatus;
}

//===============================转盘============================================
HSI_STATUS HSI_Motion::StartPlcJob(int* CamerasDis, int* BinsDis, int SubArea, int filterTime1, int filterTime2, int pluseSumDis)
{
	g_pLogger->SendAndFlushWithTime(L"[StartPlcJob] In\n");
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		m_Thread_StateData = HSI_THREAD_PAUSED;
		if (m_EncPos[2] < 0)
		{
			sEvenProp.Init();
			sEvenProp.EventType = HSI_EVENT_ERROR;
			sEvenProp.EventID = HSI_EVENT_MOTION;
			sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK;
			strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "开始前请确认当前位置大于零！");
			EventCallback(sEvenProp);
			return HSI_STATUS_FAILED;
		}
		m_Thread_StateData = HSI_THREAD_PAUSED;
		m_IbinCount = 0;
		m_cSendData[0] = CT_TURNTABLE;
		m_cSendData[1] = CT_RTSET;
		int index = 2;
		for (size_t i = 0; i < 4; i++)
		{
			m_cSendData[index++] = ((CamerasDis[i] >> 24) & 0xff);
			m_cSendData[index++] = ((CamerasDis[i] >> 16) & 0xff);
			m_cSendData[index++] = ((CamerasDis[i] >> 8) & 0xff);
			m_cSendData[index++] = (CamerasDis[i] & 0xff);
		}
		for (size_t i = 0; i < 5; i++)
		{
			m_cSendData[index++] = ((BinsDis[i] >> 24) & 0xff);
			m_cSendData[index++] = ((BinsDis[i] >> 16) & 0xff);
			m_cSendData[index++] = ((BinsDis[i] >> 8) & 0xff);
			m_cSendData[index++] = (BinsDis[i] & 0xff);
		}
		m_cSendData[index++] = SubArea;
		m_cSendData[index++] = ((filterTime1 >> 8) & 0xff);
		m_cSendData[index++] = (filterTime1 & 0xff);
		m_cSendData[index++] = ((filterTime2 >> 8) & 0xff);
		m_cSendData[index++] = (filterTime2 & 0xff);
		m_cSendData[index++] = ((pluseSumDis >> 24) & 0xff);
		m_cSendData[index++] = ((pluseSumDis >> 16) & 0xff);
		m_cSendData[index++] = ((pluseSumDis >> 8) & 0xff);
		m_cSendData[index++] = (pluseSumDis & 0xff);
		m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
		Sleep(5);
		Jog(m_UseAxisNum, 0.1);
	}
	g_pLogger->SendAndFlushWithTime(L"[StartPlcJob] Out\n");
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::SendBinResult(int* BinResult)
{
	g_pLogger->SendAndFlushWithTime(L"[SendBinResult] In\n");
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		m_IbinCount++;
		m_cSendData[0] = CT_TURNTABLE;
		m_cSendData[1] = CT_BINSDATA;
		int temp = 0;
		switch (BinResult[0])
		{
		case 1:
			temp = 0x01; break;
		case 2:
			temp = 0x02; break;
		case 3:
			temp = 0x04; break;
		case 4:
			temp = 0x08; break;
		default:
			temp = 0x00; break;
		}
		m_cSendData[2] = temp;
		m_cSendData[3] = ((m_IbinCount >> 24) & 0xff);
		m_cSendData[4] = ((m_IbinCount >> 16) & 0xff);
		m_cSendData[5] = ((m_IbinCount >> 8) & 0xff);
		m_cSendData[6] = (m_IbinCount & 0xff);
		m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
		g_pLogger->SendAndFlushWithTime(L"[GetSpeedXyz] m_IbinCount = %d\n", m_IbinCount);
		Sleep(5);
	}
	g_pLogger->SendAndFlushWithTime(L"[SendBinResult] out\n");
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::GetTriggleCount(int *nCount, int& nArea)
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		if (m_SO7_Serial.m_RecvData[0] == 3)
		{
			nArea = m_SO7_Serial.m_RecvData[1];
			nCount[0] = m_SO7_Serial.m_RecvData[2] << 24 | m_SO7_Serial.m_RecvData[3] << 16 | m_SO7_Serial.m_RecvData[4] << 8 | m_SO7_Serial.m_RecvData[5];
			nCount[1] = m_SO7_Serial.m_RecvData[6] << 24 | m_SO7_Serial.m_RecvData[7] << 16 | m_SO7_Serial.m_RecvData[8] << 8 | m_SO7_Serial.m_RecvData[9];
			nCount[2] = m_SO7_Serial.m_RecvData[10] << 24 | m_SO7_Serial.m_RecvData[11] << 16 | m_SO7_Serial.m_RecvData[12] << 8 | m_SO7_Serial.m_RecvData[13];
			nCount[3] = m_SO7_Serial.m_RecvData[14] << 24 | m_SO7_Serial.m_RecvData[15] << 16 | m_SO7_Serial.m_RecvData[16] << 8 | m_SO7_Serial.m_RecvData[17];
			nCount[4] = m_SO7_Serial.m_RecvData[18] << 24 | m_SO7_Serial.m_RecvData[19] << 16 | m_SO7_Serial.m_RecvData[20] << 8 | m_SO7_Serial.m_RecvData[21];
			nCount[5] = m_SO7_Serial.m_RecvData[22] << 24 | m_SO7_Serial.m_RecvData[23] << 16 | m_SO7_Serial.m_RecvData[24] << 8 | m_SO7_Serial.m_RecvData[25];
			nCount[6] = m_SO7_Serial.m_RecvData[26] << 24 | m_SO7_Serial.m_RecvData[27] << 16 | m_SO7_Serial.m_RecvData[28] << 8 | m_SO7_Serial.m_RecvData[29];
			nCount[7] = m_SO7_Serial.m_RecvData[30] << 24 | m_SO7_Serial.m_RecvData[31] << 16 | m_SO7_Serial.m_RecvData[32] << 8 | m_SO7_Serial.m_RecvData[33];
			nCount[8] = m_SO7_Serial.m_RecvData[34] << 24 | m_SO7_Serial.m_RecvData[35] << 16 | m_SO7_Serial.m_RecvData[36] << 8 | m_SO7_Serial.m_RecvData[37];
			nCount[9] = m_SO7_Serial.m_RecvData[38] << 24 | m_SO7_Serial.m_RecvData[39] << 16 | m_SO7_Serial.m_RecvData[40] << 8 | m_SO7_Serial.m_RecvData[41];
			g_pLogger->SendAndFlushWithTime(L"[GetTriggleCount] nArea = %d\n", nArea);
		}
	}
	return rStatus;
}
//===========================================================================
//点胶
//===========================================================================
HSI_STATUS HSI_Motion::GluePPSpnts(int* ppsDir, double* gluePPSPos, int* delayLightBefore, int* lightTime, double* lightData, int num)
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		unsigned char send_gluePPS_data[64] = { 0 };
		send_gluePPS_data[0] = CT_GLUEDISPENSER;
		send_gluePPS_data[1] = 0x01;		//擦除指令
		m_WriteByte = Send_Command(0, (const char*)send_gluePPS_data, m_SendDataLength);
		Sleep(200);
		int posIndex = 0;
		int loadFeet[4];
		int j = 0;
		int c = 0; //触发计数
		int dirType = 0;
		int saveDir = 0;
		int duanNum = 0;
		double triggerPnt[4];
		double savePPSPnt[4];
		GlueDispenserindexNum = 0;
#pragma region 拍照点
		for (size_t i = 0; i < num; i++)
		{
			if (i == 0)
			{
				dirType = ppsDir[0];
				saveDir = dirType;
				j = 7;
			}
			else
			{
				dirType = ppsDir[i];
			}
			if (dirType != saveDir)
			{
				//->
				send_gluePPS_data[0] = CT_GLUEDISPENSER;
				send_gluePPS_data[1] = 0x02;
				send_gluePPS_data[2] = GlueDispenserindexNum;
				send_gluePPS_data[3] = duanNum;
				send_gluePPS_data[4] = c;
				int axisNum = 0;
				int dirnum = 0;
				switch (saveDir)
				{
				case 0:
				{
					axisNum = 1;
					dirnum = 2;
					break;
				}
				case 1:
				{
					axisNum = 1;
					dirnum = 1;
					break;
				}
				case 2:
				{
					axisNum = 2;
					dirnum = 2;
					break;
				}
				case 3:
				{
					axisNum = 2;
					dirnum = 1;
					break;
				}
				default:
					break;
				}
				send_gluePPS_data[5] = axisNum;
				send_gluePPS_data[6] = dirnum;
				m_WriteByte = Send_Command(0, (const char*)send_gluePPS_data, m_SendDataLength);
				Sleep(20);
				GlueDispenserindexNum++;
				duanNum = 0;
				saveDir = dirType;
				c = 0;
				j = 7;
			}
			switch (dirType)
			{
			case 0:
			{
				c++;
				loadFeet[0] = (int)(gluePPSPos[posIndex++] / m_Resolution[1]);
				loadFeet[1] = (int)(gluePPSPos[posIndex++] / m_Resolution[1]);
				loadFeet[2] = (int)(gluePPSPos[posIndex++] / m_Resolution[1]);
				send_gluePPS_data[j++] = (loadFeet[0] & 0xff);
				send_gluePPS_data[j++] = ((loadFeet[0] >> 8) & 0xff);
				send_gluePPS_data[j++] = ((loadFeet[0] >> 16) & 0xff);
				send_gluePPS_data[j++] = ((loadFeet[0] >> 24) & 0xff);
				break;
			}
			case 1:
			{
				c++;
				loadFeet[0] = (int)(gluePPSPos[posIndex++] / m_Resolution[1]);
				loadFeet[1] = (int)(gluePPSPos[posIndex++] / m_Resolution[1]);
				loadFeet[2] = (int)(gluePPSPos[posIndex++] / m_Resolution[1]);
				send_gluePPS_data[j++] = (loadFeet[0] & 0xff);
				send_gluePPS_data[j++] = ((loadFeet[0] >> 8) & 0xff);
				send_gluePPS_data[j++] = ((loadFeet[0] >> 16) & 0xff);
				send_gluePPS_data[j++] = ((loadFeet[0] >> 24) & 0xff);
				break;
			}
			case 2:
			{
				c++;
				loadFeet[0] = (int)(gluePPSPos[posIndex++] / m_Resolution[1]);
				loadFeet[1] = (int)(gluePPSPos[posIndex++] / m_Resolution[1]);
				loadFeet[2] = (int)(gluePPSPos[posIndex++] / m_Resolution[1]);
				send_gluePPS_data[j++] = (loadFeet[1] & 0xff);
				send_gluePPS_data[j++] = ((loadFeet[1] >> 8) & 0xff);
				send_gluePPS_data[j++] = ((loadFeet[1] >> 16) & 0xff);
				send_gluePPS_data[j++] = ((loadFeet[1] >> 24) & 0xff);
				break;
			}
			case 3:
			{
				c++;
				loadFeet[0] = (int)(gluePPSPos[posIndex++] / m_Resolution[1]);
				loadFeet[1] = (int)(gluePPSPos[posIndex++] / m_Resolution[1]);
				loadFeet[2] = (int)(gluePPSPos[posIndex++] / m_Resolution[1]);
				send_gluePPS_data[j++] = (loadFeet[1] & 0xff);
				send_gluePPS_data[j++] = ((loadFeet[1] >> 8) & 0xff);
				send_gluePPS_data[j++] = ((loadFeet[1] >> 16) & 0xff);
				send_gluePPS_data[j++] = ((loadFeet[1] >> 24) & 0xff);
				break;
			}
			default:
				break;
			}
			if (c == 12)
			{
				//->
				send_gluePPS_data[0] = CT_GLUEDISPENSER;
				send_gluePPS_data[1] = 0x02;
				send_gluePPS_data[2] = GlueDispenserindexNum;
				send_gluePPS_data[3] = duanNum;
				send_gluePPS_data[4] = c;
				int axisNum = 0;
				int dirnum = 0;
				switch (saveDir)
				{
				case 0:
				{
					axisNum = 1;
					dirnum = 2;
					break;
				}
				case 1:
				{
					axisNum = 1;
					dirnum = 1;
					break;
				}
				case 2:
				{
					axisNum = 2;
					dirnum = 2;
					break;
				}
				case 3:
				{
					axisNum = 2;
					dirnum = 1;
					break;
				}
				default:
					break;
				}
				send_gluePPS_data[5] = axisNum;
				send_gluePPS_data[6] = dirnum;
				m_WriteByte = Send_Command(0, (const char*)send_gluePPS_data, m_SendDataLength);
				Sleep(20);
				duanNum++;
				saveDir = dirType;
				c = 0;
				j = 7;
			}
		}
		if (c != 0)
		{
			send_gluePPS_data[0] = CT_GLUEDISPENSER;
			send_gluePPS_data[1] = 0x02;
			send_gluePPS_data[2] = GlueDispenserindexNum;
			send_gluePPS_data[3] = duanNum;
			send_gluePPS_data[4] = c;
			int axisNum = 0;
			int dirnum = 0;
			switch (saveDir)
			{
			case 0:
			{
				axisNum = 1;
				dirnum = 2;
				break;
			}
			case 1:
			{
				axisNum = 1;
				dirnum = 1;
				break;
			}
			case 2:
			{
				axisNum = 2;
				dirnum = 2;
				break;
			}
			case 3:
			{
				axisNum = 2;
				dirnum = 1;
				break;
			}
			default:
				break;
			}
			send_gluePPS_data[5] = axisNum;
			send_gluePPS_data[6] = dirnum;
			m_WriteByte = Send_Command(0, (const char*)send_gluePPS_data, m_SendDataLength);
			Sleep(20);
		}
#pragma endregion

		unsigned char send_data_light2[64] = { 0 };
		unsigned char send_data_delay[64] = { 0 };
		c = 0;
		GlueDispenserindexNum = 0;
		int ldIndex = 0;
		int light1 = 0;
		int light2 = 0;
		int indexCount = 0;
		dirType = 0;
		saveDir = 0;
		j = 3;
		int k = 4;
		int d = 4;
		for (size_t i = 0; i < num; i++)
		{
			if (i == 0)
			{
				dirType = ppsDir[0];
				saveDir = dirType;
				j = 4;
			}
			else
			{
				dirType = ppsDir[i];
			}
			if (dirType != saveDir)
			{
				send_gluePPS_data[0] = CT_GLUEDISPENSER;
				send_gluePPS_data[1] = 0x03;
				send_gluePPS_data[2] = GlueDispenserindexNum;
				send_gluePPS_data[3] = 0;
				m_WriteByte = Send_Command(0, (const char*)send_gluePPS_data, m_SendDataLength);
				Sleep(20);
				send_data_light2[0] = CT_GLUEDISPENSER;
				send_data_light2[1] = 0x03;
				send_data_light2[2] = GlueDispenserindexNum;
				send_data_light2[3] = 1;
				m_WriteByte = Send_Command(0, (const char*)send_data_light2, m_SendDataLength);
				Sleep(20);
				send_data_delay[0] = CT_GLUEDISPENSER;
				send_data_delay[1] = 0x03;
				send_data_delay[2] = GlueDispenserindexNum;
				send_data_delay[3] = 2;
				m_WriteByte = Send_Command(0, (const char*)send_data_delay, m_SendDataLength);
				Sleep(20);
				c = 0;
				j = 4;
				k = 4;
				d = 4;
				indexCount = 0;
				GlueDispenserindexNum++;
			}
			light1 = (int)(lightData[ldIndex++] * 1120);
			light2 = (int)(lightData[ldIndex++] * 1120);
			//send_gluePPS_data[j++] = (delayLightBefore[i] & 0xff);
			//send_gluePPS_data[j++] = ((delayLightBefore[i] >> 8) & 0xff);
			send_gluePPS_data[j++] = (light1 & 0xff);
			send_gluePPS_data[j++] = ((light1 >> 8) & 0xff);
			send_data_light2[k++] = (light2 & 0xff);
			send_data_light2[k++] = ((light2 >> 8) & 0xff);
			send_data_delay[d++] = (lightTime[i] & 0xff);
			c++;
			if (c == 30)
			{
				send_gluePPS_data[0] = CT_GLUEDISPENSER;
				send_gluePPS_data[1] = 0x03;
				send_gluePPS_data[2] = GlueDispenserindexNum;
				send_gluePPS_data[3] = 0;
				m_WriteByte = Send_Command(0, (const char*)send_gluePPS_data, m_SendDataLength);
				Sleep(20);
				send_data_light2[0] = CT_GLUEDISPENSER;
				send_data_light2[1] = 0x03;
				send_data_light2[2] = GlueDispenserindexNum;
				send_data_light2[3] = 1;
				m_WriteByte = Send_Command(0, (const char*)send_data_light2, m_SendDataLength);
				Sleep(20);
				send_data_delay[0] = CT_GLUEDISPENSER;
				send_data_delay[1] = 0x03;
				send_data_delay[2] = GlueDispenserindexNum;
				send_data_delay[3] = 2;
				m_WriteByte = Send_Command(0, (const char*)send_data_delay, m_SendDataLength);
				Sleep(20);
				c = 0;
				j = 4;
				k = 4;
				d = 4;
				indexCount++;
				GlueDispenserindexNum++;
			}
		}
		if (c != 0)
		{
			send_gluePPS_data[0] = CT_GLUEDISPENSER;
			send_gluePPS_data[1] = 0x03;
			send_gluePPS_data[2] = GlueDispenserindexNum;
			send_gluePPS_data[3] = 0;
			m_WriteByte = Send_Command(0, (const char*)send_gluePPS_data, m_SendDataLength);
			Sleep(20);
			send_data_light2[0] = CT_GLUEDISPENSER;
			send_data_light2[1] = 0x03;
			send_data_light2[2] = GlueDispenserindexNum;
			send_data_light2[3] = 1;
			m_WriteByte = Send_Command(0, (const char*)send_data_light2, m_SendDataLength);
			Sleep(20);
			send_data_delay[0] = CT_GLUEDISPENSER;
			send_data_delay[1] = 0x03;
			send_data_delay[2] = GlueDispenserindexNum;
			send_data_delay[3] = 2;
			m_WriteByte = Send_Command(0, (const char*)send_data_delay, m_SendDataLength);
			Sleep(20);
			GlueDispenserindexNum++;
		}
	}
	return rStatus;
}
HSI_STATUS HSI_Motion::GlueDispenser(int* index, int* cirdirection, double* gluePos, int num)
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		GluerunCount = 0;
		unsigned char send_glue_data[64] = { 0 };
		send_glue_data[0] = CT_ORDER;
		send_glue_data[1] = CT_GLUEDISPENSER_CLEAR;
		send_glue_data[2] = 0x02;
		m_WriteByte = Send_Command(0, (const char*)send_glue_data, m_SendDataLength);
		Sleep(200);
		int posIndex = 0;
		int loadFeet[4];
		double startGluepos[4];
		double endGluepos[4];
		double saveGluepos[4];
		int circlepnt[4];
		unsigned char glueaxis = 0;
		for (size_t i = 0; i < num; i++)
		{
			switch (index[i])
			{
			case 0:
			{
				glueaxis = 0;
				if (i == 0)
				{
					GluerunCount++;
					endGluepos[0] = gluePos[posIndex++];
					endGluepos[1] = gluePos[posIndex++];
					endGluepos[2] = gluePos[posIndex++];
					endGluepos[3] = gluePos[posIndex++];
					saveGluepos[0] = endGluepos[0];
					saveGluepos[1] = endGluepos[1];
					saveGluepos[2] = endGluepos[2];
					saveGluepos[3] = endGluepos[3];
					loadFeet[0] = (int)(endGluepos[0] / m_Resolution[1]);
					loadFeet[1] = (int)(endGluepos[1] / m_Resolution[2]);
					loadFeet[2] = (int)(endGluepos[2] / m_Resolution[3]);
					loadFeet[3] = (int)(endGluepos[3] / m_Resolution[4]);
					if (abs(loadFeet[0]) > 20) glueaxis |= 0x01;
					if (abs(loadFeet[1]) > 20) glueaxis |= 0x02;
					if (abs(loadFeet[2]) > 20) glueaxis |= 0x04;
					if (abs(loadFeet[3]) > 20) glueaxis |= 0x08;
					send_glue_data[3] = glueaxis;
					send_glue_data[4] = 0x01;
					send_glue_data[5] = 0x00;
					int j = 6;
					for (size_t k = 0; k < 4; k++)
					{
						send_glue_data[j++] = (loadFeet[k] & 0xff);
						send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff);
						send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff);
						send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff);
					}
					send_glue_data[0] = CT_MOTOR;
					send_glue_data[1] = CT_GLUEDISPENSER_SET;
					int len = j - 3;
					send_glue_data[2] = len;
					m_WriteByte = Send_Command(0, (const char*)send_glue_data, m_SendDataLength);
					Sleep(20);
				}
				else
				{
					GluerunCount++;
					endGluepos[0] = gluePos[posIndex++];
					endGluepos[1] = gluePos[posIndex++];
					endGluepos[2] = gluePos[posIndex++];
					endGluepos[3] = gluePos[posIndex++];
					startGluepos[0] = saveGluepos[0];
					startGluepos[1] = saveGluepos[1];
					startGluepos[2] = saveGluepos[2];
					startGluepos[3] = saveGluepos[3];
					saveGluepos[0] = endGluepos[0];
					saveGluepos[1] = endGluepos[1];
					saveGluepos[2] = endGluepos[2];
					saveGluepos[3] = endGluepos[3];
					loadFeet[0] = (int)((endGluepos[0] - startGluepos[0]) / m_Resolution[1]);
					loadFeet[1] = (int)((endGluepos[1] - startGluepos[1]) / m_Resolution[2]);
					loadFeet[2] = (int)((endGluepos[2] - startGluepos[2]) / m_Resolution[3]);
					loadFeet[3] = (int)((endGluepos[3] - startGluepos[3]) / m_Resolution[4]);
					if (abs(loadFeet[0]) > 20) glueaxis |= 0x01;
					if (abs(loadFeet[1]) > 20) glueaxis |= 0x02;
					if (abs(loadFeet[2]) > 20) glueaxis |= 0x04;
					if (abs(loadFeet[3]) > 20) glueaxis |= 0x08;
					send_glue_data[3] = glueaxis;
					send_glue_data[4] = 0x01;
					send_glue_data[5] = 0x00;
					int j = 6;
					for (size_t k = 0; k < 4; k++)
					{
						send_glue_data[j++] = (loadFeet[k] & 0xff);
						send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff);
						send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff);
						send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff);
					}
					send_glue_data[0] = CT_MOTOR;
					send_glue_data[1] = CT_GLUEDISPENSER_SET;
					int len = j - 3;
					send_glue_data[2] = len;
					m_WriteByte = Send_Command(0, (const char*)send_glue_data, m_SendDataLength);
					Sleep(20);
				}
				break;
			}
			case 1:
			{
				if (i == 0)
				{
					GluerunCount++;
					endGluepos[0] = gluePos[posIndex++];
					endGluepos[1] = gluePos[posIndex++];
					endGluepos[2] = gluePos[posIndex++];
					endGluepos[3] = gluePos[posIndex++];
					saveGluepos[0] = endGluepos[0];
					saveGluepos[1] = endGluepos[1];
					saveGluepos[2] = endGluepos[2];
					saveGluepos[3] = endGluepos[3];
					loadFeet[0] = (int)(endGluepos[0] / m_Resolution[1]);
					loadFeet[1] = (int)(endGluepos[1] / m_Resolution[2]);
					loadFeet[2] = (int)(endGluepos[2] / m_Resolution[3]);
					loadFeet[3] = (int)(endGluepos[3] / m_Resolution[4]);
					if (abs(loadFeet[0]) > 20) glueaxis |= 0x01;
					if (abs(loadFeet[1]) > 20) glueaxis |= 0x02;
					if (abs(loadFeet[2]) > 20) glueaxis |= 0x04;
					if (abs(loadFeet[3]) > 20) glueaxis |= 0x08;
					send_glue_data[3] = glueaxis;
					send_glue_data[4] = 0x01;
					send_glue_data[5] = 0x00;
					int j = 6;
					for (size_t k = 0; k < 4; k++)
					{
						send_glue_data[j++] = (loadFeet[k] & 0xff);
						send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff);
						send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff);
						send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff);
					}
					GluerunCount++;
					glueaxis = 0;
					endGluepos[0] = gluePos[posIndex++];
					endGluepos[1] = gluePos[posIndex++];
					endGluepos[2] = gluePos[posIndex++];
					endGluepos[3] = gluePos[posIndex++];
					startGluepos[0] = saveGluepos[0];
					startGluepos[1] = saveGluepos[1];
					startGluepos[2] = saveGluepos[2];
					startGluepos[3] = saveGluepos[3];
					saveGluepos[0] = endGluepos[0];
					saveGluepos[1] = endGluepos[1];
					saveGluepos[2] = endGluepos[2];
					saveGluepos[3] = endGluepos[3];
					loadFeet[0] = (int)((endGluepos[0] - startGluepos[0]) / m_Resolution[1]);
					loadFeet[1] = (int)((endGluepos[1] - startGluepos[1]) / m_Resolution[2]);
					loadFeet[2] = (int)((endGluepos[2] - startGluepos[2]) / m_Resolution[3]);
					loadFeet[3] = (int)((endGluepos[3] - startGluepos[3]) / m_Resolution[4]);
					if (abs(loadFeet[0]) > 20) glueaxis |= 0x01;
					if (abs(loadFeet[1]) > 20) glueaxis |= 0x02;
					if (abs(loadFeet[2]) > 20) glueaxis |= 0x04;
					if (abs(loadFeet[3]) > 20) glueaxis |= 0x08;
					send_glue_data[j++] = glueaxis;
					send_glue_data[j++] = 0x02;
					send_glue_data[j++] = 0x01;
					for (size_t k = 0; k < 4; k++)
					{
						send_glue_data[j++] = (loadFeet[k] & 0xff);
						send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff);
						send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff);
						send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff);
					}
					send_glue_data[0] = CT_MOTOR;
					send_glue_data[1] = CT_GLUEDISPENSER_SET;
					int len = j - 3;
					send_glue_data[2] = len;
					m_WriteByte = Send_Command(0, (const char*)send_glue_data, m_SendDataLength);
					Sleep(20);
				}
				else
				{
					GluerunCount++;
					glueaxis = 0;
					endGluepos[0] = gluePos[posIndex++];
					endGluepos[1] = gluePos[posIndex++];
					endGluepos[2] = gluePos[posIndex++];
					endGluepos[3] = gluePos[posIndex++];
					startGluepos[0] = saveGluepos[0];
					startGluepos[1] = saveGluepos[1];
					startGluepos[2] = saveGluepos[2];
					startGluepos[3] = saveGluepos[3];
					saveGluepos[0] = endGluepos[0];
					saveGluepos[1] = endGluepos[1];
					saveGluepos[2] = endGluepos[2];
					saveGluepos[3] = endGluepos[3];
					loadFeet[0] = (int)((endGluepos[0] - startGluepos[0]) / m_Resolution[1]);
					loadFeet[1] = (int)((endGluepos[1] - startGluepos[1]) / m_Resolution[2]);
					loadFeet[2] = (int)((endGluepos[2] - startGluepos[2]) / m_Resolution[3]);
					loadFeet[3] = (int)((endGluepos[3] - startGluepos[3]) / m_Resolution[4]);
					if (abs(loadFeet[0]) > 20) glueaxis |= 0x01;
					if (abs(loadFeet[1]) > 20) glueaxis |= 0x02;
					if (abs(loadFeet[2]) > 20) glueaxis |= 0x04;
					if (abs(loadFeet[3]) > 20) glueaxis |= 0x08;
					send_glue_data[3] = glueaxis;
					send_glue_data[4] = 0x02;
					send_glue_data[5] = 0x00;
					int j = 6;
					for (size_t k = 0; k < 4; k++)
					{
						send_glue_data[j++] = (loadFeet[k] & 0xff);
						send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff);
						send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff);
						send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff);
					}
					GluerunCount++;
					glueaxis = 0;
					endGluepos[0] = gluePos[posIndex++];
					endGluepos[1] = gluePos[posIndex++];
					endGluepos[2] = gluePos[posIndex++];
					endGluepos[3] = gluePos[posIndex++];
					startGluepos[0] = saveGluepos[0];
					startGluepos[1] = saveGluepos[1];
					startGluepos[2] = saveGluepos[2];
					startGluepos[3] = saveGluepos[3];
					saveGluepos[0] = endGluepos[0];
					saveGluepos[1] = endGluepos[1];
					saveGluepos[2] = endGluepos[2];
					saveGluepos[3] = endGluepos[3];
					loadFeet[0] = (int)((endGluepos[0] - startGluepos[0]) / m_Resolution[1]);
					loadFeet[1] = (int)((endGluepos[1] - startGluepos[1]) / m_Resolution[2]);
					loadFeet[2] = (int)((endGluepos[2] - startGluepos[2]) / m_Resolution[3]);
					loadFeet[3] = (int)((endGluepos[3] - startGluepos[3]) / m_Resolution[4]);
					if (abs(loadFeet[0]) > 20) glueaxis |= 0x01;
					if (abs(loadFeet[1]) > 20) glueaxis |= 0x02;
					if (abs(loadFeet[2]) > 20) glueaxis |= 0x04;
					if (abs(loadFeet[3]) > 20) glueaxis |= 0x08;
					send_glue_data[j++] = glueaxis;
					send_glue_data[j++] = 0x02;
					send_glue_data[j++] = 0x01;
					for (size_t k = 0; k < 4; k++)
					{
						send_glue_data[j++] = (loadFeet[k] & 0xff);
						send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff);
						send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff);
						send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff);
					}
					send_glue_data[0] = CT_MOTOR;
					send_glue_data[1] = CT_GLUEDISPENSER_SET;
					int len = j - 3;
					send_glue_data[2] = len;
					m_WriteByte = Send_Command(0, (const char*)send_glue_data, m_SendDataLength);
					Sleep(20);
				}
				break;
			}
			case 3:
			{
				if (i == 0)
				{
					GluerunCount++;
					endGluepos[0] = gluePos[posIndex++];
					endGluepos[1] = gluePos[posIndex++];
					endGluepos[2] = gluePos[posIndex++];
					endGluepos[3] = gluePos[posIndex++];
					saveGluepos[0] = endGluepos[0];
					saveGluepos[1] = endGluepos[1];
					saveGluepos[2] = endGluepos[2];
					saveGluepos[3] = endGluepos[3];
					loadFeet[0] = (int)(endGluepos[0] / m_Resolution[1]);
					loadFeet[1] = (int)(endGluepos[1] / m_Resolution[2]);
					loadFeet[2] = (int)(endGluepos[2] / m_Resolution[3]);
					loadFeet[3] = (int)(endGluepos[3] / m_Resolution[4]);
					if (abs(loadFeet[0]) > 20) glueaxis |= 0x01;
					if (abs(loadFeet[1]) > 20) glueaxis |= 0x02;
					if (abs(loadFeet[2]) > 20) glueaxis |= 0x04;
					if (abs(loadFeet[3]) > 20) glueaxis |= 0x08;
					send_glue_data[3] = glueaxis;
					send_glue_data[4] = 0x01;
					send_glue_data[5] = 0x00;
					int j = 6;
					for (size_t k = 0; k < 4; k++)
					{
						send_glue_data[j++] = (loadFeet[k] & 0xff);
						send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff);
						send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff);
						send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff);
					}
					GluerunCount++;
					glueaxis = 0;
					endGluepos[0] = gluePos[posIndex++];
					endGluepos[1] = gluePos[posIndex++];
					endGluepos[2] = gluePos[posIndex++];
					endGluepos[3] = gluePos[posIndex++];
					startGluepos[0] = saveGluepos[0];
					startGluepos[1] = saveGluepos[1];
					startGluepos[2] = saveGluepos[2];
					startGluepos[3] = saveGluepos[3];
					circlepnt[0] = (int)((gluePos[posIndex++] - startGluepos[0]) / m_Resolution[1]);
					circlepnt[1] = (int)((gluePos[posIndex++] - startGluepos[1]) / m_Resolution[2]);
					circlepnt[2] = (int)((gluePos[posIndex++] - startGluepos[2]) / m_Resolution[3]);
					circlepnt[3] = (int)((gluePos[posIndex++] - startGluepos[3]) / m_Resolution[4]);
					saveGluepos[0] = endGluepos[0];
					saveGluepos[1] = endGluepos[1];
					saveGluepos[2] = endGluepos[2];
					saveGluepos[3] = endGluepos[3];
					loadFeet[0] = (int)((endGluepos[0] - startGluepos[0]) / m_Resolution[1]);
					loadFeet[1] = (int)((endGluepos[1] - startGluepos[1]) / m_Resolution[2]);
					loadFeet[2] = (int)((endGluepos[2] - startGluepos[2]) / m_Resolution[3]);
					loadFeet[3] = (int)((endGluepos[3] - startGluepos[3]) / m_Resolution[4]);
					if (abs(loadFeet[0]) > 20) glueaxis |= 0x01;
					if (abs(loadFeet[1]) > 20) glueaxis |= 0x02;
					if (abs(loadFeet[2]) > 20) glueaxis |= 0x04;
					if (abs(loadFeet[3]) > 20) glueaxis |= 0x08;
					send_glue_data[j++] = 0x03;
					if (cirdirection[i] == 0)
					{
						send_glue_data[j++] = 0x04;
					}
					else
					{
						send_glue_data[j++] = 0x03;
					}
					send_glue_data[j++] = 0x01;
					for (size_t k = 0; k < 4; k++)
					{
						send_glue_data[j++] = (loadFeet[k] & 0xff);
						send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff);
						send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff);
						send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff);
					}
					for (size_t k = 0; k < 4; k++)
					{
						send_glue_data[j++] = (circlepnt[k] & 0xff);
						send_glue_data[j++] = ((circlepnt[k] >> 8) & 0xff);
						send_glue_data[j++] = ((circlepnt[k] >> 16) & 0xff);
						send_glue_data[j++] = ((circlepnt[k] >> 24) & 0xff);
					}
					send_glue_data[0] = CT_MOTOR;
					send_glue_data[1] = CT_GLUEDISPENSER_SET;
					int len = j - 3;
					send_glue_data[2] = len;
					m_WriteByte = Send_Command(0, (const char*)send_glue_data, m_SendDataLength);
					Sleep(20);
				}
				else
				{
					GluerunCount++;
					glueaxis = 0;
					endGluepos[0] = gluePos[posIndex++];
					endGluepos[1] = gluePos[posIndex++];
					endGluepos[2] = gluePos[posIndex++];
					endGluepos[3] = gluePos[posIndex++];
					startGluepos[0] = saveGluepos[0];
					startGluepos[1] = saveGluepos[1];
					startGluepos[2] = saveGluepos[2];
					startGluepos[3] = saveGluepos[3];
					saveGluepos[0] = endGluepos[0];
					saveGluepos[1] = endGluepos[1];
					saveGluepos[2] = endGluepos[2];
					saveGluepos[3] = endGluepos[3];
					loadFeet[0] = (int)((endGluepos[0] - startGluepos[0]) / m_Resolution[1]);
					loadFeet[1] = (int)((endGluepos[1] - startGluepos[1]) / m_Resolution[2]);
					loadFeet[2] = (int)((endGluepos[2] - startGluepos[2]) / m_Resolution[3]);
					loadFeet[3] = (int)((endGluepos[3] - startGluepos[3]) / m_Resolution[4]);
					if (abs(loadFeet[0]) > 20) glueaxis |= 0x01;
					if (abs(loadFeet[1]) > 20) glueaxis |= 0x02;
					if (abs(loadFeet[2]) > 20) glueaxis |= 0x04;
					if (abs(loadFeet[3]) > 20) glueaxis |= 0x08;
					send_glue_data[3] = glueaxis;
					send_glue_data[4] = 0x02;
					send_glue_data[5] = 0x00;
					int j = 6;
					for (size_t k = 0; k < 4; k++)
					{
						send_glue_data[j++] = (loadFeet[k] & 0xff);
						send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff);
						send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff);
						send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff);
					}
					GluerunCount++;
					glueaxis = 0;
					endGluepos[0] = gluePos[posIndex++];
					endGluepos[1] = gluePos[posIndex++];
					endGluepos[2] = gluePos[posIndex++];
					endGluepos[3] = gluePos[posIndex++];
					startGluepos[0] = saveGluepos[0];
					startGluepos[1] = saveGluepos[1];
					startGluepos[2] = saveGluepos[2];
					startGluepos[3] = saveGluepos[3];
					circlepnt[0] = (int)((gluePos[posIndex++] - startGluepos[0]) / m_Resolution[1]);
					circlepnt[1] = (int)((gluePos[posIndex++] - startGluepos[1]) / m_Resolution[2]);
					circlepnt[2] = (int)((gluePos[posIndex++] - startGluepos[2]) / m_Resolution[3]);
					circlepnt[3] = (int)((gluePos[posIndex++] - startGluepos[3]) / m_Resolution[4]);
					saveGluepos[0] = endGluepos[0];
					saveGluepos[1] = endGluepos[1];
					saveGluepos[2] = endGluepos[2];
					saveGluepos[3] = endGluepos[3];
					loadFeet[0] = (int)((endGluepos[0] - startGluepos[0]) / m_Resolution[1]);
					loadFeet[1] = (int)((endGluepos[1] - startGluepos[1]) / m_Resolution[2]);
					loadFeet[2] = (int)((endGluepos[2] - startGluepos[2]) / m_Resolution[3]);
					loadFeet[3] = (int)((endGluepos[3] - startGluepos[3]) / m_Resolution[4]);
					if (abs(loadFeet[0]) > 20) glueaxis |= 0x01;
					if (abs(loadFeet[1]) > 20) glueaxis |= 0x02;
					if (abs(loadFeet[2]) > 20) glueaxis |= 0x04;
					if (abs(loadFeet[3]) > 20) glueaxis |= 0x08;
					send_glue_data[j++] = 0x03;
					if (cirdirection[i] == 0)
					{
						send_glue_data[j++] = 0x04;
					}
					else
					{
						send_glue_data[j++] = 0x03;
					}
					send_glue_data[j++] = 0x01;
					for (size_t k = 0; k < 4; k++)
					{
						send_glue_data[j++] = (loadFeet[k] & 0xff);
						send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff);
						send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff);
						send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff);
					}
					for (size_t k = 0; k < 4; k++)
					{
						send_glue_data[j++] = (circlepnt[k] & 0xff);
						send_glue_data[j++] = ((circlepnt[k] >> 8) & 0xff);
						send_glue_data[j++] = ((circlepnt[k] >> 16) & 0xff);
						send_glue_data[j++] = ((circlepnt[k] >> 24) & 0xff);
					}
					send_glue_data[0] = CT_MOTOR;
					send_glue_data[1] = CT_GLUEDISPENSER_SET;
					int len = j - 3;
					send_glue_data[2] = len;
					m_WriteByte = Send_Command(0, (const char*)send_glue_data, m_SendDataLength);
					Sleep(20);
				}
				break;
			}
			default:
				break;
			}
		}
		/*send_glue_data[0] = CT_MOTOR;
		send_glue_data[1] = CT_GLUEDISPENSER_START;
		send_glue_data[2] = runCount;*/
	}
	return rStatus;
}
HSI_STATUS HSI_Motion::GlueDispenserStart(double xOffset, double yOffset, double qOffset)
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		int xSkew = xOffset / m_Resolution[1];
		int ySkew = yOffset / m_Resolution[2];
		int qSkew = qOffset / m_Resolution[1];
		unsigned char send_glue_data[64] = { 0 };
		send_glue_data[0] = CT_MOTOR;
		send_glue_data[1] = CT_GLUEDISPENSER_START;
		send_glue_data[2] = GlueDispenserindexNum + 1;
		send_glue_data[3] = GluerunCount;
		send_glue_data[4] = (m_SetPotion_StartSpeed[1] & 0xff);
		send_glue_data[5] = ((m_SetPotion_StartSpeed[1] >> 8) & 0xff);
		send_glue_data[6] = (m_SetPotion_DriveSpeed[1] & 0xff);
		send_glue_data[7] = ((m_SetPotion_DriveSpeed[1] >> 8) & 0xff);
		send_glue_data[8] = (m_SetPotion_Line[1] & 0xff);
		send_glue_data[9] = ((m_SetPotion_Line[1] >> 8) & 0xff);
		send_glue_data[10] = (xSkew & 0xff);
		send_glue_data[11] = ((xSkew >> 8) & 0xff);
		send_glue_data[12] = (ySkew & 0xff);
		send_glue_data[13] = ((ySkew >> 8) & 0xff);
		send_glue_data[14] = (qSkew & 0xff);
		send_glue_data[15] = ((qSkew >> 8) & 0xff);
		g_pLogger->SendAndFlushWithTime(L"[GlueDispenser] m_SetPotion_StartSpeed = %d, m_SetPotion_DriveSpeed = %d,m_SetPotion_Line = %d \n", m_SetPotion_StartSpeed[1], m_SetPotion_DriveSpeed[1], m_SetPotion_Line[1]);
		m_WriteByte = Send_Command(0, (const char*)send_glue_data, m_SendDataLength);
		Sleep(5);
		g_IsClose = false;
		bUseGlueDispenser = true;
		m_Thread_State = HSI_THREAD_RUNNING;
		bRunGlueDispenser = HSI_THREAD_RUNNING;
		SetEvent(m_hTriggerEvent);
	}
	return rStatus;
}
void HSI_Motion::GluedispenserDone()//涂胶机完成
{
	if (bRunGlueDispenser == HSI_THREAD_RUNNING)
	{
		Sleep(100);
		int timeStart = GetTickCount();
		do
		{
			if (g_IsClose)
			{
				g_IsClose = false;
				break;
			}
			if (m_SO7_Serial.m_RecvData[0] == 2)
			{
				if ((m_SO7_Serial.m_RecvData[60] & 0x0f))
				{
					g_pLogger->SendAndFlushWithTime(L"[GluedispenserDone] Run End Normal\n");//涂胶机完成
					m_SO7_Serial.m_RecvData[60] = 0;
					break;
				}
			}
			Sleep(1);
			int timeEnd = GetTickCount();
			if (timeStart - timeEnd > 10 * 1000)
			{
				g_pLogger->SendAndFlushWithTime(L"[GluedispenserDone] Run End Abnormal\n");
				break;
			}
		} while (true);
		bRunGlueDispenser = HSI_THREAD_PAUSED;
		sEvenProp.Init();
		sEvenProp.EventType = HSI_EVENT_FUNCTION;
		sEvenProp.EventID = HSI_EVENT_MOTION_DISPENSER;
		sEvenProp.EventResponse = HSI_EVENT_FUNCTION_OK;
		EventCallback(sEvenProp);
	}
}
//===========================================================================
HSI_STATUS HSI_Motion::GetPntsDistance(double& pDistance, int& spTimeCount)
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		if (SetPotionRunEnd)
		{
			g_pLogger->SendAndFlushWithTime(L"[GetPntsDistance] PTPDistance = %.4f,spTimeCount = %d\n", PntToPntDistance, SpTimeCount);
			SetPotionRunEnd = false;
			pDistance = PntToPntDistance;
			spTimeCount = SpTimeCount;
		}
		else
		{
			pDistance = -100;
			spTimeCount = -1;
		}
	}
	return rStatus;
}
//========================运动控制参数读取及设置===================================================
int HSI_Motion::SpeedPercent(int AxisNum, double &Speed, int &DirveSpeed, int &StartSpeed, int &AccLine, int &DecLine, int &AccCurve, int &DecCurve)
{
	int MovetoSpeedGear = 0;
	int flag = 1;
	if (fabs(Speed) > 1.01)
	{
		MovetoSpeedGear = 0;
	}
	else if (fabs(Speed) > 0.81)
	{
		DirveSpeed = m_JogDriveSpeed[AxisNum][0];
		StartSpeed = m_JogStartSpeed[AxisNum][0];
		AccLine = m_JogAccLine[AxisNum][0];
		DecLine = m_JogDecLine[AxisNum][0];
		AccCurve = m_JogAccCurve[AxisNum][0];
		DecCurve = m_JogDecCurve[AxisNum][0];

	}
	else if (fabs(Speed) > 0.61)
	{
		DirveSpeed = m_JogDriveSpeed[AxisNum][1];
		StartSpeed = m_JogStartSpeed[AxisNum][1];
		AccLine = m_JogAccLine[AxisNum][1];
		DecLine = m_JogDecLine[AxisNum][1];
		AccCurve = m_JogAccCurve[AxisNum][1];
		DecCurve = m_JogDecCurve[AxisNum][1];
	}
	else if (fabs(Speed) > 0.41)
	{
		DirveSpeed = m_JogDriveSpeed[AxisNum][2];
		StartSpeed = m_JogStartSpeed[AxisNum][2];
		AccLine = m_JogAccLine[AxisNum][2];
		DecLine = m_JogDecLine[AxisNum][2];
		AccCurve = m_JogAccCurve[AxisNum][2];
		DecCurve = m_JogDecCurve[AxisNum][2];
	}
	else if (fabs(Speed) > 0.21)
	{
		DirveSpeed = m_JogDriveSpeed[AxisNum][3];
		StartSpeed = m_JogStartSpeed[AxisNum][3];
		AccLine = m_JogAccLine[AxisNum][3];
		DecLine = m_JogDecLine[AxisNum][3];
		AccCurve = m_JogAccCurve[AxisNum][3];
		DecCurve = m_JogDecCurve[AxisNum][3];
	}
	else if (fabs(Speed) > 0.01)
	{
		DirveSpeed = m_JogDriveSpeed[AxisNum][4];
		StartSpeed = m_JogStartSpeed[AxisNum][4];
		AccLine = m_JogAccLine[AxisNum][4];
		DecLine = m_JogDecLine[AxisNum][4];
		AccCurve = m_JogAccCurve[AxisNum][4];
		DecCurve = m_JogDecCurve[AxisNum][4];
	}
	else
	{
		MovetoSpeedGear = 0;
	}
	Speed = MovetoSpeedGear * flag;
	return (int)Speed;
}
//==========================JoyStick运动控制参数读取及设置=================================================
bool HSI_Motion::SpeedPercentJoyStick(int AxisNum, long &Speed, int &DirveSpeed, int &StartSpeed, int &AccLine, int &DecLine, int &AccCurve, int &DecCurve)
{
	Speed = Speed > 1000 ? 1000 : Speed;
	double dSpeedRate = (double)Speed / 1000;
	DirveSpeed = m_JogDriveSpeed[AxisNum][0] * (dSpeedRate*dSpeedRate);
	if (DirveSpeed >m_JogDriveSpeed[AxisNum][1])
	{
		DirveSpeed = m_JogDriveSpeed[AxisNum][0];
		StartSpeed = m_JogStartSpeed[AxisNum][0];
		AccLine = m_JogAccLine[AxisNum][0];
		DecLine = m_JogDecLine[AxisNum][0];
		AccCurve = m_JogAccCurve[AxisNum][0];
		DecCurve = m_JogDecCurve[AxisNum][0];
	}
	else if (DirveSpeed >m_JogDriveSpeed[AxisNum][2])
	{
		StartSpeed = m_JogStartSpeed[AxisNum][1];
		AccLine = m_JogAccLine[AxisNum][1];
		DecLine = m_JogDecLine[AxisNum][1];
		AccCurve = m_JogAccCurve[AxisNum][1];
		DecCurve = m_JogDecCurve[AxisNum][1];
	}
	else if (DirveSpeed >m_JogDriveSpeed[AxisNum][3])
	{
		StartSpeed = m_JogStartSpeed[AxisNum][2];
		AccLine = m_JogAccLine[AxisNum][2];
		DecLine = m_JogDecLine[AxisNum][2];
		AccCurve = m_JogAccCurve[AxisNum][2];
		DecCurve = m_JogDecCurve[AxisNum][2];
	}
	else if (DirveSpeed >m_JogDriveSpeed[AxisNum][4])
	{
		StartSpeed = m_JogStartSpeed[AxisNum][3];
		AccLine = m_JogAccLine[AxisNum][3];
		DecLine = m_JogDecLine[AxisNum][3];
		AccCurve = m_JogAccCurve[AxisNum][3];
		DecCurve = m_JogDecCurve[AxisNum][3];
	}
	else if (DirveSpeed == 0)
	{
		return false;
	}
	else
	{
		StartSpeed = m_JogStartSpeed[AxisNum][4];
		AccLine = m_JogAccLine[AxisNum][4];
		DecLine = m_JogDecLine[AxisNum][4];
		AccCurve = m_JogAccCurve[AxisNum][4];
		DecCurve = m_JogDecCurve[AxisNum][4];
	}
	return true;
}
//===========================================================================
HSI_STATUS HSI_Motion::GetSpeedXyz(int AxisNum, double &Speed)
{
	g_pLogger->SendAndFlushWithTime(L"[GetSpeedXyz] In\n");
	auto rStatus = HSI_STATUS_NORMAL;
	short AxisNumber = AxisConvertIndex(AxisNum);
	if (1==m_iSpeedType)
	{
		Speed = m_SetPotion_DriveSpeed[AxisNumber] * (m_Resolution[AxisNumber] * 50);
	}
	else
	{
		Speed = m_SetPotion_DriveSpeed[AxisNumber];
	}
	g_pLogger->SendAndFlushWithTime(L"[GetSpeedXyz] Out\n");
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::SetSpeedXyz(double Speed)
{
	auto rStatus = HSI_STATUS_NORMAL;
	g_pLogger->SendAndFlushWithTime(L"[SetSpeedXyz] In\n");
	if (Speed >= -1 && Speed <= 0)
	{
		g_pLogger->SendAndFlushWithTime(L"[SetSpeedXyz] Speed = %f, m_Jog_Auto_Focus = %f\n", Speed, m_Jog_Auto_Focus);
		m_JogDriveSpeed[3][4] = (int)fabs(Speed * m_Jog_Auto_Focus);
		g_pLogger->SendAndFlushWithTime(L"[SetSpeedXyz] m_Jog_SpeedZ[3][4] = %f\n", m_JogDriveSpeed[3][4]);
	}
	else
	{
		g_pLogger->SendAndFlushWithTime(L"[SetSpeedXyz] Speed = %f\n", Speed);
		if (1 == m_iSpeedType)
		{
			m_SetPotion_DriveSpeed[1] = (int)(Speed / (m_Resolution[1] * 50));
			if (m_SetPotion_DriveSpeed[1] > 1000 / (m_Resolution[1] * 50))
			{
				m_SetPotion_DriveSpeed[1] = 1000 / (m_Resolution[1] * 50);
			}
			if (m_SetPotion_DriveSpeed[1] < 0)
			{
				m_SetPotion_DriveSpeed[1] = 0;
			}
		}
		else
		{
			m_SetPotion_DriveSpeed[1] = (int)Speed;
			if (m_SetPotion_DriveSpeed[1] > 1000 / (m_Resolution[1] * 50))
			{
				m_SetPotion_DriveSpeed[1] = 1000 / (m_Resolution[1] * 50);
			}
			if (m_SetPotion_DriveSpeed[1] < 0)
			{
				m_SetPotion_DriveSpeed[1] = 0;
			}
		}
	}
	g_pLogger->SendAndFlushWithTime(L"[SetSpeedXyz] Out\n");
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::GetFocusSpeed(double &Speed)
{
	g_pLogger->SendAndFlushWithTime(L"[GetFocusSpeed] In\n");
	auto rStatus = HSI_STATUS_NORMAL;
	if (1==m_iSpeedType)
	{
		Speed = m_JogDriveSpeed[3][4] * (m_Resolution[3] * 50);
	}
	else
	{
		Speed = m_JogDriveSpeed[3][4];
	}
	g_pLogger->SendAndFlushWithTime(L"[GetFocusSpeed] Out\n");
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::SetFocusSpeed(double Speed)
{
	auto rStatus = HSI_STATUS_NORMAL;
	g_pLogger->SendAndFlushWithTime(L"[SetFocusSpeed] In\n");

	g_pLogger->SendAndFlushWithTime(L"[SetFocusSpeed] Speed = %f, m_Jog_Auto_Focus = %f\n", Speed, m_Jog_Auto_Focus);
	if (1==m_iSpeedType)
	{
		m_JogDriveSpeed[3][4] = Speed / (m_Resolution[3] * 50);
	}
	else
	{
		m_JogDriveSpeed[3][4] = Speed;
	}
	g_pLogger->SendAndFlushWithTime(L"[SetFocusSpeed] m_Jog_SpeedZ[3][4] = %f\n", m_JogDriveSpeed[4][3]);
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::GetAccelerationXyz(double &AccelX, double &AccelY, double &AccelZ)
{
	auto rStatus = HSI_STATUS_NORMAL;
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::SetAccelerationXyz(double  AccelX, double  AccelY, double  AccelZ)
{
	auto rStatus = HSI_STATUS_NORMAL;
	return rStatus;
}
//===========================================================================
double HSI_Motion::LimitOver(UINT AxisTypes, double &LimitPos)
{
	short AxisNumber = AxisConvertIndex(AxisTypes);
	if (g_pHSI_Motion)
	{
		switch (AxisNumber)
		{
		case 1: //轴1
		{
			if (LimitPos >= m_P_Work_Limit[1])
			{
				LimitPos = m_P_Work_Limit[1] - 5 * m_Resolution[1];
			}
			if (LimitPos <= m_N_Work_Limit[1])
			{
				LimitPos = m_N_Work_Limit[1] + 5 * m_Resolution[1];
			}
			break;
		}
		case 2:
		{
			if (LimitPos >= m_P_Work_Limit[2])
			{
				LimitPos = m_P_Work_Limit[2] - 5 * m_Resolution[2];
			}
			if (LimitPos <= m_N_Work_Limit[2])
			{
				LimitPos = m_N_Work_Limit[2] + 5 * m_Resolution[2];
			}
			break;
		}
		case 3:
		{
			if (LimitPos >= m_P_Work_Limit[3])
			{
				LimitPos = m_P_Work_Limit[3] - 5 * m_Resolution[3];
			}
			if (LimitPos <= m_N_Work_Limit[3])
			{
				LimitPos = m_N_Work_Limit[3] + 5 * m_Resolution[3];
			}
			break;
		}
		case 4:
		{
			if (LimitPos >= m_P_Work_Limit[4])
			{
				LimitPos = m_P_Work_Limit[4] - 5 * m_Resolution[4];
			}
			if (LimitPos <= m_N_Work_Limit[4])
			{
				LimitPos = m_N_Work_Limit[4] + 5 * m_Resolution[4];
			}
			break;
		}
		default:
			break;
		}

	}
	return LimitPos;
}
//===========================================================================
short HSI_Motion::AxisConvertIndex(UINT AxisTypes)
{
	short AxisNumber(0);
	switch (AxisTypes)
	{
	case HSI_MOTION_AXIS_X:
	{
		AxisNumber = 0x01;
		break;
	}
	case HSI_MOTION_AXIS_Y:
	{
		AxisNumber = 0x02;
		break;
	}
	case HSI_MOTION_AXIS_Z:
	{
		AxisNumber = 0x03;
		break;
	}
	case HSI_MOTION_AXIS_R:
	{
		AxisNumber = 0x04;
		break;
	}
	default:
	{
		g_pLogger->SendAndFlushWithTime(L"AxisConvertIndex failed,AxisTypes = %d,AxisNumber = %d\n", AxisTypes, AxisNumber);
		break;
	}
	}
	return AxisNumber;
}
//===========================================================================
short HSI_Motion::IndexConvertAxis(int Index)
{
	short AxisNumber(0);
	switch (Index)
	{
	case 1:
	{
		AxisNumber = AXIS_X;
		break;
	}
	case 2:
	{
		AxisNumber = AXIS_Y;
		break;
	}
	case 3:
	{
		AxisNumber = AXIS_Z;
		break;
	}
	case 4:
	{
		AxisNumber = AXIS_U;
		break;
	}
	default:
	{
		g_pLogger->SendAndFlushWithTime(L"IndexConvertAxis failed,Index = %d,AxisTypes = %d\n", Index, AxisNumber);
		break;
	}
	}
	return AxisNumber;
}
//===========================================================================
HSI_STATUS HSI_Motion::IsSupportedEx(UINT AxisTypes, UINT &Types)
{
	auto rStatus = HSI_STATUS_NORMAL;
	Types = 1;
	short AxisNumber = AxisConvertIndex(AxisTypes);
	if (AxisNumber < 1 && AxisNumber > 4)
	{
		rStatus = HSI_STATUS_NOT_SUPPORTED;
	}
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::StartupEx(UINT AxisTypes, bool bHome)
{
	auto rStatus = HSI_STATUS_NORMAL;
	g_pLogger->SendAndFlushWithTime(L"[StartupEx] In\n");

	g_pLogger->SendAndFlushWithTime(L"[StartupEx] Out\n");
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::GetScaleResolutionEx(UINT AxisTypes, double &Scale)
{
	auto rStatus = HSI_STATUS_NORMAL;
	short AxisNumber = AxisConvertIndex(AxisTypes);
	if (g_pHSI_Motion)
	{
		Scale = m_Resolution[AxisNumber];
	}
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::SetScaleResolutionEx(UINT AxisTypes, double Scale)
{
	auto rStatus = HSI_STATUS_NORMAL;
	short AxisNumber = AxisConvertIndex(AxisTypes);
	if (g_pHSI_Motion)
	{
		m_Resolution[AxisNumber] = (int)(Scale * 10000);
	}
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::GetPositionEx(UINT AxisTypes, double &Position, double &Time)
{
	auto rStatus = HSI_STATUS_NORMAL;
	UNREFERENCED_PARAMETER(AxisTypes);
	if (g_pHSI_Motion)
	{
		double position[4] = { 0.0 };
		short AxisNumber = AxisConvertIndex(AxisTypes);
		Position = m_PosForAllAxis[AxisNumber];
	}
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::SetPositionEx(UINT AxisTypes, double  Position, HSI_MOTION_MOVE_TYPE eType, double dSpeedGear)
{
	auto rStatus = HSI_STATUS_NORMAL;
	UNREFERENCED_PARAMETER(AxisTypes);
	if (g_pHSI_Motion)
	{
		g_pLogger->SendAndFlushWithTime(L"[SetPositionEx] In\n");
		short AxisNumber = AxisConvertIndex(AxisTypes);
		double Time = 0.0;
		double position[5] = { 0.0 };
		for (size_t i = 0; i < 5; i++)
		{
			position[i] = m_PosForAllAxis[i];
		}
		position[AxisNumber] = Position;
		SetPositionXyza(AxisTypes, position[1], position[2], position[3], position[4], eType, dSpeedGear);
		g_pLogger->SendAndFlushWithTime(L"[SetPositionEx] Out\n");
	}
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::SetPositionStep(UINT AxisTypes, double  Position, HSI_MOTION_MOVE_TYPE eType, double dSpeedGear)
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{

	}
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::GetSpeedEx(UINT AxisTypes, double &Speed)
{
	auto rStatus = HSI_STATUS_NORMAL;
	g_pLogger->SendAndFlushWithTime(L"[GetSpeedEx] In\n");
	short AxisNumber = AxisConvertIndex(AxisTypes);
	if (AxisNumber >= 1 && AxisNumber <= 4)
	{
		if (m_motorType == 1)
		{
			if (1==m_iSpeedType)
			{
				Speed = m_SetPotion_DriveSpeed[1] * (m_Resolution[1] * 50);
			}
			else
			{
				Speed = m_SetPotion_DriveSpeed[1];
			}
		}
		else
		{
			if (1==m_iSpeedType)
			{
				Speed = m_SetPotion_DriveSpeed[AxisNumber] * (m_Resolution[AxisNumber] * 50);
			}
			else
			{
				Speed = m_SetPotion_DriveSpeed[AxisNumber];
			}
		}
	}
	g_pLogger->SendAndFlushWithTime(L"[GetSpeedEx] Out\n");
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::SetSpeedEx(UINT AxisTypes, double Speed)
{
	auto rStatus = HSI_STATUS_NORMAL;
	g_pLogger->SendAndFlushWithTime(L"[SetSpeedEx] In\n");
	short AxisNumber = AxisConvertIndex(AxisTypes);
	if (Speed <= 0)
	{
		m_SetPotion_DriveSpeed[AxisNumber] = 0;
	}
	if (1==m_iSpeedType)
	{
		if (Speed >= 65000 * (m_Resolution[AxisNumber] * 50))
		{
			m_SetPotion_DriveSpeed[AxisNumber] = 65000;
		}
	}
	else
	{
		if (Speed >= 65000)
		{
			m_SetPotion_DriveSpeed[AxisNumber] = 65000;
		}
	}
	if (AxisNumber >= 1 && AxisNumber <= 4)
	{
		if (m_ForSoft == 1)
		{
			if (m_motorType == 1)
			{
				if (1==m_iSpeedType)
				{
					m_SetPotion_DriveSpeed[1] = (int)Speed/(m_Resolution[1] * 50);
				}
				else
				{
					m_SetPotion_DriveSpeed[1] = (int)Speed;
				}
			}
			else
			{
				g_pLogger->SendAndFlushWithTime(L"[SetSpeedEx] AxisNumber = %d,Speed = %f \n", AxisNumber, Speed);
				//if (!bSaveSpeedFlag)
				//{
				//	bSaveSpeedFlag = true;
				//	m_SaveAxisNum = AxisNumber;
				//	m_SaveAxisSpeed = m_SetPotion_DriveSpeed[AxisNumber];
				//	g_pLogger->SendAndFlushWithTime(L"[SetSpeedEx] m_SaveAxisNum = %d,m_SaveAxisSpeed = %d \n", m_SaveAxisNum, m_SaveAxisSpeed);
				//}
				//if (bSaveSpeedFlag && ((int)Speed == m_SetPotion_DriveSpeed[1] * (m_Resolution[1] * 50)))
				//{
				//	bSaveSpeedFlag = false;
				//	m_SetPotion_DriveSpeed[m_SaveAxisNum] = m_SaveAxisSpeed;
				//	return rStatus;
				//}
				if (1==m_iSpeedType)
				{
					m_SetPotion_DriveSpeed[AxisNumber] = (int)(Speed / (m_Resolution[AxisNumber] * 50));
				}
				else
				{
					m_SetPotion_DriveSpeed[AxisNumber] = (int)Speed;
				}
				g_pLogger->SendAndFlushWithTime(L"[SetSpeedEx] AxisNumber = %d\n", AxisNumber);
			}
		}
		else
		{
			if (m_motorType == 1)
			{
				m_SetPotion_DriveSpeed[AxisNumber] = (int)(Speed / (m_Resolution[AxisNumber] * 50));
			}
			else
			{
				m_SetPotion_DriveSpeed[AxisNumber] = (int)(Speed / (m_Resolution[AxisNumber] * 50));
			}
		}
	}
	g_pLogger->SendAndFlushWithTime(L"[SetSpeedEx] Out\n");
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::GetAccelerationEx(UINT AxisTypes, double &Accel)
{
	auto rStatus = HSI_STATUS_NORMAL;
	short AxisNumber = AxisConvertIndex(AxisTypes);
	if (AxisNumber >= 1 && AxisNumber <= 4)
	{
		if (m_motorType == 1)
		{
			Accel = m_SetPotion_StartSpeed[1] * (m_Resolution[1] * 500);
		}
		else
		{
			Accel = m_SetPotion_StartSpeed[AxisNumber] * (m_Resolution[AxisNumber] * 500);
		}
	}
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::SetAccelerationEx(UINT AxisTypes, double Accel)
{
	auto rStatus = HSI_STATUS_NORMAL;
	short AxisNumber = AxisConvertIndex(AxisTypes);
	if (Accel <= 0)
	{
		m_SetPotion_StartSpeed[AxisNumber] = 1;
	}
	if (AxisNumber >= 1 && AxisNumber <= 4)
	{
		if (m_motorType == 1)
		{
			m_SetPotion_StartSpeed[AxisNumber] = (int)(Accel / (m_Resolution[AxisNumber] * 500));
		}
		else
		{
			m_SetPotion_StartSpeed[AxisNumber] = (int)(Accel / (m_Resolution[AxisNumber] * 500));
		}
	}
	return rStatus;
}

//===========================================================================
VOID HSI_Motion::EventCallback(sHSIEventProperties& sEventProp)
{
	WaitForSingleObject(m_Thread_Mutex, INFINITE);
	if (g_pHSI_Sevenocean_EF3)
	{
		g_pHSI_Sevenocean_EF3->EventCallback(sEventProp);
	}
	ReleaseMutex(m_Thread_Mutex);
}
//===========================================================================
void HSI_Motion::CreateThread()
{
	if (m_Thread_Id == NULL)
	{
		m_Thread_State = HSI_THREAD_RUNNING;
		m_hTriggerEvent = CreateEvent(NULL, FALSE, NULL, NULL);
		m_Thread_Id = ::CreateThread(
			(LPSECURITY_ATTRIBUTES)NULL,
			0,
			(LPTHREAD_START_ROUTINE)m_Thread,
			(LPVOID)this,
			0,
			NULL);
		m_Thread_Mutex = CreateMutex(NULL, FALSE, NULL);
		TRACE("CreateThread!\n");
	}
}
//===========================================================================
void HSI_Motion::CloseThread()
{
	m_Thread_State = HSI_THREAD_EXIT;
	Sleep(20);
	SetEvent(m_hTriggerEvent);
	if (m_Thread_Id)
	{
		DWORD dwCode = STILL_ACTIVE;
		while (dwCode == STILL_ACTIVE)
		{
			GetExitCodeThread(m_Thread_Id, &dwCode);
			Sleep(1);
		}
	}
	SetEvent(m_hTriggerEvent);
	CloseHandle(m_hTriggerEvent);
	m_Thread_State = HSI_THREAD_EXIT;
	ReleaseMutex(m_Thread_Mutex);
	CloseHandle(m_Thread_Mutex);
	m_Thread_Id = NULL;
	m_hTriggerEvent = NULL;
	m_Thread_Mutex = NULL;
	TRACE("CloseThread!\n");

}
//===========================================================================
unsigned __stdcall HSI_Motion::m_Thread(LPVOID pThis)
{
	HSI_Motion* _This = (HSI_Motion*)pThis;
	for (;;)
	{
		TRACE("m_Thread!\n");
		if (m_Thread_State == THREAD_EXIT)
			ExitThread(0);
		WaitForSingleObject(m_hTriggerEvent, INFINITE);
		switch (m_Thread_State)
		{
		case HSI_THREAD_RUNNING:
		{
			TRACE("HSI_THREAD_RUNNING.\r\n");
			if (bRunGlueDispenser == HSI_THREAD_RUNNING)
				_This->GluedispenserDone();
			else
			{
				_This->UpdateMotionState();
				_This->UpdateMotionStateEx();
			}
			break;
		}
		case HSI_THREAD_PAUSED:
		{
			TRACE("HSI_THREAD_PAUSED.\r\n");
			break;
		}
		case HSI_THREAD_EXIT:
		{
			TRACE("HSI_THREAD_EXIT.\r\n");
			ExitThread(0);
			break;
		}
		default:
			break;
		}
	}
}
//===========================================================================
void HSI_Motion::CreateThreadProbe()
{
	if (m_Thread_IdProbe == NULL)
	{
		m_Thread_StateProbe = HSI_THREAD_RUNNING;
		m_hTriggerEventProbe = CreateEvent(NULL, FALSE, NULL, NULL);
		m_Thread_IdProbe = ::CreateThread(
			(LPSECURITY_ATTRIBUTES)NULL,
			0,
			(LPTHREAD_START_ROUTINE)m_ThreadProbe,
			(LPVOID)this,
			0,
			NULL);
		m_Thread_MutexProbe = CreateMutex(NULL, FALSE, NULL);
		TRACE("CreateThread!\n");
	}
}
//===========================================================================
void HSI_Motion::CloseThreadProbe()
{
	m_Thread_StateProbe = HSI_THREAD_EXIT;
	Sleep(20);
	SetEvent(m_hTriggerEventProbe);
	if (m_Thread_IdProbe)
	{
		DWORD dwCode = STILL_ACTIVE;
		while (dwCode == STILL_ACTIVE)
		{
			GetExitCodeThread(m_Thread_IdProbe, &dwCode);
			Sleep(1);
		}
	}
	SetEvent(m_hTriggerEventProbe);
	CloseHandle(m_hTriggerEventProbe);
	m_Thread_StateProbe = HSI_THREAD_EXIT;
	ReleaseMutex(m_Thread_MutexProbe);
	CloseHandle(m_Thread_MutexProbe);
	m_Thread_IdProbe = NULL;
	m_hTriggerEventProbe = NULL;
	m_Thread_MutexProbe = NULL;
	TRACE("CloseThread!\n");

}
//===========================================================================
unsigned __stdcall HSI_Motion::m_ThreadProbe(LPVOID pThis)
{
	HSI_Motion* _This = (HSI_Motion*)pThis;
	for (;;)
	{
		TRACE("m_Thread!\n");
		if (m_Thread_StateProbe == THREAD_EXIT)
			ExitThread(0);
		WaitForSingleObject(m_hTriggerEventProbe, INFINITE);
		switch (m_Thread_StateProbe)
		{
		case HSI_THREAD_RUNNING:
		{
			TRACE("HSI_THREAD_RUNNING.\r\n");
			_This->UpdateMotionStateProbe();
			break;
		}
		case HSI_THREAD_PAUSED:
		{
			TRACE("HSI_THREAD_PAUSED.\r\n");
			break;
		}
		case HSI_THREAD_EXIT:
		{
			TRACE("HSI_THREAD_EXIT.\r\n");
			ExitThread(0);
			break;
		}
		default:
			break;
		}
	}
}
//===========================================================================
void HSI_Motion::CreateThreadIO()
{
	if (m_Thread_IdIO == NULL)
	{
		m_Thread_StateIO = HSI_THREAD_RUNNING;
		m_hTriggerEventIO = CreateEvent(NULL, FALSE, NULL, NULL);
		m_Thread_IdIO = ::CreateThread(
			(LPSECURITY_ATTRIBUTES)NULL,
			0,
			(LPTHREAD_START_ROUTINE)m_ThreadIO,
			(LPVOID)this,
			0,
			NULL);
		m_Thread_MutexIO = CreateMutex(NULL, FALSE, NULL);
		TRACE("CreateThreadIO!\n");
	}
}
//===========================================================================
void HSI_Motion::CloseThreadIO()
{
	m_Thread_StateIO = HSI_THREAD_EXIT;
	Sleep(20);
	SetEvent(m_hTriggerEventIO);
	if (m_Thread_IdIO)
	{
		DWORD dwCode = STILL_ACTIVE;
		while (dwCode == STILL_ACTIVE)
		{
			GetExitCodeThread(m_Thread_IdIO, &dwCode);
			Sleep(1);
		}
	}
	SetEvent(m_hTriggerEventIO);
	CloseHandle(m_hTriggerEventIO);
	m_Thread_StateIO = HSI_THREAD_EXIT;
	ReleaseMutex(m_Thread_MutexIO);
	CloseHandle(m_Thread_MutexIO);
	m_Thread_IdIO = NULL;
	m_hTriggerEventIO = NULL;
	m_Thread_MutexIO = NULL;
	TRACE("CloseThreadIO!\n");

}
//===========================================================================
unsigned __stdcall HSI_Motion::m_ThreadIO(LPVOID pThis)
{
	HSI_Motion* _This = (HSI_Motion*)pThis;
	for (;;)
	{
		TRACE("m_ThreadIO!\n");
		if (m_Thread_StateIO == THREAD_EXIT)
			ExitThread(0);
		WaitForSingleObject(m_hTriggerEventIO, INFINITE);
		switch (m_Thread_StateIO)
		{
		case HSI_THREAD_RUNNING:
		{
			TRACE("HSI_THREAD_RUNNING.\r\n");
			_This->UpdateMotionStateIO();
			break;
		}
		case HSI_THREAD_PAUSED:
		{
			TRACE("HSI_THREAD_PAUSED.\r\n");
			break;
		}
		case HSI_THREAD_EXIT:
		{
			TRACE("HSI_THREAD_EXIT.\r\n");
			ExitThread(0);
			break;
		}
		default:
			break;
		}
	}
}
//===========================================================================
void HSI_Motion::CreateThreadData()
{
	if (m_Thread_IdData == NULL)
	{
		m_Thread_StateData = HSI_THREAD_RUNNING;
		m_hTriggerEventData = CreateEvent(NULL, FALSE, NULL, NULL);
		m_Thread_IdData = ::CreateThread(
			(LPSECURITY_ATTRIBUTES)NULL,
			0,
			(LPTHREAD_START_ROUTINE)m_ThreadData,
			(LPVOID)this,
			0,
			NULL);
		m_Thread_MutexData = CreateMutex(NULL, FALSE, NULL);
		TRACE("CreateThreadData!\n");
	}
}
//===========================================================================
void HSI_Motion::CloseThreadData()
{
	m_Thread_StateData = HSI_THREAD_EXIT;
	Sleep(20);
	SetEvent(m_hTriggerEventData);
	if (m_Thread_IdData)
	{
		DWORD dwCode = STILL_ACTIVE;
		while (dwCode == STILL_ACTIVE)
		{
			GetExitCodeThread(m_Thread_IdData, &dwCode);
			Sleep(1);
		}
	}
	SetEvent(m_hTriggerEventData);
	CloseHandle(m_hTriggerEventData);
	m_Thread_StateData = HSI_THREAD_EXIT;
	ReleaseMutex(m_Thread_MutexData);
	CloseHandle(m_Thread_MutexData);
	m_Thread_IdData = NULL;
	m_hTriggerEventData = NULL;
	m_Thread_MutexData = NULL;
	TRACE("CloseThreadData!\n");

}
//===========================================================================
unsigned __stdcall HSI_Motion::m_ThreadData(LPVOID pThis)
{
	HSI_Motion* _This = (HSI_Motion*)pThis;
	for (;;)
	{
		TRACE("m_ThreadData!\n");
		if (m_Thread_StateData == THREAD_EXIT)
			ExitThread(0);
		WaitForSingleObject(m_hTriggerEventData, INFINITE);
		switch (m_Thread_StateData)
		{
		case HSI_THREAD_RUNNING:
		{
			TRACE("HSI_THREAD_RUNNING.\r\n");
			_This->UpdateMotionStateData();//获取最新位置命令
			break;
		}
		case HSI_THREAD_PAUSED:
		{
			TRACE("HSI_THREAD_PAUSED.\r\n");
			break;
		}
		case HSI_THREAD_EXIT:
		{
			TRACE("HSI_THREAD_EXIT.\r\n");
			ExitThread(0);
			break;
		}
		default:
			break;
		}
	}
}
//===========================================================================

//JOG运行到软限位的运动调节
//===========================================================================
void HSI_Motion::UpdateMotionStateJOGStop()
{
	unsigned char send_JogLimt_data[64] = { 0 };
	UINT axis = 0;
	double posx = 0.0;
	double posy = 0.0;
	double posz = 0.0;
	double post = 0.0;
	int slowdowndistance = 0;
	while (m_Thread_StateJOGStop == HSI_THREAD_RUNNING)
	{
		while (jogMoving)
		{
			slowdowndistance = jogspeed * 13;
			Sleep(3);
			GetPositionXyz(axis, posx, posy, posz, post);
			switch (jogAxisNum)
			{
			case 1:
			{
				if (jogDirFlag)
				{
					double limitx = m_P_Work_Limit[1];
					if ((limitx - posx) / m_Resolution[1] < slowdowndistance)
					{
						send_JogLimt_data[0] = 0x01;
						send_JogLimt_data[1] = 0x14;
						send_JogLimt_data[2] = 0x01;
						m_WriteByte = Send_Command(0, (const char*)send_JogLimt_data, m_SendDataLength);
						Sleep(5);
						jogMoving = false;
					}
				}
				else
				{
					double limitx = m_N_Work_Limit[1];
					if ((posx - limitx) / m_Resolution[1] < slowdowndistance)
					{
						send_JogLimt_data[0] = 0x01;
						send_JogLimt_data[1] = 0x14;
						send_JogLimt_data[2] = 0x01;
						m_WriteByte = Send_Command(0, (const char*)send_JogLimt_data, m_SendDataLength);
						Sleep(5);
						jogMoving = false;
					}
				}
			}  break;
			case 2:
			{
				if (jogDirFlag)
				{
					double limity = m_P_Work_Limit[2];
					if ((limity - posy) / m_Resolution[2] < slowdowndistance)
					{
						send_JogLimt_data[0] = 0x01;
						send_JogLimt_data[1] = 0x14;
						send_JogLimt_data[2] = 0x02;
						m_WriteByte = Send_Command(0, (const char*)send_JogLimt_data, m_SendDataLength);
						Sleep(5);
						jogMoving = false;
					}
				}
				else
				{
					double limity = m_N_Work_Limit[2];
					if ((posy - limity) / m_Resolution[2] < slowdowndistance)
					{
						send_JogLimt_data[0] = 0x01;
						send_JogLimt_data[1] = 0x14;
						send_JogLimt_data[2] = 0x02;
						m_WriteByte = Send_Command(0, (const char*)send_JogLimt_data, m_SendDataLength);
						Sleep(5);
						jogMoving = false;
					}
				}
			}  break;
			case 3:
			{
				if (jogDirFlag)
				{
					double limitz = m_P_Work_Limit[3];
					if ((limitz - posz) / m_Resolution[3] < slowdowndistance)
					{
						send_JogLimt_data[0] = 0x01;
						send_JogLimt_data[1] = 0x14;
						send_JogLimt_data[2] = 0x04;
						m_WriteByte = Send_Command(0, (const char*)send_JogLimt_data, m_SendDataLength);
						Sleep(5);
						jogMoving = false;
					}
				}
				else
				{
					double limitz = m_N_Work_Limit[3];
					if ((posz - limitz) / m_Resolution[3] < slowdowndistance)
					{
						send_JogLimt_data[0] = 0x01;
						send_JogLimt_data[1] = 0x14;
						send_JogLimt_data[2] = 0x04;
						m_WriteByte = Send_Command(0, (const char*)send_JogLimt_data, m_SendDataLength);
						Sleep(5);
						jogMoving = false;
					}
				}
			}  break;
			case 4:
			{

				//第四轴未添加
			} break;
			default:
				break;
			}
		}
		Sleep(2);
	}
}
//===========================================================================
void HSI_Motion::CreateThreadJOGStop() //JOG运行到软限位的运动调节
{
	if (m_Thread_IdJOGStop == NULL)
	{
		m_Thread_StateJOGStop = HSI_THREAD_RUNNING;
		m_hTriggerEventJOGStop = CreateEvent(NULL, FALSE, NULL, NULL);
		m_Thread_IdJOGStop = ::CreateThread(
			(LPSECURITY_ATTRIBUTES)NULL,
			0,
			(LPTHREAD_START_ROUTINE)m_ThreadJOGStop,
			(LPVOID)this,
			0,
			NULL);
		m_Thread_MutexJOGStop = CreateMutex(NULL, FALSE, NULL);
		TRACE("CreateThreadJOGStop!\n");
	}
}
//===========================================================================
void HSI_Motion::CloseThreadJOGStop()
{
	m_Thread_StateJOGStop = HSI_THREAD_EXIT;
	Sleep(20);
	SetEvent(m_hTriggerEventJOGStop);
	if (m_Thread_IdData)
	{
		DWORD dwCode = STILL_ACTIVE;
		while (dwCode == STILL_ACTIVE)
		{
			GetExitCodeThread(m_Thread_IdJOGStop, &dwCode);
			Sleep(1);
		}
	}
	SetEvent(m_hTriggerEventJOGStop);
	CloseHandle(m_hTriggerEventJOGStop);
	m_Thread_StateJOGStop = HSI_THREAD_EXIT;
	ReleaseMutex(m_Thread_MutexJOGStop);
	CloseHandle(m_Thread_MutexJOGStop);
	m_Thread_IdJOGStop = NULL;
	m_hTriggerEventJOGStop = NULL;
	m_Thread_MutexJOGStop = NULL;
	TRACE("CloseThreadJOGStop!\n");

}
//===========================================================================
unsigned __stdcall HSI_Motion::m_ThreadJOGStop(LPVOID pThis) //JOG运行到软限位的运动调节
{
	HSI_Motion* _This = (HSI_Motion*)pThis;
	for (;;)
	{
		TRACE("m_ThreadJOGStop!\n");
		if (m_Thread_StateData == THREAD_EXIT)
			ExitThread(0);
		WaitForSingleObject(m_hTriggerEventJOGStop, INFINITE);
		switch (m_Thread_StateJOGStop)
		{
		case HSI_THREAD_RUNNING:
		{
			TRACE("HSI_THREAD_RUNNING.\r\n");
			_This->UpdateMotionStateJOGStop();
			break;
		}
		case HSI_THREAD_PAUSED:
		{
			TRACE("HSI_THREAD_PAUSED.\r\n");
			break;
		}
		case HSI_THREAD_EXIT:
		{
			TRACE("HSI_THREAD_EXIT.\r\n");
			ExitThread(0);
			break;
		}
		default:
			break;
		}
	}
}
//===========================================================================
//无用
HSI_STATUS HSI_Motion::IOStep(bool RunSts)
{
	auto rStatus = HSI_STATUS_NORMAL;
	m_MSTRunFlag = RunSts;
	if (g_pHSI_Motion)
	{
	}
	return rStatus;

}
//===========================================================================
HSI_STATUS HSI_Motion::IOprogram(byte* SendData, int length)
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		Send_Command(0, (const char*)SendData, m_SendDataLength);
	}
	return rStatus;

}

HSI_STATUS HSI_Motion::FindOriginTest(bool type)
{
	auto rStatus = HSI_STATUS_NORMAL;
	byte findorigdata[64] = { 0 };
	findorigdata[0] = 0x04;
	if (type == true)
	{
		findorigdata[1] = 0x18;
		findorigdata[2] = (byte)(m_Resolution[1] * 10000);
		Send_Command(0, (const char*)findorigdata, 64);
	}
	else
	{
		findorigdata[1] = 0x19;
		findorigdata[2] = (byte)(m_Resolution[1] * 10000);
		Send_Command(0, (const char*)findorigdata, 64);
	}
	return rStatus;
}

//===========================================================================
BOOL HSI_Motion::Send_Command(int com, const char* _SendData, DWORD SendDataLength)
{
	BOOL rStatus = TRUE;
	WaitForSingleObject(g_RW_Data_Mutex, INFINITE);
	if (m_bConnected && (m_IsUseEF3 == 1) && (com == 0))
	{
		int iWriteByte = m_SO7_Serial.Send(_SendData, 64);
		if (iWriteByte == 0)
		{
			int iError = GetLastError();
			g_pLogger->SendAndFlushWithTime(L"[Send_Command] GetLastError = %d\n", iError);
			if (iError == 1167)
			{
				g_pLogger->SendAndFlushWithTime(L"[Send_Command] GetLastError is 1167,EF3 device is no connect\n");
				m_Thread_StateData = THREAD_PAUSED;
				if (m_bConnected == true && g_IsClose == false)
				{
					g_pLogger->SendAndFlushWithTime(L"[Send_Command] GetLastError is 1167,EF3 device is no connect\n");
					sEvenProp.Init();
					sEvenProp.EventType = HSI_EVENT_ERROR;
					sEvenProp.EventID = HSI_EVENT_MOTION;
					sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK;
					strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "EF3控制器已断开连接");
					EventCallback(sEvenProp);
				}
			}
			g_pLogger->SendAndFlushWithTime(L"[Send_Command] m_SO7_Serial[%d].Send failed\n", com);
			rStatus = FALSE;
		}
		//	Sleep(5);
		//	m_SO7_Serial.OnReceive();
	}
	ReleaseMutex(g_RW_Data_Mutex);
	return rStatus;
};


/////////////////////////////////////////////////////////////////////////
#pragma region //网口通信8路 26路光源板通信

#pragma warning(disable:4996)
TCPIP_RETURN_CODE HSI_Motion::TCPConnect(int index, char* Address, u_short port)
{
	TCPIP_RETURN_CODE rCode = TCPIP_CONNECT_OK;
	if (!m_tcpCntFlag[index])
	{
		int retVal(0), errorCode(0);
		first = true;
		retVal = Init_Winsock();
		if (!retVal)
		{
			return TCPIP_INIT_WINSOCK_ERROR;
		}
		m_socket[index] = socket(AF_INET, SOCK_STREAM, 0);
		if (m_socket[index] == INVALID_SOCKET)
		{
			rCode = TCPIP_INVAILD_SOCKET;
		}
		else
		{
			int iMode = 1;
			int i = 0;
			int retVal = ioctlsocket(m_socket[index], FIONBIO, (u_long FAR*)&iMode);//非阻塞连接
			if (retVal == SOCKET_ERROR)
			{
				closesocket(m_socket[index]);
				WSACleanup();
				return TCPIP_INIT_WINSOCK_ERROR;
			}
			SOCKADDR_IN addSrv;
			addSrv.sin_addr.S_un.S_addr = inet_addr(Address);
			addSrv.sin_family = AF_INET;
			addSrv.sin_port = htons(port);
			while (true)
			{
				retVal = connect(m_socket[index], (SOCKADDR*)&addSrv, sizeof(SOCKADDR));
				if (retVal)
				{
					errorCode = WSAGetLastError();
					if (errorCode == WSAEWOULDBLOCK)
					{
						i++;
						if (i > 400)
							break;
						Sleep(5);
						continue;
					}
					else if (errorCode == WSAEISCONN)
					{
						break;
					}
					else
					{
						first = true;
						closesocket(m_socket[index]);
						WSACleanup();
						rCode = (TCPIP_RETURN_CODE)errorCode;
						return rCode;
					}
				}
			}
			iMode = 0;
			retVal = ioctlsocket(m_socket[index], FIONBIO, (u_long FAR*)&iMode);//设置阻塞
			if (retVal == SOCKET_ERROR)
			{
				closesocket(m_socket[index]);
				WSACleanup();
				return TCPIP_INIT_WINSOCK_ERROR;
			}
		}
	}
	return rCode;
}

void HSI_Motion::DisConnect()
{
	if (m_socket)
	{
		if (LightSend != 0 || IOSend != 0 || OrderSend != 0)
		{
			Sleep(20);
		}
		first = true;
		Exit_Thread();
		WSACleanup();
		for (size_t i = 0; i < m_bISUseMoreLights; i++)
		{
			closesocket(m_socket[i]);
			m_socket[i] = NULL;
		}
	}
}

unsigned __stdcall HSI_Motion::m_ThreadSendTCP(LPVOID pThis)
{
	HSI_Motion* _This = (HSI_Motion*)pThis;
	for (;;)
	{
		TRACE("m_Thread!\n");
		Sleep(2);
		if (m_ThreadTCP_State == TCPIP_THREAD_EXIT)
			ExitThread(0);
		switch (m_ThreadTCP_State)
		{
		case TCPIP_THREAD_RUNNING:
		{
			TRACE("TCPIP_THREAD_RUNNING.\r\n");
			_This->TCPSend();
			break;
		}
		case TCPIP_THREAD_PAUSED:
		{
			break;
		}
		case TCPIP_THREAD_EXIT:
		{
			ExitThread(0);
			break;
		}
		default:
			break;
		}
	};
}


TCPIP_RETURN_CODE HSI_Motion::TCPSend()
{
	if (m_tcpCntFlag[m_selectedIndex])
	{
		bool recvFlag = false;
		int bytesSend = 0;
		if (LightSend > 0)
		{
			g_pLogger->SendAndFlushWithTime(L"[TCPSend] LightSend = %d,m_selectedIndex = %d\n", LightSend, m_selectedIndex);
			LightSend--;
			bytesSend = send(m_socket[m_selectedIndex], (const char*)lightdata, 64, 0);
			for (size_t i = 0; i < 64; i++)
			{
				TempData[i] = lightdata[i];
			}
		}
		else if (IOSend > 0)
		{
			IOSend--;
			bytesSend = send(m_socket[m_selectedIndex], (const char*)IOdata, 64, 0);
			for (size_t i = 0; i < 64; i++)
			{
				TempData[i] = IOdata[i];
			}
		}
		else if (OrderSend > 0)
		{
			OrderSend--;
			bytesSend = send(m_socket[m_selectedIndex], (const char*)Orderdata, 64, 0);
			for (size_t i = 0; i < 64; i++)
			{
				TempData[i] = Orderdata[i];
			}
		}
		else
		{
			if (m_Led8MotionFlag[m_selectedIndex])
			{
				//g_pLogger->SendAndFlushWithTime(L"[TCPSend] IOCheck8\n");
				IOCheck[0] = 0x01;
				IOCheck[1] = 0x04;
			}
			else
			{
				//g_pLogger->SendAndFlushWithTime(L"[TCPSend] IOCheck26\n");
				IOCheck[0] = 0x01;
				IOCheck[1] = 0x01;
			}
			bytesSend = send(m_socket[m_selectedIndex], (const char*)IOCheck, 64, 0);
			for (size_t i = 0; i < 64; i++)
			{
				TempData[i] = IOCheck[i];
			}
		}
		if (bytesSend == SOCKET_ERROR)
		{
			bytesSend = WSAGetLastError();
			if (bytesSend == WSAEWOULDBLOCK || bytesSend == WSAENOTSOCK)
			{
				Sleep(2);
				bytesSend = send(m_socket[m_selectedIndex], (const char*)TempData, 64, 0);
				//have to wait for the next receive event
			}
			else
			{

				g_pLogger->SendAndFlushWithTime(L"[TCPSend] ERROR1 bytesSend = %d\n", bytesSend);
				sEvenProp.Init();
				sEvenProp.EventType = HSI_EVENT_ERROR;
				sEvenProp.EventID = HSI_EVENT_MOTION;
				sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK;
				strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "光源板已断开连接");
				EventCallback(sEvenProp);
				m_ThreadTCP_State = TCPIP_THREAD_EXIT;
				closesocket(m_socket[m_selectedIndex]);
				WSACleanup();
				return TCPIP_CONNECT_STATUS_UNKNOWN;
			}
		}
		//ZeroMemory(tReciveData, TCPIP_MAX_DAT_SIZE);
		int bytesReceived = 0;
		int num = 0;
		bytesReceived = recv(m_socket[m_selectedIndex], (char *)tReciveData, TCPIP_MAX_DAT_SIZE, 0);
		if (bytesReceived == SOCKET_ERROR)
		{
			bytesReceived = WSAGetLastError();
			if (bytesReceived == WSAEWOULDBLOCK || bytesReceived == WSAENOTSOCK)
			{
				//ZeroMemory(tReciveData, TCPIP_MAX_DAT_SIZE);
				bytesReceived = recv(m_socket[m_selectedIndex], (char *)tReciveData, TCPIP_MAX_DAT_SIZE, 0);
				if (bytesReceived == SOCKET_ERROR)
				{
					recvFlag = false;
				}
				else
				{
					recvFlag = true;
				}
				//if (num > 5)
				//{
				//	break;
				//}
				//Sleep(1);
				//num++;
				////have to wait for the next receive event
				//continue;
			}
			else
			{
				g_pLogger->SendAndFlushWithTime(L"[TCPReceived] ERROR1 bytesReceived = %d\n", bytesReceived);
				sEvenProp.Init();
				sEvenProp.EventType = HSI_EVENT_ERROR;
				sEvenProp.EventID = HSI_EVENT_MOTION;
				sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK;
				strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "光源板已断开连接");
				EventCallback(sEvenProp);
				m_ThreadTCP_State = TCPIP_THREAD_EXIT;
				closesocket(m_socket[m_selectedIndex]);
				WSACleanup();
				return TCPIP_CONNECT_STATUS_UNKNOWN;
			}
		}
		else
		{
			recvFlag = true;
		}
		if (!recvFlag)
		{
			bytesSend = send(m_socket[m_selectedIndex], (const char*)TempData, 64, 0);
			if (bytesSend == SOCKET_ERROR)
			{
				bytesSend = WSAGetLastError();
				if (bytesSend == WSAEWOULDBLOCK)
				{
					bytesSend = send(m_socket[m_selectedIndex], (const char*)TempData, 64, 0);
					if (bytesSend == SOCKET_ERROR)
					{
						g_pLogger->SendAndFlushWithTime(L"[TCPSend] ERROR2\n");
					}
					//have to wait for the next receive event
				}
			}
			bytesReceived = recv(m_socket[m_selectedIndex], (char *)tReciveData, TCPIP_MAX_DAT_SIZE, 0);
			if (bytesReceived == SOCKET_ERROR)
			{
				bytesReceived = WSAGetLastError();
				if (bytesReceived == WSAEWOULDBLOCK)
				{
					//ZeroMemory(tReciveData, TCPIP_MAX_DAT_SIZE);
					bytesReceived = recv(m_socket[m_selectedIndex], (char *)tReciveData, TCPIP_MAX_DAT_SIZE, 0);
					if (bytesReceived == SOCKET_ERROR)
					{
						g_pLogger->SendAndFlushWithTime(L"[TCPReceived] ERROR2\n");
					}
				}
			}
		}
		Sleep(5);
	}
	return TCPIP_CONNECT_OK;
}


void HSI_Motion::Create_Thread()
{
	if (!m_ThreadTCP_Id)
	{
		m_ThreadTCP_State = TCPIP_THREAD_RUNNING;
		m_ThreadTCP_Id = ::CreateThread((LPSECURITY_ATTRIBUTES)NULL, 0, (LPTHREAD_START_ROUTINE)m_ThreadSendTCP, (LPVOID)this, 0, NULL);

	}
}

void HSI_Motion::Exit_Thread()
{
	m_ThreadTCP_State = TCPIP_THREAD_EXIT;
	if (m_ThreadTCP_Id)
	{
		DWORD dwCode = STILL_ACTIVE;
		while (dwCode == STILL_ACTIVE)
		{
			GetExitCodeThread(m_ThreadTCP_Id, &dwCode);
			Sleep(1);
		}
	}
	m_ThreadTCP_State = TCPIP_THREAD_EXIT;
	m_ThreadTCP_Id = NULL;
}

int HSI_Motion::Init_Winsock()
{
	int nRet;
	if (first)
	{
		WSADATA stWSAData;    /* WinSock DLL Info */
		nRet = WSAStartup(WSA_VERSION, &stWSAData);
		if (nRet)
		{
			return FALSE;
		}
		first = false;
	}
	return TRUE;
}
#pragma endregion
//////////////////////////////////////////////////////////////////////////
//===========================================================================
HSI_STATUS HSI_Motion::CollectPos(bool isEnable, MOTOR_AXISCHOOES_CMD axis, short cycle)
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		unsigned char send_glue_data[64] = { 0 };
		send_glue_data[0] = 0x01;
		send_glue_data[1] = 0x1E;
		send_glue_data[2] = isEnable ? 1 : 2;
		send_glue_data[3] = ((cycle >> 8) & 0xff);
		send_glue_data[4] = (cycle & 0xff);
		send_glue_data[5] = axis;

		Send_Command(0, (const char*)send_glue_data, 64);

	}
	return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::SetAllGears()
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		for (size_t i = 1; i < 5; i++)
		{
			unsigned char send_glue_data[64] = { 0 };
			send_glue_data[0] = 0x01;
			send_glue_data[1] = 0x01;
			send_glue_data[2] = 0x01 << (i - 1);	//轴号
			send_glue_data[3] = 0x08;

			for (size_t j = 0; j < 5; j++)
			{
				send_glue_data[4 + 6 * j] = ((m_JogStartSpeed[i][4 - j] >> 8) & 0xff);
				send_glue_data[5 + 6 * j] = (m_JogStartSpeed[i][4 - j] & 0xff);
				send_glue_data[6 + 6 * j] = ((m_JogDriveSpeed[i][4 - j] >> 8) & 0xff);
				send_glue_data[7 + 6 * j] = (m_JogDriveSpeed[i][4 - j] & 0xff);
				send_glue_data[8 + 6 * j] = ((m_JogAccLine[i][4 - j] >> 8) & 0xff);
				send_glue_data[9 + 6 * j] = (m_JogAccLine[i][4 - j] & 0xff);
			}
			Send_Command(0, (const char*)send_glue_data, 64);
			Sleep(10);
		}
	}
	return rStatus;
}
//===========================================================================