LM-Middleware/HSI_HexagonMI_EF3/HSI_Motion.cpp

// HSI_Motion.cpp : 定义 DLL 的初始化例程。
#include "stdafx.h"
#include "HSI.h"
#include "HSI_Sevenocean_EF3.h"
#include "HSI_Motion.h"

#include <cctype>

#include "logger.h"
#include "SevenOcean/CMMIO_SERIAL.h"
#include "version.h"
#include <math.h>
#include <fstream>
#include <iomanip>
#include <conio.h>
#include <stdio.h>
#include <iostream>
#include "windows.h"
#include "ACS/ACSC.h" //引入ACS运动控制卡头文件
#include "ClMS.h"
using namespace std;

#ifdef _DEBUG
#define new DEBUG_NEW
#endif

//===========================================================================
HSI_Motion* g_pHSI_Motion = nullptr;
CLogger extern* g_pLogger = nullptr;
HANDLE hCom; //串口句柄
HANDLE handleACS; // 运动控制句柄

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 = nullptr;
HANDLE HSI_Motion::m_Thread_Mutex = nullptr;
HANDLE HSI_Motion::g_RW_Data_Mutex = nullptr;
HANDLE HSI_Motion::g_WR_ToMove_Mutex = nullptr;
HANDLE HSI_Motion::g_Lock_JogAndTrigger = nullptr;
HANDLE HSI_Motion::m_hTriggerEvent;
HANDLE HSI_Motion::m_Thread_IdIO = nullptr;
HANDLE HSI_Motion::m_Thread_MutexIO = nullptr;
HANDLE HSI_Motion::m_hTriggerEventIO;
HANDLE HSI_Motion::m_Thread_IdData = nullptr;
HANDLE HSI_Motion::m_Thread_MutexData = nullptr;
HANDLE HSI_Motion::m_hTriggerEventData;
HANDLE HSI_Motion::m_Thread_IdProbe = nullptr;
HANDLE HSI_Motion::m_Thread_MutexProbe = nullptr;
HANDLE HSI_Motion::m_hTriggerEventProbe;

HANDLE HSI_Motion::m_Thread_IdJOGStop = nullptr;
HANDLE HSI_Motion::m_Thread_MutexJOGStop = nullptr;
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 = nullptr;
int HSI_Motion::m_ThreadTCP_State = TCPIP_THREAD_PAUSED;
SOCKET m_socket[4] = { 0 };

void HSI_Motion::ErrorsHandler()
{
	if (handleACS != ACSC_INVALID)
	{
		char ErrorStr[256];
		int ErrorCode = 0;
		int Received = 0;
		ErrorCode = GetLastError();
		if (acsc_GetErrorString(handleACS, // communication handle
			ErrorCode, // error code
			ErrorStr, // buffer for the error explanation
			255, // available buffer length
			&Received // number of actually received bytes
		))
		{
			ErrorStr[Received] = '\0';
			printf("Motion Error: %d [%s]\n", ErrorCode, ErrorStr);
			g_pLogger->SendAndFlushWithTime(L"[ACS Motion] Motion Error,Code: %d, %S\n", ErrorCode, ErrorStr);

			//向上层发送错误信息
			sEvenProp.Init();
			sEvenProp.EventType = HSI_EVENT_FUNCTION;
			sEvenProp.EventID = HSI_EVENT_DEBUG_LOG;
			sEvenProp.EventResponse = HSI_EVENT_FUNCTION_FAILED;
			EventCallback(sEvenProp);
			g_pLogger->SendAndFlushWithTime(L"[ACS Motion] Motion Error Event Set\n");
		}
	}
	else
	{
		g_pLogger->SendAndFlushWithTime(L"[ACS Motion] ACS Communication handle is invalid\n");
	}
};

//===========================================================================
//运动类构造函数，涉及 运动控制参数、插补、回家、摇杆、日志等配置信息的加载
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; //0是xyz用的，1是单轴用的，2都不用
	g_IsClose = false; //用于DoEvents()的退出，而不异常
	setLightFlag = false;
	m_bISUseMoreLights = 0; //更多灯光
	m_Jog_Auto_Focus = 1.0; //变焦使用的速度,默认为1.0mm/s, 仅限Z轴
	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; //是否启用HSI进行定位补偿 0为不启用 1为启用 默认为0
	m_Compensation_Pluse = 20; //补偿脉冲数
	m_IsHardLimit = 7; //设备轴硬限位设置 为0表示所有轴都为软限位  1为X轴为硬限位 2为Y轴 3为XY轴  4为Z轴  7为XYZ轴 默认为7
	m_IsEnableAxis = 7; //设备启用轴设置 为0表示所有轴不启用  1为X轴启用 2为Y轴启用 3为XY轴启用  4为Z轴启用  7为XYZ轴启用 默认为7
	m_IsHavePattern = 15; //是否有光栅
	m_IsUseExternalTrigger = 1; //是否启用外触发功能 0为不启用 1为启用  默认为1
	m_IsUseSixRingEightArea = 0; //是否启用六环八区灯功能 0为不启用 1为启用 2为二环八区灯 默认为0
	m_IsUseTwentySixLight = 0; //是否启用26路灯光 0为不启用 1位启用 默认为0
	m_IsUseEF3 = 0; //是否启用EF3
	m_IsUseACS = 0; //是否启用ACS
	m_IsUseSimulator = false; // 增加一个是否连接虚拟机的标志

	m_IsUseRocker = 0; //是否启用摇杆 0为不启用 1为启用旧摇杆，2为新摇杆， 默认为0
	m_IsCloseRocker = 0;
	m_DeviceType = 0; //设备类型，0为通用设备，1为三激光, 2为大视野，3为转盘设备 默认为0
	m_iJoyStick = 0; //摇杆类型：0：无 1：老式摇杆
	m_IsProbe = 0;
	m_ProbeAllAxis = 3; //探针触发时，锁存的轴号，默认3表示锁存XYZ共3轴，4表示XYZA共4轴
	m_ProbeReturnPos = 10.0; //探针触发时，调试时返回的距离mm，点击启动按钮时不起作用,默认10.0mm
	m_ProbeReturnSpeed = 40; //探针触发后，轴的回退速度
	m_IsHomeEncPos = 0; //是否启动实际位置判断是否回家，默认0，1启用，0关闭
	m_IsHomePrfPos = 1; //是否启动规划位置判断是否回家，默认1，1启用，0关闭
	m_IsIOFuntion = 0; //是否启动IO功能，1为打开，0为关闭
	m_IsStartInput = 0; //是否启用脚踏开关功能，1为启用，0为关闭，默认0
	m_IsUsePPS = false;
	m_MSTRunFlag = false; //MST软件运行标志，trueMST软件已经启动，falseMST软件停止
	m_SendDataLength = 8; // 串口发送数据长度
	m_LightType = 1;
	m_IsUseFourthSpeed = 0;
	m_ETIPort = 1; //外部触发拍照输入端口号
	m_EF3LightType = 1;
	m_IbinCount = 0; //记录获取到的分bin数
	m_IsUseJerk = 0; //是否启用急停 0为不启用 1为启用
	t_start = 0; //获取jog运行的开始时间
	t_end = 0; //获取jog运行的结束时间
	m_isOKGlint = 0; //是否开启ok/ng 闪烁
	m_selectedIndex = 0;
	m_setPositionNum = 5;
	m_axisStatus = 0; //运动各轴的状态
	m_axisAlarmStatus = 0; //轴报警状态
	m_EF3COMPort = 2; //EF3板com口，默认为2
	m_AxisHomeDirection = 15; //轴回家时的方向
	m_PositionA = 0.0; //第四轴的地位位置
	m_ForSoft = 0; //针对使用软件 0为MST 1为Metus
	m_SaveAxisNum = 0; //保存轴号
	m_SaveAxisSpeed = 0; //保存速度
	bSaveSpeedFlag = false;
	m_IsUseManualRunin = 0; //是否开启手动插补（只针对步进电机）
	fourthAxisFlag = false;
	bCircleRun = false; //圆弧插补
	m_UseAxisNum = 1; //转盘设备使用轴号
	jogAxisNum = 0; //jog运动的轴号
	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; //是否不回家也能调试灯光 0为不启用 1为启用 默认为0

	for (int i = 0; i < 4; i++)
	{
		m_IsOpenTCPIP[i] = ""; //可提供的tcp通信的ip
		m_tcpCntFlag[i] = false;
		m_Led8MotionFlag[i] = false; //是否为8路光源板
	}

	for (int i = 0; i < 4; i++)
	{
		m_rockerHStartSpeed[i] = 5; //摇杆XYZ轴高初始速度
		m_rockerHDriveSpeed[i] = 20; //摇杆XYZ轴高驱动速度
		m_rockerLStartSpeed[i] = 2; //摇杆XYZ轴低初始速度
		m_rockerLDriveSpeed[i] = 10; //摇杆XYZ轴低驱动速度
		m_rockerDSpeed[i] = 100; //XYZ轴加减速2
		m_rockerASpeed[i] = 100; //XYZ轴加减速1
		begin_position[i] = 0; //外触发到初始点需要发送的脉冲数
	}

	for (size_t i = 0; i < 8; i++)
	{
		m_SixEightSubArea[i] = 0; //六环八区分区功能
	}

	//删除日志
	//DeleteDirectory()

	//是否启用日志
	CTime tm = CTime::GetCurrentTime();
	CString csTime = tm.Format("%Y%m%d_%H-%M-%S"); //构造时间字符串
	//CString csTime = tm.Format("%Y-%m-%d"); //构造时间字符串
	CString dir = L"\\Log\\" + csTime += L"_Motion.Log";
	g_pLogger = new CLogger(dir);
	g_pLogger2 = new CLogger(L"\\Log\\EF3_SumTime.Log");

	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;
	g_pLogger->SendAndFlushWithTime(L"\n");
	g_pLogger->SendAndFlushWithTime(L"==========================================================\n");

	//读取配置文件
	IS_DEBUG = GetPrivateProfileInt(L"EF3", L"ACS_PORT", 0, m_AppPath + _T("\\Config\\EF3_Config.ini"));
	g_pLogger->SendAndFlushWithTime(L"ACS_PORT = %d\n", IS_DEBUG);
	if (IS_DEBUG != 500) //非调试模式
	{
		// CLMS 授权验证
		if (!CheckLicense())
		{
			//弹窗提示
			AfxMessageBox(_T("[Check License]  Acs-Metus Middleware No Authorization Found, Please Contact the Corresponding Sales"), MB_ICONWARNING | MB_OK);
			g_pLogger->SendAndFlushWithTime(L"[Check License] Acs-Metus Middleware No Authorization Found, Please Contact the Corresponding Sales\n");
			return;
		}
	}
	g_pLogger->SendAndFlushWithTime(L"[Check License] Acs Middleware Check License OK\n");

	//档位参数
	for (int i = 0; i < 5; i++)
	{
		for (int j = 0; j < 5; j++)
		{
			m_JogDriveSpeed[j][i] = 20; //表示5个档位 轴号,从1开始；5：档位
			m_JogStartSpeed[j][i] = 20;
			m_JogAccLine[j][i] = 5;
			m_JogDecLine[j][i] = 5;
			m_JogAccCurve[j][i] = 5;
			m_JogDecCurve[j][i] = 5;
		}
		m_Home_Machine_Axis[i] = 1; //用于启动时需要回原点的轴号选择,赋值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; //从1开始,0不用
		m_Home_AddJogGears[i] = 4;
		m_Home_DecJogGears[i] = 3;
		m_SetPotion_StartSpeed[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; //SetpositionXyz的目标位置
		m_PosNow[i] = 0; //调用SetpositionXyz时，读取当前位置
		m_LogIsOpen[i] = 0; //是否打开记录，0为打开，非0为关闭
		m_StopJogMode[i] = 0; //JOG模式采用急停还是平滑停止
		m_LockPos[i] = 0.0;
		m_EncPos[i] = 0;
		m_PrfPos[i] = 0;
		m_PosForAllAxis[i] = 0.0; //记录4轴位置
		targetpos_n[i] = 0;
		targetpos_l[i] = 0;
		m_ProbeCapturePos[i] = 0; //锁存各轴的位置
		iCircleRunPnt[i] = 0; //圆弧插补时的圆心位置
		m_ijk[i] = 0;
	}

	for (int j = 0;j < 9;j++)
	{
		m_SetPotion_DriveSpeed[j] = 20; //设置定位驱动速度
		//写日志
		//g_pLogger->SendAndFlushWithTime(L"m_SetPotion_DriveSpeed[%d] = %d\n", j, m_SetPotion_DriveSpeed[j]);
	}

	m_Set_XYZA_Reserve = 0; //XYZA轴方向
	m_motorType = 0; //电机类型 1为伺服电机 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;
	m_bACSConnected = 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(nullptr, FALSE, nullptr);
	g_WR_ToMove_Mutex = CreateMutex(nullptr, FALSE, nullptr);
	g_Lock_JogAndTrigger = CreateMutex(nullptr, FALSE, nullptr);

	// 初始化当前运动模式
	m_CurrentMode = JogMode;
}

//===========================================================================
HSI_Motion::~HSI_Motion()
{
	TRACE0("HSI_Motion Destructor!\n");
}

//===========================================================================
bool HSI_Motion::PortInit(int iSerialComPort, int iBuadRate)
{
	if (hCom == nullptr)
	{
		char buf[10];
		sprintf_s(buf, "COM%d", iSerialComPort);
		CString comName(buf);
		hCom = CreateFile(comName,
			GENERIC_READ | GENERIC_WRITE, //允许读和写
			0, //独占方式,串口必须为0
			nullptr,
			OPEN_EXISTING, //打开而不是创建，串口必须为打开
			0, //同步方式,同步执行时，函数直到操作完成后才返回
			nullptr); //串口必须为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 (IS_DEBUG != 500)
	{
		// CLMS 授权验证
		if (!CheckLicense())
		{
			// 授权验证失败
			g_pLogger->SendAndFlushWithTime(L"[Check License]  Acs-Metus Middleware No Authorization Found, Please Contact the Corresponding Sales\n");
			return HSI_STATUS_FAILED; // 返回适当的错误状态
		}
	}

	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] HMQ HSI_Sevenocean_EF3.dll version = %s, date = %s\n", HSI_VERSION_CSTRING, HSI_FILE_CSDESCRIPTION); //输出HSI.dll 版本号

		GoogolMotionConfigFile = m_AppPath + _T("\\Config\\EF3_Config.ini");
		Load_EF3_Config_Inifile(GoogolMotionConfigFile); //加载 EF3_Config.ini 配置项
		GoogolMotionConfigFile = m_AppPath + _T("\\Config\\EF3_Motion.ini");
		Load_EF3_Motion_Inifile(GoogolMotionConfigFile); // 加载 EF3_Motion.ini 配置项
		//如果使用 EF3
		if (m_IsUseEF3 == 1)
		{
			if (!m_bConnected)
			{
				//直线电机平台，采用串口通信，8个数据位，1个停止位，无奇偶校验，波特率256000
				m_SO7_Serial.SetPort(m_EF3COMPort, 256000, 0, 8, 1, 0); //打开串口
#ifndef OFFLINE
				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;
				}
#endif // OFFLINE

				m_SO7_Serial.SetTimeouts(1000, 1000);
				m_bConnected = true;
				g_pLogger->SendAndFlushWithTime(L"[Startup] Serial: [COM%d] is open success\n", m_EF3COMPort);
			}
			else
			{
				g_pLogger->SendAndFlushWithTime(L"[Startup] Serial is opened\n");
			}
		}

		//如果启用ACS
		if (m_IsUseACS == 1)
		{
			if (!m_bACSConnected)
			{
				//尝试打开ACS控制器
				g_pLogger->SendAndFlushWithTime(L"[ACS Motion] In\n");
				g_pLogger->SendAndFlushWithTime(
					L"[ACS Motion] Wait for opening of communication with the controller... \n");


				if (m_IsUseSimulator == 1)
				{
					handleACS = acsc_OpenCommSimulator();
				}
				else
				{

					// 10.0.0.100 - default IP address of the controller
					handleACS = acsc_OpenCommEthernet(m_ACS_IPAddresses, ACSC_SOCKET_DGRAM_PORT);
					// for the connection to the controller via local network or Internet
					//	hComm = acsc_OpenCommEthernet("10.0.0.100", ACSC_SOCKET_STREAM_PORT);
				}
				if (handleACS == ACSC_INVALID) //打开失败
				{
					g_pLogger->SendAndFlushWithTime(L"[ACS Motion] error while opening communication.\n");
					ErrorsHandler();

					sEvenProp.Init();
					sEvenProp.EventType = HSI_EVENT_ERROR;
					sEvenProp.EventID = HSI_EVENT_MOTION;
					sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK;
					strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "ACS控制器打开失败");
					EventCallback(sEvenProp);
					return HSI_STATUS_FAILED;
				}

				//使能电机
				//int Axes[] = { ACSC_AXIS_1, ACSC_AXIS_0, ACSC_AXIS_8, -1 }; //根据电气层面定义，这里的0对应X轴，1对应Y轴，8对应Z轴
				if (!acsc_EnableM(handleACS, ACSAxisNumbers, nullptr))
				{
					g_pLogger->SendAndFlushWithTime(L"[ACS Motion] ACS Motors Enable Failed!\n");
					ErrorsHandler();
				}
				else
				{
					g_pLogger->SendAndFlushWithTime(L"[ACS Motion] ACS Motors Enable Success!\n");
				}

				m_bACSConnected = true;
				g_pLogger->SendAndFlushWithTime(L"[ACS Motion] ACS Initialization Success!\n");

				//获取ACS 控制器版本号, 待测试，2.70
				char Firmware[256];
				int Received;
				if (!acsc_GetFirmwareVersion(handleACS, Firmware, 255, &Received, nullptr))
				{
					g_pLogger->SendAndFlushWithTime(L"[ACS Motion] GET ACS Controller Version failed!\n");
					ErrorsHandler();
				}
				Firmware[Received - 1] = '\0';
				//printf("%s", Firmware);

				//https://www.cnblogs.com/MichaelOwen/articles/2128771.html
				//g_pLogger->SendAndFlushWithTime(L"%s%s\n", _T("[ACS Motion] ACS Controller Version: "), L"你好");
				g_pLogger->SendAndFlushWithTime(L"[ACS Motion] ACS Controller Version: %S\n", Firmware);

				//获取SPiiPlus C Library version
				unsigned int Ver = acsc_GetLibraryVersion();
				/*	printf("SPiiPlus C Library version is %d.%d.%d.%d\n", HIBYTE(HIWORD(Ver)), LOBYTE(HIWORD(Ver)),
						   HIBYTE(LOWORD(Ver)),
						   LOBYTE(LOWORD(Ver)));*/
				g_pLogger->SendAndFlushWithTime(L"[ACS Motion] SPiiPlus C Library version is %d.%d.%d.%d\n",
					HIBYTE(HIWORD(Ver)), LOBYTE(HIWORD(Ver)), HIBYTE(LOWORD(Ver)),
					LOBYTE(LOWORD(Ver)));
			}
			else
			{
				g_pLogger->SendAndFlushWithTime(
					L"[ACS Motion] Communication with the controller is already established successfully!\n");
			}

			//如果各个轴标志位 未回过家，需要回家
			rStatus = g_pHSI_Motion->HomeMachine(TRUE); //执行回家
		}

		//   04 05 0F 4A 04 00 04 00 00 00 00 00 00 00 00 00
		//
		//01 01 0F 04 00 00 00 00 00 00 00 00 00 00 00 00
		//01 01 0F 04 00 00 00 00 00 00 00 00 00 00 00 00
		//
		//01 01 01 07 02 01 01 00 00 00 00 00 00 00 00 00
		//01 09 01 01 01 07 07 00 00 00 07 04 00 00 00 00
		//
		//01 01 01 05 01 07 07 00 00 00 07 04 00 00 00 00
		//01 01 02 05 01 07 07 00 00 00 07 04 00 00 00 00
		//01 01 04 05 01 07 07 00 00 00 07 04 00 00 00 00
		//01 01 08 05 01 07 07 00 00 00 07 04 00 00 00 00
		//01 01 01 06 01 07 07 00 00 00 07 04 00 00 00 00
		//01 01 01 04 00 00 00 00 00 00 00 00 00 00 00 00
		//01 01 01 02 69 00 00 00 E8 03 00 00 F4 01 00 00
		//01 01 01 03 69 00 00 00 E8 03 00 00 F4 01 00 00
		//01 01 02 04 00 00 00 00 00 00 00 00 F4 01 00 00
		//01 01 02 02 00 00 00 00 F0 00 00 00 F4 01 00 00
		//01 01 02 03 00 00 00 00 F0 00 00 00 F4 01 00 00
		//01 01 04 04 00 00 00 00 00 00 00 00 F4 01 00 00
		//01 01 04 02 00 00 00 00 F4 01 00 00 F4 01 00 00
		//01 01 04 03 00 00 00 00 F4 01 00 00 F4 01 00 00
		//01 06 07 53 01 0F 00 00 F4 01 00 00 F4 01 00 00

		//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 success\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");

		//读取EF3数据状态线程
		//CreateThreadData();
		//SetEvent(m_hTriggerEventData);
		//m_Thread_StateData = HSI_THREAD_RUNNING;
		//g_pLogger->SendAndFlushWithTime(L"[Startup] Read EF3 Status Run\n");

		//JOG运行到软限位的运动调节线程
		//if (m_DeviceType != 3)
		//{
		//	CreateThreadJOGStop();
		//	SetEvent(m_hTriggerEventJOGStop);
		//	m_Thread_StateJOGStop = HSI_THREAD_PAUSED;
		//}

		//是否启用IO功能线程，通过配置文件启用
		if (m_IsIOFuntion == 1)
		{
			m_Thread_StateIO = HSI_THREAD_RUNNING;
			CreateThreadIO(); //IO发消息使用
			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");

		//启用EF3锁存模式设置，定时模式
		if (m_IsUseEF3)
		{
			//设置锁存频率 1秒钟（）
			m_cSendData[0] = 0x01;
			m_cSendData[1] = 0x01;
			m_cSendData[2] = 0x02;
			//m_cSendData[3] = 0x03; // 10000 （0x27 0x10）对应1秒，3-4字节表示锁存周期
			//m_cSendData[4] = 0xE8;

			//m_cSendData[3] = 0x01; // 10000 （0x27 0x10）对应1秒，3-4字节表示锁存周期
			//m_cSendData[4] = 0xF4;

			// 10000 （0x27 0x10）对应1秒，3-4字节表示锁存周期
			m_cSendData[3] = (m_axisLatchFrequency >> 8) & 0xFF;
			m_cSendData[4] = m_axisLatchFrequency & 0xFF;
			m_WriteByte = Send_Command(0, (const char*)m_cSendData, 5, 2);
			Sleep(10);

			g_pLogger->SendAndFlushWithTime(L"[Startup] Set EF3 Timing latch Success\n");
		}
	}
	else
	{
		g_pLogger->SendAndFlushWithTime(L"[Startup] No enable controller\n");
		rStatus = HSI_STATUS_FAILED;
	}
	return rStatus;
}

//===========================================================================
/**
 * \brief 获取EF3固件版本
 * \param version
 * \return
 */
HSI_STATUS HSI_Motion::GetFirmwareVersion(byte* version)
{
	m_Thread_StateData = HSI_THREAD_PAUSED;
	Sleep(3);
	int waite_count = 0;
	unsigned char senddata[8] = { 0 };
	//senddata[0] = 0x04;
	//senddata[1] = 0x03;

	senddata[0] = 0x01;
	senddata[1] = 0x08;
	m_SO7_Serial.m_RecvData[0] = 0;
	m_WriteByte = Send_Command(0, (const char*)senddata, 2, 20);
	Sleep(30);
	//m_SO7_Serial.m_RecvData[0] = 0;
	//m_WriteByte = Send_Command(0, (const char*)senddata, sizeof(senddata));
	//Sleep(30);
	//m_SO7_Serial.m_RecvData[0] = 0;
	//m_WriteByte = Send_Command(0, (const char*)senddata, sizeof(senddata));
	//Sleep(30);

	while ((m_SO7_Serial.m_RecvData[0] != 0x32) && (m_SO7_Serial.m_RecvData[1] != 0x30))
	{
		waite_count++;
		if (waite_count > 100)
			break;
		Sleep(1);
	}
	if (waite_count > 100)
	{
		waite_count = 0;
		m_WriteByte = Send_Command(0, (const char*)senddata, 2, 20); //再次重发，并等待回应
		while ((m_SO7_Serial.m_RecvData[0] != 0x32) && (m_SO7_Serial.m_RecvData[1] != 0x30))
		{
			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];
		}
	}

	g_pLogger->SendAndFlushWithTime(L"[GetFirmwareVersion] EF3 Firmware Version %S \n", version);
	m_Thread_StateData = HSI_THREAD_RUNNING;
	SetEvent(m_hTriggerEventData); //触发事件，其中hEvent表示句柄，返回值：如果操作成功，则返回非零值，否则为0。
	return HSI_STATUS_NORMAL;
}

//===========================================================================
/**
 * \brief 回家
 * \param bHomed
 * \return
 */
HSI_STATUS HSI_Motion::HomeMachineOld(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 = static_cast<byte>(AxisConvertIndex(AxisTypes)); //HomeMachineOld
				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] = static_cast<int>(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) & 0xff;
				m_cSendData[5] = (static_cast<int>(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 8) & 0xff;
				m_cSendData[6] = (static_cast<int>(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 16) & 0xff;
				m_cSendData[7] = (static_cast<int>(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 24) & 0xff;

				m_cSendData[8] = static_cast<int>(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) & 0xff;
				m_cSendData[9] = (static_cast<int>(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 8) & 0xff;
				m_cSendData[10] = (static_cast<int>(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 16) &
					0xff;
				m_cSendData[11] = (static_cast<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::HomeMachine(bool bHomed)
{
	auto rStatus = HSI_STATUS_NORMAL;
	g_pLogger->SendAndFlushWithTime(L"[HomeMachine] In\n");
	if (!bHomed)
	{
		g_pLogger->SendAndFlushWithTime(L"[HomeMachine] bHomed No Need Reture\n");
		return HSI_STATUS_NORMAL;
	}

	if (g_pHSI_Motion && (handleACS != ACSC_INVALID))
	{
		//判断是否需要回家
		bool home(false);
		IsHomed(home);
	
		if (home == true && !m_IsHomeEveryTime) //不需要每次都回家，且已经回家了
		{
			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; //正在回家中

		//运行 ACS 控制器内 buffer6 自动回家动作
		g_pLogger->SendAndFlushWithTime(L"[HomeMachine] RunBuffer 6 \n");
		//回家后，启用正负限位
		if (!acsc_RunBuffer(handleACS, ACSC_BUFFER_6, nullptr, ACSC_SYNCHRONOUS))
		{
			g_pLogger->SendAndFlushWithTime(L"[HomeMachine] ACS Run Buffer %d error\n", ACSC_BUFFER_6);
			ErrorsHandler();
			return HSI_STATUS_FAILED;
		}

		//等待运动结束 ，方式1
		/*	acsc_WaitMotionEnd(handleACS, ACSC_AXIS_1, INFINITE);
			g_pLogger->SendAndFlushWithTime(L"[HomeMachine] X homed\n");

			acsc_WaitMotionEnd(handleACS, ACSC_AXIS_0, INFINITE);
			g_pLogger->SendAndFlushWithTime(L"[HomeMachine] Y homed\n");

			acsc_WaitMotionEnd(handleACS, ACSC_AXIS_4, INFINITE);
			g_pLogger->SendAndFlushWithTime(L"[HomeMachine] Z homed\n");*/

			//等待运动结束 ，方式2
		do
		{
			//再次读取回家标志位，或者上个动作完成回调
			IsHomed(home);
			Sleep(200);
		} while (!home);

		if (m_IsUseEF3) //启用EF3锁存功能，需要再回家完成后，设置锁存板的位置
		{
			//回家后，将锁存板的位置设置为0
			unsigned char m_cSendData[8] = { 0 };
			m_cSendData[0] = 0x01;
			m_cSendData[1] = 0x06;
			m_WriteByte = Send_Command(0, (const char*)m_cSendData, 2);
			Sleep(5);

			//清除锁存板Flash区
			m_cSendData[0] = 0x01;
			m_cSendData[1] = 0x04;
			m_WriteByte = Send_Command(0, (const char*)m_cSendData, 2);
			Sleep(5);
		}

		//回家表示改为1
		m_Home_Machine_Axis[1] = 1;
		m_Home_Machine_Axis[0] = 1;
		m_Home_Machine_Axis[8] = 1;
		g_pLogger->SendAndFlushWithTime(L"[HomeMachine] Go Home success\n");
		g_pLogger->SendAndFlushWithTime(L"[HomeMachine] Out\n");
	}
	else
	{
		g_pLogger->SendAndFlushWithTime(L"[HomeMachine] faild, g_pHSI_Motion or handleACS not invaild! \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;
}

//===========================================================================
/**
 * \brief 取消限位，设置初始速度，加减速等参数
 * \return
 */
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++;
			g_pLogger->SendAndFlushWithTime(L"[HomeFindIndex] Going Home Count=%d\n", 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 是系统的安全函数，微软在 2005 后建议用一系统所谓安全的函数，这中间就有 strcat_s 取代了 strcat ，原来 strcat 函数，没有方法来保证有效的缓冲区尺寸，所以它只能假定缓冲足够大来容纳要拷贝的字符串, 容易产生程序崩溃。而strcat_s函数能很好的规避这个问题*/
			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(nullptr, 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::IsHomedOld(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::IsHomed(bool& bHomed)
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion && handleACS != ACSC_INVALID)
	{
		g_pLogger->SendAndFlushWithTime(L"[IsHomed] In\n");
		int isHomed[5] = { 0, 1, 1, 1, 1 }; //暂定只有一个回家标志位，即全部回家完成，没有按单个轴回家来看

		//所有轴都不需要回家
		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;
		}

		// 判断是否需要回家，读取ACS控制器回家标志位，来判断本次上电是否已经回过家 ， 1：已经回过家，0：未回过家
		if (!acsc_ReadInteger(handleACS, ACSC_NONE, "YAW_HOME_DONE", ACSC_NONE, ACSC_NONE, ACSC_NONE, ACSC_NONE,
			isHomed,
			nullptr))
		{
			g_pLogger->SendAndFlushWithTime(L"[IsHomed] ACS Read ISHOMED Flag Error\n");
			ErrorsHandler();

			CurrentHomeMachineState = E_EF3_HOME_NONE;
			rStatus = HSI_STATUS_FAILED;
			bHomed = false;
		}

		g_pLogger->SendAndFlushWithTime(L"[IsHomed] ACS Read YAW_HOME_DONE X:[%d] Y:[%d] Z:[%d]\n", isHomed[0],
			isHomed[1], isHomed[2]);

		//如果各个轴标志位 已经回过家
		if (isHomed[0] == 1 && isHomed[1] == 1 && isHomed[2] == 1 && isHomed[3] == 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");
	}
	else
	{
		g_pLogger->SendAndFlushWithTime(L"[IsHomed] ACS Communication error\n");
		CurrentHomeMachineState = E_EF3_HOME_NONE;
		rStatus = HSI_STATUS_FAILED;
		bHomed = false;
	}
	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;
}

//===========================================================================
/**
 * \brief JOG模式
 * \param AxisTypes 单轴
 * \param Speed 速度，Speed > 0 正移动
 * \return
 */
HSI_STATUS HSI_Motion::JogOld(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 = static_cast<byte>(AxisConvertIndex(AxisTypes)); //JogOld
		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] = static_cast<int>(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) & 0xff; //正限位
			m_cSendData[5] = (static_cast<int>(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 8) & 0xff;
			m_cSendData[6] = (static_cast<int>(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 16) & 0xff;
			m_cSendData[7] = (static_cast<int>(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 24) & 0xff;

			m_cSendData[8] = static_cast<int>(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) & 0xff; //负限位
			m_cSendData[9] = (static_cast<int>(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 8) & 0xff;
			m_cSendData[10] = (static_cast<int>(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 16) & 0xff;
			m_cSendData[11] = (static_cast<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 = static_cast<int>(now_pos[1] / m_Resolution[1]) - static_cast<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 = static_cast<int>(m_P_Work_Limit[1] / m_Resolution[1]) - static_cast<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 = static_cast<int>(now_pos[2] / m_Resolution[2]) - static_cast<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 = static_cast<int>(m_P_Work_Limit[2] / m_Resolution[2]) - static_cast<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 = static_cast<int>(now_pos[3] / m_Resolution[3]) - static_cast<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 = static_cast<int>(m_P_Work_Limit[3] / m_Resolution[3]) - static_cast<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 = static_cast<int>(now_pos[4] / m_Resolution[4]) - static_cast<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 = static_cast<int>(m_P_Work_Limit[4] / m_Resolution[4]) - static_cast<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 = %s\n", bJOGDir);
	}
	return rStatus;
}

HSI_STATUS HSI_Motion::Jog(UINT AxisTypes, double Speed)
{
	auto rStatus = HSI_STATUS_NORMAL;
	g_pLogger->SendAndFlushWithTime(L"[Jog] Aixs: [%d]  SpeedGear: [%lf]\n", AxisTypes, Speed);
	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; //运动方向

		jogDirFlag = bJOGDir;
		m_Thread_State = HSI_THREAD_PAUSED;

		if (CurrentHomeMachineState == E_EF3_HOME_FINISHED)
		{
			//软限位
			g_pLogger->SendAndFlushWithTime(
				L"[Jog] Limit Enable, Axis = %d, m_P_Work_Limit = %f,m_N_Work_Limit = %f\n",
				AxisTypes, m_P_Work_Limit[AxisTypes], m_N_Work_Limit[AxisTypes]);
		}
		else
		{
			//无效软限位
			g_pLogger->SendAndFlushWithTime(L"[Jog] Limit No Enable\n");
		}

		//是否回过家判断
		if (m_Home_Machine_Axis[AxisTypes] == 0)
		{
			g_pLogger->SendAndFlushWithTime(L"[Jog] Current Axis[%d] not homed \n", AxisTypes);
			return rStatus;
		}

		//设置 JOG运动参数 加减速 JOG_SPEED_ACC_DEC
		double now_pos[5] = { 0 };
		double Prf_pos[5] = { 0 };
		double limitpos[4] = { 0 };
		int RemainPul = 0;
		int limitSDPul = 0;
		double time;

		//速度限制
		JogSpeed = abs(SpeedPercent(AxisTypes, Speed, DriveSpeed, StartSpeed, AccLine, DecLine, AccCurve, DecCurve));

		g_pLogger->SendAndFlushWithTime(
			L"[Jog] Speed: [%d], DriveSpeed: [%d],AccLine: [%d], DecLine: [%d] AccCurve: [%d],  DecCurve: [%d],\n",
			Speed, DriveSpeed, AccLine, DecLine, AccCurve, DecCurve);

		//转到真实ACS平台轴号，并开始执行
		byte AxisNumber = static_cast<byte>(AxisConvertIndex(AxisTypes)); //Jog
		double motionParam[5] = { DriveSpeed,AccLine ,  DecLine, AccCurve,DecCurve }; //速度，加速度，减速度，Kill, jerk
		SetSingleAxisMotionParams(AxisNumber, motionParam);

		// 急停判断
		if ((StartSpeed < 250) && (DriveSpeed < 6))
		{
			m_IsUseJerk = 1;
			g_pLogger->SendAndFlushWithTime(L"[Jog] Use Jerk\n");
		}
		else
		{
			m_IsUseJerk = 0;
			g_pLogger->SendAndFlushWithTime(L"[Jog] No Use Jerk\n");
		}

		//使能电机
		//int Axes[] = { ACSC_AXIS_1, ACSC_AXIS_0, ACSC_AXIS_8, -1 }; //根据电气层面定义，这里的0对应X轴，1对应Y轴，8对应Z轴
		if (!acsc_EnableM(handleACS, ACSAxisNumbers, nullptr))
		{
			g_pLogger->SendAndFlushWithTime(L"[ACS Motion] ACS Motors Enable Failed!\n");
			ErrorsHandler();
		}
		else
		{
			g_pLogger->SendAndFlushWithTime(L"[ACS Motion] ACS Motors Enable Success!\n");
		}

		SwitchMode(JogMode); //切换模式,设置当前模式为JOG模式

		//开始JOG运动
		if (!bJOGDir)
		{
			DriveSpeed = DriveSpeed * (-1); // Negative direction : Using - (minus) velocity //正方向，或 负方向
		}

		if (!acsc_Jog(handleACS, 0, AxisNumber, DriveSpeed, nullptr))
		{
			g_pLogger->SendAndFlushWithTime(L"[Jog] 方向移动失败, 轴:[%d] JOGDir:[%S]\n", AxisTypes, bJOGDir ? "Positive" : "Negative");
			ErrorsHandler();
		}

		jogMoving = true;
		g_pLogger->SendAndFlushWithTime(L"[Jog] Out, AxisNumber = %d, DriveSpeed = %d   AccCurve:[%d]  DecCurve:[%d]\n", AxisNumber, DriveSpeed,
			AccLine, AccLine);
	}
	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 = static_cast<byte>(AxisConvertIndex(AxisTypes)); //JoyStick
		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] = static_cast<int>(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) & 0xff;
			m_cSendData[5] = (static_cast<int>(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 8) & 0xff;
			m_cSendData[6] = (static_cast<int>(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 16) & 0xff;
			m_cSendData[7] = (static_cast<int>(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 24) & 0xff;

			m_cSendData[8] = static_cast<int>(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) & 0xff;
			m_cSendData[9] = (static_cast<int>(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 8) & 0xff;
			m_cSendData[10] = (static_cast<int>(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 16) & 0xff;
			m_cSendData[11] = (static_cast<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 = static_cast<int>(now_pos[1] / m_Resolution[1]) - static_cast<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 = static_cast<int>(m_P_Work_Limit[1] / m_Resolution[1]) - static_cast<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 = static_cast<int>(now_pos[2] / m_Resolution[2]) - static_cast<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 = static_cast<int>(m_P_Work_Limit[2] / m_Resolution[2]) - static_cast<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 = static_cast<int>(now_pos[3] / m_Resolution[3]) - static_cast<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 = static_cast<int>(m_P_Work_Limit[3] / m_Resolution[3]) - static_cast<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 = static_cast<int>(now_pos[4] / m_Resolution[4]) - static_cast<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 = static_cast<int>(m_P_Work_Limit[4] / m_Resolution[4]) - static_cast<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;
}

//===========================================================================
/**
 * \brief 停止Jog运动
 * \return
 */
HSI_STATUS HSI_Motion::StopJogOld()
{
	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::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);
		}*/

		SwitchMode(JogMode); //设置当前模式为JOG模式 

		//int Axes[] = { ACSC_AXIS_1, ACSC_AXIS_0, ACSC_AXIS_8, -1 };
		if (handleACS != ACSC_INVALID)
		{
			if (!acsc_HaltM(handleACS, ACSAxisNumbers, nullptr)) //停止JOG运动
			{
				g_pLogger->SendAndFlushWithTime(L"[StopJog] ACS acsc_HaltM error!\n");
				ErrorsHandler();
			}
			//
			g_pLogger->SendAndFlushWithTime(L"[StopJog] ACS acsc_HaltM success!\n");
		}

		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 = static_cast<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;
}

//===========================================================================
//运动控制部分
//===========================================================================
/**
 * \brief 对比现在编码器位置和期望位置, position给上层用的, HSI内部用的是EncPrf
 * \param AxisTypes
 * \param EncPos
 * \param PrfPos
 * \param Count
 * \return
 */
HSI_STATUS HSI_Motion::GetPositionEncPrfMultiOld(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::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;
}

//===========================================================================
/**
 * \brief 获取轴当前运动位置
 * \param AxisTypes
 * \param PositionX
 * \param PositionY
 * \param PositionZ
 * \param Time 耗时
 * \return
 */
HSI_STATUS HSI_Motion::GetPositionXyzOld(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::GetPositionXyz(UINT AxisTypes, double& PositionX, double& PositionY, double& PositionZ,
	double& Time)
{
	auto rStatus = HSI_STATUS_NORMAL;
	//UNREFERENCED_PARAMETER(AxisTypes)的意思就是告诉编译器，
	//	AxisTypes参数我使用过了，别报警告了，仅此而已。
	UNREFERENCED_PARAMETER(AxisTypes);
	//读取3个轴的位置
	CString tempStr;
	bool bGetPosition = true;

	if (g_pHSI_Motion && (handleACS != ACSC_INVALID)) //句柄有效
	{
		if (!acsc_GetFPosition(handleACS, ACSAxisNumbers[0], &PositionX, nullptr))
		{
			g_pLogger->SendAndFlushWithTime(L"[GetPositionEncPrfMulti] get PositionX failed\n");
			ErrorsHandler();
			bGetPosition = false;
			return HSI_ACS_ERROR;
		}
		if (!acsc_GetFPosition(handleACS, ACSAxisNumbers[1], &PositionY, nullptr))
		{
			g_pLogger->SendAndFlushWithTime(L"[GetPositionEncPrfMulti] get PositionY failed\n");
			ErrorsHandler();
			bGetPosition = false;
			return HSI_ACS_ERROR;
		}
		if (!acsc_GetFPosition(handleACS, ACSAxisNumbers[2], &PositionZ, nullptr))
		{
			g_pLogger->SendAndFlushWithTime(L"[GetPositionEncPrfMulti] get PositionZ failed\n");
			ErrorsHandler();
			bGetPosition = false;
			return HSI_ACS_ERROR;
		}

		if (bGetPosition) //读取成功
		{
			m_EncPos[1] = PositionX;
			m_EncPos[2] = PositionY;
			m_EncPos[3] = PositionZ;
			m_EncPos[4] = m_PosForAllAxis[4]; // m_PosForAllAxis 记录4轴位置
			m_PosForAllAxis[1] = PositionX;
			m_PosForAllAxis[2] = PositionY;
			m_PosForAllAxis[3] = PositionZ;

			//g_pLogger->SendAndFlushWithTime(
			//	L"[GetPositionEncPrfMulti] GetPosition Success, Pos[1] = %.4f, Pos[2] = %.4f, Pos[3] = %.4f\n",
			//	PositionX, PositionY, PositionZ);
		}
		else //读取将之前的值进行返回
		{
			PositionX = m_EncPos[1];
			PositionY = m_EncPos[2];
			PositionZ = m_EncPos[3];
			m_PosForAllAxis[1] = m_EncPos[1]; // m_PosForAllAxis 记录4轴位置
			m_PosForAllAxis[2] = m_EncPos[2];
			m_PosForAllAxis[3] = m_EncPos[3];
			m_PosForAllAxis[4] = m_EncPos[4];

			g_pLogger->SendAndFlushWithTime(
				L"[GetPositionEncPrfMulti] GetPosition failed, return position record before, Pos[1] = %.4f,Pos[2] = %.4f,Pos[3] = %.4f,Pos[4] = %.4f\n",
				m_EncPos[1], m_EncPos[2], m_EncPos[3], m_PosForAllAxis[4]);
		}

		Time = set_end - set_start;
	}
	return rStatus;
}

//===========================================================================
HSI_STATUS HSI_Motion::GetEncoderXyzOld(long* lEncoderVal)//原读取编码器值使用串口获取EF3的光栅尺读数，待测试
{
	auto rStatus = HSI_STATUS_NORMAL;
	//读取3个轴的编码器值
	if (g_pHSI_Motion)
	{
		g_pLogger->SendAndFlushWithTime(L"[GetEncoderXyz] In\n");
		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]);
					g_pLogger->SendAndFlushWithTime(L"[GetEncoderXyz] lEncoderVal[0] %ld \n", lEncoderVal[0]);
				}
				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]);
					g_pLogger->SendAndFlushWithTime(L"[GetEncoderXyz] lEncoderVal[00] %ld \n", lEncoderVal[0]);
				}
				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]);
					g_pLogger->SendAndFlushWithTime(L"[GetEncoderXyz] lEncoderVal[1] %ld \n", lEncoderVal[1]);
				}
				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]);
					g_pLogger->SendAndFlushWithTime(L"[GetEncoderXyz] lEncoderVal[01] %ld \n", lEncoderVal[1]);
				}
				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]);
					g_pLogger->SendAndFlushWithTime(L"[GetEncoderXyz] lEncoderVal[2] %ld \n", lEncoderVal[2]);
				}
				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]);
					g_pLogger->SendAndFlushWithTime(L"[GetEncoderXyz] lEncoderVal[02] %ld \n", lEncoderVal[2]);
				}
			}
			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]);
				g_pLogger->SendAndFlushWithTime(L"[GetEncoderXyz] lEncoderVal[0]= %ld,EncoderVal[1]= %ld,,EncoderVal[2]= %ld,     \n", lEncoderVal[0], lEncoderVal[1], lEncoderVal[2]);
			}
		}
		else
		{
			auto rStatus = HSI_STATUS_FAILED;
			g_pLogger->SendAndFlushWithTime(L"[GetEncoderXyz] failed\n");
		}
		g_pLogger->SendAndFlushWithTime(L"[GetEncoderXyz] Out\n");
	}
	return rStatus;
}

HSI_STATUS HSI_Motion::GetEncoderXyz(long* lEncoderVal)//原读取编码器值使用串口获取EF3的光栅尺读数，待测试
{
	auto rStatus = HSI_STATUS_NORMAL;
	//读取3个轴的编码器值
	if (g_pHSI_Motion)
	{
		g_pLogger->SendAndFlushWithTime(L"[GetEncoderXyz] In\n");

		{
			lEncoderVal[0] = m_EncPos[1] / m_Resolution[1];//PositionX
			lEncoderVal[1] = m_EncPos[2] / m_Resolution[1];// PositionY;
			lEncoderVal[2] = m_EncPos[3] / m_Resolution[1]; // PositionZ;

			g_pLogger->SendAndFlushWithTime(L"[GetEncoderXyz] lEncoderVal[0]= %ld,EncoderVal[1]= %ld,,EncoderVal[2]= %ld,     \n", lEncoderVal[0], lEncoderVal[1], lEncoderVal[2]);
		}
		//else
		//{
		//	auto rStatus = HSI_STATUS_FAILED;
		//	g_pLogger->SendAndFlushWithTime(L"[GetEncoderXyz] failed\n");
		//}
		g_pLogger->SendAndFlushWithTime(L"[GetEncoderXyz] Out\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;
}

//===========================================================================
/**
 * \brief 设置多轴运动到指定位置
 * \param AxisTypes
 * \param PositionX
 * \param PositionY
 * \param PositionZ
 * \param eType
 * \param dFlyRadius
 * \return
 */
 //HSI_STATUS HSI_Motion::SetPositionXyzOld(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; //SetpositionXyz的目标位置
 //			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
 //			{
 //				/*	编译器隐式执行的任何类型转换都可以由static_cast显式完成。
 //					static_cast可以用来将枚举类型转换成整型，或者整型转换成浮点型*/
 //				Pos_t[1] = NowPos[1] = static_cast<int>(m_EncPos[1] / m_Resolution[1]);
 //				Pos_t[2] = NowPos[2] = static_cast<int>(m_EncPos[2] / m_Resolution[2]);
 //				Pos_t[3] = NowPos[3] = static_cast<int>(m_EncPos[3] / m_Resolution[3]);
 //				Pos_t[4] = NowPos[4] = static_cast<int>(m_EncPos[4] / m_Resolution[4]);
 //			}
 //
 //			if (m_motorType & 0x01) //步进电机
 //				Pos[1] = static_cast<int>(PositionX / m_Resolution[1]) - NowPos[1];
 //			else
 //				Pos[1] = static_cast<int>(PositionX / m_Resolution[1]) - Pos_t[1];
 //			if (m_motorType & 0x02) //步进电机
 //				Pos[2] = static_cast<int>(PositionX / m_Resolution[2]) - NowPos[2];
 //			else
 //				Pos[2] = static_cast<int>(PositionY / m_Resolution[2]) - Pos_t[2];
 //			if (m_motorType & 0x04) //步进电机
 //				Pos[3] = static_cast<int>(PositionX / m_Resolution[3]) - NowPos[3];
 //			else
 //				Pos[3] = static_cast<int>(PositionZ / m_Resolution[3]) - Pos_t[3];
 //			if (m_motorType & 0x08) //步进电机
 //				Pos[4] = static_cast<int>(PositionX / m_Resolution[4]) - NowPos[4];
 //			else
 //				Pos[4] = static_cast<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] = static_cast<int>(PositionX / m_Resolution[1]); //计算到目标位置
 //			target_pos[2] = static_cast<int>(PositionY / m_Resolution[2]);
 //			target_pos[3] = static_cast<int>(PositionZ / m_Resolution[3]);
 //			target_pos[4] = static_cast<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]) / static_cast<float>(MaxPos);
 //			scale[1] = abs(Pos[2]) / static_cast<float>(MaxPos);
 //			scale[2] = abs(Pos[3]) / static_cast<float>(MaxPos);
 //			scale[3] = abs(Pos[4]) / static_cast<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) //第4轴
 //			{
 //				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;
 //}

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] AxisTypes = %d, PositionX = %.4f,PositionY = %.4f,PositionZ = %.4f\n", AxisTypes, PositionX, PositionY, PositionZ);

		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);
			g_pLogger->SendAndFlushWithTime(
				L"[SetPositionXyz] LimitOverCheck, PositionX = %.4f, PositionY = %.4f, PositionZ = %.4f, m_PositionA = %.4f\n",
				PositionX, PositionY, PositionZ, m_PositionA);

			//SetpositionXyz的目标位置
			m_PosThread[1] = PositionX;
			m_PosThread[2] = PositionY;
			m_PosThread[3] = PositionZ;
			m_PosThread[4] = m_PositionA;

			//目标位置
			g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] PositionX = %.4f, PositionY = %.4f, PositionZ = %.4f\n", PositionX, PositionY, PositionZ);

			//设置速度，对应配置文件中定位合成速度 SET_POTION_DRIVESPEED_1
			g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] DriveSpeed[0] = %d, DriveSpeed[1] = %d, DriveSpeed[2] = %d, DriveSpeed[3] = %d, DriveSpeed[4] = %d\n",
				m_SetPotion_DriveSpeed[0], m_SetPotion_DriveSpeed[1], m_SetPotion_DriveSpeed[2], m_SetPotion_DriveSpeed[3], m_SetPotion_DriveSpeed[4]);

			g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] 设置X轴定位速度 %d\n", m_SetPotion_DriveSpeed[1]);
			double X_SetmotionParam[5] = {
				m_SetPotion_DriveSpeed[1],
				m_SetPotion_DriveSpeed[1] * 10,
				m_SetPotion_DriveSpeed[1] * 10,
				m_SetPotion_DriveSpeed[1] * 100,
				m_SetPotion_DriveSpeed[1] * 100
			};

			SetSingleAxisMotionParams(ACSAxisNumbers[0], X_SetmotionParam);//设置X轴定位速度
			m_SetPotion_DriveSpeed[ACSAxisNumbers[0]] = X_SetmotionParam[0];// 记录X轴速度

			g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] 设置Y轴定位速度 %d\n", m_SetPotion_DriveSpeed[2]);
			double Y_SetmotionParam[5] = {
				m_SetPotion_DriveSpeed[2],
				m_SetPotion_DriveSpeed[2] * 10 ,
				m_SetPotion_DriveSpeed[2] * 10,
				m_SetPotion_DriveSpeed[2] * 100,
				m_SetPotion_DriveSpeed[2] * 100
			};

			SetSingleAxisMotionParams(ACSAxisNumbers[1], Y_SetmotionParam);//设置Y轴定位速度
			m_SetPotion_DriveSpeed[ACSAxisNumbers[1]] = Y_SetmotionParam[0];// 记录Y轴速度

			g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] 设置Z轴定位速度 %d\n", m_SetPotion_DriveSpeed[3]);
			double Z_SetmotionParam[5] = {
				m_SetPotion_DriveSpeed[3],		//速度
				m_SetPotion_DriveSpeed[3] * 10 ,//加速度
				m_SetPotion_DriveSpeed[3] * 10, //减速度
				m_SetPotion_DriveSpeed[3] * 100,//加加速度
				m_SetPotion_DriveSpeed[3] * 100 //减减速度
			};

			SetSingleAxisMotionParams(ACSAxisNumbers[2], Z_SetmotionParam);//设置Z轴定位速度
			m_SetPotion_DriveSpeed[ACSAxisNumbers[2]] = Z_SetmotionParam[0]; //记录Z轴速度

			//使能电机
			//int Axes[] = { ACSC_AXIS_1, ACSC_AXIS_0, ACSC_AXIS_8, -1 }; //根据电气层面定义，这里的0对应X轴，1对应Y轴，8对应Z轴
			if (!acsc_EnableM(handleACS, ACSAxisNumbers, nullptr))
			{
				g_pLogger->SendAndFlushWithTime(L"[ACS Motion] ACS Motors Enable Failed!\n");
				ErrorsHandler();
			}
			else
			{
				g_pLogger->SendAndFlushWithTime(L"[ACS Motion] ACS Motors Enable Success!\n");
			}

		
			SwitchMode(PTPMode); //设置当前模式为JOG模式 

			//开始运动到指定位置，多轴运动
			double Points[] = { PositionX, PositionY, PositionZ }; //目标位置点

		
			if (m_isSynchronized ==1)

			{
				//使用三个轴的分别运动，替代整体运动 acsc_ToPointM，来解决多轴不同速度运动的问题
				if (!acsc_ToPoint(handleACS, 0, ACSAxisNumbers[0], PositionX, nullptr)) //移动到绝对位置
				{
					g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] X轴移动出错 \n");
					ErrorsHandler();
				}
				if (!acsc_ToPoint(handleACS, 0, ACSAxisNumbers[1], PositionY, nullptr)) //移动到绝对位置
				{
					g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Y轴移动出错\n");
					ErrorsHandler();
				}
				if (!acsc_ToPoint(handleACS, 0, ACSAxisNumbers[2], PositionZ, nullptr)) //移动到绝对位置
				{
					g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Z轴移动出错\n");
					ErrorsHandler();
				}
			}
			else
			{
				if (!acsc_ToPointM(handleACS, 0, ACSAxisNumbers, Points, nullptr)) //移动到绝对位置		
					{
						g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] ACS Multi Motion Error\n");
						ErrorsHandler();
					}

			}

		
			//状态更新
			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 = static_cast<double>(abs(pos)) / 1000;
	int acc = (mpos * mpos) / 20 + mpos / 2 + 8;
	if (acc < minacc)
		acc = minacc;
	if (acc > maxacc)
		acc = maxacc;
	return acc;
}

//===========================================================================
//&符号用来处理一个变量，但不是通常的 - 访问这个变量的内容，而是取出这个变量的地址
//
//int* b；此时b是一个指向int空间的指针，也就是说它是一个未分配的地址
//int* 只用来定义，定义的变量是一个地址（索引），可以通过这个变量来对这段空间操作
//
//而 & 是对一个已存在的变量取地址，取完地址之后同样可以通过地址操作
//
//* x是找到x地址的变量，取它的值，所以 * 和 & 是反操作， & x得到x的地址 * ， * x得到x的值x
//————————————————
//版权声明：本文为CSDN博主「nick__huang」的原创文章，遵循CC 4.0 BY - SA版权协议，转载请附上原文出处链接及本声明。
//原文链接：https ://blog.csdn.net/u012610237/article/details/58599083

//void func3(void); 		//利用全局变量返回数组
//void func2(uchar* s); 	//利用指针返回数组
//uchar* func1();			//利用指针函数返回数组
//void func0(uchar*& r);	//利用引用返回数组

/**
 * \brief 获取单轴运动参数
 * \param AXIS
 * \param motionParam
 * \return
 */
HSI_STATUS HSI_Motion::GetSingleAxisParam(int AXIS, double motionParam[5])
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (handleACS != ACSC_INVALID)
	{
		//g_pLogger->SendAndFlushWithTime(L"[GetSingleAxisParam] In\n");

		//依次是 速度
		if (!acsc_GetVelocity(handleACS, AXIS, motionParam + 0, nullptr))
		{
			g_pLogger->SendAndFlushWithTime(L"[GetSingleAxisParam] acsc_GetVelocity error\n");
			rStatus = HSI_ACS_ERROR;
			ErrorsHandler();
		}
		// 加速度
		if (!acsc_GetAcceleration(handleACS, AXIS, motionParam + 1, nullptr))
		{
			g_pLogger->SendAndFlushWithTime(L"[GetSingleAxisParam] acsc_GetAcceleration error\n");
			rStatus = HSI_ACS_ERROR;
			ErrorsHandler();
		}
		//减速、
		if (!acsc_GetDeceleration(handleACS, AXIS, motionParam + 2, nullptr))
		{
			g_pLogger->SendAndFlushWithTime(L"[GetSingleAxisParam] acsc_GetDeceleration error\n");
			rStatus = HSI_ACS_ERROR;
			ErrorsHandler();
		}
		// 杀死速度
		if (!acsc_GetKillDeceleration(handleACS, AXIS, motionParam + 3, nullptr))
		{
			g_pLogger->SendAndFlushWithTime(L"[GetSingleAxisParam] acsc_GetKillDeceleration error\n");
			rStatus = HSI_ACS_ERROR;
			ErrorsHandler();
		}
		//抖动
		if (!acsc_GetJerk(handleACS, AXIS, motionParam + 4, nullptr))
		{
			g_pLogger->SendAndFlushWithTime(L"[GetSingleAxisParam] acsc_GetJerk error\n");
			rStatus = HSI_ACS_ERROR;
			ErrorsHandler();
		}
		//打印数组 motionParam[5]
	/*	g_pLogger->SendAndFlushWithTime(L"[GetSingleAxisParam] AXIS = %d, Vel = %.4f, ACC = %.4f, DCC = %.4f, KillDec = %.4f, Jerk = %.4f\n",
			AXIS,motionParam[0], motionParam[1], motionParam[2], motionParam[3], motionParam[4]);*/
			//g_pLogger->SendAndFlushWithTime(L"[GetSingleAxisParam] Out\n");
	}

	return rStatus;
}

//===========================================================================
HSI_STATUS HSI_Motion::SetSingleAxisParam(int AXIS, double motionParam[5]) //设置单轴运动参数
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (handleACS != ACSC_INVALID)
	{
		//g_pLogger->SendAndFlushWithTime(L"[SetSingleAxisParam] In\n");

		//依次是 速度
		if (!acsc_SetVelocity(handleACS, AXIS, motionParam[0], nullptr))
		{
			g_pLogger->SendAndFlushWithTime(L"[SetSingleAxisParam] acsc_SetVelocity error\n");
			rStatus = HSI_ACS_ERROR;
			ErrorsHandler();
		}
		// 加速度
		if (!acsc_SetAcceleration(handleACS, AXIS, motionParam[1], nullptr))
		{
			g_pLogger->SendAndFlushWithTime(L"[SetSingleAxisParam] acsc_SetAcceleration error\n");
			rStatus = HSI_ACS_ERROR;
			ErrorsHandler();
		}
		//减速度
		if (!acsc_SetDeceleration(handleACS, AXIS, motionParam[2], nullptr))
		{
			g_pLogger->SendAndFlushWithTime(L"[SetSingleAxisParam] acsc_SetDeceleration error\n");
			rStatus = HSI_ACS_ERROR;
			ErrorsHandler();
		}
		ACSC_WAITBLOCK* Wait = NULL;
		// 杀死速度
		if (!acsc_SetKillDeceleration(handleACS, AXIS, motionParam[3], Wait))
		{
			g_pLogger->SendAndFlushWithTime(L"[SetSingleAxisParam] acsc_SetKillDeceleration error\n");
			rStatus = HSI_ACS_ERROR;
			ErrorsHandler();
		}
		//抖动
		if (!acsc_SetJerk(handleACS, AXIS, motionParam[4], Wait))
		{
			g_pLogger->SendAndFlushWithTime(L"[SetSingleAxisParam] acsc_SetJerk error\n");
			rStatus = HSI_ACS_ERROR;
			ErrorsHandler();
		}

		//打印	motionParam[5]
		g_pLogger->SendAndFlushWithTime(L"[SetSingleAxisParam] AXIS = %d, Vel = %.4f, ACC = %.4f, DCC = %.4f, KillDec = %.4f, Jerk = %.4f\n",
			AXIS, motionParam[0], motionParam[1], motionParam[2], motionParam[3], motionParam[4]);

		//g_pLogger->SendAndFlushWithTime(L"[SetSingleAxisParam] Out\n");
	}

	return rStatus;
}

//===========================================================================
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;
}

//===========================================================================
/**
 * \brief 获取缓存点
 * \param CacheData
 * \param DataCount
 * \return
 */
HSI_STATUS HSI_Motion::GetPositionXyzCache(unsigned char* CacheData, int& DataCount)
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		g_pLogger->SendAndFlushWithTime(L"[GetPositionXyzCache] start\n");

		if (m_SO7_Serial.IsOpen())
		{
			//发送获取点数量命令 0x01 0x05
			unsigned char m_cSendData[8] = { 0 };
			m_cSendData[0] = 0x01;
			m_cSendData[1] = 0x05;
			m_WriteByte = Send_Command(0, (const char*)m_cSendData, 2, 8); //期望回复8个字节，其中 4-7字节为点数量

			//解析缓存点数量
			if (m_SO7_Serial.m_iRecvState)
			{
				//特殊帧头, 表示该内容为回复点数
				if ((m_SO7_Serial.m_RecvData[0] == 0x01) && (m_SO7_Serial.m_RecvData[1] == 0x1B) && (m_SO7_Serial.
					m_iRecvBytes == 8))
				{
					DataCount = (m_SO7_Serial.m_RecvData[4] << 24 | m_SO7_Serial.m_RecvData[5] << 16 | m_SO7_Serial.
						m_RecvData[6] << 8 | m_SO7_Serial.m_RecvData[7]);

					//详细 https://blog.csdn.net/hebbely/article/details/79577880
					g_pLogger->SendAndFlushWithTime(L"[GetPositionXyzCache] DataCount：%d, %s\n", DataCount,
						m_SO7_Serial.HexToStr((const char*)m_SO7_Serial.m_RecvData, 8));
				}
				else //查询失败
				{
					g_pLogger->SendAndFlushWithTime(L"[GetPositionXyzCache] DataCount：%d, %s\n", DataCount,
						m_SO7_Serial.HexToStr((const char*)m_SO7_Serial.m_RecvData, 8));
				}
			}

			//读取锁存点，给出期望结果
			m_cSendData[0] = 0x01;
			m_cSendData[1] = 0x09;
			m_WriteByte = Send_Command(0, (const char*)m_cSendData, 2, DataCount * 12);
			Sleep(100);

			//读取点的字节数，每个点占用12个字节
			if (m_SO7_Serial.m_iRecvBytes != (DataCount * 12))
			{
				m_SO7_Serial.m_RecvData[0] = 0xff;
				memcpy(CacheData, m_SO7_Serial.m_RecvData, m_SO7_Serial.m_iRecvBytes); //返回内容

				g_pLogger->SendAndFlushWithTime(L"[GetPositionXyzCache] m_iRecvBytes: %d != Points: %d\n", m_SO7_Serial.m_iRecvBytes, DataCount * 12);
				rStatus = HSI_STATUS_FAILED;
			}
			else
			{
				m_SO7_Serial.m_RecvData[0] = 0xff;
				memcpy(CacheData, m_SO7_Serial.m_RecvData, m_SO7_Serial.m_iRecvBytes); //返回内容

				DataCount = m_SO7_Serial.m_iRecvBytes; //返回点数
				g_pLogger->SendAndFlushWithTime(L"[GetPositionXyzCache] m_iRecvBytes ok, Points: %d\n",
					m_SO7_Serial.m_iRecvBytes);
				m_SO7_Serial.m_iRecvState = FALSE; //将接收标志重置
				rStatus = HSI_STATUS_NORMAL;
			}
		}
		else
		{
			g_pLogger->SendAndFlushWithTime(L"[GetPositionXyzCache] Serial Port is unavailable! \n");
			rStatus = HSI_STATUS_FAILED;
		}

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

//===========================================================================
/**
 * \brief 圆弧插补
 * \param PositionX
 * \param PositionY
 * \param PositionZ
 * \return
 */
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] = static_cast<int>(PositionX / m_Resolution[1]);
		iCircleRunPnt[2] = static_cast<int>(PositionY / m_Resolution[2]);
		iCircleRunPnt[3] = static_cast<int>(PositionZ / m_Resolution[3]);
		g_pLogger->SendAndFlushWithTime(L"SetCircleInterpolate : end\n");
	}
	return rStatus;
}

//===========================================================================
/**
 * \brief 探针接口
 * \param RetractManDist
 */
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 = static_cast<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;
}

//===========================================================================
/**
 * \brief 读取配置
 * \param GoogolIniFile
 * \return
 */
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", nullptr))
		{
			g_pLogger->SendAndFlushWithTime(L"[Load_EF3_Motion_Inifile] Create Log Directory\n");
			CreateDirectory(m_AppPath + L"\\Log", nullptr);
		}
		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] = static_cast<long>(GetPrivateProfileInt(L"PRECISION", L"PRECISION_TIME_" + axisNum[i],
				14000,
				csAppPath)); //超时时间(0.1ms)
			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)));//各轴正限位

			//打印正负限位
			g_pLogger->SendAndFlushWithTime(L"[Load_EF3_Motion_Inifile] m_N_Work_Limit[%d]: %.2f, m_P_Work_Limit[%d]: %.2f\n",
				i, m_N_Work_Limit[i], i, m_P_Work_Limit[i]);

			m_Home_Time[i] = static_cast<long>(GetPrivateProfileInt(L"HOME", L"HOME_TIME_" + axisNum[i], 1500,
				csAppPath)); //回家超时时间(0.1ms)
			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);
			//打印定位合成速度
			g_pLogger->SendAndFlushWithTime(L"[Load_EF3_Motion_Inifile] m_SetPotion_DriveSpeed[%d]: %d\n", i,
				m_SetPotion_DriveSpeed[i]);

			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++) // i 轴
		{
			for (int j = 1; j < 5; j++) //j 档位
			{
				GetPrivateProfileString(L"JOG_SPEED", L"JOG_DRIVESPEED_" + strGear[i] + axisNum[j], L"10",
					temp.GetBufferSetLength(50), 10, csAppPath);
				float speed = (atof(T2A(temp)));
				//printf("spedd:%.2F %S\n",speed, L"JOG_DRIVESPEED_" + strGear[i] + axisNum[j]); //调试打印
				//m_JogDriveSpeed[j][i] = GetPrivateProfileInt(L"JOG_SPEED", L"JOG_DRIVESPEED_" , 10, csAppPath);
				//m_JogDriveSpeed[j][i] = speed / (m_Resolution[j] * 50);//速度转换为脉冲
				m_JogDriveSpeed[j][i] = speed; //直接读取速度
				g_pLogger->SendAndFlushWithTime(L"[Load_EF3_Motion_Inifile] m_JogDriveSpeed[%d][%d]: %f %ld\n", j, i,
					speed, m_JogDriveSpeed[j][i]); //打印配置文件 档位速度

				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_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_IsUseACS = GetPrivateProfileInt(L"EF3", L"IS_USEACS", 0, csAppPath);
		m_IsUseSimulator = GetPrivateProfileInt(L"EF3", L"IS_USE_SIMULATOR", 0, csAppPath);
		m_isSynchronized = GetPrivateProfileInt(L"EF3", L"IS_Synchronized", 1, csAppPath);
		g_pLogger->SendAndFlushWithTime(L"[Load_EF3_Config_Inifile] m_IsUseEF3: %d, m_IsUseACS: %d, m_IsUseSimulator: %d, m_isSynchronized: %d\n",
			m_IsUseEF3, m_IsUseACS, m_IsUseSimulator, m_isSynchronized);

		// 定义变量，保存 是否每次都回家
		m_IsHomeEveryTime = GetPrivateProfileInt(L"EF3", L"IS_HOME_EVERYTIME", 1, csAppPath);
		g_pLogger->SendAndFlushWithTime(L"[Load_EF3_Config_Inifile] m_IsHomeEveryTime: %d\n", m_IsHomeEveryTime);

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

		// 从配置文件中读取IP地址
		TCHAR buffer[50];
		GetPrivateProfileString(L"EF3", L"ACS_ADDRESS", L"100.0.0.100",
			buffer, 50, csAppPath);
		// 将TCHAR类型的buffer转换为char类型的m_ACS_IPAddresses
		size_t convertedChars = 0;
		wcstombs_s(&convertedChars, m_ACS_IPAddresses, buffer, _TRUNCATE);
		g_pLogger->SendAndFlushWithTime(L"[Load_EF3_Config_Inifile] ACS IP Address: %S\n", m_ACS_IPAddresses);

		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_axisLatchFrequency = GetPrivateProfileInt(L"ASIX", L"AXIS_LATCHFREQUENCY", 1000, csAppPath);
		//switch (m_axisLatchFrequency)
		//{
		//case 1:
		//	m_axisLatchFrequency = 1000;
		//	break;
		//case 2:
		//	m_axisLatchFrequency = 2000;
		//	break;
		//case 3:
		//	m_axisLatchFrequency = 4000;
		//	break;
		//case 4:
		//	m_axisLatchFrequency = 8000;
		//	break;
		//case 5:
		//	m_axisLatchFrequency = 16000;
		//	break;
		//} //锁存频率处理

		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); //是否启用IO功能，1为启用，0为关闭，默认0
		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); // 是否启用探针捕获功能，1启用，默认0关闭
		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); //是否启用摇杆功能 0不启用 1为启用 默认为0
		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);

		// 加载ACS 轴号定义
		int x = GetPrivateProfileInt(L"ACS_AXIS", L"ACS_AXIS_X", 0, csAppPath);
		int y = GetPrivateProfileInt(L"ACS_AXIS", L"ACS_AXIS_Y", 0, csAppPath);
		int z = GetPrivateProfileInt(L"ACS_AXIS", L"ACS_AXIS_Z", 0, csAppPath);

		ACSAxisNumbers[0] = ACSAxisDefault[x];
		ACSAxisNumbers[1] = ACSAxisDefault[y];
		ACSAxisNumbers[2] = ACSAxisDefault[z];
		ACSAxisNumbers[3] = -1;
		g_pLogger->SendAndFlushWithTime(L"[Load_EF3_Config_Inifile] 加载ACS轴定义 ACSAxisNumbers[0]: %d, ACSAxisNumbers[1]: %d, ACSAxisNumbers[2]: %d, ACSAxisNumbers[3]: %d\n",
			ACSAxisNumbers[0], ACSAxisNumbers[1], ACSAxisNumbers[2], ACSAxisNumbers[3]);
	}
	return rStatus;
}

//===========================================================================
/**
 * \brief 读取/设置光栅尺精度
 * \param _ScaleX
 * \param _ScaleY
 * \param _ScaleZ
 * \return
 */
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] = static_cast<int>(_ScaleX * 10000);
		m_Resolution[2] = static_cast<int>(_ScaleY * 10000);
		m_Resolution[3] = static_cast<int>(_ScaleZ * 10000);
	}
	return rStatus;
}

//===========================================================================
/**
 * \brief 定位完成事件
 */
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);
}

//===========================================================================
/**
 * \brief 回调探针运行事件
 */
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);
}

//===========================================================================
/**
 * \brief 刷新运动状态，运动命令发出后，通过EF3板判断是否运动停止，读脉冲数进行到位轴判断（是否超时），欧式距离判断（是否超时），发送运动完成。
 */
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();

			//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
			//		if ((m_SO7_Serial.m_RecvData[39] == axis_start)/* &&(m_motorType == 0)*/)
			//		{
			//			m_SO7_Serial.m_RecvData[39] = 0;
			//			SpCompleteTStart = GetTickCount();
			//			interpolationflag = true;
			//			break;
			//		}
			//		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] Now wait Run End\n");

			printf("\nWaiting for motion end\n");
			acsc_WaitMotionEnd(handleACS, ACSAxisNumbers[0], INFINITE);//依次等待 X,Y,Z轴运动到位
			acsc_WaitMotionEnd(handleACS, ACSAxisNumbers[1], INFINITE);
			acsc_WaitMotionEnd(handleACS, ACSAxisNumbers[2], INFINITE);
			printf("\nMotion 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 };

			interpolationflag = true;
			if (interpolationflag && m_motorType)
			{
				while (Count < m_SetPotion_Count[1]) //到位次数判断, 读配置文件240
				{
					Sleep(2); //延时2毫秒，1秒 =1000 毫秒
					//获取当前位置
					GetPositionXyz(HSI_MOTION_AXIS_ALL, prfpos[1], prfpos[2], prfpos[3], prfpos[0]);

					//-----------TEST Begin------------------
					g_pLogger->SendAndFlushWithTime(
						L"[UpdateMotionState] Moving to target, TargetPos[1] = %.4f, NowPos[2] = %.4f, fabs[3] = %.4f\n",
						m_PosThread[1], prfpos[1], fabs(m_PosThread[1] - prfpos[1]));

					//-----------TEST End--------------------
					//目标位置  和当前位置 小于回家误差脉冲数

					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 PntToPntDistance = %.4f\n", prfpos[1],
				prfpos[2], prfpos[3], PntToPntDistance);

			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::UpdateMotionStateOld()
{
	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
					if ((m_SO7_Serial.m_RecvData[39] == axis_start)/* &&(m_motorType == 0)*/)
					{
						m_SO7_Serial.m_RecvData[39] = 0;
						SpCompleteTStart = GetTickCount();
						interpolationflag = true;
						break;
					}
					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);
					}
				}
			}
		}
	}
}

//===========================================================================
/**
 * \brief 获取运动状态
 */
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];
			}
			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;
			}
			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;
	//GetMessage:从系统获取消息，将消息从系统中移除，属于阻塞函数。当系统无消息时，GetMessage会等待下一条消息。

	//PeekMesssge是以查看的方式从系统中获取消息，可以不将消息从系统中移除，是非阻塞函数；
	//当系统无消息时，返回FALSE，继续执行后续代码
	while (PeekMessage(&msg, nullptr, 0, 0, PM_REMOVE))
	{
		__try
		{
			//DispatchMessage 函数分发一个消息给窗口程序。通常消息从GetMessage函数获得或者TranslateMessage函数传递的。
			// 消息被分发到回调函数（过程函数)，作用是消息传递给操作系统，
			// 然后操作系统去调用我们的回调函数，也就是说我们在窗体的过程函数中处理消息。
			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===================================================
/**
 * \brief
 * \param IOChannel
 * \param _Status
 * \return
 */
HSI_STATUS HSI_Motion::GetDIOOld(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::GetDIO(UINT IOChannel, UINT& _Status)
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		//g_pLogger->SendAndFlushWithTime(L"[GetDIO] In\n");
		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");
		}
		//g_pLogger->SendAndFlushWithTime(L"[GetDIO] Out\n");
	}
	//-----------TEST Begin------------------
	_Status = 0;

	//-----------TEST End------------------
	return rStatus;
}

//===========================================================================
/**
 * \brief
 * \param IOChannel
 * \param _Status
 * \return
 */
HSI_STATUS HSI_Motion::SetDIOOld(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::SetDIO(UINT IOChannel, UINT _Status)
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		//g_pLogger->SendAndFlushWithTime(L"[SetDIO] In\n");
		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);
		}
		//g_pLogger->SendAndFlushWithTime(L"[SetDIO] Out\n");
	}

	//-----------TEST Begin------------------
	_Status = 0;
	//-----------TEST End------------------
	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;
}

//===========================================================================
/**
 * \brief 暂停和关闭
 * \return
 */
HSI_STATUS HSI_Motion::AbortMotionOld() //需要运动实现
{
	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::AbortMotion() //需要运动实现
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		g_IsClose = true;
		g_pLogger->SendAndFlushWithTime(L"[AbortMotion] In\n");

		//int Axes[] = { ACSC_AXIS_1, ACSC_AXIS_0, ACSC_AXIS_8, -1 };
		if (handleACS != ACSC_INVALID)
		{
			if (!acsc_HaltM(handleACS, ACSAxisNumbers, nullptr)) //停止JOG运动
			{
				g_pLogger->SendAndFlushWithTime(L"[AbortMotion] ACS acsc_HaltM error!\n");
				ErrorsHandler();
			}
		}

		// 增加一个阻塞延时 500ms
		Sleep(500);


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

//===========================================================================
/**
 * \brief 关闭
 * \return
 */
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;
}

//===========================================================================
/**
 * \brief 触发灯光
 * \param triggleNum
 * \param delayLighting
 * \param delayLightBefor
 * \param triggleMode
 * \param Intensities
 * \return
 */
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++] = (static_cast<int>(Intensities[j] * 11.2) >> 8) & 0xff;
				m_SetTriggerLightData[z++] = static_cast<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++] = (static_cast<int>(Intensities[j] * 11.2) >> 8) & 0xff;
				m_SetTriggerLightData[z++] = static_cast<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++] = (static_cast<int>(Intensities[j] * 11.2) >> 8) & 0xff;
				m_SetTriggerLightData[z++] = static_cast<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;
}

//===========================================================================
/**
 * \brief 硬件触发拍照
 * \param startPoint
 * \return
 */
HSI_STATUS HSI_Motion::DCCPPStartPoint(double* startPoint)
{
	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
		m_IsUsePPS = true;
		begin_position[1] = static_cast<int>(startPoint[1] / m_Resolution[1]);
		begin_position[2] = static_cast<int>(startPoint[2] / m_Resolution[2]);
		begin_position[3] = static_cast<int>(startPoint[3] / m_Resolution[3]);
	}
	return rStatus;
}

//===========================================================================
/*
 * 设置扫描数据并指定三轴坐标点
 * @param AxisTypes 三轴类型（如X、Y、Z），用于指定要设置的触发轴，预留
 * @param speed 运动速度
 * @param points 三维坐标点数组
 * @param numPoints 坐标点数量
 */
HSI_STATUS HSI_Motion::DCCScanSetDataWithPoints(UINT AxisTypes, double speed, Point3D* points, UINT numPoints)
{
	auto rStatus = HSI_STATUS_NORMAL;
	// 处理传入的三维坐标点数组
	for (UINT i = 0; i < numPoints; ++i)
	{
		Point3D point = points[i];
		// 在这里处理每个点，例如打印或存储
		printf("Point %u: x = %f, y = %f, z = %f\n", i, point.x, point.y, point.z);
	}

	// 控制平台的运动，（安全限位的问题，导入100个点，判断是否在运动空间内）

	// 其他处理逻辑
	// ...

	return rStatus;
}

//===========================================================================
HSI_STATUS HSI_Motion::DCCScanSetDataOld(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;
			}
			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++] = static_cast<long>(dTrigDis[j] / m_Resolution[axisNum]) & 0xff;
					m_SendDCCData[z++] = (static_cast<long>(dTrigDis[j] / m_Resolution[axisNum]) >> 8) & 0xff;
					m_SendDCCData[z++] = (static_cast<long>(dTrigDis[j] / m_Resolution[axisNum]) >> 16) & 0xff;
					m_SendDCCData[z++] = static_cast<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++] = static_cast<long>(dTrigDis[j] / m_Resolution[axisNum]) & 0xff;
					m_SendDCCData[z++] = (static_cast<long>(dTrigDis[j] / m_Resolution[axisNum]) >> 8) & 0xff;
					m_SendDCCData[z++] = (static_cast<long>(dTrigDis[j] / m_Resolution[axisNum]) >> 16) & 0xff;
					m_SendDCCData[z++] = static_cast<long>(dTrigDis[j] / m_Resolution[axisNum]) >> 24;
				}
				m_SendDCCData[z++] = static_cast<long>(limit / m_Resolution[axisNum]) & 0xff;
				m_SendDCCData[z++] = (static_cast<long>(limit / m_Resolution[axisNum]) >> 8) & 0xff;
				m_SendDCCData[z++] = (static_cast<long>(limit / m_Resolution[axisNum]) >> 16) & 0xff;
				m_SendDCCData[z++] = static_cast<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++] = static_cast<long>(dTrigDis[j] / m_Resolution[axisNum]) & 0xff;
					m_SendDCCData[z++] = (static_cast<long>(dTrigDis[j] / m_Resolution[axisNum]) >> 8) & 0xff;
					m_SendDCCData[z++] = (static_cast<long>(dTrigDis[j] / m_Resolution[axisNum]) >> 16) & 0xff;
					m_SendDCCData[z++] = static_cast<long>(dTrigDis[j] / m_Resolution[axisNum]) >> 24;
				}
				m_SendDCCData[z++] = static_cast<long>(limit / m_Resolution[axisNum]) & 0xff;
				m_SendDCCData[z++] = (static_cast<long>(limit / m_Resolution[axisNum]) >> 8) & 0xff;
				m_SendDCCData[z++] = (static_cast<long>(limit / m_Resolution[axisNum]) >> 16) & 0xff;
				m_SendDCCData[z++] = static_cast<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] = static_cast<long>(dTrigDis[0] / m_Resolution[axisNum]) & 0xff;
			m_SendDCCData[5] = (static_cast<long>(dTrigDis[0] / m_Resolution[axisNum]) >> 8) & 0xff;
			m_SendDCCData[6] = (static_cast<long>(dTrigDis[0] / m_Resolution[axisNum]) >> 16) & 0xff;
			m_SendDCCData[7] = static_cast<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++] = static_cast<long>(dTrigDis[i] / m_Resolution[axisNum]) & 0xff;
				m_SendDCCData[j++] = (static_cast<long>(dTrigDis[i] / m_Resolution[axisNum]) >> 8) & 0xff;
				m_SendDCCData[j++] = (static_cast<long>(dTrigDis[i] / m_Resolution[axisNum]) >> 16) & 0xff;
				m_SendDCCData[j++] = static_cast<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::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 AxisTypes:%d eType:%d lTrigNumber:%d dTrigDis:ld\n",
			AxisTypes, eType, lTrigNumber, dTrigDis);
		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;
			}
			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++] = static_cast<long>(dTrigDis[j] / m_Resolution[axisNum]) & 0xff;
					m_SendDCCData[z++] = (static_cast<long>(dTrigDis[j] / m_Resolution[axisNum]) >> 8) & 0xff;
					m_SendDCCData[z++] = (static_cast<long>(dTrigDis[j] / m_Resolution[axisNum]) >> 16) & 0xff;
					m_SendDCCData[z++] = static_cast<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++] = static_cast<long>(dTrigDis[j] / m_Resolution[axisNum]) & 0xff;
					m_SendDCCData[z++] = (static_cast<long>(dTrigDis[j] / m_Resolution[axisNum]) >> 8) & 0xff;
					m_SendDCCData[z++] = (static_cast<long>(dTrigDis[j] / m_Resolution[axisNum]) >> 16) & 0xff;
					m_SendDCCData[z++] = static_cast<long>(dTrigDis[j] / m_Resolution[axisNum]) >> 24;
				}
				m_SendDCCData[z++] = static_cast<long>(limit / m_Resolution[axisNum]) & 0xff;
				m_SendDCCData[z++] = (static_cast<long>(limit / m_Resolution[axisNum]) >> 8) & 0xff;
				m_SendDCCData[z++] = (static_cast<long>(limit / m_Resolution[axisNum]) >> 16) & 0xff;
				m_SendDCCData[z++] = static_cast<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++] = static_cast<long>(dTrigDis[j] / m_Resolution[axisNum]) & 0xff;
					m_SendDCCData[z++] = (static_cast<long>(dTrigDis[j] / m_Resolution[axisNum]) >> 8) & 0xff;
					m_SendDCCData[z++] = (static_cast<long>(dTrigDis[j] / m_Resolution[axisNum]) >> 16) & 0xff;
					m_SendDCCData[z++] = static_cast<long>(dTrigDis[j] / m_Resolution[axisNum]) >> 24;
				}
				m_SendDCCData[z++] = static_cast<long>(limit / m_Resolution[axisNum]) & 0xff;
				m_SendDCCData[z++] = (static_cast<long>(limit / m_Resolution[axisNum]) >> 8) & 0xff;
				m_SendDCCData[z++] = (static_cast<long>(limit / m_Resolution[axisNum]) >> 16) & 0xff;
				m_SendDCCData[z++] = static_cast<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] = static_cast<long>(dTrigDis[0] / m_Resolution[axisNum]) & 0xff;
			m_SendDCCData[5] = (static_cast<long>(dTrigDis[0] / m_Resolution[axisNum]) >> 8) & 0xff;
			m_SendDCCData[6] = (static_cast<long>(dTrigDis[0] / m_Resolution[axisNum]) >> 16) & 0xff;
			m_SendDCCData[7] = static_cast<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++] = static_cast<long>(dTrigDis[i] / m_Resolution[axisNum]) & 0xff;
				m_SendDCCData[j++] = (static_cast<long>(dTrigDis[i] / m_Resolution[axisNum]) >> 8) & 0xff;
				m_SendDCCData[j++] = (static_cast<long>(dTrigDis[i] / m_Resolution[axisNum]) >> 16) & 0xff;
				m_SendDCCData[j++] = static_cast<long>(dTrigDis[i] / m_Resolution[axisNum]) >> 24;
			}
			//m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
			Sleep(10);
			break;
		}
		default:
			break;
		}

		//启动定时锁存的同时，启动扫描 外部IO
		unsigned char m_cSendData[8] = { 0 };

		//清空EF3缓存Flash
		m_cSendData[0] = 0x01;
		m_cSendData[1] = 0x04;
		m_WriteByte = Send_Command(0, (const char*)m_cSendData, 2);
		Sleep(2000);//延时2秒，等待清空完成，EF3清空Flash比较慢
		g_pLogger->SendAndFlushWithTime(L"[DCCScanSetData] Out\n");
	}
	ReleaseMutex(g_WR_ToMove_Mutex);
	return rStatus;
}

//===========================================================================
HSI_STATUS HSI_Motion::DCCScanStartOld()
{
	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::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;
		}
		g_pLogger->SendAndFlushWithTime(
			L"[DCCScanStart] iaxisNum:%d, iTriggleNum:%d, iMotionDirection:%d, begin_position:%d  \n", iaxisNum,
			iTriggleNum, iMotionDirection, begin_position[1]);
		//m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);

		//启动定时扫描，此处清空旧缓存值
		unsigned char m_cSendData[8] = { 0 };
		m_cSendData[0] = 0x01;
		m_cSendData[1] = 0x04;
		m_WriteByte = Send_Command(0, (const char*)m_cSendData, 2, 2);

		Sleep(100);
		//启动定时锁存的同时启动扫描外部IO
		m_cSendData[0] = 0x01;
		m_cSendData[1] = 0x02;
		m_WriteByte = Send_Command(0, (const char*)m_cSendData, 2, 2);

		g_pLogger->SendAndFlushWithTime(L"[DCCScanStart] Out\n");
		m_IsUsePPS = false;
	}
	ReleaseMutex(g_WR_ToMove_Mutex);
	return rStatus;
}

//===========================================================================
HSI_STATUS HSI_Motion::DCCScanStopOld()
{
	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::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] = 0x01;
		m_SendDCCData[1] = 0x03;
		m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, 2, 2);
		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;
}

//===========================================================================
/**
 * \brief 转盘
 * \param CamerasDis
 * \param BinsDis
 * \param SubArea
 * \param filterTime1
 * \param filterTime2
 * \param pluseSumDis
 * \return
 */
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"[SendBinResult] 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] = static_cast<int>(gluePPSPos[posIndex++] / m_Resolution[1]);
				loadFeet[1] = static_cast<int>(gluePPSPos[posIndex++] / m_Resolution[1]);
				loadFeet[2] = static_cast<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] = static_cast<int>(gluePPSPos[posIndex++] / m_Resolution[1]);
				loadFeet[1] = static_cast<int>(gluePPSPos[posIndex++] / m_Resolution[1]);
				loadFeet[2] = static_cast<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] = static_cast<int>(gluePPSPos[posIndex++] / m_Resolution[1]);
				loadFeet[1] = static_cast<int>(gluePPSPos[posIndex++] / m_Resolution[1]);
				loadFeet[2] = static_cast<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] = static_cast<int>(gluePPSPos[posIndex++] / m_Resolution[1]);
				loadFeet[1] = static_cast<int>(gluePPSPos[posIndex++] / m_Resolution[1]);
				loadFeet[2] = static_cast<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 = static_cast<int>(lightData[ldIndex++] * 1120);
			light2 = static_cast<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] = static_cast<int>(endGluepos[0] / m_Resolution[1]);
					loadFeet[1] = static_cast<int>(endGluepos[1] / m_Resolution[2]);
					loadFeet[2] = static_cast<int>(endGluepos[2] / m_Resolution[3]);
					loadFeet[3] = static_cast<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] = static_cast<int>((endGluepos[0] - startGluepos[0]) / m_Resolution[1]);
					loadFeet[1] = static_cast<int>((endGluepos[1] - startGluepos[1]) / m_Resolution[2]);
					loadFeet[2] = static_cast<int>((endGluepos[2] - startGluepos[2]) / m_Resolution[3]);
					loadFeet[3] = static_cast<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] = static_cast<int>(endGluepos[0] / m_Resolution[1]);
					loadFeet[1] = static_cast<int>(endGluepos[1] / m_Resolution[2]);
					loadFeet[2] = static_cast<int>(endGluepos[2] / m_Resolution[3]);
					loadFeet[3] = static_cast<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] = static_cast<int>((endGluepos[0] - startGluepos[0]) / m_Resolution[1]);
					loadFeet[1] = static_cast<int>((endGluepos[1] - startGluepos[1]) / m_Resolution[2]);
					loadFeet[2] = static_cast<int>((endGluepos[2] - startGluepos[2]) / m_Resolution[3]);
					loadFeet[3] = static_cast<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] = static_cast<int>((endGluepos[0] - startGluepos[0]) / m_Resolution[1]);
					loadFeet[1] = static_cast<int>((endGluepos[1] - startGluepos[1]) / m_Resolution[2]);
					loadFeet[2] = static_cast<int>((endGluepos[2] - startGluepos[2]) / m_Resolution[3]);
					loadFeet[3] = static_cast<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] = static_cast<int>((endGluepos[0] - startGluepos[0]) / m_Resolution[1]);
					loadFeet[1] = static_cast<int>((endGluepos[1] - startGluepos[1]) / m_Resolution[2]);
					loadFeet[2] = static_cast<int>((endGluepos[2] - startGluepos[2]) / m_Resolution[3]);
					loadFeet[3] = static_cast<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] = static_cast<int>(endGluepos[0] / m_Resolution[1]);
					loadFeet[1] = static_cast<int>(endGluepos[1] / m_Resolution[2]);
					loadFeet[2] = static_cast<int>(endGluepos[2] / m_Resolution[3]);
					loadFeet[3] = static_cast<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] = static_cast<int>((gluePos[posIndex++] - startGluepos[0]) / m_Resolution[1]);
					circlepnt[1] = static_cast<int>((gluePos[posIndex++] - startGluepos[1]) / m_Resolution[2]);
					circlepnt[2] = static_cast<int>((gluePos[posIndex++] - startGluepos[2]) / m_Resolution[3]);
					circlepnt[3] = static_cast<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] = static_cast<int>((endGluepos[0] - startGluepos[0]) / m_Resolution[1]);
					loadFeet[1] = static_cast<int>((endGluepos[1] - startGluepos[1]) / m_Resolution[2]);
					loadFeet[2] = static_cast<int>((endGluepos[2] - startGluepos[2]) / m_Resolution[3]);
					loadFeet[3] = static_cast<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] = static_cast<int>((endGluepos[0] - startGluepos[0]) / m_Resolution[1]);
					loadFeet[1] = static_cast<int>((endGluepos[1] - startGluepos[1]) / m_Resolution[2]);
					loadFeet[2] = static_cast<int>((endGluepos[2] - startGluepos[2]) / m_Resolution[3]);
					loadFeet[3] = static_cast<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] = static_cast<int>((gluePos[posIndex++] - startGluepos[0]) / m_Resolution[1]);
					circlepnt[1] = static_cast<int>((gluePos[posIndex++] - startGluepos[1]) / m_Resolution[2]);
					circlepnt[2] = static_cast<int>((gluePos[posIndex++] - startGluepos[2]) / m_Resolution[3]);
					circlepnt[3] = static_cast<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] = static_cast<int>((endGluepos[0] - startGluepos[0]) / m_Resolution[1]);
					loadFeet[1] = static_cast<int>((endGluepos[1] - startGluepos[1]) / m_Resolution[2]);
					loadFeet[2] = static_cast<int>((endGluepos[2] - startGluepos[2]) / m_Resolution[3]);
					loadFeet[3] = static_cast<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;
}

//===========================================================================
/**
 * \brief 界面挡位获取速度
 * \param AxisNum
 * \param Speed
 * \param DirveSpeed
 * \param StartSpeed
 * \param AccLine
 * \param DecLine
 * \param AccCurve
 * \param DecCurve
 * \return
 */
int HSI_Motion::SpeedPercent(int AxisNum, double& Speed, int& DirveSpeed, int& StartSpeed, int& AccLine, int& DecLine,
	int& AccCurve, int& DecCurve)
{
	g_pLogger->SendAndFlushWithTime(L"[SpeedPercent]  AxisNum: [%d]\n", AxisNum);
	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_JogAccLine[AxisNum][0] * 10;
		DecCurve = m_JogDecLine[AxisNum][0] * 10;
		g_pLogger->SendAndFlushWithTime(
			L"[SpeedPercent] 四挡 DriveSpeed: [%d], StartSpeed: [%d], AccLine: [%d], DecLine: [%d], AccCurve: [%d], DecCurve: [%d]\n",
			DirveSpeed, StartSpeed, AccLine, DecLine, AccCurve, DecCurve);
	}
	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_JogAccLine[AxisNum][1] * 10;
		DecCurve = m_JogDecLine[AxisNum][1] * 10;
		g_pLogger->SendAndFlushWithTime(
			L"[SpeedPercent] 三档 DriveSpeed: [%d], StartSpeed: [%d], AccLine: [%d], DecLine: [%d], AccCurve: [%d], DecCurve: [%d]\n",
			DirveSpeed, StartSpeed, AccLine, DecLine, AccCurve, DecCurve);
	}
	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_JogAccLine[AxisNum][2] * 10;
		DecCurve = m_JogDecLine[AxisNum][2] * 10;
		g_pLogger->SendAndFlushWithTime(
			L"[SpeedPercent] 二档 DriveSpeed: [%d], StartSpeed: [%d], AccLine: [%d], DecLine: [%d], AccCurve: [%d], DecCurve: [%d]\n",
			DirveSpeed, StartSpeed, AccLine, DecLine, AccCurve, DecCurve);
	}
	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_JogAccLine[AxisNum][3] * 10;
		DecCurve = m_JogDecLine[AxisNum][3] * 10;
		g_pLogger->SendAndFlushWithTime(
			L"[SpeedPercent] 一档 DriveSpeed: [%d], StartSpeed: [%d], AccLine: [%d], DecLine: [%d], AccCurve: [%d], DecCurve: [%d]\n",
			DirveSpeed, StartSpeed, AccLine, DecLine, AccCurve, DecCurve);
	}
	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_JogAccLine[AxisNum][4] * 10;
		DecCurve = m_JogDecLine[AxisNum][4] * 10;
		g_pLogger->SendAndFlushWithTime(
			L"[SpeedPercent] 零档 DriveSpeed: [%d], StartSpeed: [%d], AccLine: [%d], DecLine: [%d], AccCurve: [%d], DecCurve: [%d]\n",
			DirveSpeed, StartSpeed, AccLine, DecLine, AccCurve, DecCurve);
	}
	else
	{
		MovetoSpeedGear = 0;
	}
	Speed = MovetoSpeedGear * flag;
	return static_cast<int>(Speed);
}

//===========================================================================
/**
 * \brief JoyStick运动控制参数读取及设置
 * \param AxisNum
 * \param Speed
 * \param DirveSpeed
 * \param StartSpeed
 * \param AccLine
 * \param DecLine
 * \param AccCurve
 * \param DecCurve
 * \return
 */
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 = static_cast<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;
}

//===========================================================================
/**
 * \brief 获取单轴设置的速度
 * \param AxisNum
 * \param Speed
 * \return
 */
HSI_STATUS HSI_Motion::GetSpeedXyzOld(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::GetSpeedXyz(int AxisNum, double& Speed)
{
	g_pLogger->SendAndFlushWithTime(L"[GetSpeedXyz] In ================================ \n");
	auto rStatus = HSI_STATUS_NORMAL;
	short AxisNumber = AxisConvertIndex(AxisNum); //将轴号转换为 平台轴号
	//打印轴号换算后的值
	g_pLogger->SendAndFlushWithTime(L"[GetSpeedXyz] AxisNum = %d, AxisNumber = %d\n", AxisNum, AxisNumber);

	// 方式1
	for (int j = 0;j < 9;j++)
	{
		//写日志
		g_pLogger->SendAndFlushWithTime(L"m_SetPotion_DriveSpeed[%d] = %d\n", j, m_SetPotion_DriveSpeed[j]);
	}
	Speed = m_SetPotion_DriveSpeed[AxisNumber]; //从m_SetPotion_DriveSpeed[AxisNumber]中获取速度

	//方式2，从底层查询
	double getMotionParam[5] = { 0 };
	GetSingleAxisParam(AxisNumber, getMotionParam); //获取单轴运动参数
	g_pLogger->SendAndFlushWithTime(
		L"[GetSingleAxisParam] Axis= %d, Velocity = %.2f, Acceleration= %.2f, Deceleration= %.2f, KillDeceleration= %.2f, Jerk= %.2f\n",
		AxisNumber, getMotionParam[0], getMotionParam[1], getMotionParam[2], getMotionParam[3], getMotionParam[4]);

	Speed = getMotionParam[0]; //从getMotionParam[0]中获取速度

	g_pLogger->SendAndFlushWithTime(L"[GetSpeedXyz] AxisNum = %d, Speed=%.2f\n", AxisNumber, Speed);
	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] = static_cast<int>(fabs(Speed * m_Jog_Auto_Focus));
		g_pLogger->SendAndFlushWithTime(L"[SetSpeedXyz] m_Jog_SpeedZ[3][4] = %f\n", m_JogDriveSpeed[3][4]);
	}
	else
	{
		if (1 == m_iSpeedType)
		{
			//m_SetPotion_DriveSpeed[1] = static_cast<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;
			//}
			m_SetPotion_DriveSpeed[1] = static_cast<int>(Speed);
			//打印 m_setotion_drivespeed[1]的值
			g_pLogger->SendAndFlushWithTime(L"[SetSpeedXyz] m_iSpeedType=1, m_SetPotion_DriveSpeed[1] = %.2f\n", m_SetPotion_DriveSpeed[1]);
		}
		else
		{
			m_SetPotion_DriveSpeed[1] = static_cast<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;
			//}
			//打印 m_setotion_drivespeed[1]的值
			g_pLogger->SendAndFlushWithTime(L"[SetSpeedXyz] m_SetPotion_DriveSpeed[1] = %.2f\n", m_SetPotion_DriveSpeed[1]);
		}
	}
	g_pLogger->SendAndFlushWithTime(L"[SetSpeedXyz] Out ================================ \n");
	return rStatus;
}

//===========================================================================
// 切换运动模式函数
void HSI_Motion::SwitchMode(MotionMode newMode)
{
	g_pLogger->SendAndFlushWithTime(L"[SwitchMode] In ================================ \n");

	// 打印新模式
	g_pLogger->SendAndFlushWithTime(L"[SwitchMode] New mode: %d\n", newMode);
	// 打印当前模式
	g_pLogger->SendAndFlushWithTime(L"[SwitchMode] Current mode: %d\n", m_CurrentMode);
	if (m_CurrentMode != newMode)
	{
		// 根据新模式进行准备工作
		//switch (newMode)
		//{
		//case MODE_A:
		//	PrepareForModeA();
		//	break;
		//case MODE_B:
		//	PrepareForModeB();
		//	break;
		//}

		AbortMotion(); //模式切换前先停止运动


		// 更新当前模式
		m_CurrentMode = newMode;
		g_pLogger->SendAndFlushWithTime(L"Switched to new mode: %d\n", newMode);
	}
	g_pLogger->SendAndFlushWithTime(L"[SwitchMode] Out ================================\n");
}

//===========================================================================
//HSI_STATUS HSI_Motion::GetFocusSpeedOld(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::GetFocusSpeed(double& Speed)
{
	auto rStatus = HSI_STATUS_NORMAL;

	// 方式1
	Speed = m_Jog_Auto_Focus;
	g_pLogger->SendAndFlushWithTime(L"[GetFocusSpeed]  m_Jog_Auto_Focus = %f\n", m_Jog_Auto_Focus);

	// 方式2，从底层查询
	// 正常这里要读取z轴的速度，但是这里直接返回m_Jog_Auto_Focus
	//double getMotionParam[5] = { 0 };
	//GetSingleAxisParam(ACSAxisNumbers[2], getMotionParam); //获取单轴运动参数
	//g_pLogger->SendAndFlushWithTime(
	//	L"[GetFocusSpeed] Axis= %d, Velocity = %.2f, Acceleration= %.2f, Deceleration= %.2f, KillDeceleration= %.2f, Jerk= %.2f\n",
	//	ACSAxisNumbers[2], getMotionParam[0], getMotionParam[1], getMotionParam[2], getMotionParam[3], getMotionParam[4]);

	//Speed = getMotionParam[0]; //从getMotionParam[0]中获取速度
	g_pLogger->SendAndFlushWithTime(L"[GetFocusSpeed] AxisNum = %d, Speed=%.2f\n", ACSAxisNumbers[2], Speed);
	return rStatus;
}

//===========================================================================
//HSI_STATUS HSI_Motion::SetFocusSpeedOld(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::SetFocusSpeed(double Speed)
{
	auto rStatus = HSI_STATUS_NORMAL;
	g_pLogger->SendAndFlushWithTime(L"[SetFocusSpeed] In ================================\n");
	g_pLogger->SendAndFlushWithTime(L"[SetFocusSpeed] Speed = %f\n", Speed);
	m_Jog_Auto_Focus = Speed;
	g_pLogger->SendAndFlushWithTime(L"[SetFocusSpeed] m_Jog_Auto_Focus= %f\n", m_Jog_Auto_Focus);

	// 正常这里要设置z轴的速度
	g_pLogger->SendAndFlushWithTime(L"[SetFocusSpeed] 设置Z轴定位速度 %f\n", m_Jog_Auto_Focus);
	double Z_SetmotionParam[5] = {
		m_Jog_Auto_Focus,		//速度
		m_Jog_Auto_Focus * 10 ,//加速度
		m_Jog_Auto_Focus * 10, //减速度
		m_Jog_Auto_Focus * 100,//加加速度
		m_Jog_Auto_Focus * 100 //减减速度
	};

	SetSingleAxisMotionParams(ACSAxisNumbers[2], Z_SetmotionParam);//设置Z轴定位速度
	m_SetPotion_DriveSpeed[ACSAxisNumbers[2]] = Z_SetmotionParam[0]; //记录Z轴速度

	for (int j = 0;j < 9;j++)
	{
		//写日志
		g_pLogger->SendAndFlushWithTime(L"m_SetPotion_DriveSpeed[%d] = %d\n", j, m_SetPotion_DriveSpeed[j]);
	}

	g_pLogger->SendAndFlushWithTime(L"[SetFocusSpeed] Out ================================\n");
	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 HSI_MOTION_AXIS_X: // X 轴
		{
			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 HSI_MOTION_AXIS_Y: //Y 轴
		{
			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 HSI_MOTION_AXIS_Z:
		{
			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 HSI_MOTION_AXIS_R:
		{
			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 m_Axis(0);
	switch (AxisTypes)
	{
	case HSI_MOTION_AXIS_X:
	{
		m_Axis = ACSAxisNumbers[0]; //对应ACS 平台 1轴
		break;
	}
	case HSI_MOTION_AXIS_Y:
	{
		m_Axis = ACSAxisNumbers[1]; //对应 ACS 平台 0轴	(龙门轴)
		break;
	}
	case HSI_MOTION_AXIS_Z:
	{
		m_Axis = ACSAxisNumbers[2];//对应 ACS平台 8轴 （ecat通讯）
		break;
	}
	case HSI_MOTION_AXIS_R:
	{
		m_Axis = 0x04;
		break;
	}
	default:
	{
		g_pLogger->SendAndFlushWithTime(L"[AxisConvertIndex] failed, AxisTypes = %d,AxisNumber = %d\n", AxisTypes,
			m_Axis);
		break;
	}
	}
	return m_Axis;
}

//===========================================================================
void HSI_Motion::SetSingleAxisMotionParams(UINT AxisTypes, double SetMotionParam[5]) //设置单轴运动参数
{
	g_pLogger->SendAndFlushWithTime(L"[SetSingleAxisMotionParams] In >>>>>>>>>>>>>>>>>>>>>> \n");
	double getMotionParam[5] = { 0 };

	GetSingleAxisParam(AxisTypes, getMotionParam); //获取单轴运动参数
	g_pLogger->SendAndFlushWithTime(
		L"[before] Axis= %d, Velocity = %.2f, Acceleration= %.2f, Deceleration= %.2f, KillDeceleration= %.2f, Jerk= %.2f\n",
		AxisTypes, getMotionParam[0], getMotionParam[1], getMotionParam[2], getMotionParam[3], getMotionParam[4]);

	//设置单轴参数
	SetSingleAxisParam(AxisTypes, SetMotionParam);

	//打印轴当前运动参数
	GetSingleAxisParam(AxisTypes, getMotionParam); //获取单轴运动参数
	g_pLogger->SendAndFlushWithTime(
		L"[after] Axis= %d, Velocity = %.2f, Acceleration= %.2f, Deceleration= %.2f, KillDeceleration= %.2f, Jerk= %.2f\n",
		AxisTypes, getMotionParam[0], getMotionParam[1], getMotionParam[2], getMotionParam[3], getMotionParam[4]);

	g_pLogger->SendAndFlushWithTime(L"[SetSingleAxisMotionParams] Out >>>>>>>>>>>>>>>>>>>>>>\n");
}

//===========================================================================
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;
	}
	//写日志
	g_pLogger->SendAndFlushWithTime(L"[IsSupportedEx] AxisNumber = %d, Types = %d\n", AxisNumber, Types);
	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];
	}
	//写日志
	g_pLogger->SendAndFlushWithTime(L"[GetScaleResolutionEx] AxisNumber = %d, Scale = %f\n", AxisNumber, Scale);
	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] = static_cast<int>(Scale * 10000);
	}
	//写日志
	g_pLogger->SendAndFlushWithTime(L"[SetScaleResolutionEx] AxisNumber = %d, Scale = %f\n", AxisNumber, m_Resolution[AxisNumber]);
	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];
		//g_pLogger->SendAndFlushWithTime(L"[GetPositionEx] Position: %d\n", Position);
	}
	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)
		{
			Speed = m_SetPotion_DriveSpeed[1];

			g_pLogger->SendAndFlushWithTime(L"[GetSpeedEx] m_motorType =1， Speed=%d\n", Speed);
		}
		else
		{
			Speed = m_SetPotion_DriveSpeed[AxisNumber];

			g_pLogger->SendAndFlushWithTime(L"[GetSpeedEx] m_iSpeedType=1, Speed=%d\n", Speed);
		}
	}
	g_pLogger->SendAndFlushWithTime(L"[GetSpeedEx] Out\n");
	return rStatus;
}

//===========================================================================
/**
 * \brief 设置单轴运动速度
 * \param AxisTypes
 * \param Speed
 * \return
 */
HSI_STATUS HSI_Motion::SetSpeedExOld(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] = static_cast<int>(Speed) / (m_Resolution[1] * 50);
				}
				else
				{
					m_SetPotion_DriveSpeed[1] = static_cast<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] = static_cast<int>(Speed / (m_Resolution[AxisNumber] * 50));
				}
				else
				{
					m_SetPotion_DriveSpeed[AxisNumber] = static_cast<int>(Speed);
				}
				g_pLogger->SendAndFlushWithTime(L"[SetSpeedEx] AxisNumber = %d\n", AxisNumber);
			}
		}
		else
		{
			if (m_motorType == 1)
			{
				m_SetPotion_DriveSpeed[AxisNumber] = static_cast<int>(Speed / (m_Resolution[AxisNumber] * 50));
			}
			else
			{
				m_SetPotion_DriveSpeed[AxisNumber] = static_cast<int>(Speed / (m_Resolution[AxisNumber] * 50));
			}
		}
	}
	g_pLogger->SendAndFlushWithTime(L"[SetSpeedEx] Out\n");
	return rStatus;
}

HSI_STATUS HSI_Motion::SetSpeedEx(UINT AxisTypes, double Speed)
{
	auto rStatus = HSI_STATUS_NORMAL;
	g_pLogger->SendAndFlushWithTime(L"[SetSpeedEx] Method not implemented\n");
	//short AxisNumber = AxisConvertIndex(AxisTypes);
	//if (Speed <= 0)
	//{
	//	m_SetPotion_DriveSpeed[AxisNumber] = 0;
	//	g_pLogger->SendAndFlushWithTime(L"[SetSpeedEx] Speed <= 0 \n");
	//}

	//if (AxisNumber >= 0 && AxisNumber <= 8)
	//{
	//	m_SetPotion_DriveSpeed[1] = static_cast<int>(Speed);
	//	g_pLogger->SendAndFlushWithTime(L"[SetSpeedEx] AxisNumber = %d,Speed = %d \n", AxisNumber, m_SetPotion_DriveSpeed[1]);
	//}
	//else
	//{
	//	//打印日志 轴号不在1-4之间
	//	g_pLogger->SendAndFlushWithTime(L"[SetSpeedEx] 轴号不在0-8之间\n");
	//}
	//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);
		}
	}
	g_pLogger->SendAndFlushWithTime(L"[GetAccelerationEx] Accel = %d\n", Accel);
	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] = static_cast<int>(Accel / (m_Resolution[AxisNumber] * 500));
		}
		else
		{
			m_SetPotion_StartSpeed[AxisNumber] = static_cast<int>(Accel / (m_Resolution[AxisNumber] * 500));
		}
	}
	g_pLogger->SendAndFlushWithTime(L"[SetAccelerationEx] Accel = %d\n", m_SetPotion_StartSpeed[AxisNumber]);
	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 == nullptr)
	{
		m_Thread_State = HSI_THREAD_RUNNING;
		m_hTriggerEvent = CreateEvent(nullptr, FALSE, NULL, nullptr);
		m_Thread_Id = ::CreateThread(
			nullptr,
			0,
			(LPTHREAD_START_ROUTINE)m_Thread,
			this,
			0,
			nullptr);
		m_Thread_Mutex = CreateMutex(nullptr, FALSE, nullptr);
		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 = nullptr;
	m_hTriggerEvent = nullptr;
	m_Thread_Mutex = nullptr;
	TRACE("CloseThread!\n");
}

//===========================================================================
unsigned __stdcall HSI_Motion::m_Thread(LPVOID pThis)
{
	auto _This = static_cast<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 == nullptr)
	{
		m_Thread_StateProbe = HSI_THREAD_RUNNING;
		m_hTriggerEventProbe = CreateEvent(nullptr, FALSE, NULL, nullptr);
		m_Thread_IdProbe = ::CreateThread(
			nullptr,
			0,
			(LPTHREAD_START_ROUTINE)m_ThreadProbe,
			this,
			0,
			nullptr);
		m_Thread_MutexProbe = CreateMutex(nullptr, FALSE, nullptr);
		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 = nullptr;
	m_hTriggerEventProbe = nullptr;
	m_Thread_MutexProbe = nullptr;
	TRACE("CloseThread!\n");
}

//===========================================================================
unsigned __stdcall HSI_Motion::m_ThreadProbe(LPVOID pThis)
{
	auto _This = static_cast<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 == nullptr)
	{
		m_Thread_StateIO = HSI_THREAD_RUNNING;
		m_hTriggerEventIO = CreateEvent(nullptr, FALSE, NULL, nullptr);
		m_Thread_IdIO = ::CreateThread(
			nullptr,
			0,
			(LPTHREAD_START_ROUTINE)m_ThreadIO,
			this,
			0,
			nullptr);
		m_Thread_MutexIO = CreateMutex(nullptr, FALSE, nullptr);
		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 = nullptr;
	m_hTriggerEventIO = nullptr;
	m_Thread_MutexIO = nullptr;
	TRACE("CloseThreadIO!\n");
}

//===========================================================================
unsigned __stdcall HSI_Motion::m_ThreadIO(LPVOID pThis)
{
	auto _This = static_cast<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 == nullptr)
	{
		m_Thread_StateData = HSI_THREAD_RUNNING;
		m_hTriggerEventData = CreateEvent(nullptr, FALSE, NULL, nullptr);
		m_Thread_IdData = ::CreateThread(
			nullptr,
			0,
			(LPTHREAD_START_ROUTINE)m_ThreadData,
			this,
			0,
			nullptr);
		m_Thread_MutexData = CreateMutex(nullptr, FALSE, nullptr);
		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 = nullptr;
	m_hTriggerEventData = nullptr;
	m_Thread_MutexData = nullptr;
	TRACE("CloseThreadData!\n");
}

//===========================================================================
unsigned __stdcall HSI_Motion::m_ThreadData(LPVOID pThis)
{
	auto _This = static_cast<HSI_Motion*>(pThis);
	for (;;)
	{
		TRACE("m_ThreadData!\n");
		if (m_Thread_StateData == THREAD_EXIT)
			ExitThread(0);
		//WaitForSingleObject函数用来检测hHandle事件的信号状态，
		//在某一线程中调用该函数时，线程暂时挂起，
		//如果在挂起的dwMilliseconds毫秒内，线程所等待的对象变为有信号状态，则该函数立即返回；
		//如果时间已经到达dwMilliseconds毫秒，但hHandle所指向的对象还没有变成有信号状态，函数照样返回。
		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;
		}
	}
}

//===========================================================================
/**
 * \brief 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 == nullptr)
	{
		m_Thread_StateJOGStop = HSI_THREAD_RUNNING;
		m_hTriggerEventJOGStop = CreateEvent(nullptr, FALSE, NULL, nullptr);
		m_Thread_IdJOGStop = ::CreateThread(
			nullptr,
			0,
			(LPTHREAD_START_ROUTINE)m_ThreadJOGStop,
			this,
			0,
			nullptr);
		m_Thread_MutexJOGStop = CreateMutex(nullptr, FALSE, nullptr);
		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 = nullptr;
	m_hTriggerEventJOGStop = nullptr;
	m_Thread_MutexJOGStop = nullptr;
	TRACE("CloseThreadJOGStop!\n");
}

//===========================================================================
unsigned __stdcall HSI_Motion::m_ThreadJOGStop(LPVOID pThis) //JOG运行到软限位的运动调节
{
	auto _This = static_cast<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] = static_cast<byte>(m_Resolution[1] * 10000);
		Send_Command(0, (const char*)findorigdata, 64);
	}
	else
	{
		findorigdata[1] = 0x19;
		findorigdata[2] = static_cast<byte>(m_Resolution[1] * 10000);
		Send_Command(0, (const char*)findorigdata, 64);
	}
	return rStatus;
}

//===========================================================================
/**
 * \brief  串口发送命令
 * \param com
 * \param _SendData 发送内容指针
 * \param SendDataLength 发送字节长度
 * \param expectType 期待返回长度，2：返回ok，8：返回8个字节，N:返回N字节，如果本次返回内容不是期待字节，等待或重发该命令。
 * \return
 */
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))
	{
		g_pLogger->SendAndFlushWithTime(L"[Send_Command] lenth:%d, %s\n", SendDataLength,
			m_SO7_Serial.HexToStr(_SendData, SendDataLength));
		int iWriteByte = m_SO7_Serial.Send(_SendData, SendDataLength);
		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;
};
//分3个层次处理该问题：ZZX
//1、用户层：业务逻辑层面处理，并在发送的时候，给出期望回应值
//2、发送层：保证发送成功，并对返回数据进行一定的判断，如果通讯超时，进行重发逻辑
//3、接收数据线程，获得单包数据后，根据期望值进行拼包判断处理，返回拼包后的数据
BOOL HSI_Motion::Send_Command(int com, const char* _SendData, DWORD SendDataLength, int expectType)
{
	BOOL rStatus = TRUE;
	WaitForSingleObject(g_RW_Data_Mutex, INFINITE);

	//指令回复分两种情况:
	//1、正常2字节，回复 OK，（0x6F 0x6B ）
	//2、返回多字节

	if (m_bConnected && (m_IsUseEF3 == 1) /*&& (com == 0)*/)
	{
		//------------------------------调试区-----------------------------------//
		//原文链接：https://blog.csdn.net/qq_31073871/article/details/85696092
		//printf("Send_Command: ");
		//for (int i = 0; i < SendDataLength; i++)
		//{
		//	printf("%02X ", ((uint8_t*)_SendData)[i]);
		//}
		//printf("\r\n");
		//------------------------------调试区-----------------------------------//

		g_pLogger->SendAndFlushWithTime(L"[Send_Command] lenth:%d, expectType:%d, %s\n", SendDataLength, expectType,
			m_SO7_Serial.HexToStr(_SendData, SendDataLength));
		int iWriteByte = m_SO7_Serial.Send(_SendData, SendDataLength);

		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;
		}

		if (iWriteByte != SendDataLength) // 写入字节数不等于发送字节数
		{
			rStatus = FALSE;
			g_pLogger->SendAndFlushWithTime(L"[Send_Command] WriteByte:%d,!= SendDataLength:%d\n", iWriteByte,
				SendDataLength);
		}
		m_SO7_Serial.m_iRecvState = false; //发送完毕，接收状态置为false，并开始等待EF3回复
		m_SO7_Serial.m_iExpectBytes = expectType; //给出期望结果，传递到下层，由下层进行拼包

		//判断回复值，是否为ok 6F 6B
		int iRetrys = 0;
		while (!m_SO7_Serial.m_iRecvState && iRetrys < 100) //再次接收等待
		{
			iRetrys++;
			Sleep(3);
		}
		g_pLogger->SendAndFlushWithTime(L"[Send_Command] Timeout Retrys:%d\n", iRetrys);

		//两种情况，1、成功获得期望值；2、如果超时
		if (m_SO7_Serial.m_iRecvState && (m_SO7_Serial.m_iRecvBytes == expectType))
		{
			rStatus = TRUE;
			g_pLogger->SendAndFlushWithTime(L"[Send_Command] EF3 Reply OK, RecvBytes:%d, %S\n",
				m_SO7_Serial.m_iRecvBytes,
				m_SO7_Serial.HexToStr((const char*)m_SO7_Serial.m_RecvData,
					m_SO7_Serial.m_iRecvBytes));
			/*		g_pLogger->SendAndFlushWithTime(L"[Send_Command] EF3 Reply OK, RecvBytes:%d\n",
						m_SO7_Serial.m_iRecvBytes);*/
		}
		else if (iRetrys > 100) //获取超时，重发
		{
			iRetrys = 0;
			g_pLogger->SendAndFlushWithTime(L"[Send_Command] EF3 Reply Timeout ,ReSend\n");
			int iWriteByte = m_SO7_Serial.Send(_SendData, SendDataLength); //再次重发，并等待回应
			m_SO7_Serial.m_iRecvState = false; //发送完毕，接收状态置为false，并开始等待EF3回复
			m_SO7_Serial.m_iExpectBytes = expectType; //给出期望结果，传递到下层，由下层进行拼包

			while (!m_SO7_Serial.m_iRecvState && iRetrys < 100)
			{
				iRetrys++;
				Sleep(2);
			}

			if (iRetrys > 100)
			{
				rStatus = HSI_STATUS_FAILED;
				g_pLogger->SendAndFlushWithTime(L"[Send_Command] EF3 Reply Error!");
			}
			else //获得期望结果
			{
				rStatus = HSI_STATUS_NORMAL;
				g_pLogger->SendAndFlushWithTime(L"[Send_Command] EF3 Reply OK, RecvBytes:%d, %s\n",
					m_SO7_Serial.m_iRecvBytes,
					m_SO7_Serial.HexToStr((const char*)m_SO7_Serial.m_RecvData,
						m_SO7_Serial.m_iRecvBytes));
			}
		}

		//发送完当前指令后，第一次结束完成标志
		//if (m_SO7_Serial.m_iRecvBytes != expectType) //获取数量不等于期望数量，打印错误，并重发
		//{
		//	g_pLogger->SendAndFlushWithTime(L"[Send_Command] EF3 Reply Error, RecvBytes:%d, %s\n",
		//	                                m_SO7_Serial.m_iRecvBytes,
		//	                                m_SO7_Serial.HexToStr((const char*)m_SO7_Serial.m_RecvData,
		//	                                                      m_SO7_Serial.m_iRecvBytes));
		//	m_SO7_Serial.m_iRecvState = false;
		//	int iWriteByte = m_SO7_Serial.Send(_SendData, SendDataLength); //再次重发，并等待回应
		//	while (!m_SO7_Serial.m_iRecvState && iRetrys < 100) //等待300毫秒
		//	{
		//		iRetrys++;
		//		Sleep(3);
		//	}
		//	if (m_SO7_Serial.m_iRecvState)
		//	{
		//		if (iRetrys > 100)
		//		{
		//			iRetrys = 0;
		//			while (m_SO7_Serial.m_iRecvBytes != expectType)
		//			{
		//				iRetrys++;
		//				if (iRetrys > 100)
		//					break;
		//				Sleep(2);
		//			}
		//		}
		//		if (iRetrys > 100)
		//		{
		//			rStatus = HSI_STATUS_FAILED;
		//			g_pLogger->SendAndFlushWithTime(L"[Send_Command] EF3 Reply Error!");
		//		}
		//		else //获得期望结果
		//		{
		//			rStatus = HSI_STATUS_NORMAL;
		//			g_pLogger->SendAndFlushWithTime(L"[Send_Command] EF3 Reply OK, RecvBytes:%d, %s\n",
		//			                                m_SO7_Serial.m_iRecvBytes,
		//			                                m_SO7_Serial.HexToStr((const char*)m_SO7_Serial.m_RecvData,
		//			                                                      m_SO7_Serial.m_iRecvBytes));
		//		}
		//	}
		//}
		//else
		//{
		//	rStatus = FALSE;
		//	g_pLogger->SendAndFlushWithTime(L"[Send_Command] EF3 Reply OK, RecvBytes:%d, %s\n",
		//	                                m_SO7_Serial.m_iRecvBytes,
		//	                                m_SO7_Serial.HexToStr((const char*)m_SO7_Serial.m_RecvData,
		//	                                                      m_SO7_Serial.m_iRecvBytes));
		//}
	}

	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;
					}
					if (errorCode == WSAEISCONN)
					{
						break;
					}
					first = true;
					closesocket(m_socket[index]);
					WSACleanup();
					rCode = static_cast<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)
{
	auto _This = static_cast<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(nullptr, 0, (LPTHREAD_START_ROUTINE)m_ThreadSendTCP,
			this, 0, nullptr);
	}
}

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 = nullptr;
}

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;
}

//===========================================================================
HSI_STATUS HSI_Motion::SetMotionDirectionFlag(int CurrentDirection) //用于向ACS特定变量写入 -1 或 1
{
	g_pLogger->SendAndFlushWithTime(L"[SetMotionDirectionFlag] CurrentDirection = %d\n", CurrentDirection);

	// 判断输入是否为1或-1
	if (CurrentDirection != 1 && CurrentDirection != -1)
	{
		g_pLogger->SendAndFlushWithTime(L"[SetMotionDirectionFlag] Invalid direction flag\n");
		return HSI_STATUS_FAILED;
	}

	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion && handleACS != ACSC_INVALID)
	{
		int waitTowrite[1] = { CurrentDirection };
		if (!acsc_WriteInteger(handleACS, // communication handle
			ACSC_NONE, // standard variable
			"CurrentDirection", // variable name
			ACSC_NONE, ACSC_NONE, // first dimension indexes
			ACSC_NONE, ACSC_NONE, // no second dimension
			waitTowrite, // pointer to the buffer contained values
			// that must be written
			NULL // waiting call
		))
		{

			g_pLogger->SendAndFlushWithTime(L"[SetMotionDirectionFlag] transaction error : % d\n", acsc_GetLastError());
			ErrorsHandler();
			CurrentHomeMachineState = E_EF3_HOME_NONE;
			rStatus = HSI_STATUS_FAILED;
		}

		g_pLogger->SendAndFlushWithTime(L"[SetMotionDirectionFlag] ACS write OK\n");
		return rStatus;

	}
	else
	{
		g_pLogger->SendAndFlushWithTime(L"[SetMotionDirectionFlag] ACS is not connected\n");
		rStatus = HSI_STATUS_FAILED;
	}

}


//===========================================================================
HSI_STATUS HSI_Motion::GetMotionDirectionFlag(int& CurrentDirection)
{

	auto rStatus = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion && handleACS != ACSC_INVALID)
	{
		int waitToRead[1] = { 0 };
		if (!acsc_ReadInteger(handleACS, // communication handle
			ACSC_NONE, // standard variable
			"CurrentDirection", // variable name
			ACSC_NONE, ACSC_NONE, // first dimension indexes
			ACSC_NONE, ACSC_NONE, // no second dimension
			waitToRead, // pointer to the buffer contained values
			// that must be written
			NULL // waiting call
		))
		{
			g_pLogger->SendAndFlushWithTime(L"[GetMotionDirectionFlag] transaction error : % d\n", acsc_GetLastError());
			ErrorsHandler();
			rStatus = HSI_STATUS_FAILED;
		}
		else
		{
			g_pLogger->SendAndFlushWithTime(L"[GetMotionDirectionFlag] ACS read OK\n");
			CurrentDirection = waitToRead[0];
		}
		return rStatus;
	}
	else
	{
		g_pLogger->SendAndFlushWithTime(L"[GetMotionDirectionFlag] ACS is not connected\n");
		rStatus = HSI_STATUS_FAILED;
	}
}