xhouse/LM-Middleware
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
6d2b284f36b49cc49105135a7f44baec056cd039
LM-Middleware/HSI_HexagonMI_EF3/HSI_Motion.cpp
T

6843 lines
214 KiB
C++
Raw Blame History

// HSI_Motion.cpp : 定义 DLL 的初始化例程。
#include "stdafx.h"
#include "HSI.h"
#include "HSI_Sevenocean_EF3.h"
#include "HSI_Motion.h"
#include "logger.h"
#include "SevenOcean\CMMIO_SERIAL.h"
#include "version.h"
#include <math.h>
#include <fstream>
#include <iomanip>
using namespace std;
#ifdef _DEBUG
#define new DEBUG_NEW
#endif
//===========================================================================
HSI_Motion *g_pHSI_Motion = nullptr;
CLogger extern *g_pLogger = nullptr;
HANDLE hCom;//串口句柄
const int WSA_MAJOR_VERSION = 2;
const int WSA_MINOR_VERSION = 2;
#define WSA_VERSION MAKEWORD(WSA_MAJOR_VERSION, WSA_MINOR_VERSION)
#define SOCKADDR_LEN sizeof(SOCKADDR_IN)
//===========================================================================
HANDLE HSI_Motion::m_Thread_Id = NULL;
HANDLE HSI_Motion::m_Thread_Mutex = NULL;
HANDLE HSI_Motion::g_RW_Data_Mutex = NULL;
HANDLE HSI_Motion::g_WR_ToMove_Mutex = NULL;
HANDLE HSI_Motion::g_Lock_JogAndTrigger = NULL;
HANDLE HSI_Motion::m_hTriggerEvent;
HANDLE HSI_Motion::m_Thread_IdIO = NULL;
HANDLE HSI_Motion::m_Thread_MutexIO = NULL;
HANDLE HSI_Motion::m_hTriggerEventIO;
HANDLE HSI_Motion::m_Thread_IdData = NULL;
HANDLE HSI_Motion::m_Thread_MutexData = NULL;
HANDLE HSI_Motion::m_hTriggerEventData;
HANDLE HSI_Motion::m_Thread_IdProbe = NULL;
HANDLE HSI_Motion::m_Thread_MutexProbe = NULL;
HANDLE HSI_Motion::m_hTriggerEventProbe;
HANDLE HSI_Motion::m_Thread_IdJOGStop = NULL;
HANDLE HSI_Motion::m_Thread_MutexJOGStop = NULL;
HANDLE HSI_Motion::m_hTriggerEventJOGStop;
int HSI_Motion::m_Thread_StateJOGStop = THREAD_PAUSED;
int HSI_Motion::m_Thread_State = THREAD_PAUSED;
int HSI_Motion::m_Thread_StateIO = THREAD_PAUSED;
int HSI_Motion::m_Thread_StateData = THREAD_PAUSED;
int HSI_Motion::m_Thread_StateProbe = THREAD_PAUSED;
int HSI_Motion::bRunGlueDispenser = THREAD_PAUSED;
//===========================================================================
HANDLE HSI_Motion::m_ThreadTCP_Id = NULL;
int HSI_Motion::m_ThreadTCP_State = TCPIP_THREAD_PAUSED;
SOCKET m_socket[4] = { 0 };
//===========================================================================
HSI_Motion::HSI_Motion()
{
TRACE0("HSI_Motion Constructor!\n");
sEvenProp.Init();
sEvenProp.EventCallbackID = 0;
sEvenProp.EventResponse = HSI_EVENT_FUNCTION_OK;
CurrentMotionState = E_SO7_MOTION_NONE;
CurrentReadDataType = E_DATA_TYPE_NONE;
GluerunCount = 0;
m_IsExMotion = 2;
g_IsClose = false;
setLightFlag = false;
m_bISUseMoreLights = 0;
m_Jog_Auto_Focus = 1;
m_StartInputPort = 1;
iaxisNum = 0;
iScanMotionType = 0;
iTriggleNum = 0;
iMotionDirection = 1;
m_InputStatus = 0;
m_ForStatus = 0;
m_setPositionDelay = 0;
m_setPositionPrecision = 0;
m_IsUse_HSICompensation = 0;
m_Compensation_Pluse = 20;
m_IsHardLimit = 7;
m_IsEnableAxis = 7;
m_IsHavePattern = 15;
m_IsUseExternalTrigger = 1;
m_IsUseSixRingEightArea = 0;
m_IsUseTwentySixLight = 0;
m_IsUseEF3 = 0;
m_IsUseRocker = 0;
m_IsCloseRocker = 0;
m_DeviceType = 0;
m_iJoyStick = 0;
m_IsProbe = 0;
m_ProbeAllAxis = 3;
m_ProbeReturnPos = 10.0;
m_ProbeReturnSpeed = 40;
m_IsHomeEncPos = 0;
m_IsHomePrfPos = 1;
m_IsIOFuntion = 0;
m_IsStartInput = 0;
m_IsUsePPS = false;
m_MSTRunFlag = false;
m_SendDataLength = 64;
m_LightType = 1;
m_IsUseFourthSpeed = 0;
m_ETIPort = 1;
m_EF3LightType = 1;
m_IbinCount = 0;
m_IsUseJerk = 0;
t_start = 0;
t_end = 0;
m_isOKGlint = 0;
m_selectedIndex = 0;
m_setPositionNum = 5;
m_axisStatus = 0;
m_axisAlarmStatus = 0;
m_EF3COMPort = 2;
m_AxisHomeDirection = 15;
m_PositionA = 0.0;
m_ForSoft = 0;
m_SaveAxisNum = 0;
m_SaveAxisSpeed = 0;
bSaveSpeedFlag = false;
m_IsUseManualRunin = 0;
fourthAxisFlag = false;
bCircleRun = false;//圆弧插补
m_UseAxisNum = 1;
jogAxisNum = 0;
jogDirFlag = false;
jogMoving = false;
jogspeed = 0;
set_start = 0;
set_end = 0;
m_iSpeedType = 0;
m_axisDirX = 0;//探针运动时X轴的运动方向
m_axisDirY = 0;//探针运动时Y轴的运动方向
m_axisDirZ = 0;//探针运动时Z轴的运动方向
m_probeSeekSpeed = 0;
bUseGlueDispenser = false;
m_iGlueStartSpeed = 1;
m_iGlueDriveSpeed = 1;
m_iGlueAccSpeed = 1;
GlueDispenserindexNum = 0;
m_isUseAport = 0;//A串口
m_portAnum = 0;
m_isUseBport = 0;//B串口
m_portBnum = 0;
m_bEmergencyState = false;
SpCompleteTStart = 0;
SpCompleteTEnd = 0;
SpTimeCount = 0;
SetPotionRunEnd = false;
PntToPntDistance = 0.0;
m_IsLightDebug = 0;
for (int i = 0; i < 4; i++)
{
m_IsOpenTCPIP[i] = "";
m_tcpCntFlag[i] = false;
m_Led8MotionFlag[i] = false;
}
for (int i = 0; i < 4; i++)
{
m_rockerHStartSpeed[i] = 5;
m_rockerHDriveSpeed[i] = 20;
m_rockerLStartSpeed[i] = 2;
m_rockerLDriveSpeed[i] = 10;
m_rockerDSpeed[i] = 100;
m_rockerASpeed[i] = 100;
begin_position[i] = 0;
}
for (size_t i = 0; i < 8; i++)
{
m_SixEightSubArea[i] = 0;
}
CTime tm = CTime::GetCurrentTime();
CString csTime = tm.Format("%Y-%m-%d");//显示年月日
CString dir = L"\\Log\\" + csTime += L".SO7_EF3.Log";
g_pLogger = new CLogger(dir);
g_pLogger2 = new CLogger(L"\\Log\\EF3_SumTime.Log");
for (int i = 0; i < 5; i++)
{
for (int j = 0; j < 5; j++)
{
m_JogDriveSpeed[j][i] = 10; //表示5个档位
m_JogStartSpeed[j][i] = 10;
m_JogAccLine[j][i] = 5;
m_JogAccCurve[j][i] = 0;
m_JogDecLine[j][i] = 5;
m_JogDecCurve[j][i] = 0;
}
m_Home_Machine_Axis[i] = 1;
m_Home_Pos_Axis[i] = 0;
}
m_Home_Machine_Axis[4] = 0;
for (int i = 0; i < 5; i++)
{
m_N_Work_Limit[i] = -40;
m_P_Work_Limit[i] = 160;
m_Resolution[i] = 0.0004;
m_Home_AddJogGears[i] = 4;
m_Home_DecJogGears[i] = 3;
m_SetPotion_StartSpeed[i] = 20;
m_SetPotion_DriveSpeed[i] = 20;
m_SetPotion_Line[i] = 150;
m_SetPotion_Buffer[i] = 40;
m_SetPotion_AccCurve[i] = 0;
m_SetPotion_DecCurve[i] = 0;
m_SpeedAdjustPeriod[i] = 1;
m_SpeedMax[i] = 150;
m_precisionCount[i] = 5;
m_precisionTime[i] = 350;
m_Home_Time[i] = 1500;
m_SetPotion_Count[i] = 200;
m_PosThread[i] = 0;
m_PosNow[i] = 0;
m_LogIsOpen[i] = 0;
m_StopJogMode[i] = 0;
m_LockPos[i] = 0.0;
m_EncPos[i] = 0;
m_PrfPos[i] = 0;
m_PosForAllAxis[i] = 0.0;
targetpos_n[i] = 0;
targetpos_l[i] = 0;
m_ProbeCapturePos[i] = 0;
iCircleRunPnt[i] = 0;
m_ijk[i] = 0;
}
GetAppPath(m_AppPath);
m_LogIsOpen[0] = GetPrivateProfileInt(L"LOG", L"LOG_IS_OPEN_0", 0, m_AppPath + _T("\\Config\\EF3_Motion.ini"));
g_pLogger->IsEnabledLog = m_LogIsOpen[0] == 1 ? true : false;
m_Set_XYZA_Reserve = 0;
m_motorType = 0;
m_AxisHex[0] = 0;
m_direct_pos = 0;
m_AxisHex[1] = AXIS_X;
m_AxisHex[2] = AXIS_Y;
m_AxisHex[3] = AXIS_Z;
m_AxisHex[4] = AXIS_U;
for (int i = 0; i < 64; i++)
{
m_cSendData[i] = 0;
}
m_cSendData[0] = 2;
m_cSendData[1] = 2;
m_cSendData[2] = 2;
m_cSendData[3] = 48;
m_cSendData[4] = 4;
m_cSendData[6] = 4;
m_bConnected = false;
first = true;
LightSend = 0;
OrderSend = 0;
IOSend = 0;
for (int i = 0; i < 64; i++)
{
lightdata[i] = 0;
IOdata[i] = 0;
Orderdata[i] = 0;
IOCheck[i] = 0;
TempData[i] = 0;
}
IOCheck[0] = 0x01;
IOCheck[1] = 0x04;
memset(tReciveData, 0x00, TCPIP_MAX_DAT_SIZE);
g_RW_Data_Mutex = CreateMutex(NULL, FALSE, NULL);
g_WR_ToMove_Mutex = CreateMutex(NULL, FALSE, NULL);
g_Lock_JogAndTrigger = CreateMutex(NULL, FALSE, NULL);
}
//===========================================================================
HSI_Motion::~HSI_Motion()
{
TRACE0("HSI_Motion Destructor!\n");
}
//===========================================================================
bool HSI_Motion::PortInit(int iSerialComPort, int iBuadRate)
{
if (hCom == NULL)
{
char buf[10];
sprintf_s(buf, "COM%d", iSerialComPort);
CString comName(buf);
hCom = CreateFile(comName,
GENERIC_READ | GENERIC_WRITE, //允许读和写
0, //独占方式,串口必须为0
NULL,
OPEN_EXISTING, //打开而不是创建，串口必须为打开
0, //同步方式,同步执行时，函数直到操作完成后才返回
NULL);//串口必须为NULL
if (hCom != (HANDLE)-1)
{
PurgeComm(hCom, PURGE_TXCLEAR | PURGE_RXCLEAR);
}
SetupComm(hCom, 1024, 1024);
DCB dcb;
FillMemory(&dcb, sizeof(dcb), 0);
dcb.DCBlength = sizeof(dcb);
BuildCommDCB(L"2400,n,8,1", &dcb);
if (!SetCommState(hCom, &dcb))
{
return false;//Setting Error!!!!
}
//设置读写超时时间
COMMTIMEOUTS to;
memset(&to, 0, sizeof(to));
to.ReadIntervalTimeout = 100;
to.ReadTotalTimeoutMultiplier = 10;
to.ReadTotalTimeoutConstant = 10;
SetCommTimeouts(hCom, &to);
PurgeComm(hCom, PURGE_TXCLEAR | PURGE_RXCLEAR);
}
return true;
}
//===========================================================================
HSI_STATUS HSI_Motion::IsSupported(UINT &Types)
{
auto rStatus = HSI_STATUS_NORMAL;
Types = 1;
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::Startup(HWND _hWnd, bool _bOfflineOnly)
{
auto rStatus = HSI_STATUS_NORMAL;
hWnd = _hWnd;
bOfflineOnly = _bOfflineOnly;
if (!bOfflineOnly)
{
CString GoogolMotionConfigFile(_T(""));
if (!g_pHSI_Motion)
{
g_pHSI_Motion = new HSI_Motion();
}
if (CurrentHomeMachineState == E_EF3_HOME_ING)
{
g_pLogger->SendAndFlushWithTime(L"[Startup] Going Home\n");
return HSI_STATUS_NORMAL;
}
g_pLogger->SendAndFlushWithTime(L"[Startup] In\n");
g_pLogger->SendAndFlushWithTime(L"[Startup] EF3 HSI.dll version = %s, date = %s\n", HSI_VERSION_CSTRING, HSI_FILE_CSDESCRIPTION);
GoogolMotionConfigFile = m_AppPath + _T("\\Config\\EF3_Config.ini");
Load_EF3_Config_Inifile(GoogolMotionConfigFile);
GoogolMotionConfigFile = m_AppPath + _T("\\Config\\EF3_Motion.ini");
Load_EF3_Motion_Inifile(GoogolMotionConfigFile);
if (m_IsUseEF3 == 1)
{
if (!m_bConnected)
{
m_SO7_Serial.SetPort(m_EF3COMPort, 115200, 0, 8, 1, 0);
if (!m_SO7_Serial.Open())
{
g_pLogger->SendAndFlushWithTime(L"[Startup] m_SO7_Serial.Open is false,connected failed\n");
sEvenProp.Init();
sEvenProp.EventType = HSI_EVENT_ERROR;
sEvenProp.EventID = HSI_EVENT_MOTION;
sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK;
strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "EF3控制器打开失败,串口");
EventCallback(sEvenProp);
return HSI_STATUS_FAILED;
}
m_SO7_Serial.SetTimeouts(1000, 1000);
m_bConnected = true;
}
else
{
g_pLogger->SendAndFlushWithTime(L"[Startup] Serial is opened\n");
}
}
//AbortMotion();
m_cSendData[0] = CT_ORDER;
m_cSendData[1] = CT_STOP;
m_cSendData[2] = AXIS_XYZU;
m_StopJogMode[1] == 0 ? m_cSendData[3] = 0X4A : m_cSendData[3] = 0X05;
m_StopJogMode[2] == 0 ? m_cSendData[3] = 0X4A : m_cSendData[3] = 0X05;
m_StopJogMode[3] == 0 ? m_cSendData[3] = 0X4A : m_cSendData[3] = 0X05;
m_StopJogMode[4] == 0 ? m_cSendData[3] = 0X4A : m_cSendData[3] = 0X05;
m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
Sleep(5);
if (m_DeviceType == 3)
{
m_cSendData[0] = CT_TURNTABLE;
m_cSendData[1] = CT_RTSTOP;
m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
Sleep(5);
m_Thread_StateData = HSI_THREAD_RUNNING;
SetEvent(m_hTriggerEventData);
}
if (bUseGlueDispenser)
{
bUseGlueDispenser = false;
m_cSendData[0] = CT_MOTOR;
m_cSendData[1] = CT_GLUEDISPENSER_STOP;
m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
Sleep(5);
}
g_pLogger->SendAndFlushWithTime(L"[Startup] Connected scuuess\n");
if (HSI_STATUS_FAILED == DriverAlarmStatus())
{
g_pLogger->SendAndFlushWithTime(L"[Startup] DriverAlarmStatus HSI_STATUS_FAILED\n");
//return HSI_STATUS_FAILED;
}
//无效软限位
m_cSendData[0] = CT_MOTOR;
m_cSendData[1] = CT_MOTOR_SET;
m_cSendData[2] = AXIS_XYZU;
m_cSendData[3] = SOFT_LIMIT_POS_NEG;
m_cSendData[4] = 0;
m_cSendData[5] = 0;
m_cSendData[6] = 0;
m_cSendData[7] = 0;
m_cSendData[8] = 0;
m_cSendData[9] = 0;
m_cSendData[10] = 0;
m_cSendData[11] = 0;
m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);//初始化防止第一次无效
Sleep(5);
m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
g_pLogger->SendAndFlushWithTime(L"[Startup] Limit no Enable\n");
//设置方向4个轴的方向，按位
m_cSendData[0] = CT_MOTOR;
m_cSendData[1] = CT_MOTOR_SET;
m_cSendData[2] = AXIS_X;
m_cSendData[3] = ENC_POS_DIR;
m_cSendData[4] = m_Set_XYZA_Reserve;
m_cSendData[5] = m_setPositionDelay;
m_cSendData[6] = m_setPositionPrecision;
m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
Sleep(10);
m_cSendData[0] = CT_MOTOR;
m_cSendData[1] = CT_MDATA_INIT;
m_cSendData[2] = m_motorType & 0xff; //电机类型（步进电机或伺服电机）
m_cSendData[3] = m_IsUseExternalTrigger; //是否启用外触发功能
m_cSendData[4] = m_IsUseSixRingEightArea; //是否启用六环八区灯功能
m_cSendData[5] = m_IsHardLimit;
m_cSendData[6] = m_IsEnableAxis;
m_cSendData[7] = m_IsProbe; //是否启用探针
m_cSendData[8] = m_EF3LightType; //5V高频灯光配置
m_cSendData[9] = m_IsUseRocker; //是否启用摇杆
m_cSendData[10] = m_IsHavePattern; //光栅
m_cSendData[11] = m_AxisHomeDirection; //轴回家方向
m_cSendData[12] = m_IsCollectPos; //是否从串口打印位置
m_cSendData[16] = m_IsLightDebug; //是否不回家也能调试灯光
if (m_IsStartInput == 1 && m_IsUseRocker == 2)
{
m_cSendData[14] = m_StartInputPort >> 8 & 0xff; //外部启动端口号 H
m_cSendData[15] = m_StartInputPort & 0xff; //外部启动端口号 L
}
m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
Sleep(10);
////步进电机补偿值设定
//if (m_motorType == 0)
//{
// m_cSendData[0] = CT_MOTOR;
// m_cSendData[1] = CT_MDATA_COM;
// for (int i = 1, j = 2; i < 5; i++)
// {
// m_cSendData[j++] = (m_SpeedMax[i] & 0xff);
// m_cSendData[j++] = ((m_SpeedMax[i] >> 8) & 0xff);
// m_cSendData[j++] = ((m_SpeedMax[i] >> 16) & 0xff);
// m_cSendData[j++] = ((m_SpeedMax[i] >> 24) & 0xff);
// }
// m_WriteByte = Send_Command(0,(const char*)m_cSendData, m_SendDataLength);
// g_pLogger->SendAndFlushWithTime(L"[Startup] Set Encoder Dir\n");
//}
//多光源板
if (m_bISUseMoreLights > 0)
{
for (int i = 0; i < m_bISUseMoreLights; i++)
{
if (m_tcpCntFlag[i])
{
continue;
}
USES_CONVERSION;
char* ip = T2A(m_IsOpenTCPIP[i]);
u_short port = 8088;
TCPIP_RETURN_CODE rCode = TCPConnect(i, ip, port);
if (rCode != TCPIP_CONNECT_OK)
{
g_pLogger->SendAndFlushWithTime(L"[Startup] m_SO7_Serial.Open is false,connected failed\n");
sEvenProp.Init();
sEvenProp.EventType = HSI_EVENT_ERROR;
sEvenProp.EventID = HSI_EVENT_MOTION;
sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK;
strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "光源板打开网口失败");
EventCallback(sEvenProp);
return HSI_STATUS_FAILED;
}
m_tcpCntFlag[i] = true;
}
Create_Thread();
for (int i = 0; i < m_bISUseMoreLights; i++)
{
if (m_Led8MotionFlag[i])
{
m_selectedIndex = i;
Orderdata[0] = 0x01;
Orderdata[1] = 0x05;
Orderdata[2] = m_LightType;
OrderSend++;
Orderdata[0] = 0x01;
Orderdata[1] = 0x06;
Orderdata[2] = 0x00;
OrderSend++;
}
}
}
//摇杆速度设置
if (m_IsUseRocker == 1)
{
m_cSendData[0] = CT_MOTOR;
m_cSendData[1] = CT_ROCKER;
int j = 2;
for (int i = 0; i < 3; i++)
{
m_cSendData[j++] = (m_rockerHStartSpeed[i] & 0xff);
m_cSendData[j++] = ((m_rockerHStartSpeed[i] >> 8) & 0xff);
m_cSendData[j++] = (m_rockerHDriveSpeed[i] & 0xff);
m_cSendData[j++] = ((m_rockerHDriveSpeed[i] >> 8) & 0xff);
m_cSendData[j++] = (m_rockerASpeed[i] & 0xff);
m_cSendData[j++] = ((m_rockerASpeed[i] >> 8) & 0xff);
}
for (int i = 0; i < 3; i++)
{
m_cSendData[j++] = (m_rockerLStartSpeed[i] & 0xff);
m_cSendData[j++] = ((m_rockerLStartSpeed[i] >> 8) & 0xff);
m_cSendData[j++] = (m_rockerLDriveSpeed[i] & 0xff);
m_cSendData[j++] = ((m_rockerLDriveSpeed[i] >> 8) & 0xff);
m_cSendData[j++] = (m_rockerDSpeed[i] & 0xff);
m_cSendData[j++] = ((m_rockerDSpeed[i] >> 8) & 0xff);
}
m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
Sleep(5);
g_pLogger->SendAndFlushWithTime(L"[Startup] Set Rocker Success\n");
}
//摇杆2下载档位
if (m_IsUseRocker == 2)
{
SetAllGears();
Sleep(5);
g_pLogger->SendAndFlushWithTime(L"[Startup] Set Gears Success\n");
}
CreateThread();
SetEvent(m_hTriggerEvent);
m_Thread_State = HSI_THREAD_PAUSED;
g_pLogger->SendAndFlushWithTime(L"[Startup] SetpositionXyz Enable\n");
CreateThreadData();
SetEvent(m_hTriggerEventData);
m_Thread_StateData = HSI_THREAD_RUNNING;
g_pLogger->SendAndFlushWithTime(L"[Startup] Read EF3 Status Run\n");
if (m_DeviceType != 3)
{
CreateThreadJOGStop();
SetEvent(m_hTriggerEventJOGStop);
m_Thread_StateJOGStop = HSI_THREAD_PAUSED;
}
if (m_IsIOFuntion == 1)
{
m_Thread_StateIO = HSI_THREAD_RUNNING;
CreateThreadIO();
SetEvent(m_hTriggerEventIO);
g_pLogger->SendAndFlushWithTime(L"[Startup] m_IsIOFuntion Enable\n");
SetDIO(HSI_MOTION_OUTPUT_CH1, 0xfffff);
}
if (m_IsProbe == 1)
{
CreateThreadProbe();
m_Thread_StateProbe = HSI_THREAD_RUNNING;
SetEvent(m_hTriggerEventProbe);
g_pLogger->SendAndFlushWithTime(L"[Startup] m_IsProbe Enable\n");
}
g_pLogger->SendAndFlushWithTime(L"[Startup] Out\n");
}
else
{
g_pLogger->SendAndFlushWithTime(L"[Startup] No enable controller\n");
rStatus = HSI_STATUS_FAILED;
}
return rStatus;
}
//=============================获取EF3固件版本===============================
HSI_STATUS HSI_Motion::GetFirmwareVersion(byte *version)
{
m_Thread_StateData = HSI_THREAD_PAUSED;
Sleep(3);
int waite_count = 0;
unsigned char senddata[64] = { 0 };
senddata[0] = 0x04;
senddata[1] = 0x03;
m_SO7_Serial.m_RecvData[0] = 0;
m_WriteByte = Send_Command(0, (const char*)senddata, m_SendDataLength);
Sleep(30);
m_SO7_Serial.m_RecvData[0] = 0;
m_WriteByte = Send_Command(0, (const char*)senddata, m_SendDataLength);
Sleep(30);
m_SO7_Serial.m_RecvData[0] = 0;
m_WriteByte = Send_Command(0, (const char*)senddata, m_SendDataLength);
Sleep(30);
while ((m_SO7_Serial.m_RecvData[0] != 3) && (m_SO7_Serial.m_RecvData[0] != 2))
{
waite_count++;
if (waite_count > 100)
break;
Sleep(1);
}
if (waite_count > 100)
{
waite_count = 0;
m_WriteByte = Send_Command(0, (const char*)senddata, m_SendDataLength);
while ((m_SO7_Serial.m_RecvData[0] != 3) && (m_SO7_Serial.m_RecvData[0] != 2))
{
waite_count++;
if (waite_count > 100)
break;
Sleep(1);
}
}
if (waite_count > 100)
{
version[0] = 't';
version[1] = 'i';
version[2] = 'm';
version[3] = 'e';
version[4] = 'o';
version[5] = 'u';
version[6] = 't';
}
else
{
for (int i = 0; i < 20; i++)
{
version[i] = m_SO7_Serial.m_RecvData[i];
}
}
m_Thread_StateData = HSI_THREAD_RUNNING;
SetEvent(m_hTriggerEventData); //触发事件，其中hEvent表示句柄，返回值：如果操作成功，则返回非零值，否则为0。
return HSI_STATUS_NORMAL;
}
//================================回家=======================================
HSI_STATUS HSI_Motion::HomeMachine(bool bHomed)
{
auto rStatus = HSI_STATUS_NORMAL;
if (!bHomed)
{
g_pLogger->SendAndFlushWithTime(L"[HomeMachine] bHomed No Need Reture\n");
return HSI_STATUS_NORMAL;
}
if (m_IsUseEF3 == 0)
{
return HSI_STATUS_NORMAL;
}
if (g_pHSI_Motion)
{
g_pLogger->SendAndFlushWithTime(L"[HomeMachine] In\n");
//判断是否需要回家
bool home(false);
IsHomed(home);
if (home == true)
{
g_pLogger->SendAndFlushWithTime(L"[HomeMachine] IsHomed No Need Go Home\n");
return HSI_STATUS_NORMAL;
}
sEvenProp.Init();
sEvenProp.EventType = HSI_EVENT_FUNCTION;
sEvenProp.EventID = HSI_EVENT_MOTION_DCC_HOME;
sEvenProp.EventResponse = HSI_EVENT_RESPONSE_CANCEL;
EventCallback(sEvenProp);
if (sEvenProp.EventCallbackID == HSI_EVENT_RESPONSE_CANCEL)
{
g_pLogger->SendAndFlushWithTime(L"[HomeMachine] Cancel\n");
return HSI_STATUS_NORMAL;
}
if (m_bEmergencyState)
{
AfxMessageBox(_T("急停或安全门或安全光幕触发！"));
return HSI_STATUS_FAILED;
}
CurrentHomeMachineState = E_EF3_HOME_ING;
int GetHomePos[5] = { 0 };
if (m_SO7_Serial.m_RecvData[0] == 2)
{
if (m_IsHavePattern & 0x01 == 0x01)
GetHomePos[1] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial.m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]);
else
GetHomePos[1] = (m_SO7_Serial.m_RecvData[17] << 24 | m_SO7_Serial.m_RecvData[18] << 16 | m_SO7_Serial.m_RecvData[19] << 8 | m_SO7_Serial.m_RecvData[20]);
if (m_IsHavePattern & 0x02)
GetHomePos[2] = (m_SO7_Serial.m_RecvData[5] << 24 | m_SO7_Serial.m_RecvData[6] << 16 | m_SO7_Serial.m_RecvData[7] << 8 | m_SO7_Serial.m_RecvData[8]);
else
GetHomePos[2] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial.m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]);
if (m_IsHavePattern & 0x04)
GetHomePos[3] = (m_SO7_Serial.m_RecvData[9] << 24 | m_SO7_Serial.m_RecvData[10] << 16 | m_SO7_Serial.m_RecvData[11] << 8 | m_SO7_Serial.m_RecvData[12]);
else
GetHomePos[3] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial.m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]);
if (m_IsHavePattern & 0x08)
GetHomePos[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | m_SO7_Serial.m_RecvData[15] << 8 | m_SO7_Serial.m_RecvData[16]);
else
GetHomePos[4] = (m_SO7_Serial.m_RecvData[29] << 24 | m_SO7_Serial.m_RecvData[30] << 16 | m_SO7_Serial.m_RecvData[31] << 8 | m_SO7_Serial.m_RecvData[32]);
//GetHomePos[1] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial.m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]);
//GetHomePos[2] = (m_SO7_Serial.m_RecvData[5] << 24 | m_SO7_Serial.m_RecvData[6] << 16 | m_SO7_Serial.m_RecvData[7] << 8 | m_SO7_Serial.m_RecvData[8]);
//GetHomePos[3] = (m_SO7_Serial.m_RecvData[9] << 24 | m_SO7_Serial.m_RecvData[10] << 16 | m_SO7_Serial.m_RecvData[11] << 8 | m_SO7_Serial.m_RecvData[12]);
//GetHomePos[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | m_SO7_Serial.m_RecvData[15] << 8 | m_SO7_Serial.m_RecvData[16]);
}
if (HSI_STATUS_NORMAL != HomeFindIndex())
{
return HSI_STATUS_FAILED;
}
int SavePos[5] = { 0 };
if (m_SO7_Serial.m_RecvData[0] == 2)
{
SavePos[1] = (m_SO7_Serial.m_RecvData[41] << 24 | m_SO7_Serial.m_RecvData[42] << 16 | m_SO7_Serial.m_RecvData[43] << 8 | m_SO7_Serial.m_RecvData[44]);
SavePos[2] = (m_SO7_Serial.m_RecvData[45] << 24 | m_SO7_Serial.m_RecvData[46] << 16 | m_SO7_Serial.m_RecvData[47] << 8 | m_SO7_Serial.m_RecvData[48]);
SavePos[3] = (m_SO7_Serial.m_RecvData[49] << 24 | m_SO7_Serial.m_RecvData[50] << 16 | m_SO7_Serial.m_RecvData[51] << 8 | m_SO7_Serial.m_RecvData[52]);
SavePos[4] = (m_SO7_Serial.m_RecvData[53] << 24 | m_SO7_Serial.m_RecvData[54] << 16 | m_SO7_Serial.m_RecvData[55] << 8 | m_SO7_Serial.m_RecvData[56]);
}
double PrinfPos[5] = { 0 };
PrinfPos[1] = SavePos[1] * m_Resolution[1];
PrinfPos[2] = SavePos[2] * m_Resolution[2];
PrinfPos[3] = SavePos[3] * m_Resolution[3];
PrinfPos[4] = SavePos[4] * m_Resolution[4];
g_pLogger->SendAndFlushWithTime(L"[HomeMachine] SavePos[1] = %d,SavePos[2] = %d,SavePos[3] = %d,SavePos[4] = %d\n", SavePos[1], SavePos[2], SavePos[3], SavePos[4]);
g_pLogger->SendAndFlushWithTime(L"[HomeMachine] PrinfPos[1] = %.4f,PrinfPos[2] = %.4f,PrinfPos[3] = %.4f,PrinfPos[4] = %.4f\n", PrinfPos[1], PrinfPos[2], PrinfPos[3], PrinfPos[4]);
int LastPos[5] = { 0 };
if (m_SO7_Serial.m_RecvData[0] == 2)
{
LastPos[1] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial.m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]);
LastPos[2] = (m_SO7_Serial.m_RecvData[5] << 24 | m_SO7_Serial.m_RecvData[6] << 16 | m_SO7_Serial.m_RecvData[7] << 8 | m_SO7_Serial.m_RecvData[8]);
LastPos[3] = (m_SO7_Serial.m_RecvData[9] << 24 | m_SO7_Serial.m_RecvData[10] << 16 | m_SO7_Serial.m_RecvData[11] << 8 | m_SO7_Serial.m_RecvData[12]);
LastPos[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | m_SO7_Serial.m_RecvData[15] << 8 | m_SO7_Serial.m_RecvData[16]);
}
m_Thread_State = HSI_THREAD_RUNNING;
g_pLogger->SendAndFlushWithTime(L"[HomeMachine] SetPositionXyz\n");
double PrinfMovePos[5] = { 0 };
PrinfMovePos[1] = (GetHomePos[1] - SavePos[1])*m_Resolution[1];
PrinfMovePos[2] = (GetHomePos[2] - SavePos[2])*m_Resolution[2];
PrinfMovePos[3] = (GetHomePos[3] - SavePos[3])*m_Resolution[3];
PrinfMovePos[4] = (GetHomePos[4] - SavePos[4])*m_Resolution[4];
g_pLogger->SendAndFlushWithTime(L"[HomeMachine] SetPos[1] = %.4fmm,SetPos[2] = %.4fmm,SetPos[3] = %.4fmm,SetPos[4] = %.4fmm\n", PrinfMovePos[1], PrinfMovePos[2], PrinfMovePos[3], PrinfMovePos[4]);
if (m_DeviceType == 0 || m_DeviceType == 1)
{
for (short i = 1; i < 5; i++)
{
if (m_Home_Machine_Axis[i] == 0)
{
PrinfMovePos[i] = GetHomePos[i] * m_Resolution[i];
}
}
Sleep(20);
SetPositionXyza(0, PrinfMovePos[1], PrinfMovePos[2], PrinfMovePos[3], PrinfMovePos[4], HSI_MOTION_MOVE_WAIT, 0);
}
CurrentHomeMachineState = E_EF3_HOME_FINISHED;
for (int AxisTypes = 1; AxisTypes <= 8;)
{
if (CurrentHomeMachineState == E_EF3_HOME_FINISHED)
{
byte AxisNumber = (byte)AxisConvertIndex(AxisTypes);
m_cSendData[0] = CT_MOTOR;
m_cSendData[1] = CT_MOTOR_SET;
m_cSendData[2] = AxisTypes;
m_cSendData[3] = SOFT_LIMIT_POS_NEG;
m_cSendData[4] = (int)(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) & 0xff;
m_cSendData[5] = ((int)(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 8) & 0xff;
m_cSendData[6] = ((int)(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 16) & 0xff;
m_cSendData[7] = ((int)(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 24) & 0xff;
m_cSendData[8] = (int)(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) & 0xff;
m_cSendData[9] = ((int)(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 8) & 0xff;
m_cSendData[10] = ((int)(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 16) & 0xff;
m_cSendData[11] = ((int)(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 24) & 0xff;
m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
Sleep(10);
AxisTypes = 2 * AxisTypes;
g_pLogger->SendAndFlushWithTime(L"[HomeMachine] Limit Enable, m_P_Work_Limit[AxisNumber] = %f,m_N_Work_Limit[AxisNumber] = %f\n", m_P_Work_Limit[AxisNumber], m_N_Work_Limit[AxisNumber]);
}
}
//SetPositionXyz(0, -10,-10,-10, HSI_MOTION_MOVE_NOWAIT, 0);
//Sleep(100);
//SetPositionXyz(0, PrinfMovePos[1], PrinfMovePos[2], PrinfMovePos[3], HSI_MOTION_MOVE_NOWAIT, 0);
g_pLogger->SendAndFlushWithTime(L"[HomeMachine] Out\n");
}
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::HomeJog(short AxisNumber, short Dir, bool Wait)
{
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
g_pLogger->SendAndFlushWithTime(L"[HomeJog] In\n");
g_pLogger->SendAndFlushWithTime(L"[HomeJog] Out\n");
}
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::HomeFindIndex()
{
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
g_pLogger->SendAndFlushWithTime(L"[HomeFindIndex] In\n");
int AxisTypes = 1;
int AxisAll = 0;
g_IsClose = false;
g_pLogger->SendAndFlushWithTime(L"[HomeFindIndex] Limit no Enable\n");
ZeroPos(true);
for (short i = 1; i < 5; i++)
{
if (m_Home_Machine_Axis[i] == 1)
{
AxisTypes = IndexConvertAxis(i);
//无效软限位
m_cSendData[0] = CT_MOTOR;
m_cSendData[1] = CT_MOTOR_SET;
m_cSendData[2] = AxisTypes;
m_cSendData[3] = SOFT_LIMIT_POS_NEG;
m_cSendData[4] = 0;
m_cSendData[5] = 0;
m_cSendData[6] = 0;
m_cSendData[7] = 0;
m_cSendData[8] = 0;
m_cSendData[9] = 0;
m_cSendData[10] = 0;
m_cSendData[11] = 0;
m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
Sleep(10);
int DriveSpeed(1);
int StartSpeed(1);
int AccLine(1);
int DecLine(1);
int AccCurve(1);
int DecCurve(1);
m_cSendData[0] = CT_MOTOR;
m_cSendData[1] = CT_MOTOR_SET;
m_cSendData[2] = AxisTypes;
m_cSendData[3] = HOME_LIMIT_SPEED_ACC_DEC;
HomeJogGearsChoice(i, m_Home_AddJogGears[i], DriveSpeed, StartSpeed, AccLine, DecLine, AccCurve, DecCurve);
m_cSendData[4] = (StartSpeed & 0xff);
m_cSendData[5] = ((StartSpeed >> 8) & 0xff);
m_cSendData[6] = ((StartSpeed >> 16) & 0xff);
m_cSendData[7] = ((StartSpeed >> 24) & 0xff);
m_cSendData[8] = (DriveSpeed & 0xff);
m_cSendData[9] = ((DriveSpeed >> 8) & 0xff);
m_cSendData[10] = ((DriveSpeed >> 16) & 0xff);
m_cSendData[11] = ((DriveSpeed >> 24) & 0xff);
m_cSendData[12] = (AccCurve & 0xff);
m_cSendData[13] = ((AccCurve >> 8) & 0xff);
m_cSendData[14] = ((AccCurve >> 16) & 0xff);
m_cSendData[15] = ((AccCurve >> 24) & 0xff);
m_cSendData[16] = (AccLine & 0xff);
m_cSendData[17] = ((AccLine >> 8) & 0xff);
m_cSendData[18] = ((AccLine >> 16) & 0xff);
m_cSendData[19] = ((AccLine >> 24) & 0xff);
m_cSendData[20] = (DecCurve & 0xff);
m_cSendData[21] = ((DecCurve >> 8) & 0xff);
m_cSendData[22] = ((DecCurve >> 16) & 0xff);
m_cSendData[23] = ((DecCurve >> 24) & 0xff);
m_cSendData[24] = (DecLine & 0xff);
m_cSendData[25] = ((DecLine >> 8) & 0xff);
m_cSendData[26] = ((DecLine >> 16) & 0xff);
m_cSendData[27] = ((DecLine >> 24) & 0xff);
m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
g_pLogger->SendAndFlushWithTime(L"[HomeFindIndex] m_Home_Speed_High Go Limit\n");
Sleep(10);
m_cSendData[0] = CT_MOTOR;
m_cSendData[1] = CT_MOTOR_SET;
m_cSendData[2] = AxisTypes;
m_cSendData[3] = HOME_ORG_SPEED_ACC_DEC;
HomeJogGearsChoice(i, m_Home_DecJogGears[i], DriveSpeed, StartSpeed, AccLine, DecLine, AccCurve, DecCurve);
m_cSendData[4] = (StartSpeed & 0xff);
m_cSendData[5] = ((StartSpeed >> 8) & 0xff);
m_cSendData[6] = ((StartSpeed >> 16) & 0xff);
m_cSendData[7] = ((StartSpeed >> 24) & 0xff);
m_cSendData[8] = (DriveSpeed & 0xff);
m_cSendData[9] = ((DriveSpeed >> 8) & 0xff);
m_cSendData[10] = ((DriveSpeed >> 16) & 0xff);
m_cSendData[11] = ((DriveSpeed >> 24) & 0xff);
m_cSendData[12] = (AccCurve & 0xff);
m_cSendData[13] = ((AccCurve >> 8) & 0xff);
m_cSendData[14] = ((AccCurve >> 16) & 0xff);
m_cSendData[15] = ((AccCurve >> 24) & 0xff);
m_cSendData[16] = (AccLine & 0xff);
m_cSendData[17] = ((AccLine >> 8) & 0xff);
m_cSendData[18] = ((AccLine >> 16) & 0xff);
m_cSendData[19] = ((AccLine >> 24) & 0xff);
m_cSendData[20] = (DecCurve & 0xff);
m_cSendData[21] = ((DecCurve >> 8) & 0xff);
m_cSendData[22] = ((DecCurve >> 16) & 0xff);
m_cSendData[23] = ((DecCurve >> 24) & 0xff);
m_cSendData[24] = (DecLine & 0xff);
m_cSendData[25] = ((DecLine >> 8) & 0xff);
m_cSendData[26] = ((DecLine >> 16) & 0xff);
m_cSendData[27] = ((DecLine >> 24) & 0xff);
m_cSendData[28] = (m_Home_Time[i] & 0xff);
m_cSendData[29] = ((m_Home_Time[i] >> 8) & 0xff);
m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
g_pLogger->SendAndFlushWithTime(L"[HomeFindIndex] m_Home_Speed_High Go Limit and find index signal\n");
Sleep(10);
AxisAll += (1 << (i - 1));
}
}
m_cSendData[0] = CT_MOTOR;
m_cSendData[1] = CT_GOHOME;
m_cSendData[2] = AxisAll;
m_cSendData[3] = 0x53;
m_cSendData[4] = m_motorType & 0xff;
m_cSendData[5] = 0x0f;
m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
g_pLogger->SendAndFlushWithTime(L"[HomeFindIndex] Starting, AxisAll = %d\n", AxisAll);
Sleep(1000);
int Count = 0;
do
{
DoEvents();
if (g_IsClose)
{
g_IsClose = false;
return HSI_STATUS_FAILED;
}
Sleep(1);
if (m_SO7_Serial.m_RecvData[0] == 2)
{
if (m_SO7_Serial.m_RecvData[38] & 16)
{
g_pLogger->SendAndFlushWithTime(L"[HomeFindIndex] Go Home Motion Done\n");
break;
}
}
if (m_bEmergencyState)
{
AfxMessageBox(_T("急停或安全门或安全光幕触发！"));
return HSI_STATUS_FAILED;
}
if (Count > 25000)
{
g_pLogger->SendAndFlushWithTime(L"[HomeFindIndex] Go Home Timeout HSI_STATUS_FAILED\n");
AbortMotion();
sEvenProp.EventType = HSI_EVENT_ERROR;
sEvenProp.EventID = HSI_EVENT_MOTION;
sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK;
strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "回家超时");
EventCallback(sEvenProp);
return HSI_STATUS_FAILED;
}
Count++;
} while (true);
char MessageHome[100] = "";
bool bHomed = true;
if ((m_SO7_Serial.m_RecvData[38] & 0x01) == 0 && m_Home_Machine_Axis[1] == 1)
{
strcat_s(MessageHome, 30, "1、");
bHomed = false;
}
if ((m_SO7_Serial.m_RecvData[38] & 0x02) == 0 && m_Home_Machine_Axis[2] == 1)
{
strcat_s(MessageHome, 30, "2、");
bHomed = false;
}
if ((m_SO7_Serial.m_RecvData[38] & 0x04) == 0 && m_Home_Machine_Axis[3] == 1)
{
strcat_s(MessageHome, 30, "3、");
bHomed = false;
}
if ((m_SO7_Serial.m_RecvData[38] & 0x08) == 0 && m_Home_Machine_Axis[4] == 1)
{
strcat_s(MessageHome, 30, "4、");
bHomed = false;
}
if (!bHomed)
{
sEvenProp.Init();
sEvenProp.EventType = HSI_EVENT_ERROR;
sEvenProp.EventID = HSI_EVENT_MOTION;
sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK;
strcat_s(MessageHome, 100, "轴回家失败!");
strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, MessageHome);
EventCallback(sEvenProp);
return HSI_STATUS_FAILED;
}
g_pLogger->SendAndFlushWithTime(L"[HomeFindIndex] Go Home Success\n");
g_pLogger->SendAndFlushWithTime(L"[HomeFindIndex] Out\n");
}
return rStatus;
}
//===========================================================================
void HSI_Motion::HomeJogGearsChoice(int AxisTypes, int JogGears, int &DriveSpeed, int &StartSpeed, int &AccLine, int &DecLine, int &AccCurve, int &DecCurve)
{
DriveSpeed = m_JogDriveSpeed[AxisTypes][4 - JogGears];
StartSpeed = m_JogStartSpeed[AxisTypes][4 - JogGears];
AccLine = m_JogAccLine[AxisTypes][4 - JogGears];
DecLine = m_JogDecLine[AxisTypes][4 - JogGears];
AccCurve = m_JogAccCurve[AxisTypes][4 - JogGears];
DecCurve = m_JogDecCurve[AxisTypes][4 - JogGears];
}
//===========================================================================
HSI_STATUS HSI_Motion::GetAppPath(CString &Path)
{
auto rStatus = HSI_STATUS_NORMAL;
Path = _T("");
if (Path.IsEmpty())
{
CString tmpPath;
GetModuleFileName(NULL, tmpPath.GetBuffer(255), 255);
tmpPath.ReleaseBuffer();
tmpPath.TrimRight();
int nLastSlash = tmpPath.ReverseFind('\\');
if (nLastSlash >= 0)
tmpPath = tmpPath.Left(nLastSlash);
else
tmpPath.Empty();
Path = tmpPath;
}
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::IsHomed(bool &bHomed)
{
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
g_pLogger->SendAndFlushWithTime(L"[IsHomed] In\n");
short isHomed[5] = { 1, 1, 1, 1, 1 };
//int Count = 0;
//if (bHomed == true)//定位是增大判断精度
//{
// Count = 1000;
//}
//所有轴都不需要回家
if (m_Home_Machine_Axis[1] == 0 && m_Home_Machine_Axis[2] == 0 && m_Home_Machine_Axis[3] == 0 && m_Home_Machine_Axis[4] == 0)
{
g_pLogger->SendAndFlushWithTime(L"[IsHomed] No Axis Go Home E_GTS_HOME_FINISHED\n");
CurrentHomeMachineState = E_EF3_HOME_FINISHED;
bHomed = true;
return HSI_STATUS_NORMAL;
}
//判断是否需要回家
int Delay = 0;
while (m_SO7_Serial.m_RecvData[0] != 2)
{
DoEvents();
Sleep(1);
if (m_SO7_Serial.m_RecvData[0] == 2)
{
g_pLogger->SendAndFlushWithTime(L"[IsHomed] normal break Delay = %d\n", Delay);
break;
}
if (Delay > 300)
{
g_pLogger->SendAndFlushWithTime(L"[IsHomed] timeout break Delay = %d\n", Delay);
break;
}
Delay++;
}
for (short i = 1; i < 5; i++)
{
if (m_Home_Machine_Axis[i] == 1)
{
if (m_SO7_Serial.m_RecvData[0] == 2)
{
isHomed[i] = (m_SO7_Serial.m_RecvData[38] & m_AxisHex[i] ? 1 : 0);
}
else
{
isHomed[i] = 0;
}
}
}
if (isHomed[1] == 1 && isHomed[2] == 1 && isHomed[3] == 1 && isHomed[4] == 1)
{
g_pLogger->SendAndFlushWithTime(L"[IsHomed] E_GTS_HOME_FINISHED\n");
CurrentHomeMachineState = E_EF3_HOME_FINISHED;
bHomed = true;
}
else
{
g_pLogger->SendAndFlushWithTime(L"[IsHomed] Is No Go Home E_GTS_HOME_NONE\n");
CurrentHomeMachineState = E_EF3_HOME_NONE;
bHomed = false;
}
g_pLogger->SendAndFlushWithTime(L"[IsHomed] Out\n");
}
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::ZeroPos(bool bZeroPos)
{
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
g_pLogger->SendAndFlushWithTime(L"[ZeroPos] In\n");
if (bZeroPos == true)
{
m_cSendData[0] = CT_MOTOR;
m_cSendData[1] = CT_MOTOR_SET;
m_cSendData[2] = AXIS_X;
m_cSendData[3] = POS_CLEAR;
Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
Sleep(8);
m_cSendData[0] = CT_MOTOR;
m_cSendData[1] = CT_MOTOR_SET;
m_cSendData[2] = AXIS_Y;
m_cSendData[3] = POS_CLEAR;
Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
Sleep(8);
m_cSendData[0] = CT_MOTOR;
m_cSendData[1] = CT_MOTOR_SET;
m_cSendData[2] = AXIS_Z;
m_cSendData[3] = POS_CLEAR;
Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
Sleep(8);
m_cSendData[0] = CT_MOTOR;
m_cSendData[1] = CT_MOTOR_SET;
m_cSendData[2] = AXIS_U;
m_cSendData[3] = POS_CLEAR;
Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
Sleep(8);
g_pLogger->SendAndFlushWithTime(L"[ZeroPos] All Pos ZeroPos\n");
}
m_cSendData[0] = CT_MOTOR;
m_cSendData[1] = CT_MOTOR_SET;
m_cSendData[2] = AXIS_X;
m_cSendData[3] = HOME_CLEAR;
Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
g_pLogger->SendAndFlushWithTime(L"[ZeroPos] All HomeFlag ZeroPos\n");
g_pLogger->SendAndFlushWithTime(L"[ZeroPos] Out\n");
}
return rStatus;
}
//===============================JOG模式============================================
HSI_STATUS HSI_Motion::Jog(UINT AxisTypes, double Speed)
{
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
if (m_DeviceType != 3)
{
m_Thread_StateJOGStop = HSI_THREAD_RUNNING;
SetEvent(m_hTriggerEventJOGStop);
}
int DriveSpeed(1);
int StartSpeed(1);
int AccLine(1);
int DecLine(1);
int AccCurve(1);
int DecCurve(1);
int JogSpeed(1);
bool bJOGDir = Speed > 0 ? true : false;
byte AxisNumber = (byte)AxisConvertIndex(AxisTypes);
jogAxisNum = AxisNumber;
jogDirFlag = bJOGDir;
m_Thread_State = HSI_THREAD_PAUSED;
if (CurrentHomeMachineState == E_EF3_HOME_FINISHED)
{
m_cSendData[0] = CT_MOTOR;
m_cSendData[1] = CT_MOTOR_SET;
m_cSendData[2] = AxisTypes;
m_cSendData[3] = SOFT_LIMIT_POS_NEG;
m_cSendData[4] = (int)(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) & 0xff;//正限位
m_cSendData[5] = ((int)(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 8) & 0xff;
m_cSendData[6] = ((int)(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 16) & 0xff;
m_cSendData[7] = ((int)(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 24) & 0xff;
m_cSendData[8] = (int)(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) & 0xff; //负限位
m_cSendData[9] = ((int)(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 8) & 0xff;
m_cSendData[10] = ((int)(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 16) & 0xff;
m_cSendData[11] = ((int)(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 24) & 0xff;
m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
g_pLogger->SendAndFlushWithTime(L"[Jog] Limit Enable, m_P_Work_Limit[AxisNumber] = %f,m_N_Work_Limit[AxisNumber] = %f\n", m_P_Work_Limit[AxisNumber], m_N_Work_Limit[AxisNumber]);
}
else
{
//无效软限位
m_cSendData[0] = CT_MOTOR;
m_cSendData[1] = CT_MOTOR_SET;
m_cSendData[2] = AxisTypes;
m_cSendData[3] = SOFT_LIMIT_POS_NEG;
m_cSendData[4] = 0;
m_cSendData[5] = 0;
m_cSendData[6] = 0;
m_cSendData[7] = 0;
m_cSendData[8] = 0;
m_cSendData[9] = 0;
m_cSendData[10] = 0;
m_cSendData[11] = 0;
m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
g_pLogger->SendAndFlushWithTime(L"[Jog] Limit No Enable\n");
}
if (m_Home_Machine_Axis[AxisNumber] == 0)
{
return rStatus;
}
double now_pos[5] = { 0 };
double Prf_pos[5] = { 0 };
double limitpos[4] = { 0 };
int RemainPul = 0;
int limitSDPul = 0;
double time;
/*GetPositionXyz(1, now_pos[0], now_pos[1], now_pos[2], time);*/
GetPositionEncPrfMulti(1, now_pos, Prf_pos, 1);
JogSpeed = abs(SpeedPercent(AxisNumber, Speed, DriveSpeed, StartSpeed, AccLine, DecLine, AccCurve, DecCurve));
if (m_DeviceType != 3)
{
if (AxisTypes == AXIS_X && m_motorType & 0x01)
{
if (!bJOGDir)//负方向
{
RemainPul = (int)(now_pos[1] / m_Resolution[1]) - (int)(m_N_Work_Limit[1] / m_Resolution[1]);
limitSDPul = (DriveSpeed - StartSpeed) * 13;
if ((RemainPul < limitSDPul * 2) && (RemainPul>0))
{
float SpeedRatio = limitSDPul * 2 / RemainPul;
DriveSpeed = DriveSpeed / SpeedRatio;
if (DriveSpeed < StartSpeed)
{
DriveSpeed = StartSpeed;
}
}
}
else
{
RemainPul = (int)(m_P_Work_Limit[1] / m_Resolution[1]) - (int)(now_pos[1] / m_Resolution[1]);
limitSDPul = (DriveSpeed - StartSpeed) * 13;
if ((RemainPul < limitSDPul * 2) && (RemainPul>0))
{
float SpeedRatio = limitSDPul * 2 / RemainPul;
DriveSpeed = DriveSpeed / SpeedRatio;
if (DriveSpeed < StartSpeed)
{
DriveSpeed = StartSpeed;
}
}
}
}
else if (AxisTypes == AXIS_Y && m_motorType & 0x02)
{
if (!bJOGDir)//负方向
{
RemainPul = (int)(now_pos[2] / m_Resolution[2]) - (int)(m_N_Work_Limit[2] / m_Resolution[2]);
limitSDPul = (DriveSpeed - StartSpeed) * 13;
if ((RemainPul < limitSDPul * 2) && (RemainPul>0))
{
float SpeedRatio = 1 + limitSDPul * 2 / RemainPul;
DriveSpeed = DriveSpeed / SpeedRatio;
if (DriveSpeed < StartSpeed)
{
DriveSpeed = StartSpeed;
}
}
}
else
{
RemainPul = (int)(m_P_Work_Limit[2] / m_Resolution[2]) - (int)(now_pos[2] / m_Resolution[2]);
limitSDPul = (DriveSpeed - StartSpeed) * 13;
if ((RemainPul < limitSDPul * 2) && (RemainPul>0))
{
float SpeedRatio = limitSDPul * 2 / RemainPul;
DriveSpeed = DriveSpeed / SpeedRatio;
if (DriveSpeed < StartSpeed)
{
DriveSpeed = StartSpeed;
}
}
}
}
if (AxisTypes == AXIS_Z && m_motorType & 0x04)
{
if (!bJOGDir)//负方向
{
RemainPul = (int)(now_pos[3] / m_Resolution[3]) - (int)(m_N_Work_Limit[3] / m_Resolution[3]);
limitSDPul = (DriveSpeed - StartSpeed) * 13;
if ((RemainPul < limitSDPul * 2) && (RemainPul>0))
{
float SpeedRatio = limitSDPul * 2 / RemainPul;
DriveSpeed = DriveSpeed / SpeedRatio;
if (DriveSpeed < StartSpeed)
{
DriveSpeed = StartSpeed;
}
}
}
else
{
RemainPul = (int)(m_P_Work_Limit[3] / m_Resolution[3]) - (int)(now_pos[3] / m_Resolution[3]);
limitSDPul = (DriveSpeed - StartSpeed) * 13;
if ((RemainPul < limitSDPul * 2) && (RemainPul>0))
{
float SpeedRatio = limitSDPul * 2 / RemainPul;
DriveSpeed = DriveSpeed / SpeedRatio;
if (DriveSpeed < StartSpeed)
{
DriveSpeed = StartSpeed;
}
}
}
}
if (AxisTypes == AXIS_U && m_motorType & 0x08)
{
if (!bJOGDir)//负方向
{
RemainPul = (int)(now_pos[4] / m_Resolution[4]) - (int)(m_N_Work_Limit[4] / m_Resolution[4]);
limitSDPul = (DriveSpeed - StartSpeed) * 13;
if ((RemainPul < limitSDPul * 2) && (RemainPul>0))
{
float SpeedRatio = limitSDPul * 2 / RemainPul;
DriveSpeed = DriveSpeed / SpeedRatio;
if (DriveSpeed < StartSpeed)
{
DriveSpeed = StartSpeed;
}
}
}
else
{
RemainPul = (int)(m_P_Work_Limit[4] / m_Resolution[4]) - (int)(now_pos[4] / m_Resolution[4]);
limitSDPul = (DriveSpeed - StartSpeed) * 13;
if ((RemainPul < limitSDPul * 2) && (RemainPul>0))
{
float SpeedRatio = limitSDPul * 2 / RemainPul;
DriveSpeed = DriveSpeed / SpeedRatio;
if (DriveSpeed < StartSpeed)
{
DriveSpeed = StartSpeed;
}
}
}
}
}
m_cSendData[0] = CT_MOTOR;
m_cSendData[1] = CT_MOTOR_SET;
m_cSendData[2] = AxisTypes;
m_cSendData[3] = JOG_SPEED_ACC_DEC;
m_cSendData[4] = (StartSpeed & 0xff);
m_cSendData[5] = ((StartSpeed >> 8) & 0xff);
m_cSendData[6] = ((StartSpeed >> 16) & 0xff);
m_cSendData[7] = ((StartSpeed >> 24) & 0xff);
m_cSendData[8] = (DriveSpeed & 0xff);
m_cSendData[9] = ((DriveSpeed >> 8) & 0xff);
m_cSendData[10] = ((DriveSpeed >> 16) & 0xff);
m_cSendData[11] = ((DriveSpeed >> 24) & 0xff);
m_cSendData[12] = (AccCurve & 0xff);
m_cSendData[13] = ((AccCurve >> 8) & 0xff);
m_cSendData[14] = ((AccCurve >> 16) & 0xff);
m_cSendData[15] = ((AccCurve >> 24) & 0xff);
m_cSendData[16] = (AccLine & 0xff);
m_cSendData[17] = ((AccLine >> 8) & 0xff);
m_cSendData[18] = ((AccLine >> 16) & 0xff);
m_cSendData[19] = ((AccLine >> 24) & 0xff);
m_cSendData[20] = (DecCurve & 0xff);
m_cSendData[21] = ((DecCurve >> 8) & 0xff);
m_cSendData[22] = ((DecCurve >> 16) & 0xff);
m_cSendData[23] = ((DecCurve >> 24) & 0xff);
m_cSendData[24] = (DecLine & 0xff);
m_cSendData[25] = ((DecLine >> 8) & 0xff);
m_cSendData[26] = ((DecLine >> 16) & 0xff);
m_cSendData[27] = ((DecLine >> 24) & 0xff);
jogspeed = DriveSpeed;
if ((StartSpeed < 250) && (DriveSpeed < 6))
{
m_IsUseJerk = 1;
}
else
{
m_IsUseJerk = 0;
}
m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
Sleep(5);
m_cSendData[0] = CT_MOTOR;
if (AxisNumber == 1 || AxisNumber == 2)
{
if (!bJOGDir)//方向
{
m_cSendData[1] = CT_START_JOG_NEG;
}
else
{
m_cSendData[1] = CT_START_JOG_POS;
}
}
else
{
if (bJOGDir)//方向
{
m_cSendData[1] = CT_START_JOG_POS;
}
else
{
m_cSendData[1] = CT_START_JOG_NEG;
}
}
m_cSendData[2] = AxisTypes;
m_cSendData[3] = 0x53;
t_start = GetTickCount();
m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
jogMoving = true;
g_pLogger->SendAndFlushWithTime(L"[Jog] Out,bJOGDir = %d\n", bJOGDir);
}
return rStatus;
}
//JOG模式
//===========================================================================
HSI_STATUS HSI_Motion::JoyStick(UINT AxisTypes, long Speed)
{
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
if (m_DeviceType != 3)
{
m_Thread_StateJOGStop = HSI_THREAD_RUNNING;
SetEvent(m_hTriggerEventJOGStop);
}
int DriveSpeed(1);
int StartSpeed(1);
int AccLine(1);
int DecLine(1);
int AccCurve(1);
int DecCurve(1);
int JogSpeed(1);
bool bJOGDir = Speed > 0 ? true : false;
byte AxisNumber = (byte)AxisConvertIndex(AxisTypes);
jogAxisNum = AxisNumber;
jogDirFlag = bJOGDir;
m_Thread_State = HSI_THREAD_PAUSED;
if (CurrentHomeMachineState == E_EF3_HOME_FINISHED)
{
m_cSendData[0] = CT_MOTOR;
m_cSendData[1] = CT_MOTOR_SET;
m_cSendData[2] = AxisTypes;
m_cSendData[3] = SOFT_LIMIT_POS_NEG;
m_cSendData[4] = (int)(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) & 0xff;
m_cSendData[5] = ((int)(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 8) & 0xff;
m_cSendData[6] = ((int)(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 16) & 0xff;
m_cSendData[7] = ((int)(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 24) & 0xff;
m_cSendData[8] = (int)(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) & 0xff;
m_cSendData[9] = ((int)(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 8) & 0xff;
m_cSendData[10] = ((int)(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 16) & 0xff;
m_cSendData[11] = ((int)(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 24) & 0xff;
m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
g_pLogger->SendAndFlushWithTime(L"[Jog] Limit Enable, m_P_Work_Limit[AxisNumber] = %f,m_N_Work_Limit[AxisNumber] = %f\n", m_P_Work_Limit[AxisNumber], m_N_Work_Limit[AxisNumber]);
}
else
{
//无效软限位
m_cSendData[0] = CT_MOTOR;
m_cSendData[1] = CT_MOTOR_SET;
m_cSendData[2] = AxisTypes;
m_cSendData[3] = SOFT_LIMIT_POS_NEG;
m_cSendData[4] = 0;
m_cSendData[5] = 0;
m_cSendData[6] = 0;
m_cSendData[7] = 0;
m_cSendData[8] = 0;
m_cSendData[9] = 0;
m_cSendData[10] = 0;
m_cSendData[11] = 0;
m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
g_pLogger->SendAndFlushWithTime(L"[Jog] Limit No Enable\n");
}
if (m_Home_Machine_Axis[AxisNumber] == 0)
{
return rStatus;
}
double now_pos[5] = { 0 };
double Prf_pos[5] = { 0 };
double limitpos[4] = { 0 };
int RemainPul = 0;
int limitSDPul = 0;
double time;
/*GetPositionXyz(1, now_pos[0], now_pos[1], now_pos[2], time);*/
GetPositionEncPrfMulti(1, now_pos, Prf_pos, 1);
long lSpeed = abs(Speed);
if (!abs(SpeedPercentJoyStick(AxisNumber, lSpeed, DriveSpeed, StartSpeed, AccLine, DecLine, AccCurve, DecCurve)))
{
return rStatus;
}
if (m_DeviceType != 3)
{
if (AxisTypes == AXIS_X && m_motorType & 0x01)
{
if (!bJOGDir)//负方向
{
RemainPul = (int)(now_pos[1] / m_Resolution[1]) - (int)(m_N_Work_Limit[1] / m_Resolution[1]);
limitSDPul = (DriveSpeed - StartSpeed) * 13;
if ((RemainPul < limitSDPul * 2) && (RemainPul>0))
{
float SpeedRatio = limitSDPul * 2 / RemainPul;
DriveSpeed = DriveSpeed / SpeedRatio;
if (DriveSpeed < StartSpeed)
{
DriveSpeed = StartSpeed;
}
}
}
else
{
RemainPul = (int)(m_P_Work_Limit[1] / m_Resolution[1]) - (int)(now_pos[1] / m_Resolution[1]);
limitSDPul = (DriveSpeed - StartSpeed) * 13;
if ((RemainPul < limitSDPul * 2) && (RemainPul>0))
{
float SpeedRatio = limitSDPul * 2 / RemainPul;
DriveSpeed = DriveSpeed / SpeedRatio;
if (DriveSpeed < StartSpeed)
{
DriveSpeed = StartSpeed;
}
}
}
}
else if (AxisTypes == AXIS_Y && m_motorType & 0x02)
{
if (!bJOGDir)//负方向
{
RemainPul = (int)(now_pos[2] / m_Resolution[2]) - (int)(m_N_Work_Limit[2] / m_Resolution[2]);
limitSDPul = (DriveSpeed - StartSpeed) * 13;
if ((RemainPul < limitSDPul * 2) && (RemainPul>0))
{
float SpeedRatio = 1 + limitSDPul * 2 / RemainPul;
DriveSpeed = DriveSpeed / SpeedRatio;
if (DriveSpeed < StartSpeed)
{
DriveSpeed = StartSpeed;
}
}
}
else
{
RemainPul = (int)(m_P_Work_Limit[2] / m_Resolution[2]) - (int)(now_pos[2] / m_Resolution[2]);
limitSDPul = (DriveSpeed - StartSpeed) * 13;
if ((RemainPul < limitSDPul * 2) && (RemainPul>0))
{
float SpeedRatio = limitSDPul * 2 / RemainPul;
DriveSpeed = DriveSpeed / SpeedRatio;
if (DriveSpeed < StartSpeed)
{
DriveSpeed = StartSpeed;
}
}
}
}
if (AxisTypes == AXIS_Z && m_motorType & 0x04)
{
if (!bJOGDir)//负方向
{
RemainPul = (int)(now_pos[3] / m_Resolution[3]) - (int)(m_N_Work_Limit[3] / m_Resolution[3]);
limitSDPul = (DriveSpeed - StartSpeed) * 13;
if ((RemainPul < limitSDPul * 2) && (RemainPul>0))
{
float SpeedRatio = limitSDPul * 2 / RemainPul;
DriveSpeed = DriveSpeed / SpeedRatio;
if (DriveSpeed < StartSpeed)
{
DriveSpeed = StartSpeed;
}
}
}
else
{
RemainPul = (int)(m_P_Work_Limit[3] / m_Resolution[3]) - (int)(now_pos[3] / m_Resolution[3]);
limitSDPul = (DriveSpeed - StartSpeed) * 13;
if ((RemainPul < limitSDPul * 2) && (RemainPul>0))
{
float SpeedRatio = limitSDPul * 2 / RemainPul;
DriveSpeed = DriveSpeed / SpeedRatio;
if (DriveSpeed < StartSpeed)
{
DriveSpeed = StartSpeed;
}
}
}
}
if (AxisTypes == AXIS_U && m_motorType & 0x08)
{
if (!bJOGDir)//负方向
{
RemainPul = (int)(now_pos[4] / m_Resolution[4]) - (int)(m_N_Work_Limit[4] / m_Resolution[4]);
limitSDPul = (DriveSpeed - StartSpeed) * 13;
if ((RemainPul < limitSDPul * 2) && (RemainPul>0))
{
float SpeedRatio = limitSDPul * 2 / RemainPul;
DriveSpeed = DriveSpeed / SpeedRatio;
if (DriveSpeed < StartSpeed)
{
DriveSpeed = StartSpeed;
}
}
}
else
{
RemainPul = (int)(m_P_Work_Limit[4] / m_Resolution[4]) - (int)(now_pos[4] / m_Resolution[4]);
limitSDPul = (DriveSpeed - StartSpeed) * 13;
if ((RemainPul < limitSDPul * 2) && (RemainPul>0))
{
float SpeedRatio = limitSDPul * 2 / RemainPul;
DriveSpeed = DriveSpeed / SpeedRatio;
if (DriveSpeed < StartSpeed)
{
DriveSpeed = StartSpeed;
}
}
}
}
}
m_cSendData[0] = CT_MOTOR;
m_cSendData[1] = CT_MOTOR_SET;
m_cSendData[2] = AxisTypes;
m_cSendData[3] = JOG_SPEED_ACC_DEC;
m_cSendData[4] = (StartSpeed & 0xff);
m_cSendData[5] = ((StartSpeed >> 8) & 0xff);
m_cSendData[6] = ((StartSpeed >> 16) & 0xff);
m_cSendData[7] = ((StartSpeed >> 24) & 0xff);
m_cSendData[8] = (DriveSpeed & 0xff);
m_cSendData[9] = ((DriveSpeed >> 8) & 0xff);
m_cSendData[10] = ((DriveSpeed >> 16) & 0xff);
m_cSendData[11] = ((DriveSpeed >> 24) & 0xff);
m_cSendData[12] = (AccCurve & 0xff);
m_cSendData[13] = ((AccCurve >> 8) & 0xff);
m_cSendData[14] = ((AccCurve >> 16) & 0xff);
m_cSendData[15] = ((AccCurve >> 24) & 0xff);
m_cSendData[16] = (AccLine & 0xff);
m_cSendData[17] = ((AccLine >> 8) & 0xff);
m_cSendData[18] = ((AccLine >> 16) & 0xff);
m_cSendData[19] = ((AccLine >> 24) & 0xff);
m_cSendData[20] = (DecCurve & 0xff);
m_cSendData[21] = ((DecCurve >> 8) & 0xff);
m_cSendData[22] = ((DecCurve >> 16) & 0xff);
m_cSendData[23] = ((DecCurve >> 24) & 0xff);
m_cSendData[24] = (DecLine & 0xff);
m_cSendData[25] = ((DecLine >> 8) & 0xff);
m_cSendData[26] = ((DecLine >> 16) & 0xff);
m_cSendData[27] = ((DecLine >> 24) & 0xff);
jogspeed = DriveSpeed;
if ((StartSpeed < 250) && (DriveSpeed < 6))
{
m_IsUseJerk = 1;
}
else
{
m_IsUseJerk = 0;
}
m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
Sleep(5);
m_cSendData[0] = CT_MOTOR;
if (AxisNumber == 1 || AxisNumber == 2)
{
if (!bJOGDir)//方向
{
m_cSendData[1] = CT_START_JOG_NEG;
}
else
{
m_cSendData[1] = CT_START_JOG_POS;
}
}
else
{
if (bJOGDir)//方向
{
m_cSendData[1] = CT_START_JOG_POS;
}
else
{
m_cSendData[1] = CT_START_JOG_NEG;
}
}
m_cSendData[2] = AxisTypes;
m_cSendData[3] = 0x53;
t_start = GetTickCount();
m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
jogMoving = true;
g_pLogger->SendAndFlushWithTime(L"[Jog] Out,bJOGDir = %d\n", bJOGDir);
}
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::StopJog()
{
WaitForSingleObject(g_Lock_JogAndTrigger, INFINITE);
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
g_pLogger->SendAndFlushWithTime(L"[StopJog] In\n");
t_end = GetTickCount();
g_pLogger->SendAndFlushWithTime(L"[StopJog] t_start = %d, t_end = %d\n", t_start, t_end);
jogMoving = false;
if (t_end - t_start < 100)
{
DWORD t_use = 100 - (t_end - t_start);
g_pLogger->SendAndFlushWithTime(L"[StopJog] PushButtonTime = %d\n", t_use);
Sleep(t_use);
}
unsigned char m_SendJogData[64] = { 0 };
if (m_IsUseJerk == 0)
{
m_SendJogData[0] = CT_MOTOR;
m_SendJogData[1] = CT_STOP;
m_SendJogData[2] = AXIS_XYZU;
m_StopJogMode[1] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49;
m_StopJogMode[2] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49;
m_StopJogMode[3] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49;
m_StopJogMode[4] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49;
m_WriteByte = Send_Command(0, (const char*)m_SendJogData, m_SendDataLength);
}
else
{
m_SendJogData[0] = CT_MOTOR;
m_SendJogData[1] = CT_STOP;
m_SendJogData[2] = AXIS_XYZU;
m_StopJogMode[1] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49;
m_StopJogMode[2] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49;
m_StopJogMode[3] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49;
m_StopJogMode[4] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49;
m_WriteByte = Send_Command(0, (const char*)m_SendJogData, m_SendDataLength);
}
m_Thread_StateJOGStop = HSI_THREAD_PAUSED;
g_pLogger->SendAndFlushWithTime(L"[StopJog] Out\n");
ReleaseMutex(g_Lock_JogAndTrigger);
}
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::StopJogEx(UINT AxisTypes)
{
WaitForSingleObject(g_Lock_JogAndTrigger, INFINITE);
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
byte AxisNumber = (byte)IndexConvertAxis(AxisTypes);
g_pLogger->SendAndFlushWithTime(L"[StopJog] In\n");
t_end = GetTickCount();
g_pLogger->SendAndFlushWithTime(L"[StopJog] t_start = %d, t_end = %d\n", t_start, t_end);
jogMoving = false;
if (t_end - t_start < 100)
{
DWORD t_use = 100 - (t_end - t_start);
g_pLogger->SendAndFlushWithTime(L"[StopJog] PushButtonTime = %d\n", t_use);
Sleep(t_use);
}
unsigned char m_SendJogData[64] = { 0 };
if (m_IsUseJerk == 0)
{
m_SendJogData[0] = CT_MOTOR;
m_SendJogData[1] = CT_STOP;
m_SendJogData[2] = AxisNumber;
m_StopJogMode[1] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49;
m_StopJogMode[2] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49;
m_StopJogMode[3] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49;
m_StopJogMode[4] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49;
for (int i = 0; i < 5; i++)
{
m_WriteByte = Send_Command(0, (const char*)m_SendJogData, m_SendDataLength);
Sleep(5);
}
}
else
{
m_SendJogData[0] = CT_MOTOR;
m_SendJogData[1] = CT_STOP;
m_SendJogData[2] = AxisNumber;
m_StopJogMode[1] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49;
m_StopJogMode[2] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49;
m_StopJogMode[3] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49;
m_StopJogMode[4] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49;
for (int i = 0; i < 5; i++)
{
m_WriteByte = Send_Command(0, (const char*)m_SendJogData, m_SendDataLength);
Sleep(5);
}
}
m_Thread_StateJOGStop = HSI_THREAD_PAUSED;
g_pLogger->SendAndFlushWithTime(L"[StopJog] Out\n");
ReleaseMutex(g_Lock_JogAndTrigger);
}
return rStatus;
}
//===========================================================================
int HSI_Motion::P2P(short AxisNumber, long Pos, double Speed, double Acc)
{
if (g_pHSI_Motion)
{
}
return 0;
}
//===========================================================================
//运动控制部分
//===========================================================================
HSI_STATUS HSI_Motion::GetPositionEncPrfMulti(UINT AxisTypes, double *EncPos, double *PrfPos, int Count)
{
auto rStatus = HSI_STATUS_NORMAL;
UNREFERENCED_PARAMETER(AxisTypes);
CString tempStr;
if (g_pHSI_Motion)
{
//g_pLogger->SendAndFlushWithTime(L"[GetPositionEncPrfMulti] In\n");
if (m_SO7_Serial.m_RecvData[0] == 2)
{
EncPos[1] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial.m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]) * m_Resolution[1];
EncPos[2] = (m_SO7_Serial.m_RecvData[5] << 24 | m_SO7_Serial.m_RecvData[6] << 16 | m_SO7_Serial.m_RecvData[7] << 8 | m_SO7_Serial.m_RecvData[8]) * m_Resolution[2];
EncPos[3] = (m_SO7_Serial.m_RecvData[9] << 24 | m_SO7_Serial.m_RecvData[10] << 16 | m_SO7_Serial.m_RecvData[11] << 8 | m_SO7_Serial.m_RecvData[12]) * m_Resolution[3];
EncPos[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | m_SO7_Serial.m_RecvData[15] << 8 | m_SO7_Serial.m_RecvData[16]) * m_Resolution[4];
PrfPos[1] = (m_SO7_Serial.m_RecvData[17] << 24 | m_SO7_Serial.m_RecvData[18] << 16 | m_SO7_Serial.m_RecvData[19] << 8 | m_SO7_Serial.m_RecvData[20]) * m_Resolution[1];
PrfPos[2] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial.m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]) * m_Resolution[2];
PrfPos[3] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial.m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]) * m_Resolution[3];
PrfPos[4] = (m_SO7_Serial.m_RecvData[29] << 24 | m_SO7_Serial.m_RecvData[30] << 16 | m_SO7_Serial.m_RecvData[31] << 8 | m_SO7_Serial.m_RecvData[32]) * m_Resolution[4];
if (m_IsHavePattern & 0x01)
m_EncPos[1] = EncPos[1];
else
m_EncPos[1] = PrfPos[1];
if (m_IsHavePattern & 0x02)
m_EncPos[2] = EncPos[2];
else
m_EncPos[2] = PrfPos[2];
if (m_IsHavePattern & 0x04)
m_EncPos[3] = EncPos[3];
else
m_EncPos[3] = PrfPos[3];
if (m_IsHavePattern & 0x08)
m_EncPos[4] = EncPos[4];
else
m_EncPos[4] = PrfPos[4];
m_PrfPos[1] = PrfPos[1];
m_PrfPos[2] = PrfPos[2];
m_PrfPos[3] = PrfPos[3];
m_PrfPos[4] = PrfPos[4];
//begin_position[1] = EncPos[1] / m_Resolution[1];
//begin_position[2] = EncPos[2] / m_Resolution[1];
//begin_position[3] = EncPos[3] / m_Resolution[1];
//begin_position[4] = EncPos[4] / m_Resolution[1];
}
else
{
EncPos[1] = m_EncPos[1];
EncPos[2] = m_EncPos[2];
EncPos[2] = m_EncPos[3];
EncPos[4] = m_EncPos[4];
PrfPos[1] = m_PrfPos[1];
PrfPos[2] = m_PrfPos[2];
PrfPos[2] = m_PrfPos[3];
PrfPos[4] = m_PrfPos[4];
g_pLogger->SendAndFlushWithTime(L"[GetPositionEncPrfMulti] failed\n");
}
//g_pLogger->SendAndFlushWithTime(L"[GetPositionEncPrfMulti] Out\n");
}
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::GetPositionXyz(UINT AxisTypes, double &PositionX, double &PositionY, double &PositionZ, double &Time)
{
auto rStatus = HSI_STATUS_NORMAL;
UNREFERENCED_PARAMETER(AxisTypes);
//读取3个轴的位置
CString tempStr;
if (g_pHSI_Motion)
{
if (m_SO7_Serial.m_RecvData[0] == 2)
{
if (m_DeviceType != 1)
{
if (m_IsHavePattern & 0x01)
PositionX = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial.m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]) * m_Resolution[1];
else
PositionX = (m_SO7_Serial.m_RecvData[17] << 24 | m_SO7_Serial.m_RecvData[18] << 16 | m_SO7_Serial.m_RecvData[19] << 8 | m_SO7_Serial.m_RecvData[20]) * m_Resolution[1];
if (m_IsHavePattern & 0x02)
PositionY = (m_SO7_Serial.m_RecvData[5] << 24 | m_SO7_Serial.m_RecvData[6] << 16 | m_SO7_Serial.m_RecvData[7] << 8 | m_SO7_Serial.m_RecvData[8]) * m_Resolution[2];
else
PositionY = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial.m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]) * m_Resolution[2];
if (m_IsHavePattern & 0x04)
PositionZ = (m_SO7_Serial.m_RecvData[9] << 24 | m_SO7_Serial.m_RecvData[10] << 16 | m_SO7_Serial.m_RecvData[11] << 8 | m_SO7_Serial.m_RecvData[12]) * m_Resolution[3];
else
PositionZ = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial.m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]) * m_Resolution[3];
if (m_IsHavePattern & 0x08)
m_PosForAllAxis[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | m_SO7_Serial.m_RecvData[15] << 8 | m_SO7_Serial.m_RecvData[16]) * m_Resolution[4];
else
m_PosForAllAxis[4] = (m_SO7_Serial.m_RecvData[29] << 24 | m_SO7_Serial.m_RecvData[30] << 16 | m_SO7_Serial.m_RecvData[31] << 8 | m_SO7_Serial.m_RecvData[32]) * m_Resolution[4];
}
else
{
PositionX = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial.m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]) * m_Resolution[1];
PositionY = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial.m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]) * m_Resolution[2];
PositionZ = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial.m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]) * m_Resolution[3];
}
m_EncPos[1] = PositionX;
m_EncPos[2] = PositionY;
m_EncPos[3] = PositionZ;
m_EncPos[4] = m_PosForAllAxis[4];
m_PosForAllAxis[1] = PositionX;
m_PosForAllAxis[2] = PositionY;
m_PosForAllAxis[3] = PositionZ;
}
else
{
PositionX = m_EncPos[1];
PositionY = m_EncPos[2];
PositionZ = m_EncPos[3];
m_PosForAllAxis[1] = m_EncPos[1];
m_PosForAllAxis[2] = m_EncPos[2];
m_PosForAllAxis[3] = m_EncPos[3];
m_PosForAllAxis[4] = m_EncPos[4];
g_pLogger->SendAndFlushWithTime(L"[GetPositionEncPrfMulti] failed\n");
}
//LARGE_INTEGER tima;
//QueryPerformanceCounter(&tima);
//Time = static_cast<double>(tima.QuadPart);
Time = set_end - set_start;
}
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::GetEncoderXyz(long *lEncoderVal)
{
auto rStatus = HSI_STATUS_NORMAL;
//读取3个轴的编码器值
if (g_pHSI_Motion)
{
if (m_SO7_Serial.m_RecvData[0] == 2)
{
if (m_DeviceType != 1)
{
if (m_IsHavePattern & 0x01)
lEncoderVal[0] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial.m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]);
else
lEncoderVal[0] = (m_SO7_Serial.m_RecvData[17] << 24 | m_SO7_Serial.m_RecvData[18] << 16 | m_SO7_Serial.m_RecvData[19] << 8 | m_SO7_Serial.m_RecvData[20]);
if (m_IsHavePattern & 0x02)
lEncoderVal[1] = (m_SO7_Serial.m_RecvData[5] << 24 | m_SO7_Serial.m_RecvData[6] << 16 | m_SO7_Serial.m_RecvData[7] << 8 | m_SO7_Serial.m_RecvData[8]);
else
lEncoderVal[1] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial.m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]);
if (m_IsHavePattern & 0x04)
lEncoderVal[2] = (m_SO7_Serial.m_RecvData[9] << 24 | m_SO7_Serial.m_RecvData[10] << 16 | m_SO7_Serial.m_RecvData[11] << 8 | m_SO7_Serial.m_RecvData[12]);
else
lEncoderVal[2] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial.m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]);
}
else
{
lEncoderVal[0] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial.m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]);
lEncoderVal[1] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial.m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]);
lEncoderVal[2] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial.m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]);
}
}
else
{
auto rStatus = HSI_STATUS_FAILED;
g_pLogger->SendAndFlushWithTime(L"[GetEncoderXyz] failed\n");
}
}
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::GetPositionXyzaProbe(UINT AxisTypes, double &PositionX, double &PositionY, double &PositionZ, double &PositionA)
{
auto rStatus = HSI_STATUS_NORMAL;
UNREFERENCED_PARAMETER(AxisTypes);
if (g_pHSI_Motion)
{
PositionX = m_LockPos[1];
PositionY = m_LockPos[2];
PositionZ = m_LockPos[3];
PositionA = m_LockPos[4];
}
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::SetPositionXyz(UINT AxisTypes, double PositionX, double PositionY, double PositionZ, HSI_MOTION_MOVE_TYPE eType, double dFlyRadius)
{
WaitForSingleObject(g_WR_ToMove_Mutex, INFINITE);
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] In\n");
g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Pos[1] = %.4f,Pos[2] = %.4f,Pos[3] = %.4f\n", PositionX, PositionY, PositionZ);
unsigned char send_pos_data[64] = { 0 };
axis_start = 0;
unsigned char direct_pos = 0;
unsigned char xyzAxis = 0;
if (CurrentMotionState != E_SO7_MOTION_MOVETO)
{
CurrentMotionState = E_SO7_MOTION_MOVETO;
LimitOver(HSI_MOTION_AXIS_X, PositionX);
LimitOver(HSI_MOTION_AXIS_Y, PositionY);
LimitOver(HSI_MOTION_AXIS_Z, PositionZ);
LimitOver(HSI_MOTION_AXIS_R, m_PositionA);
m_PosThread[1] = PositionX;
m_PosThread[2] = PositionY;
m_PosThread[3] = PositionZ;
m_PosThread[4] = m_PositionA;
targetpos_n[1] = PositionX;
targetpos_n[2] = PositionY;
targetpos_n[3] = PositionZ;
targetpos_n[4] = m_PositionA;
int Pos_t[5] = { 0 };
int Pos_n[5] = { 0 };
int Pos[5] = { 0 };
int NowPos[5] = { 0 };
int target_pos[5] = { 0 };
if (m_SO7_Serial.m_RecvData[0] == 2)
{
if (m_DeviceType != 1)
{
if (m_IsHavePattern & 0x01 == 0x01)
NowPos[1] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial.m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]);
else
NowPos[1] = (m_SO7_Serial.m_RecvData[17] << 24 | m_SO7_Serial.m_RecvData[18] << 16 | m_SO7_Serial.m_RecvData[19] << 8 | m_SO7_Serial.m_RecvData[20]);
if (m_IsHavePattern & 0x02)
NowPos[2] = (m_SO7_Serial.m_RecvData[5] << 24 | m_SO7_Serial.m_RecvData[6] << 16 | m_SO7_Serial.m_RecvData[7] << 8 | m_SO7_Serial.m_RecvData[8]);
else
NowPos[2] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial.m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]);
if (m_IsHavePattern & 0x04)
NowPos[3] = (m_SO7_Serial.m_RecvData[9] << 24 | m_SO7_Serial.m_RecvData[10] << 16 | m_SO7_Serial.m_RecvData[11] << 8 | m_SO7_Serial.m_RecvData[12]);
else
NowPos[3] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial.m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]);
if (m_IsHavePattern & 0x08)
NowPos[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | m_SO7_Serial.m_RecvData[15] << 8 | m_SO7_Serial.m_RecvData[16]);
else
NowPos[4] = (m_SO7_Serial.m_RecvData[29] << 24 | m_SO7_Serial.m_RecvData[30] << 16 | m_SO7_Serial.m_RecvData[31] << 8 | m_SO7_Serial.m_RecvData[32]);
}
else
{
NowPos[1] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial.m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]);
NowPos[2] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial.m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]);
NowPos[3] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial.m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]);
NowPos[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | m_SO7_Serial.m_RecvData[15] << 8 | m_SO7_Serial.m_RecvData[16]);
}
//NowPos[1] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial.m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]);
//NowPos[2] = (m_SO7_Serial.m_RecvData[5] << 24 | m_SO7_Serial.m_RecvData[6] << 16 | m_SO7_Serial.m_RecvData[7] << 8 | m_SO7_Serial.m_RecvData[8]);
//NowPos[3] = (m_SO7_Serial.m_RecvData[9] << 24 | m_SO7_Serial.m_RecvData[10] << 16 | m_SO7_Serial.m_RecvData[11] << 8 | m_SO7_Serial.m_RecvData[12]);
//NowPos[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | m_SO7_Serial.m_RecvData[15] << 8 | m_SO7_Serial.m_RecvData[16]);
Pos_t[1] = (m_SO7_Serial.m_RecvData[17] << 24 | m_SO7_Serial.m_RecvData[18] << 16 | m_SO7_Serial.m_RecvData[19] << 8 | m_SO7_Serial.m_RecvData[20]);
Pos_t[2] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial.m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]);
Pos_t[3] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial.m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]);
Pos_t[4] = (m_SO7_Serial.m_RecvData[29] << 24 | m_SO7_Serial.m_RecvData[30] << 16 | m_SO7_Serial.m_RecvData[31] << 8 | m_SO7_Serial.m_RecvData[32]);
}
else
{
Pos_t[1] = NowPos[1] = (int)(m_EncPos[1] / m_Resolution[1]);
Pos_t[2] = NowPos[2] = (int)(m_EncPos[2] / m_Resolution[2]);
Pos_t[3] = NowPos[3] = (int)(m_EncPos[3] / m_Resolution[3]);
Pos_t[4] = NowPos[4] = (int)(m_EncPos[4] / m_Resolution[4]);
}
if (m_motorType & 0x01) //步进电机
Pos[1] = (int)(PositionX / m_Resolution[1]) - NowPos[1];
else
Pos[1] = (int)(PositionX / m_Resolution[1]) - Pos_t[1];
if (m_motorType & 0x02) //步进电机
Pos[2] = (int)(PositionX / m_Resolution[2]) - NowPos[2];
else
Pos[2] = (int)(PositionY / m_Resolution[2]) - Pos_t[2];
if (m_motorType & 0x04) //步进电机
Pos[3] = (int)(PositionX / m_Resolution[3]) - NowPos[3];
else
Pos[3] = (int)(PositionZ / m_Resolution[3]) - Pos_t[3];
if (m_motorType & 0x08) //步进电机
Pos[4] = (int)(PositionX / m_Resolution[4]) - NowPos[4];
else
Pos[4] = (int)(m_PositionA / m_Resolution[4]) - Pos_t[4];
/*if (m_motorType==1)
{
if (m_IsUse_HSICompensation)
{
}
else
{
Pos[1] = (int)(PositionX / m_Resolution[1]) - NowPos[1];
Pos[2] = (int)(PositionY / m_Resolution[2]) - NowPos[2];
Pos[3] = (int)(PositionZ / m_Resolution[3]) - NowPos[3];
Pos[4] = (int)(m_PositionA / m_Resolution[4]) - NowPos[4];
}
}
else
{
Pos_n[1] = (int)(targetpos_n[1] / m_Resolution[1]) - NowPos[1];
Pos_n[2] = (int)(targetpos_n[2] / m_Resolution[2]) - NowPos[2];
Pos_n[3] = (int)(targetpos_n[3] / m_Resolution[3]) - NowPos[3];
Pos_t[1] = (int)(targetpos_n[1] / m_Resolution[1]) - (int)(targetpos_l[1] / m_Resolution[1]);
Pos_t[2] = (int)(targetpos_n[2] / m_Resolution[2]) - (int)(targetpos_l[2] / m_Resolution[1]);
Pos_t[3] = (int)(targetpos_n[3] / m_Resolution[3]) - (int)(targetpos_l[3] / m_Resolution[1]);
if (m_IsUse_HSICompensation)
{
for (int k = 1; k < 4; k++)
{
if (abs(Pos_n[k] - Pos_t[k]) > m_Compensation_Pluse)
{
Pos[k] = Pos_n[k];
}
else
{
Pos[k] = Pos_t[k];
}
if (abs(Pos[k]) < m_Compensation_Pluse)
{
Pos[k] = 0;
}
}
}
else
{
Pos[1] = (int)(PositionX / m_Resolution[1]) - NowPos[1];
Pos[2] = (int)(PositionY / m_Resolution[2]) - NowPos[2];
Pos[3] = (int)(PositionZ / m_Resolution[3]) - NowPos[3];
Pos[4] = (int)(m_PositionA / m_Resolution[4]) - NowPos[4];
}
}*/
target_pos[1] = (int)(PositionX / m_Resolution[1]);
target_pos[2] = (int)(PositionY / m_Resolution[2]);
target_pos[3] = (int)(PositionZ / m_Resolution[3]);
target_pos[4] = (int)(m_PositionA / m_Resolution[4]);
begin_position[1] = target_pos[1];
begin_position[2] = target_pos[2];
begin_position[3] = target_pos[3];
begin_position[4] = target_pos[4];
float scale[4] = { 0 };
int Stepdriverspeed[5] = { 0 };
int StepStartspeed[5] = { 0 };
int StepAcc[5] = { 0 };
//if (Pos[1] > 0) direct_pos |= 0x01;
//if (Pos[2] > 0) direct_pos |= 0x02;
//if (Pos[3] > 0) direct_pos |= 0x04;
if (abs(Pos[1]) > 2) axis_start |= 0x01;
if (abs(Pos[2]) > 2) axis_start |= 0x02;
if (abs(Pos[3]) > 2) axis_start |= 0x04;
int MaxPos = abs(Pos[1]);
for (int i = 2; i < 5; i++)
{
if (MaxPos < abs(Pos[i]))
{
MaxPos = abs(Pos[i]);
}
}
//MaxPos = abs(MaxPos);
scale[0] = abs(Pos[1]) / (float)MaxPos;
scale[1] = abs(Pos[2]) / (float)MaxPos;
scale[2] = abs(Pos[3]) / (float)MaxPos;
scale[3] = abs(Pos[4]) / (float)MaxPos;
g_pLogger->SendAndFlushWithTime(L"[SetPositionXyzNowPos] Pos[1] = %.4f,Pos[2] = %.4f,Pos[3] = %.4f,Pos[4] = %.4f\n", NowPos[1] * m_Resolution[1], NowPos[2] * m_Resolution[2], NowPos[3] * m_Resolution[3], NowPos[4] * m_Resolution[4]);
g_pLogger->SendAndFlushWithTime(L"[SetPositionXyzTagPos] Pos[1] = %.4f,Pos[2] = %.4f,Pos[3] = %.4f,Pos[4] = %.4f\n", PositionX, PositionY, PositionZ, m_PositionA);
int stepinterpolation = 0x03;
//if (!m_motorType)
//{
// /*if ((stepinterpolation & 0x01) && m_IsUseManualRunin)
// {
// Stepdriverspeed[1] = m_SetPotion_DriveSpeed[1] * scale[0];
// if (Stepdriverspeed[1] < m_SetPotion_StartSpeed[1])
// {
// Stepdriverspeed[1] = m_SetPotion_StartSpeed[1];
// }
// }
// else
// {
// Stepdriverspeed[1] = m_SetPotion_DriveSpeed[1];
// }
// if ((stepinterpolation & 0x02) && m_IsUseManualRunin)
// {
// Stepdriverspeed[2] = m_SetPotion_DriveSpeed[2] * scale[1];
// if (Stepdriverspeed[2] < m_SetPotion_StartSpeed[2])
// {
// Stepdriverspeed[2] = m_SetPotion_StartSpeed[2];
// }
// }
// else
// {
// Stepdriverspeed[2] = m_SetPotion_DriveSpeed[2];
// }
// if ((stepinterpolation & 0x04) && m_IsUseManualRunin)
// {
// Stepdriverspeed[3] = m_SetPotion_DriveSpeed[3] * scale[2];
// if (Stepdriverspeed[3] < m_SetPotion_StartSpeed[3])
// {
// Stepdriverspeed[3] = m_SetPotion_DriveSpeed[3];
// }
// }
// else
// {
// Stepdriverspeed[3] = m_SetPotion_DriveSpeed[3];
// }
// if ((stepinterpolation & 0x08) && m_IsUseManualRunin)
// {
// Stepdriverspeed[4] = m_SetPotion_DriveSpeed[4] * scale[3];
// if (Stepdriverspeed[4] < m_SetPotion_StartSpeed[4])
// {
// Stepdriverspeed[4] = m_SetPotion_DriveSpeed[4];
// }
// }
// else
// {
// Stepdriverspeed[4] = m_SetPotion_DriveSpeed[4];
// }*/
// for (int i = 1; i < 3; i++)
// {
// if (abs(Pos[i]) < m_stepPosition_Load[0])
// {
// StepStartspeed[i] = m_stepPosition_L_speed[0];
// Stepdriverspeed[i] = m_stepPosition_H_speed[0];
// StepAcc[i] = m_stepPosition_acc[0];
// }
// else if (abs(Pos[i]) < m_stepPosition_Load[1])
// {
// StepStartspeed[i] = m_stepPosition_L_speed[1];
// Stepdriverspeed[i] = m_stepPosition_H_speed[1];
// StepAcc[i] = m_stepPosition_acc[1];
// }
// else if (abs(Pos[i]) < m_stepPosition_Load[2])
// {
// StepStartspeed[i] = m_stepPosition_L_speed[2];
// Stepdriverspeed[i] = m_stepPosition_H_speed[2];
// StepAcc[i] = m_stepPosition_acc[2];
// }
// else if (abs(Pos[i]) < m_stepPosition_Load[3])
// {
// StepStartspeed[i] = m_stepPosition_L_speed[3];
// Stepdriverspeed[i] = m_stepPosition_H_speed[3];
// StepAcc[i] = m_stepPosition_acc[3];
// }
// else if (abs(Pos[i]) < m_stepPosition_Load[4])
// {
// StepStartspeed[i] = m_stepPosition_L_speed[4];
// Stepdriverspeed[i] = m_stepPosition_H_speed[4];
// StepAcc[i] = m_stepPosition_acc[4];
// }
// else if (abs(Pos[i]) < m_stepPosition_Load[5])
// {
// StepStartspeed[i] = m_stepPosition_L_speed[5];
// Stepdriverspeed[i] = m_stepPosition_H_speed[5];
// StepAcc[i] = m_stepPosition_acc[5];
// }
// else if (abs(Pos[i]) < m_stepPosition_Load[6])
// {
// StepStartspeed[i] = m_stepPosition_L_speed[6];
// Stepdriverspeed[i] = m_stepPosition_H_speed[6];
// StepAcc[i] = m_stepPosition_acc[6];
// }
// else if (abs(Pos[i]) < m_stepPosition_Load[7])
// {
// StepStartspeed[i] = m_stepPosition_L_speed[7];
// Stepdriverspeed[i] = m_stepPosition_H_speed[7];
// StepAcc[i] = m_stepPosition_acc[7];
// }
// else if (abs(Pos[i]) < m_stepPosition_Load[8])
// {
// StepStartspeed[i] = m_stepPosition_L_speed[8];
// Stepdriverspeed[i] = m_stepPosition_H_speed[8];
// StepAcc[i] = m_stepPosition_acc[8];
// }
// else if (abs(Pos[i]) < m_stepPosition_Load[9])
// {
// StepStartspeed[i] = m_stepPosition_L_speed[9];
// Stepdriverspeed[i] = m_stepPosition_H_speed[9];
// StepAcc[i] = m_stepPosition_acc[9];
// }
// else
// {
// StepStartspeed[i] = m_SetPotion_StartSpeed[i];
// Stepdriverspeed[i] = m_SetPotion_DriveSpeed[i];
// StepAcc[i] = m_SetPotion_Line[i];
// }
// }
// StepStartspeed[3] = m_SetPotion_StartSpeed[3];
// StepAcc[3] = m_SetPotion_Line[3];
// Stepdriverspeed[3] = m_SetPotion_DriveSpeed[3];
//}
int axisCount = 4;
if (fourthAxisFlag)
{
fourthAxisFlag = false;
if (Pos[4] > 0) direct_pos |= 0x08;
if (abs(Pos[4]) > 20) axis_start |= 0x08;
axisCount = 5;
xyzAxis = AXIS_XYZU;
}
else
xyzAxis = AXIS_XYZ;
for (int i = 1; i < axisCount; i++)
{
int time_out_send = 0;
send_pos_data[0] = CT_MOTOR;
send_pos_data[1] = CT_MOTOR_SET;
send_pos_data[2] = 1 << (i - 1);
send_pos_data[3] = POSITION_SPEED_ACC_DEC_POS;
/*if (m_motorType == 1)
{*/
send_pos_data[4] = (m_SetPotion_StartSpeed[i] & 0xff);
send_pos_data[5] = ((m_SetPotion_StartSpeed[i] >> 8) & 0xff);
send_pos_data[6] = ((m_SetPotion_StartSpeed[i] >> 16) & 0xff);
send_pos_data[7] = ((m_SetPotion_StartSpeed[i] >> 24) & 0xff);
if ((xyzAxis == AXIS_XYZU) && (m_IsUseFourthSpeed == 1))
{
send_pos_data[8] = (m_SetPotion_DriveSpeed[4] & 0xff);
send_pos_data[9] = ((m_SetPotion_DriveSpeed[4] >> 8) & 0xff);
send_pos_data[10] = ((m_SetPotion_DriveSpeed[4] >> 16) & 0xff);
send_pos_data[11] = ((m_SetPotion_DriveSpeed[4] >> 24) & 0xff);
}
else
{
send_pos_data[8] = (m_SetPotion_DriveSpeed[i] & 0xff);
send_pos_data[9] = ((m_SetPotion_DriveSpeed[i] >> 8) & 0xff);
send_pos_data[10] = ((m_SetPotion_DriveSpeed[i] >> 16) & 0xff);
send_pos_data[11] = ((m_SetPotion_DriveSpeed[i] >> 24) & 0xff);
}
send_pos_data[12] = (m_SetPotion_Line[i] & 0xff);
send_pos_data[13] = ((m_SetPotion_Line[i] >> 8) & 0xff);
send_pos_data[14] = ((m_SetPotion_Line[i] >> 16) & 0xff);
send_pos_data[15] = ((m_SetPotion_Line[i] >> 24) & 0xff);
send_pos_data[16] = (m_SetPotion_Buffer[i] & 0xff);
send_pos_data[17] = ((m_SetPotion_Buffer[i] >> 8) & 0xff);
send_pos_data[18] = ((m_SetPotion_Buffer[i] >> 16) & 0xff);
send_pos_data[19] = ((m_SetPotion_Buffer[i] >> 24) & 0xff);
send_pos_data[28] = (target_pos[i] & 0xff);
send_pos_data[29] = ((target_pos[i] >> 8) & 0xff);
send_pos_data[30] = ((target_pos[i] >> 16) & 0xff);
send_pos_data[31] = ((target_pos[i] >> 24) & 0xff);
send_pos_data[32] = (Pos[i] & 0xff);
send_pos_data[33] = ((Pos[i] >> 8) & 0xff);
send_pos_data[34] = ((Pos[i] >> 16) & 0xff);
send_pos_data[35] = ((Pos[i] >> 24) & 0xff);
g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] m_SetPotion_StartSpeed[%d] = %d,m_SetPotion_DriveSpeed[%d] = %d\n", i, m_SetPotion_StartSpeed[i], i, m_SetPotion_DriveSpeed[i]);
//send_pos_data[32] = 0x21;
/*}*/
/*else
{
send_pos_data[4] = (StepStartspeed[i] & 0xff);
send_pos_data[5] = ((StepStartspeed[i] >> 8) & 0xff);
send_pos_data[6] = ((StepStartspeed[i] >> 16) & 0xff);
send_pos_data[7] = ((StepStartspeed[i] >> 24) & 0xff);
send_pos_data[8] = (Stepdriverspeed[i] & 0xff);
send_pos_data[9] = ((Stepdriverspeed[i] >> 8) & 0xff);
send_pos_data[10] = ((Stepdriverspeed[i] >> 16) & 0xff);
send_pos_data[11] = ((Stepdriverspeed[i] >> 24) & 0xff);
send_pos_data[12] = (StepAcc[i] & 0xff);
send_pos_data[13] = ((StepAcc[i] >> 8) & 0xff);
send_pos_data[14] = ((StepAcc[i] >> 16) & 0xff);
send_pos_data[15] = ((StepAcc[i] >> 24) & 0xff);
send_pos_data[16] = (m_SetPotion_Buffer[i] & 0xff);
send_pos_data[17] = ((m_SetPotion_Buffer[i] >> 8) & 0xff);
send_pos_data[18] = ((m_SetPotion_Buffer[i] >> 16) & 0xff);
send_pos_data[19] = ((m_SetPotion_Buffer[i] >> 24) & 0xff);
send_pos_data[28] = (target_pos[i] & 0xff);
send_pos_data[29] = ((target_pos[i] >> 8) & 0xff);
send_pos_data[30] = ((target_pos[i] >> 16) & 0xff);
send_pos_data[31] = ((target_pos[i] >> 24) & 0xff);
send_pos_data[32] = (Pos[i] & 0xff);
send_pos_data[33] = ((Pos[i] >> 8) & 0xff);
send_pos_data[34] = ((Pos[i] >> 16) & 0xff);
send_pos_data[35] = ((Pos[i] >> 24) & 0xff);
g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] m_SetPotion_StartSpeed[%d] = %d,m_SetPotion_DriveSpeed[%d] = %d\n", i, m_SetPotion_StartSpeed[i], i, m_SetPotion_DriveSpeed[i]);
}*/
m_WriteByte = Send_Command(0, (const char*)send_pos_data, m_SendDataLength);
Sleep(6);
}
while (m_SO7_Serial.m_RecvData[39])
{
send_pos_data[0] = CT_ORDER;
send_pos_data[1] = CT_POSFLAG_CLEAR;
m_WriteByte = Send_Command(0, (const char*)send_pos_data, m_SendDataLength);
Sleep(10);
}
Sleep(10);
//启动插补和定位功能
/* if (m_motorType == 1)
{
send_pos_data[0] = CT_MOTOR;
send_pos_data[1] = CT_START_POSITION;
send_pos_data[2] = xyzAxis;
send_pos_data[3] = 0x53;
send_pos_data[4] = INTERPOLATION;
}
else
{*/
send_pos_data[0] = CT_MOTOR;
send_pos_data[1] = CT_START_POSITION;
send_pos_data[2] = axis_start;
send_pos_data[3] = 0x53;
send_pos_data[4] = m_motorType & 0xff;
/* }*/
for (int j = 1; j < axisCount; j++)
{
send_pos_data[9 + 4 * (j - 1)] = (target_pos[j] & 0xff);
send_pos_data[10 + 4 * (j - 1)] = ((target_pos[j] >> 8) & 0xff);
send_pos_data[11 + 4 * (j - 1)] = ((target_pos[j] >> 16) & 0xff);
send_pos_data[12 + 4 * (j - 1)] = ((target_pos[j] >> 24) & 0xff);
}
//send_pos_data[25] = direct_pos;
int stepmotoracc = 0;
stepmotoracc = CaculateStepMotorACC(Pos[1], m_SetPotion_Line[1], 10);
send_pos_data[25] = stepmotoracc;
stepmotoracc = CaculateStepMotorACC(Pos[2], m_SetPotion_Line[2], 10);
send_pos_data[26] = stepmotoracc;
stepmotoracc = CaculateStepMotorACC(Pos[3], m_SetPotion_Line[3], 10);
send_pos_data[27] = stepmotoracc;
stepmotoracc = CaculateStepMotorACC(Pos[4], m_SetPotion_Line[4], 10);
/*send_pos_data[25] = m_SetPotion_Line[1];
send_pos_data[26] = m_SetPotion_Line[2];
send_pos_data[27] = m_SetPotion_Line[3];
send_pos_data[28] = m_SetPotion_Line[4];*/
send_pos_data[29] = m_SpeedAdjustPeriod[1];
send_pos_data[30] = m_SpeedAdjustPeriod[2];
send_pos_data[31] = m_SpeedAdjustPeriod[3];
send_pos_data[32] = m_SpeedAdjustPeriod[4];
for (size_t i = 1; i < 5; i++)
{
Stepdriverspeed[i] = m_SetPotion_DriveSpeed[i] * m_Resolution[i] * 50;
}
send_pos_data[33] = (Stepdriverspeed[1] >> 8) & 0xff;
send_pos_data[34] = Stepdriverspeed[1] & 0xff;
send_pos_data[35] = (Stepdriverspeed[2] >> 8) & 0xff;
send_pos_data[36] = Stepdriverspeed[2] & 0xff;
send_pos_data[37] = (Stepdriverspeed[3] >> 8) & 0xff;
send_pos_data[38] = Stepdriverspeed[3] & 0xff;
send_pos_data[39] = (Stepdriverspeed[4] >> 8) & 0xff;
send_pos_data[40] = Stepdriverspeed[4] & 0xff;
if (bCircleRun)
{
bCircleRun = false;
send_pos_data[1] = CT_CIRCLERUN_POSITION;
send_pos_data[2] = 0x03;
send_pos_data[4] = CIRCLER;
for (size_t i = 1; i < 3; i++)
{
iCircleRunPnt[i] = iCircleRunPnt[i] - NowPos[i];
send_pos_data[26 + 4 * (i - 1)] = (iCircleRunPnt[i] & 0xff);
send_pos_data[27 + 4 * (i - 1)] = ((iCircleRunPnt[i] >> 8) & 0xff);
send_pos_data[28 + 4 * (i - 1)] = ((iCircleRunPnt[i] >> 16) & 0xff);
send_pos_data[29 + 4 * (i - 1)] = ((iCircleRunPnt[i] >> 24) & 0xff);
}
}
g_IsClose = false;
m_WriteByte = Send_Command(0, (const char*)send_pos_data, m_SendDataLength);
set_start = GetTickCount();
Sleep(3);//
if (eType == HSI_MOTION_MOVE_NOWAIT)
{
g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Nowait SetEvent\n");
g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Nowait move!\n");
g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Out Success Nowait\n");
m_IsExMotion = 0;
bRunGlueDispenser = HSI_THREAD_PAUSED;
m_Thread_State = HSI_THREAD_RUNNING;
SetEvent(m_hTriggerEvent);
}
if (eType == HSI_MOTION_MOVE_WAIT)
{
g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Out Success Wait Mode\n");
m_Thread_State = HSI_THREAD_RUNNING;
m_IsExMotion = 0;
UpdateMotionState();
m_Thread_State = HSI_THREAD_PAUSED;
}
}
else
{
g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] HSI_STATUS_MOTION_MOVING\n");
g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Out\n");
rStatus = HSI_STATUS_MOTION_MOVING;
}
g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Out\n");
targetpos_l[1] = PositionX;
targetpos_l[2] = PositionY;
targetpos_l[3] = PositionZ;
}
ReleaseMutex(g_WR_ToMove_Mutex);
return rStatus;
}
//===========================================================================
int HSI_Motion::CaculateStepMotorACC(int pos, int maxacc, int minacc)
{
double mpos = (double)abs(pos) / 1000;
int acc = (mpos * mpos) / 20 + mpos / 2 + 8;
if (acc < minacc)
acc = minacc;
if (acc > maxacc)
acc = maxacc;
return acc;
}
//===========================================================================
HSI_STATUS HSI_Motion::SetPositionXyza(UINT AxisTypes, double PositionX, double PositionY, double PositionZ, double PositionA, HSI_MOTION_MOVE_TYPE eType, double dFlyRadius)
{
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
g_pLogger->SendAndFlushWithTime(L"[SetPositionXyza] SetPositionXyza\n");
fourthAxisFlag = true;
m_PositionA = PositionA;
rStatus = SetPositionXyz(AxisTypes, PositionX, PositionY, PositionZ, eType, dFlyRadius);
}
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::SetPositionXyzCache(UINT AxisTypes, HSI_MOTION_MOVE_TYPE eType, int DataCount, Point *CacheData)
{
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
g_pLogger->SendAndFlushWithTime(L"SetPositionXyzCache : start\n");
g_pLogger->SendAndFlushWithTime(L"SetPositionXyzCache : end\n");
}
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::SetCircleInterpolate(double PositionX, double PositionY, double PositionZ)
{
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
g_pLogger->SendAndFlushWithTime(L"SetCircleInterpolate : start\n");
bCircleRun = true;
iCircleRunPnt[1] = (int)(PositionX / m_Resolution[1]);
iCircleRunPnt[2] = (int)(PositionY / m_Resolution[2]);
iCircleRunPnt[3] = (int)(PositionZ / m_Resolution[3]);
g_pLogger->SendAndFlushWithTime(L"SetCircleInterpolate : end\n");
}
return rStatus;
}
//===========================探针接口================================================
void HSI_Motion::ProbeRetractManDist(int RetractManDist)
{
if (g_pHSI_Motion)
{
m_cSendData[0] = 0x01;
m_cSendData[1] = 23;
m_cSendData[2] = (RetractManDist >> 24 & 0xff);
m_cSendData[3] = ((RetractManDist >> 16) & 0xff);
m_cSendData[4] = ((RetractManDist >> 8) & 0xff);
m_cSendData[5] = ((RetractManDist)& 0xff);
m_cSendData[6] = (m_ProbeReturnSpeed >> 8) & 0xff;
m_cSendData[7] = m_ProbeReturnSpeed & 0xff;
m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
Sleep(10);
}
}
HSI_STATUS HSI_Motion::JogProbe(UINT AxisTypes, double Speed)
{
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
while (m_SO7_Serial.m_RecvData[57])
{
m_cSendData[0] = CT_ORDER;
m_cSendData[1] = CT_POSFLAG_CLEAR;
m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
Sleep(10);
}
unsigned char send_probe_data[64] = { 0 };
unsigned char motionAxis = 0;
unsigned char motionDir = 0;
for (size_t i = 0; i < 3; i++)
{
if ((i == 0) && (m_axisDirX != 0) || (i == 1) && (m_axisDirY != 0) || (i == 2) && (m_axisDirZ != 0))
{
if ((m_axisDirX == 1) && (i == 0))
{
motionAxis |= 0x01;
motionDir |= 0x01;
}
else if ((i == 0) && (m_axisDirX == 2))
{
motionAxis |= 0x01;
}
if ((m_axisDirY == 1) && (i == 1))
{
motionAxis |= 0x02;
motionDir |= 0x02;
}
else if ((i == 1) && (m_axisDirY == 2))
{
motionAxis |= 0x02;
}
if ((m_axisDirZ == 1) && (i == 2))
{
motionAxis |= 0x04;
motionDir |= 0x04;
}
else if ((i == 2) && (m_axisDirZ == 2))
{
motionAxis |= 0x04;
}
int DriveSpeed(1);
int StartSpeed(1);
int AccLine(1);
int DecLine(1);
int AccCurve(1);
int DecCurve(1);
int JogSpeed(1);
byte AxisNumber = (byte)IndexConvertAxis(i + 1);
m_Thread_State = HSI_THREAD_PAUSED;
JogSpeed = abs(SpeedPercent(AxisNumber, Speed, DriveSpeed, StartSpeed, AccLine, DecLine, AccCurve, DecCurve));
send_probe_data[0] = CT_MOTOR;
send_probe_data[1] = CT_MOTOR_SET;
send_probe_data[2] = AxisNumber;
send_probe_data[3] = JOG_SPEED_ACC_DEC;
send_probe_data[4] = (StartSpeed & 0xff);
send_probe_data[5] = ((StartSpeed >> 8) & 0xff);
send_probe_data[6] = ((StartSpeed >> 16) & 0xff);
send_probe_data[7] = ((StartSpeed >> 24) & 0xff);
send_probe_data[8] = (m_probeSeekSpeed & 0xff);
send_probe_data[9] = ((m_probeSeekSpeed >> 8) & 0xff);
send_probe_data[10] = ((m_probeSeekSpeed >> 16) & 0xff);
send_probe_data[11] = ((m_probeSeekSpeed >> 24) & 0xff);
send_probe_data[12] = (AccCurve & 0xff);
send_probe_data[13] = ((AccCurve >> 8) & 0xff);
send_probe_data[14] = ((AccCurve >> 16) & 0xff);
send_probe_data[15] = ((AccCurve >> 24) & 0xff);
send_probe_data[16] = (AccLine & 0xff);
send_probe_data[17] = ((AccLine >> 8) & 0xff);
send_probe_data[18] = ((AccLine >> 16) & 0xff);
send_probe_data[19] = ((AccLine >> 24) & 0xff);
send_probe_data[20] = (DecCurve & 0xff);
send_probe_data[21] = ((DecCurve >> 8) & 0xff);
send_probe_data[22] = ((DecCurve >> 16) & 0xff);
send_probe_data[23] = ((DecCurve >> 24) & 0xff);
send_probe_data[24] = (DecLine & 0xff);
send_probe_data[25] = ((DecLine >> 8) & 0xff);
send_probe_data[26] = ((DecLine >> 16) & 0xff);
send_probe_data[27] = ((DecLine >> 24) & 0xff);
m_WriteByte = Send_Command(0, (const char*)send_probe_data, m_SendDataLength);
Sleep(5);
}
}
send_probe_data[0] = CT_MOTOR;
send_probe_data[1] = CT_START_LATCH;
send_probe_data[2] = motionAxis;
send_probe_data[3] = motionDir;
int returnPos = m_ProbeReturnPos / m_Resolution[AxisTypes];
send_probe_data[4] = (returnPos >> 8) & 0xff;
send_probe_data[5] = returnPos & 0xff;
send_probe_data[6] = (m_ProbeReturnSpeed >> 8) & 0xff;
send_probe_data[7] = m_ProbeReturnSpeed & 0xff;
m_WriteByte = Send_Command(0, (const char*)send_probe_data, m_SendDataLength);
Sleep(3);//
}
return rStatus;
}
//=============================读取配置==============================================
HSI_STATUS HSI_Motion::Load_EF3_Motion_Inifile(CString GoogolIniFile)
{
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
CString csAppPath = GoogolIniFile;
USES_CONVERSION;
CString temp = L"";
CString strGear[5] = { L"GEAR0_", L"GEAR1_", L"GEAR2_", L"GEAR3_", L"GEAR4_" };
CString axisNum[5] = { L"0", L"1", L"2", L"3", L"4" };
//判断Log目录是否存在，不存在就创建
if (CreateDirectory(m_AppPath + L"\\Log", NULL))
{
g_pLogger->SendAndFlushWithTime(L"[Load_EF3_Motion_Inifile] Create Log Directory\n");
CreateDirectory(m_AppPath + L"\\Log", NULL);
}
if (GetFileAttributes(GoogolIniFile) == -1)
{
g_pLogger->SendAndFlushWithTime(L"[Load_EF3_Motion_Inifile] EF3_Motion.ini file no find\n");
sEvenProp.Init();
sEvenProp.EventType = HSI_EVENT_ERROR;
sEvenProp.EventID = HSI_EVENT_MOTION;
sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK;
strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "EF3_Motion.ini文件不存在！");
EventCallback(sEvenProp);
return HSI_STATUS_FAILED;
}
m_motorType = GetPrivateProfileInt(L"MOTORTYPE", L"SERVOMOTOR", 1, csAppPath);
for (int i = 1; i < 5; i++)
{
m_precisionCount[i] = GetPrivateProfileInt(L"PRECISION", L"PRECISION_COUNT_" + axisNum[i], 14000, csAppPath);
m_precisionTime[i] = (long)GetPrivateProfileInt(L"PRECISION", L"PRECISION_TIME_" + axisNum[i], 14000, csAppPath);
GetPrivateProfileString(L"RESOLUTION", L"SCALE_RESOLUTION_" + axisNum[i], L"0.0004", temp.GetBufferSetLength(50), 50, csAppPath);
m_Resolution[i] = (atof(T2A(temp)));
GetPrivateProfileString(L"LIMIT", L"NEG_WORKING_LIMIT_" + axisNum[i], L"-40", temp.GetBufferSetLength(50), 50, csAppPath);
m_N_Work_Limit[i] = (atof(T2A(temp)));
GetPrivateProfileString(L"LIMIT", L"POS_WORKING_LIMIT_" + axisNum[i], L"160", temp.GetBufferSetLength(50), 50, csAppPath);
m_P_Work_Limit[i] = (atof(T2A(temp)));
m_Home_Time[i] = (long)GetPrivateProfileInt(L"HOME", L"HOME_TIME_" + axisNum[i], 1500, csAppPath);
m_Home_AddJogGears[i] = GetPrivateProfileInt(L"HOME", L"HOME_ADD_JOGCHOICE_" + axisNum[i], 100, csAppPath);
m_Home_DecJogGears[i] = GetPrivateProfileInt(L"HOME", L"HOME_DEC_JOGCHOICE_" + axisNum[i], 100, csAppPath);
m_SetPotion_Count[i] = GetPrivateProfileInt(L"SET_POSITION_SPEED", L"SET_POSITION_COUNT_" + axisNum[i], 10, csAppPath);
m_SetPotion_DriveSpeed[i] = GetPrivateProfileInt(L"SET_POSITION_SPEED", L"SET_POTION_DRIVESPEED_" + axisNum[i], 100, csAppPath);
m_SetPotion_DriveSpeed[i] = m_SetPotion_DriveSpeed[i] / (m_Resolution[i] * 50);
m_SetPotion_Line[i] = GetPrivateProfileInt(L"SET_POSITION_SPEED", L"SET_POTION_LINE_" + axisNum[i], 100, csAppPath);
m_SetPotion_StartSpeed[i] = GetPrivateProfileInt(L"SET_POSITION_SPEED", L"SET_POTION_STARTSPEED_" + axisNum[i], 100, csAppPath);
m_SetPotion_StartSpeed[i] = m_SetPotion_StartSpeed[i] / (m_Resolution[i] * 500);
m_SetPotion_Buffer[i] = GetPrivateProfileInt(L"SET_POSITION_SPEED", L"SET_POTION_BUFFER_" + axisNum[i], 105, csAppPath);
m_LogIsOpen[i] = GetPrivateProfileInt(L"LOG", L"LOG_IS_OPEN_" + axisNum[i], 0, csAppPath);
m_Home_Machine_Axis[i] = GetPrivateProfileInt(L"HOME", L"HOME_MACHINE_AXIS_" + axisNum[i], 0, csAppPath);
m_Home_Pos_Axis[i] = GetPrivateProfileInt(L"HOME", L"HOME_POS_AXIS_" + axisNum[i], 0, csAppPath);
m_StopJogMode[i] = GetPrivateProfileInt(L"JOG_SPEED", L"JOG_STOP_MODE_" + axisNum[i], 0, csAppPath);
m_SpeedAdjustPeriod[i] = GetPrivateProfileInt(L"SET_POSITION_SPEED", L"SPEED_ADJUSTMENT_" + axisNum[i], 1, csAppPath);
m_SpeedMax[i] = GetPrivateProfileInt(L"SET_POSITION_SPEED", L"SPEED_RUNMAX_" + axisNum[i], 150, csAppPath);
}
for (int i = 0; i < 5; i++)
{
for (int j = 1; j < 5; j++)
{
GetPrivateProfileString(L"JOG_SPEED", L"JOG_DRIVESPEED_" + strGear[i] + axisNum[j], L"10", temp.GetBufferSetLength(50), 10, csAppPath);
float speed = (atof(T2A(temp)));
//m_JogDriveSpeed[j][i] = GetPrivateProfileInt(L"JOG_SPEED", L"JOG_DRIVESPEED_" , 10, csAppPath);
m_JogDriveSpeed[j][i] = speed / (m_Resolution[j] * 50);
GetPrivateProfileString(L"JOG_SPEED", L"JOG_STARTSPEED_" + strGear[i] + axisNum[j], L"10", temp.GetBufferSetLength(50), 10, csAppPath);
speed = (atof(T2A(temp)));
//m_JogStartSpeed[j][i] = GetPrivateProfileInt(L"JOG_SPEED", L"JOG_STARTSPEED_" + strGear[i] + axisNum[j], 10, csAppPath);
m_JogStartSpeed[j][i] = speed / (m_Resolution[j] * 50);
m_JogAccLine[j][i] = GetPrivateProfileInt(L"JOG_SPEED", L"JOG_LINE_" + strGear[i] + axisNum[j], 10, csAppPath);
m_JogDecLine[j][i] = GetPrivateProfileInt(L"JOG_SPEED", L"JOG_LINE_" + strGear[i] + axisNum[j], 10, csAppPath);
}
}
m_Jog_Auto_Focus = m_JogDriveSpeed[3][4];
m_Home_Machine_Axis[1] = GetPrivateProfileInt(L"HOME", L"HOME_MACHINE_AXIS_1", 1, csAppPath);
m_Home_Machine_Axis[2] = GetPrivateProfileInt(L"HOME", L"HOME_MACHINE_AXIS_2", 1, csAppPath);
m_Home_Machine_Axis[3] = GetPrivateProfileInt(L"HOME", L"HOME_MACHINE_AXIS_3", 1, csAppPath);
m_Home_Machine_Axis[4] = GetPrivateProfileInt(L"HOME", L"HOME_MACHINE_AXIS_4", 1, csAppPath);
m_IsHomeEncPos = GetPrivateProfileInt(L"HOME", L"IS_HOME_ENC_POS", 0, csAppPath);
m_IsHomePrfPos = GetPrivateProfileInt(L"HOME", L"IS_HOME_PRF_POS", 1, csAppPath);
m_Set_XYZA_Reserve = GetPrivateProfileInt(L"SET_XYZ", L"SET_XYZ_RESERVE", 2, csAppPath);
m_setPositionDelay = GetPrivateProfileInt(L"SETPOSITION", L"SETPOSITION_DELAY", 1, csAppPath);
m_setPositionPrecision = GetPrivateProfileInt(L"SETPOSITION", L"SETPPSITION_PRECISION", 1, csAppPath);
m_setPositionNum = GetPrivateProfileInt(L"SETPOSITION", L"SETPOSITION_NUMBER", 1, csAppPath);
//两块四路光源板
m_isUseAport = GetPrivateProfileInt(L"COMPORT", L"IS_COM_PORT_A", 0, csAppPath);
m_portAnum = GetPrivateProfileInt(L"COMPORT", L"COM_PORT_A", 0, csAppPath);
m_isUseBport = GetPrivateProfileInt(L"COMPORT", L"IS_COM_PORT_B", 0, csAppPath);
m_portBnum = GetPrivateProfileInt(L"COMPORT", L"COM_PORT_B", 0, csAppPath);
m_iGlueStartSpeed = GetPrivateProfileInt(L"GLUEDISPENSER", L"DISPENSER_STARTSPEED", 0, csAppPath);
m_iGlueStartSpeed = m_iGlueStartSpeed / (m_Resolution[1] * 500);
m_iGlueDriveSpeed = GetPrivateProfileInt(L"GLUEDISPENSER", L"DISPENSER_DRIVESPEED", 0, csAppPath);
m_iGlueDriveSpeed = m_iGlueDriveSpeed / (m_Resolution[1] * 50);
m_iGlueAccSpeed = GetPrivateProfileInt(L"GLUEDISPENSER", L"DISPENSER_ACCSPEED", 0, csAppPath);
m_LogIsOpen[0] = GetPrivateProfileInt(L"LOG", L"LOG_IS_OPEN_0", 0, csAppPath);
g_pLogger->IsEnabledLog = m_LogIsOpen[0] == 1 ? true : false;
m_iSpeedType = GetPrivateProfileInt(L"SET_SPEED", L"SPEEDTYPE", 0, csAppPath);
CString comNum[5] = { L"0", L"1", L"2", L"3" };
for (int i = 0; i < 4; i++)
{
GetPrivateProfileString(L"ROCKER_SPEED", L"ROCKER_HSTARTSPEED_" + comNum[i], L"5", temp.GetBufferSetLength(50), 10, csAppPath);
float speed = (atof(T2A(temp)));
m_rockerHStartSpeed[i] = speed / (m_Resolution[i] * 50);
GetPrivateProfileString(L"ROCKER_SPEED", L"ROCKER_HDRIVESPEED_" + comNum[i], L"20", temp.GetBufferSetLength(50), 10, csAppPath);
speed = (atof(T2A(temp)));
m_rockerHDriveSpeed[i] = speed / (m_Resolution[i] * 50);
GetPrivateProfileString(L"ROCKER_SPEED", L"ROCKER_LSTARTSPEED_" + comNum[i], L"2", temp.GetBufferSetLength(50), 10, csAppPath);
speed = (atof(T2A(temp)));
m_rockerLStartSpeed[i] = speed / (m_Resolution[i] * 50);
GetPrivateProfileString(L"ROCKER_SPEED", L"ROCKER_LDRIVESPEED_" + comNum[i], L"10", temp.GetBufferSetLength(50), 10, csAppPath);
speed = (atof(T2A(temp)));
m_rockerLDriveSpeed[i] = speed / (m_Resolution[i] * 50);
GetPrivateProfileString(L"ROCKER_SPEED", L"ROCKER_ASPEED_" + comNum[i], L"100", temp.GetBufferSetLength(50), 10, csAppPath);
m_rockerASpeed[i] = (atoi(T2A(temp)));
GetPrivateProfileString(L"ROCKER_SPEED", L"ROCKER_DSPEED_" + comNum[i], L"100", temp.GetBufferSetLength(50), 10, csAppPath);
m_rockerDSpeed[i] = (atoi(T2A(temp)));
}
CString strAxis[10] = { L"0", L"1", L"2", L"3", L"4", L"5", L"6", L"7", L"8", L"9" };
for (int i = 0; i < 10; i++)
{
m_stepPosition_L_speed[i] = GetPrivateProfileInt(L"STEPMOTORPOSITIONINIT", L"StepPositionLSpeed_" + strAxis[i], 1, csAppPath);
m_stepPosition_L_speed[i] = m_stepPosition_L_speed[i] / (m_Resolution[1] * 50);
m_stepPosition_H_speed[i] = GetPrivateProfileInt(L"STEPMOTORPOSITIONINIT", L"StepPositionHSpeed_" + strAxis[i], 1, csAppPath);
m_stepPosition_H_speed[i] = m_stepPosition_H_speed[i] / (m_Resolution[1] * 50);
m_stepPosition_Load[i] = GetPrivateProfileInt(L"STEPMOTORPOSITIONINIT", L"StepPositionLoad_" + strAxis[i], 1, csAppPath);
m_stepPosition_Load[i] = m_stepPosition_Load[i] / (m_Resolution[1]);
m_stepPosition_acc[i] = GetPrivateProfileInt(L"STEPMOTORPOSITIONINIT", L"StepPositionAcc_" + strAxis[i], 1, csAppPath);
}
CString SubArea[8] = { L"0", L"1", L"2", L"3", L"4", L"5", L"6", L"7" };
for (int i = 0; i < 8; i++)
{
m_SixEightSubArea[i] = GetPrivateProfileInt(L"SIXEIGHTSUBAREA", L"SIXEIGHT_SUBARE_" + SubArea[i], 0, csAppPath);
}
}
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::Load_EF3_Config_Inifile(CString GoogolIniFile)
{
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
CString csAppPath = GoogolIniFile;
USES_CONVERSION;
CString temp;
if (GetFileAttributes(GoogolIniFile) == -1)
{
g_pLogger->SendAndFlushWithTime(L"[Load_EF3_Config_Inifile] EF3_Config.ini file no find\n");
sEvenProp.Init();
sEvenProp.EventType = HSI_EVENT_ERROR;
sEvenProp.EventID = HSI_EVENT_MOTION;
sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK;
strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "EF3_Config.ini文件不存在！");
EventCallback(sEvenProp);
return HSI_STATUS_FAILED;
}
m_IsUseEF3 = GetPrivateProfileInt(L"EF3", L"IS_USEEF3", 0, csAppPath);
m_ForSoft = GetPrivateProfileInt(L"SOFTWARE", L"USE_SOFTWARE", 0, csAppPath);
m_IsUse_HSICompensation = GetPrivateProfileInt(L"ASIX", L"IS_USE_HSICOMPENSATION", 0, csAppPath);
m_Compensation_Pluse = GetPrivateProfileInt(L"ASIX", L"COMPENSATE_PLUSE", 20, csAppPath);
m_IsHardLimit = GetPrivateProfileInt(L"ASIX", L"IS_HARD_LIMIT", 7, csAppPath);
m_IsEnableAxis = GetPrivateProfileInt(L"ASIX", L"ISENABLE_AXIS", 7, csAppPath);
m_IsHavePattern = GetPrivateProfileInt(L"ASIX", L"ISHAVE_AXISPATTERN", 15, csAppPath);
m_AxisHomeDirection = GetPrivateProfileInt(L"ASIX", L"AXIS_HOMEDIRECTION", 15, csAppPath);
m_IsUseManualRunin = GetPrivateProfileInt(L"ASIX", L"IS_USEMANUALRUNIN", 0, csAppPath);
m_IsUseExternalTrigger = GetPrivateProfileInt(L"FUNCTION", L"IS_USEEXTERNALTRIGGER", 1, csAppPath);
m_IsUseSixRingEightArea = GetPrivateProfileInt(L"FUNCTION", L"IS_USESIXRINGEIGHTAREA", 0, csAppPath);
m_IsUseTwentySixLight = GetPrivateProfileInt(L"FUNCTION", L"IS_USETWENTYSIXLIGHT", 0, csAppPath);
m_EF3LightType = GetPrivateProfileInt(L"FUNCTION", L"IS_LIGHTTYPE", 1, csAppPath);
m_IsCollectPos = GetPrivateProfileInt(L"FUNCTION", L"IS_COLLECT_POS", 0, csAppPath);
m_IsLightDebug = GetPrivateProfileInt(L"FUNCTION", L"IS_LIGHT_DEBUG", 0, csAppPath);
m_IsIOFuntion = GetPrivateProfileInt(L"IO", L"IS_IO_FUNTION", 0, csAppPath);
m_IsStartInput = GetPrivateProfileInt(L"IO", L"IS_START_INPUT", 0, csAppPath);
m_StartInputPort = GetPrivateProfileInt(L"IO", L"START_INPUT_PORT", 1, csAppPath);
m_IsProbe = GetPrivateProfileInt(L"PROBE", L"IS_PROBE", 0, csAppPath);
m_ProbeAllAxis = GetPrivateProfileInt(L"PROBE", L"PROBE_All_AXIS", 3, csAppPath);
m_ProbeReturnSpeed = GetPrivateProfileInt(L"PROBE", L"PROBE_RETURN_SPEED", 40, csAppPath);
m_ProbeReturnSpeed = m_ProbeReturnSpeed * 50;
GetPrivateProfileString(L"PROBE", L"PROBE_RETURN_POS", L"10.0", temp.GetBufferSetLength(50), 50, csAppPath);
m_ProbeReturnPos = atof(T2A(temp));
m_IsUseRocker = GetPrivateProfileInt(L"ROCKER", L"IS_USEROCKER", 0, csAppPath);
m_IsCloseRocker = GetPrivateProfileInt(L"ROCKER", L"IS_CLOSEROCKER", 0, csAppPath);
m_bISUseMoreLights = GetPrivateProfileInt(L"IPADDRESS", L"IS_USENUM", 0, csAppPath);
m_LightType = GetPrivateProfileInt(L"LIGHTCONTROL", L"IS_LIGHTTYPE", 1, csAppPath);
m_IsUseFourthSpeed = GetPrivateProfileInt(L"FOURTHSPEED", L"IS_USEFOURTHSPEED", 1, csAppPath);
m_isOKGlint = GetPrivateProfileInt(L"LIGHTCONTROL", L"IS_OKGLINT", 0, csAppPath);
m_ETIPort = GetPrivateProfileInt(L"LIGHTCONTROL", L"IS_ETIPORT", 0, csAppPath);
m_DeviceType = GetPrivateProfileInt(L"DEVICE", L"DEVICE_TYPE", 1, csAppPath);
m_UseAxisNum = GetPrivateProfileInt(L"DEVICE", L"USE_AXISNUM", 0, csAppPath);
m_iJoyStick = GetPrivateProfileInt(L"JOYSTICK", L"JOY_STICK_TYPE", 0, csAppPath);
CString comNum[5] = { L"0", L"1", L"2", L"3" };
for (int i = 0; i < 4; i++)
{
GetPrivateProfileString(L"IPADDRESS", L"IP_ADDRESS_" + comNum[i], L"192.168.0.1", temp.GetBufferSetLength(50), 50, csAppPath);
m_IsOpenTCPIP[i] = temp;
temp = temp.Trim();
int iNext = temp.Find(':');
CString ss = temp.Mid(0, iNext);
if (ss == "8")
{
m_Led8MotionFlag[i] = true;
}
m_IsOpenTCPIP[i] = temp.Mid(iNext + 1, temp.GetLength() - iNext);
}
m_EF3COMPort = GetPrivateProfileInt(L"EF3COM", L"COMPORT", 0, csAppPath);
}
return rStatus;
}
//===============================读取/设置光栅尺精度============================================
HSI_STATUS HSI_Motion::GetScaleResolution(double &_ScaleX, double &_ScaleY, double &_ScaleZ)
{
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
_ScaleX = m_Resolution[1];
_ScaleY = m_Resolution[2];
_ScaleZ = m_Resolution[3];
}
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::SetScaleResolution(double _ScaleX, double _ScaleY, double _ScaleZ)
{
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
m_Resolution[1] = (int)(_ScaleX * 10000);
m_Resolution[2] = (int)(_ScaleY * 10000);
m_Resolution[3] = (int)(_ScaleZ * 10000);
}
return rStatus;
}
//============================回调定位完成===============================================
void HSI_Motion::SendMsgMotionFinished()
{
sEvenProp.Init();
sEvenProp.EventType = HSI_EVENT_FUNCTION;
sEvenProp.EventID = HSI_EVENT_MOVE_POINT;
sEvenProp.EventResponse = HSI_EVENT_FUNCTION_OK;
EventCallback(sEvenProp);
}
//=============================回调探针运行==============================================
void HSI_Motion::SendMsgProbeFinished()
{
sEvenProp.Init();
sEvenProp.EventType = HSI_EVENT_FUNCTION;
sEvenProp.EventID = HSI_EVENT_MOTION_PROBE;
sEvenProp.EventResponse = HSI_EVENT_FUNCTION_OK;
EventCallback(sEvenProp);
}
//===========================================================================
void HSI_Motion::UpdateMotionState()
{
if (g_pHSI_Motion && m_IsExMotion == 0)
{
g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] In\n");
while (m_Thread_State == HSI_THREAD_RUNNING)
{
g_IsClose = false;
bool interpolationflag = false;
bool timeoutflag = false;
int timeStart = GetTickCount();
// bool singleaxisflag_x = false;
// bool singleaxisflag_y = false;
// bool singleaxisflag_z = false;
// bool singleaxisflag_a = false;
do
{
if (g_IsClose)
{
g_IsClose = false;
break;
}
if (m_SO7_Serial.m_RecvData[0] == 2)
{
if ((m_SO7_Serial.m_RecvData[39] & 0x10))
{
m_SO7_Serial.m_RecvData[39] = 0;
SpCompleteTStart = GetTickCount();
interpolationflag = true;
break;
}
//else if ((m_SO7_Serial.m_RecvData[39] == 0x07))//axis_start
else if ((m_SO7_Serial.m_RecvData[39] == axis_start)/* &&(m_motorType == 0)*/)
{
m_SO7_Serial.m_RecvData[39] = 0;
SpCompleteTStart = GetTickCount();
interpolationflag = true;
break;
}
else if (m_SO7_Serial.m_RecvData[39] & 0x20)
{
m_SO7_Serial.m_RecvData[39] = 0;
timeoutflag = true;
break;
}
}
Sleep(1);
int timeEnd = GetTickCount();
if (timeStart - timeEnd > 10 * 1000)
{
timeoutflag = true;
break;
}
} while (true);
g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Nowait Run End\n");
unsigned int tempPrecision[5] = { 0, m_precisionCount[1], m_precisionCount[2], m_precisionCount[3], m_precisionCount[4] };
tempPrecision[1] = m_Home_Machine_Axis[1] == 0 ? 10000000 : m_precisionCount[1];
tempPrecision[2] = m_Home_Machine_Axis[2] == 0 ? 10000000 : m_precisionCount[2];
tempPrecision[3] = m_Home_Machine_Axis[3] == 0 ? 10000000 : m_precisionCount[3];
tempPrecision[4] = m_Home_Machine_Axis[4] == 0 ? 10000000 : m_precisionCount[4];
int i = 0, j = 0;
unsigned long Count = 0;
double prfpos[5] = { 0 };
//GetPositionEncPrfMulti(1, m_EncPos, m_PrfPos, 4);
//double EncPos[5] = { 0.0 };
// double ProPulse[5] = { 0.0 };
if (interpolationflag&&m_motorType)
{
while (Count < m_SetPotion_Count[1])//到位次数判断
{
Sleep(2);
GetPositionXyz(HSI_MOTION_AXIS_ALL, prfpos[1], prfpos[2], prfpos[3], prfpos[0]);
if ((fabs(m_PosThread[1] - prfpos[1]) <= tempPrecision[1] * m_Resolution[1]) && (fabs(m_PosThread[2] - prfpos[2]) <= tempPrecision[2] * m_Resolution[2]) && (fabs(m_PosThread[3] - prfpos[3]) <= tempPrecision[3] * m_Resolution[3]) && (fabs(m_PosThread[4] - m_PosForAllAxis[4]) <= tempPrecision[4] * m_Resolution[4]))
{
i++;
if (m_SetPotion_Count[1] > m_setPositionNum)
{
j = m_setPositionNum;
}
else
{
j = m_SetPotion_Count[1];
}
if (i == j)
{
set_end = GetTickCount();
break;
}
}
else
{
}
Count++;
g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] m_SetPotion_Count = %d\n", Count);
}
//if (Count == m_SetPotion_Count[1]) //超时退出
//{
// if (g_IsClose == false)
// {
// g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Nowait Timeout\n");
// sEvenProp.Init();
// sEvenProp.EventType = HSI_EVENT_ERROR;
// sEvenProp.EventID = HSI_EVENT_MOTION;
// sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK;
// strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "Nowait_HSI定位超时!");
// EventCallback(sEvenProp);
// }
// else
// {
// g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Nowait Timeout Is AbortMotion\n");
// }
//}
}
if (interpolationflag)
{
SpCompleteTEnd = GetTickCount();
SpTimeCount = SpCompleteTEnd - SpCompleteTStart;
double dis = pow(m_PrfPos[1] - m_EncPos[1], 2.0) + pow(m_PrfPos[2] - m_EncPos[2], 2.0) + pow(m_PrfPos[3] - m_EncPos[3], 2.0);
if (dis > 0)
{
PntToPntDistance = sqrt(dis);
}
else
{
PntToPntDistance = 0.0;
}
SetPotionRunEnd = true;
}
g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] m_PosThread[1] = %.4f,m_PosThread[2] = %.4f,m_PosThread[3] = %.4f\n", m_PosThread[1], m_PosThread[2], m_PosThread[3]);
g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Pos[1] = %.4f,Pos[2] = %.4f,Pos[3] = %.4f\n", prfpos[1], prfpos[2], prfpos[3]);
if (timeoutflag)
{
g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Nowait Timeout\n");
sEvenProp.Init();
sEvenProp.EventType = HSI_EVENT_ERROR;
sEvenProp.EventID = HSI_EVENT_MOTION;
sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK;
strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "Nowait_EF3定位超时!");
EventCallback(sEvenProp);
}
switch (CurrentMotionState)
{
case E_SO7_MOTION_MOVETO:
{
g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Nowait CurrentMotionState E_SO7_MOTION_MOVETO\n");
m_Thread_State = HSI_THREAD_PAUSED;
CurrentMotionState = E_SO7_MOTION_NONE;
m_IsExMotion = 2;
SendMsgMotionFinished();
break;
}
default:
{
g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Nowait CurrentMotionState default\n");
break;
}
}
}
g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Out\n");
}
}
//===========================================================================
void HSI_Motion::UpdateMotionStateEx()
{
if (g_pHSI_Motion && m_IsExMotion == 1)
{
g_pLogger->SendAndFlushWithTime(L"[UpdateMotionStateEx] In\n");
g_pLogger->SendAndFlushWithTime(L"[UpdateMotionStateEx] Out\n");
}
}
//===========================================================================
void HSI_Motion::UpdateMotionStateIO()
{
if (g_pHSI_Motion)
{
int StartFlag = 0;
UINT recvData = 0;
while (m_Thread_StateIO == HSI_THREAD_RUNNING)
{
//1个脚踏开关
Sleep(3);
GetDIO(HSI_MOTION_INPUT_CH1, m_InputStatus);
if (m_IsStartInput == 1)
{
if ((m_InputStatus & m_StartInputPort) == 0)
{
StartFlag = 1;
}
else if (StartFlag == 1 && (m_InputStatus & m_StartInputPort) == m_StartInputPort)
{
if (m_isOKGlint == 1)
{
memset(m_cSendData, 0x00, 63);
m_cSendData[0] = 0x02;
m_cSendData[1] = 0x02;
m_cSendData[2] = 0x01;
m_cSendData[6] = 0x04;
m_cSendData[7] = 0xff;
m_cSendData[8] = 0xff;
m_cSendData[9] = 0xff;
m_cSendData[10] = 0xff;
m_cSendData[14] = 0xff;
m_WriteByte = Send_Command(1, (const char*)m_cSendData, m_SendDataLength);
Sleep(5);
}
g_pLogger->SendAndFlushWithTime(L"[UpdateMotionStateIO] One Foot Switch Trigger\n");
StartFlag = 0;
sEvenProp.Init();
sEvenProp.EventType = HSI_EVENT_NOTIFY;
sEvenProp.EventID = HSI_NOTIFY_PROGRAM_EXECUTION_START;
sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK;
EventCallback(sEvenProp);
}
}
if ((m_axisStatus & 0xAA) == 0xAA)
{
if (!m_bEmergencyState)
{
Sleep(100);
GetDIO(HSI_MOTION_INPUT_CH1, m_InputStatus);
if ((m_axisStatus & 0xAA) == 0xAA)
{
sEvenProp.Init();
sEvenProp.EventType = HSI_EVENT_NOTIFY;
sEvenProp.EventID = HSI_NOTIFY_EMERGENCY_STATE;
sEvenProp.EventData[0] = 1;
m_bEmergencyState = true;
EventCallback(sEvenProp);
}
}
}
else
{
if (m_bEmergencyState)
{
Sleep(100);
GetDIO(HSI_MOTION_INPUT_CH1, m_InputStatus);
if ((m_axisStatus & 0xAA) != 0xAA)
{
sEvenProp.Init();
sEvenProp.EventType = HSI_EVENT_NOTIFY;
sEvenProp.EventID = HSI_NOTIFY_EMERGENCY_STATE;
sEvenProp.EventData[0] = 0;
m_bEmergencyState = false;
EventCallback(sEvenProp);
}
}
}
}
}
}
//===========================================================================
void HSI_Motion::UpdateMotionStateData()
{
while (m_Thread_StateData == HSI_THREAD_RUNNING)
{
if (m_bConnected)
{
Sleep(3);
m_cSendData[0] = CT_ORDER;
m_cSendData[1] = MOTORPOSITION;
Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
}
else
{
}
}
}
//===========================================================================
void HSI_Motion::UpdateMotionStateProbe()
{
if (g_pHSI_Motion)
{
bool hitFlag = false;
while (m_Thread_StateProbe == HSI_THREAD_RUNNING)
{
Sleep(3);
if (g_IsClose)
{
g_IsClose = false;
break;
}
if (m_SO7_Serial.m_RecvData[57] == 0)
{
hitFlag = true;
}
else if (m_SO7_Serial.m_RecvData[57] == 1)
{
//AbortMotion();
//Beep(300,1000);
//printf("\a");
// m_SO7_Serial.m_RecvData[57] = 0;
m_LockPos[1] = (m_SO7_Serial.m_RecvData[41] << 24 | m_SO7_Serial.m_RecvData[42] << 16 | m_SO7_Serial.m_RecvData[43] << 8 | m_SO7_Serial.m_RecvData[44])*m_Resolution[1];
m_LockPos[2] = (m_SO7_Serial.m_RecvData[45] << 24 | m_SO7_Serial.m_RecvData[46] << 16 | m_SO7_Serial.m_RecvData[47] << 8 | m_SO7_Serial.m_RecvData[48])*m_Resolution[2];
m_LockPos[3] = (m_SO7_Serial.m_RecvData[49] << 24 | m_SO7_Serial.m_RecvData[50] << 16 | m_SO7_Serial.m_RecvData[51] << 8 | m_SO7_Serial.m_RecvData[52])*m_Resolution[3];
m_LockPos[4] = (m_SO7_Serial.m_RecvData[53] << 24 | m_SO7_Serial.m_RecvData[54] << 16 | m_SO7_Serial.m_RecvData[55] << 8 | m_SO7_Serial.m_RecvData[56])*m_Resolution[4];
continue;
}
else if (m_SO7_Serial.m_RecvData[57] == 2)
{
Sleep(400);
m_LockPos[1] = 0.0;
m_LockPos[2] = 0.0;
m_LockPos[3] = 0.0;
m_LockPos[4] = 0.0;
//m_SO7_Serial.m_RecvData[57] == 0;
continue;
}
else if (m_SO7_Serial.m_RecvData[57] == 3 && hitFlag)
{
Sleep(200);
hitFlag = false;
double ProbeHitCompletePos[4] = { 0 };
GetPositionXyz(0, ProbeHitCompletePos[0], ProbeHitCompletePos[1], ProbeHitCompletePos[2], ProbeHitCompletePos[3]);
m_ijk[0] = ProbeHitCompletePos[0];
m_ijk[1] = ProbeHitCompletePos[1];
m_ijk[2] = ProbeHitCompletePos[2];
g_pLogger->SendAndFlushWithTime(L"[UpdateMotionStateProbe] Probe hit\n");
SendMsgProbeFinished();
}
}
}
}
//===========================================================================
void HSI_Motion::DoEvents()
{
MSG msg;
while (PeekMessage(&msg, NULL, 0, 0, PM_REMOVE))
{
__try
{
DispatchMessage(&msg);
TranslateMessage(&msg);
}
__except (EXCEPTION_EXECUTE_HANDLER)
{
g_pLogger->SendAndFlushWithTime(msg.message + L"----------DoEvents---------- : Exception");
sEvenProp.Init();
sEvenProp.EventType = HSI_EVENT_ERROR;
sEvenProp.EventID = HSI_EVENT_MOTION;
sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK;
strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "DoEvents_异常");
EventCallback(sEvenProp);
}
if (g_IsClose)
{
break;
}
}
}
//===========================================================================
HSI_STATUS HSI_Motion::DriverAlarmStatus()
{
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
g_pLogger->SendAndFlushWithTime(L"[DriverAlarmStatus] In\n");
g_pLogger->SendAndFlushWithTime(L"[DriverAlarmStatus] Out\n");
}
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::FirstHome()
{
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
g_pLogger->SendAndFlushWithTime(L"[FirstHome] In\n");
g_pLogger->SendAndFlushWithTime(L"[FirstHome] Out\n");
}
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::SpecialMotorHome(short AxisNum)
{
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
g_pLogger->SendAndFlushWithTime(L"[SpecialMotorHome] In\n");
g_pLogger->SendAndFlushWithTime(L"[SpecialMotorHome] Out\n");
}
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::SpecialMotorMove(short AxisNumber, double Position)
{
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
g_pLogger->SendAndFlushWithTime(L"[SpecialMotorMove] In\n");
g_pLogger->SendAndFlushWithTime(L"[SpecialMotorMove] Out\n");
}
return rStatus;
}
//========================IO===================================================
HSI_STATUS HSI_Motion::GetDIO(UINT IOChannel, UINT& _Status)
{
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
if (m_SO7_Serial.m_RecvData[0] == 2)
{
if (IOChannel == HSI_MOTION_INPUT_LIMIT_SWITCH)
{
_Status = m_SO7_Serial.m_RecvData[33];
}
if (IOChannel == HSI_MOTION_INPUT_ALARM)
{
_Status = m_SO7_Serial.m_RecvData[40];
}
if (IOChannel == HSI_MOTION_INPUT_CH1) //获取通用输入
{
_Status = m_SO7_Serial.m_RecvData[34];
_Status = (m_SO7_Serial.m_RecvData[35] | (_Status << 8)) & 0xffff;
}
if (IOChannel == HSI_MOTION_OUTPUT_CH1) //获取通用输出
{
_Status = m_SO7_Serial.m_RecvData[36];
_Status = (m_SO7_Serial.m_RecvData[37] | (_Status << 8)) & 0xffff;
}
m_axisStatus = m_SO7_Serial.m_RecvData[58];
m_axisAlarmStatus = m_SO7_Serial.m_RecvData[59];
}
else if (m_bISUseMoreLights > 0)
{
if (m_Led8MotionFlag[m_selectedIndex])
{
if (tReciveData[0] == 0x05)
{
_Status = tReciveData[2];
}
}
else
{
if (tReciveData[0] == 0x05)
{
_Status = tReciveData[1] << 24 | tReciveData[1] << 16 | tReciveData[3] << 8 | tReciveData[4];
}
}
}
else
{
g_pLogger->SendAndFlushWithTime(L"[GetDIO] failed\n");
}
}
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::SetDIO(UINT IOChannel, UINT _Status)
{
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
if (m_bISUseMoreLights > 0 && (m_ForStatus != _Status))
{
if (m_Led8MotionFlag[m_selectedIndex])
{
m_ForStatus = _Status;
IOdata[0] = 0x02;
IOdata[1] = 0x02;
IOdata[2] = 0x01;
IOdata[6] = 0x04;
IOdata[7] = 0xff;
IOdata[8] = 0xff;
IOdata[9] = 0xff;
IOdata[10] = 0xff;
IOdata[14] = _Status & 0xff;
IOSend++;
}
else
{
m_ForStatus = _Status;
IOdata[0] = 0x02;
IOdata[1] = 0x02;
IOdata[2] = 0x01;
IOdata[3] = 0x36;
IOdata[4] = 0x04;
IOdata[7] = 0xff;
IOdata[8] = 0xff;
IOdata[9] = 0xff;
IOdata[10] = 0xff;
IOdata[11] = (_Status >> 24) & 0xff;
IOdata[12] = (_Status >> 16) & 0xff;
IOdata[13] = (_Status >> 8) & 0xff;
IOdata[14] = _Status & 0xff;
IOSend++;
}
}
if ((IOChannel == HSI_MOTION_OUTPUT_CH1)/* && (m_ForStatus != _Status)*/)
{
m_ForStatus = _Status;
m_cSendData[0] = CT_PORT;
m_cSendData[1] = 0x02;
m_cSendData[2] = (_Status >> 8) & 0xff;
m_cSendData[3] = _Status & 0xff;
m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
Sleep(5);
}
}
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::GetAxisStatus(int* _Status)
{
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
_Status[0] = m_axisStatus;
_Status[1] = m_axisAlarmStatus;
}
return rStatus;
}
//=============================暂停和关闭==============================================
HSI_STATUS HSI_Motion::AbortMotion()
{
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
g_IsClose = true;
g_pLogger->SendAndFlushWithTime(L"[AbortMotion] In\n");
m_cSendData[0] = CT_ORDER;
m_cSendData[1] = CT_STOP;
m_cSendData[2] = AXIS_XYZU;
m_StopJogMode[1] == 0 ? m_cSendData[3] = 0X4A : m_cSendData[3] = 0X05;
m_StopJogMode[2] == 0 ? m_cSendData[3] = 0X4A : m_cSendData[3] = 0X05;
m_StopJogMode[3] == 0 ? m_cSendData[3] = 0X4A : m_cSendData[3] = 0X05;
m_StopJogMode[4] == 0 ? m_cSendData[3] = 0X4A : m_cSendData[3] = 0X05;
m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
Sleep(5);
if (m_DeviceType == 3)
{
m_cSendData[0] = CT_TURNTABLE;
m_cSendData[1] = CT_RTSTOP;
m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
Sleep(5);
m_Thread_StateData = HSI_THREAD_RUNNING;
SetEvent(m_hTriggerEventData);
}
if (bUseGlueDispenser)
{
bUseGlueDispenser = false;
m_cSendData[0] = CT_MOTOR;
m_cSendData[1] = CT_GLUEDISPENSER_STOP;
m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
Sleep(5);
}
g_pLogger->SendAndFlushWithTime(L"[AbortMotion] Out\n");
}
return rStatus;
}
//===============================关闭============================================
HSI_STATUS HSI_Motion::Shutdown()
{
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
g_pLogger->SendAndFlushWithTime(L"[Shutdown] In\n");
g_IsClose = true;
SetDIO(HSI_MOTION_OUTPUT_CH1, 0xffff);
if (m_bConnected)
{
CloseHandle(hCom);
hCom = nullptr;
//m_SO7_Serial.Close();
}
if (m_IsCloseRocker == 0)
{
memset(m_cSendData, 0x00, 64);
m_cSendData[0] = CT_SOFTSTOP;
m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
Sleep(5);
}
if (m_bISUseMoreLights > 0)
{
for (size_t i = 0; i < m_bISUseMoreLights; i++)
{
if (m_Led8MotionFlag[i])
{
m_selectedIndex = i;
Orderdata[0] = 0x01;
Orderdata[1] = 0x07;
Orderdata[2] = 0x00;
OrderSend++;
}
Sleep(20);
}
DisConnect();
}
AbortMotion();
m_bConnected = false;
CurrentMotionState = E_SO7_MOTION_NONE;
CurrentHomeMachineState = E_EF3_HOME_NONE;
m_Thread_StateData = HSI_THREAD_EXIT;
m_Thread_StateData = HSI_THREAD_EXIT;
m_Thread_StateIO = HSI_THREAD_EXIT;
m_Thread_State = HSI_THREAD_EXIT;
m_Thread_StateJOGStop = HSI_THREAD_EXIT;
SendMsgMotionFinished();
CloseThread();
CloseThreadIO();
CloseThreadData();
CloseThreadProbe();
CloseThreadJOGStop();
ReleaseMutex(g_RW_Data_Mutex);
CloseHandle(g_RW_Data_Mutex);
ReleaseMutex(g_WR_ToMove_Mutex);
CloseHandle(g_WR_ToMove_Mutex);
g_pLogger->SendAndFlushWithTime(L"[Shutdown] Out\n");
}
return rStatus;
}
//==============================触发灯光=============================================
HSI_STATUS HSI_Motion::SetTriggerLight(int triggleNum, int delayLighting, int delayLightBefor, int triggleMode, double* Intensities)
{
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
setLightFlag = true;
unsigned char m_SetTriggerLightData[64] = { 0 };
m_SetTriggerLightData[0] = CT_LIGHT;
m_SetTriggerLightData[1] = 0x03;
m_SetTriggerLightData[2] = triggleNum;
int num = triggleNum / 3;
int i = 0;
for (i = 0; i < num; i++)
{
m_SetTriggerLightData[3] = i;
int z = 4;
for (int j = 24 * i; j < 24 * (i + 1); ++j)
{
m_SetTriggerLightData[z++] = ((int(Intensities[j] * 11.2)) >> 8) & 0xff;
m_SetTriggerLightData[z++] = (int(Intensities[j] * 11.2)) & 0xff;
}
m_SetTriggerLightData[z++] = delayLighting & 0xff;
m_SetTriggerLightData[z++] = triggleMode & 0xff;
m_SetTriggerLightData[z] = delayLightBefor & 0xff;
m_WriteByte = Send_Command(0, (const char*)m_SetTriggerLightData, m_SendDataLength);
Sleep(1);
}
if ((num > 0) && (triggleNum % 3 != 0))
{
m_SetTriggerLightData[3] = i;
int z = 4;
for (int j = 24 * i; j < 8 * triggleNum; ++j)
{
m_SetTriggerLightData[z++] = ((int(Intensities[j] * 11.2)) >> 8) & 0xff;
m_SetTriggerLightData[z++] = (int(Intensities[j] * 11.2)) & 0xff;
}
m_SetTriggerLightData[52] = delayLighting & 0xff;
m_SetTriggerLightData[53] = triggleMode & 0xff;
m_SetTriggerLightData[54] = delayLightBefor & 0xff;
m_WriteByte = Send_Command(0, (const char*)m_SetTriggerLightData, m_SendDataLength);
Sleep(1);
}
else if (num == 0)
{
m_SetTriggerLightData[3] = 0;
int z = 4;
for (int j = 0; j < 8 * triggleNum; ++j)
{
m_SetTriggerLightData[z++] = ((int(Intensities[j] * 11.2)) >> 8) & 0xff;
m_SetTriggerLightData[z++] = (int(Intensities[j] * 11.2)) & 0xff;
}
m_SetTriggerLightData[52] = delayLighting & 0xff;
m_SetTriggerLightData[53] = triggleMode & 0xff;
m_SetTriggerLightData[54] = delayLightBefor & 0xff;
m_WriteByte = Send_Command(0, (const char*)m_SetTriggerLightData, m_SendDataLength);
Sleep(1);
}
}
return rStatus;
}
//=============================硬件触发拍照==============================================
HSI_STATUS HSI_Motion::DCCPPStartPoint(double *startPoint)
{
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
m_IsUsePPS = true;
begin_position[1] = (int)(startPoint[1] / m_Resolution[1]);
begin_position[2] = (int)(startPoint[2] / m_Resolution[2]);
begin_position[3] = (int)(startPoint[3] / m_Resolution[3]);
}
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::DCCScanSetData(UINT AxisTypes, HSI_SCAN_MOTION_TYPE eType, UINT lTrigNumber, double* dTrigDis)
{
WaitForSingleObject(g_WR_ToMove_Mutex, INFINITE);
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
//触发的位置为相对位置,用法一般是移动到起点位置，再开始设置触发位置(相对位置)，最终设置终点位置
g_pLogger->SendAndFlushWithTime(L"[DCCScanSetData] In\n");
int axisNum;
unsigned char m_SendDCCData[64] = { 0 };
m_SendDCCData[0] = CT_MOTOR;
m_SendDCCData[1] = CT_TRIGGER_DATA;
m_SendDCCData[2] = 0x01;
iTriggleNum = lTrigNumber;
double limit = 0.0;
//memset(m_cSendData,0,64);
if (AxisTypes & HSI_MOTION_AXIS_X)
{
iaxisNum = AXIS_X;
axisNum = 1;
}
else if (AxisTypes & HSI_MOTION_AXIS_Y)
{
iaxisNum = AXIS_Y;
axisNum = 2;
}
else if (AxisTypes & HSI_MOTION_AXIS_Z)
{
iaxisNum = AXIS_Z;
axisNum = 3;
}
else
{
iaxisNum = AXIS_X;
axisNum = 1;
}
switch (eType)
{
case HSI_SCAN_MOTION_SPEC_LOCA:
{
if (dTrigDis[0] > 0.0001)
{
iMotionDirection = 0;
}
else
{
iMotionDirection = 1;
}
switch (iaxisNum)
{
case AXIS_X:
{
if (!m_IsUsePPS)
{
begin_position[1] = begin_position[1] + dTrigDis[0] / m_Resolution[axisNum];
}
else
{
if (iMotionDirection == 0)
{
begin_position[1] = begin_position[1] - 20;
}
else
{
begin_position[1] = begin_position[1] + 20;
}
}
if (iMotionDirection == 0)
{
limit = 1000.0;
}
else
{
limit = -1000.0;
}
break;
}
case AXIS_Y:
{
if (!m_IsUsePPS)
{
begin_position[2] = begin_position[2] + dTrigDis[0] / m_Resolution[axisNum];
}
else
{
if (iMotionDirection == 0)
{
begin_position[2] = begin_position[2] - 20;
}
else
{
begin_position[2] = begin_position[2] + 20;
}
}
if (iMotionDirection == 0)
{
limit = 1000.0;
}
else
{
limit = -1000.0;
}
break;
}
case AXIS_Z:
{
if (!m_IsUsePPS)
{
begin_position[3] = begin_position[3] + dTrigDis[0] / m_Resolution[axisNum];
}
else
{
if (iMotionDirection == 0)
{
begin_position[3] = begin_position[3] - 20;
}
else
{
begin_position[3] = begin_position[3] + 20;
}
}
if (iMotionDirection == 0)
{
limit = 1000.0;
}
else
{
limit = -1000.0;
}
break;
}
default:
break;
}
iScanMotionType = HSI_SCAN_MOTION_SPEC_LOCA;
if (lTrigNumber > 100)
{
return HSI_STATUS_FAILED;
}
else
{
int num = lTrigNumber / 14;
int i = 0;
for (i = 0; i < num; i++)
{
m_SendDCCData[3] = i;
for (size_t j = 14 * i, z = 4; j < 14 * (i + 1); ++j)
{
m_SendDCCData[z++] = ((long)(dTrigDis[j] / m_Resolution[axisNum])) & 0xff;
m_SendDCCData[z++] = (((long)(dTrigDis[j] / m_Resolution[axisNum])) >> 8) & 0xff;
m_SendDCCData[z++] = (((long)(dTrigDis[j] / m_Resolution[axisNum])) >> 16) & 0xff;
m_SendDCCData[z++] = ((long)(dTrigDis[j] / m_Resolution[axisNum])) >> 24;
}
m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
Sleep(1);
}
if ((num > 0) && (lTrigNumber % 14 != 0))
{
m_SendDCCData[3] = i;
int j = 0, z = 0;
for (j = 14 * i, z = 4; j < lTrigNumber; ++j)
{
m_SendDCCData[z++] = ((long)(dTrigDis[j] / m_Resolution[axisNum])) & 0xff;
m_SendDCCData[z++] = (((long)(dTrigDis[j] / m_Resolution[axisNum])) >> 8) & 0xff;
m_SendDCCData[z++] = (((long)(dTrigDis[j] / m_Resolution[axisNum])) >> 16) & 0xff;
m_SendDCCData[z++] = ((long)(dTrigDis[j] / m_Resolution[axisNum])) >> 24;
}
m_SendDCCData[z++] = ((long)(limit / m_Resolution[axisNum])) & 0xff;
m_SendDCCData[z++] = (((long)(limit / m_Resolution[axisNum])) >> 8) & 0xff;
m_SendDCCData[z++] = (((long)(limit / m_Resolution[axisNum])) >> 16) & 0xff;
m_SendDCCData[z++] = ((long)(limit / m_Resolution[axisNum])) >> 24;
m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
Sleep(10);
}
else if (num == 0)
{
m_SendDCCData[3] = 0;
int j = 0, z = 0;
for (j = 0, z = 4; j < lTrigNumber; ++j)
{
m_SendDCCData[z++] = ((long)(dTrigDis[j] / m_Resolution[axisNum])) & 0xff;
m_SendDCCData[z++] = (((long)(dTrigDis[j] / m_Resolution[axisNum])) >> 8) & 0xff;
m_SendDCCData[z++] = (((long)(dTrigDis[j] / m_Resolution[axisNum])) >> 16) & 0xff;
m_SendDCCData[z++] = ((long)(dTrigDis[j] / m_Resolution[axisNum])) >> 24;
}
m_SendDCCData[z++] = ((long)(limit / m_Resolution[axisNum])) & 0xff;
m_SendDCCData[z++] = (((long)(limit / m_Resolution[axisNum])) >> 8) & 0xff;
m_SendDCCData[z++] = (((long)(limit / m_Resolution[axisNum])) >> 16) & 0xff;
m_SendDCCData[z++] = ((long)(limit / m_Resolution[axisNum])) >> 24;
m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
Sleep(10);
}
}
break;
}
case HSI_SCAN_MOTION_EQ_DIS:
{
if (dTrigDis[0] > 0.0001)
{
iMotionDirection = 0;
}
else
{
iMotionDirection = 1;
}
switch (iaxisNum)
{
case AXIS_X:
{
if (!m_IsUsePPS)
{
begin_position[1] = begin_position[1] + dTrigDis[0] / m_Resolution[axisNum];
}
else
{
if (iMotionDirection == 0)
{
begin_position[1] = begin_position[1] - 20;
}
else
{
begin_position[1] = begin_position[1] + 20;
}
}
if (iMotionDirection == 0)
{
limit = 1000;
}
else
{
limit = -1000;
}
break;
}
case AXIS_Y:
{
if (!m_IsUsePPS)
{
begin_position[2] = begin_position[2] + dTrigDis[0] / m_Resolution[axisNum];
}
else
{
if (iMotionDirection == 0)
{
begin_position[2] = begin_position[2] - 20;
}
else
{
begin_position[2] = begin_position[2] + 20;
}
}
if (iMotionDirection == 0)
{
limit = 1000;
}
else
{
limit = -1000;
}
break;
}
case AXIS_Z:
{
if (!m_IsUsePPS)
{
begin_position[3] = begin_position[3] + dTrigDis[0] / m_Resolution[axisNum];
}
else
{
if (iMotionDirection == 0)
{
begin_position[3] = begin_position[3] - 20;
}
else
{
begin_position[3] = begin_position[3] + 20;
}
}
if (iMotionDirection == 0)
{
limit = 1000;
}
else
{
limit = -1000;
}
break;
}
default:
break;
}
iScanMotionType = HSI_SCAN_MOTION_EQ_DIS;
m_SendDCCData[3] = 0x00;
m_SendDCCData[4] = ((long)(dTrigDis[0] / m_Resolution[axisNum])) & 0xff;
m_SendDCCData[5] = (((long)(dTrigDis[0] / m_Resolution[axisNum])) >> 8) & 0xff;
m_SendDCCData[6] = (((long)(dTrigDis[0] / m_Resolution[axisNum])) >> 16) & 0xff;
m_SendDCCData[7] = ((long)(dTrigDis[0] / m_Resolution[axisNum])) >> 24;
m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
Sleep(10);
break;
}
case HSI_SCAN_MOTION_EQ_DIS_II:
{
if (dTrigDis[1] > 0.0001)
{
iMotionDirection = 0;
}
else
{
iMotionDirection = 1;
}
switch (iaxisNum)
{
case AXIS_X:
{
if (!m_IsUsePPS)
{
begin_position[1] = begin_position[1] + dTrigDis[0] / m_Resolution[axisNum];
}
else
{
if (iMotionDirection == 0)
{
begin_position[1] = begin_position[1] - 20;
}
else
{
begin_position[1] = begin_position[1] + 20;
}
}
if (iMotionDirection == 0)
{
limit = 1000;
}
else
{
limit = -1000;
}
break;
}
case AXIS_Y:
{
if (!m_IsUsePPS)
{
begin_position[2] = begin_position[2] + dTrigDis[0] / m_Resolution[axisNum];
}
else
{
if (iMotionDirection == 0)
{
begin_position[2] = begin_position[2] - 20;
}
else
{
begin_position[2] = begin_position[2] + 20;
}
}
if (iMotionDirection == 0)
{
limit = 1000;
}
else
{
limit = -1000;
}
break;
}
case AXIS_Z:
{
if (!m_IsUsePPS)
{
begin_position[3] = begin_position[3] + dTrigDis[0] / m_Resolution[axisNum];
}
else
{
if (iMotionDirection == 0)
{
begin_position[3] = begin_position[3] - 20;
}
else
{
begin_position[3] = begin_position[3] + 20;
}
}
if (iMotionDirection == 0)
{
limit = 1000;
}
else
{
limit = -1000;
}
break;
}
default:
break;
}
iScanMotionType = HSI_SCAN_MOTION_EQ_DIS_II;
m_SendDCCData[3] = 0x00;
int i = 0, j = 0;
for (i = 0, j = 4; i < 2; ++i)
{
m_SendDCCData[j++] = ((long)(dTrigDis[i] / m_Resolution[axisNum])) & 0xff;
m_SendDCCData[j++] = (((long)(dTrigDis[i] / m_Resolution[axisNum])) >> 8) & 0xff;
m_SendDCCData[j++] = (((long)(dTrigDis[i] / m_Resolution[axisNum])) >> 16) & 0xff;
m_SendDCCData[j++] = ((long)(dTrigDis[i] / m_Resolution[axisNum])) >> 24;
}
m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
Sleep(10);
break;
}
default:
break;
}
g_pLogger->SendAndFlushWithTime(L"[DCCScanSetData] Out\n");
}
ReleaseMutex(g_WR_ToMove_Mutex);
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::DCCScanStart()
{
WaitForSingleObject(g_WR_ToMove_Mutex, INFINITE);
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
if (!setLightFlag)
{
unsigned char m_SetTriggerLightData[64] = { 0 };
m_SetTriggerLightData[0] = CT_LIGHT;
m_SetTriggerLightData[1] = 0x03;
m_SetTriggerLightData[53] = 0;
m_WriteByte = Send_Command(0, (const char*)m_SetTriggerLightData, m_SendDataLength);
Sleep(1);
}
setLightFlag = false;
g_pLogger->SendAndFlushWithTime(L"[DCCScanStart] In\n");
unsigned char m_SendDCCData[64] = { 0 };
m_SendDCCData[0] = CT_MOTOR;
m_SendDCCData[1] = CT_TRIGGER_DATA;
m_SendDCCData[2] = 0x00;
m_SendDCCData[3] = iaxisNum;
m_SendDCCData[4] = iScanMotionType;
m_SendDCCData[5] = ((iTriggleNum)& 0xff);
m_SendDCCData[6] = (iTriggleNum >> 8) & 0xff;
m_SendDCCData[7] = iMotionDirection;
switch (iaxisNum)
{
case AXIS_X: m_SendDCCData[8] = begin_position[1] & 0xff;
m_SendDCCData[9] = (begin_position[1] >> 8) & 0xff;
m_SendDCCData[10] = (begin_position[1] >> 16) & 0xff;
m_SendDCCData[11] = (begin_position[1] >> 24) & 0xff;
m_SendDCCData[12] = m_SetPotion_DriveSpeed[1] & 0xff;
m_SendDCCData[13] = (m_SetPotion_DriveSpeed[1] >> 8) & 0xff;
break;
case AXIS_Y: m_SendDCCData[8] = begin_position[2] & 0xff;
m_SendDCCData[9] = (begin_position[2] >> 8) & 0xff;
m_SendDCCData[10] = (begin_position[2] >> 16) & 0xff;
m_SendDCCData[11] = (begin_position[2] >> 24) & 0xff;
m_SendDCCData[12] = m_SetPotion_DriveSpeed[2] & 0xff;
m_SendDCCData[13] = (m_SetPotion_DriveSpeed[2] >> 8) & 0xff;
break;
case AXIS_Z:
m_SendDCCData[8] = begin_position[3] & 0xff;
m_SendDCCData[9] = (begin_position[3] >> 8) & 0xff;
m_SendDCCData[10] = (begin_position[3] >> 16) & 0xff;
m_SendDCCData[11] = (begin_position[3] >> 24) & 0xff;
m_SendDCCData[12] = m_SetPotion_DriveSpeed[3] & 0xff;
m_SendDCCData[13] = (m_SetPotion_DriveSpeed[3] >> 8) & 0xff;
break;
default:
break;
}
m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
Sleep(2);
g_pLogger->SendAndFlushWithTime(L"[DCCScanStart] Out\n");
m_IsUsePPS = false;
}
ReleaseMutex(g_WR_ToMove_Mutex);
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::DCCScanStop()
{
WaitForSingleObject(g_WR_ToMove_Mutex, INFINITE);
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
g_pLogger->SendAndFlushWithTime(L"[DCCScanStop] In\n");
unsigned char m_SendDCCData[64] = { 0 };
m_SendDCCData[0] = CT_MOTOR;
m_SendDCCData[1] = CT_TRIGGER_DATA;
m_SendDCCData[2] = 0x02;
m_SendDCCData[3] = iaxisNum;
m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
g_pLogger->SendAndFlushWithTime(L"[DCCScanStop] Out\n");
}
ReleaseMutex(g_WR_ToMove_Mutex);
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::DCCForLightPlate()
{
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
g_pLogger->SendAndFlushWithTime(L"[DCCForLightPlate] In\n");
memset(m_cSendData, 0x00, 63);
m_cSendData[0] = 0x02;
m_cSendData[1] = 0x02;
m_cSendData[2] = 0x01;
m_cSendData[6] = 0x04;
m_cSendData[7] = 0xff;
m_cSendData[8] = 0xff;
m_cSendData[9] = 0xff;
m_cSendData[10] = 0xff;
m_cSendData[14] = 0x00;
m_WriteByte = Send_Command(1, (const char*)m_cSendData, m_SendDataLength);
Sleep(5);
memset(m_cSendData, 0x00, 63);
m_cSendData[0] = 0x02;
m_cSendData[1] = 0x02;
m_cSendData[2] = 0x01;
m_cSendData[6] = 0x04;
m_cSendData[7] = 0xff;
m_cSendData[8] = 0xff;
m_cSendData[9] = 0xff;
m_cSendData[10] = 0xff;
m_cSendData[14] = m_ETIPort & 0xff;
m_WriteByte = Send_Command(1, (const char*)m_cSendData, m_SendDataLength);
Sleep(5);
g_pLogger->SendAndFlushWithTime(L"[DCCForLightPlate] Out\n");
}
return rStatus;
}
//===============================转盘============================================
HSI_STATUS HSI_Motion::StartPlcJob(int* CamerasDis, int* BinsDis, int SubArea, int filterTime1, int filterTime2, int pluseSumDis)
{
g_pLogger->SendAndFlushWithTime(L"[StartPlcJob] In\n");
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
m_Thread_StateData = HSI_THREAD_PAUSED;
if (m_EncPos[2] < 0)
{
sEvenProp.Init();
sEvenProp.EventType = HSI_EVENT_ERROR;
sEvenProp.EventID = HSI_EVENT_MOTION;
sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK;
strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "开始前请确认当前位置大于零！");
EventCallback(sEvenProp);
return HSI_STATUS_FAILED;
}
m_Thread_StateData = HSI_THREAD_PAUSED;
m_IbinCount = 0;
m_cSendData[0] = CT_TURNTABLE;
m_cSendData[1] = CT_RTSET;
int index = 2;
for (size_t i = 0; i < 4; i++)
{
m_cSendData[index++] = ((CamerasDis[i] >> 24) & 0xff);
m_cSendData[index++] = ((CamerasDis[i] >> 16) & 0xff);
m_cSendData[index++] = ((CamerasDis[i] >> 8) & 0xff);
m_cSendData[index++] = (CamerasDis[i] & 0xff);
}
for (size_t i = 0; i < 5; i++)
{
m_cSendData[index++] = ((BinsDis[i] >> 24) & 0xff);
m_cSendData[index++] = ((BinsDis[i] >> 16) & 0xff);
m_cSendData[index++] = ((BinsDis[i] >> 8) & 0xff);
m_cSendData[index++] = (BinsDis[i] & 0xff);
}
m_cSendData[index++] = SubArea;
m_cSendData[index++] = ((filterTime1 >> 8) & 0xff);
m_cSendData[index++] = (filterTime1 & 0xff);
m_cSendData[index++] = ((filterTime2 >> 8) & 0xff);
m_cSendData[index++] = (filterTime2 & 0xff);
m_cSendData[index++] = ((pluseSumDis >> 24) & 0xff);
m_cSendData[index++] = ((pluseSumDis >> 16) & 0xff);
m_cSendData[index++] = ((pluseSumDis >> 8) & 0xff);
m_cSendData[index++] = (pluseSumDis & 0xff);
m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
Sleep(5);
Jog(m_UseAxisNum, 0.1);
}
g_pLogger->SendAndFlushWithTime(L"[StartPlcJob] Out\n");
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::SendBinResult(int* BinResult)
{
g_pLogger->SendAndFlushWithTime(L"[SendBinResult] In\n");
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
m_IbinCount++;
m_cSendData[0] = CT_TURNTABLE;
m_cSendData[1] = CT_BINSDATA;
int temp = 0;
switch (BinResult[0])
{
case 1:
temp = 0x01; break;
case 2:
temp = 0x02; break;
case 3:
temp = 0x04; break;
case 4:
temp = 0x08; break;
default:
temp = 0x00; break;
}
m_cSendData[2] = temp;
m_cSendData[3] = ((m_IbinCount >> 24) & 0xff);
m_cSendData[4] = ((m_IbinCount >> 16) & 0xff);
m_cSendData[5] = ((m_IbinCount >> 8) & 0xff);
m_cSendData[6] = (m_IbinCount & 0xff);
m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
g_pLogger->SendAndFlushWithTime(L"[GetSpeedXyz] m_IbinCount = %d\n", m_IbinCount);
Sleep(5);
}
g_pLogger->SendAndFlushWithTime(L"[SendBinResult] out\n");
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::GetTriggleCount(int *nCount, int& nArea)
{
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
if (m_SO7_Serial.m_RecvData[0] == 3)
{
nArea = m_SO7_Serial.m_RecvData[1];
nCount[0] = m_SO7_Serial.m_RecvData[2] << 24 | m_SO7_Serial.m_RecvData[3] << 16 | m_SO7_Serial.m_RecvData[4] << 8 | m_SO7_Serial.m_RecvData[5];
nCount[1] = m_SO7_Serial.m_RecvData[6] << 24 | m_SO7_Serial.m_RecvData[7] << 16 | m_SO7_Serial.m_RecvData[8] << 8 | m_SO7_Serial.m_RecvData[9];
nCount[2] = m_SO7_Serial.m_RecvData[10] << 24 | m_SO7_Serial.m_RecvData[11] << 16 | m_SO7_Serial.m_RecvData[12] << 8 | m_SO7_Serial.m_RecvData[13];
nCount[3] = m_SO7_Serial.m_RecvData[14] << 24 | m_SO7_Serial.m_RecvData[15] << 16 | m_SO7_Serial.m_RecvData[16] << 8 | m_SO7_Serial.m_RecvData[17];
nCount[4] = m_SO7_Serial.m_RecvData[18] << 24 | m_SO7_Serial.m_RecvData[19] << 16 | m_SO7_Serial.m_RecvData[20] << 8 | m_SO7_Serial.m_RecvData[21];
nCount[5] = m_SO7_Serial.m_RecvData[22] << 24 | m_SO7_Serial.m_RecvData[23] << 16 | m_SO7_Serial.m_RecvData[24] << 8 | m_SO7_Serial.m_RecvData[25];
nCount[6] = m_SO7_Serial.m_RecvData[26] << 24 | m_SO7_Serial.m_RecvData[27] << 16 | m_SO7_Serial.m_RecvData[28] << 8 | m_SO7_Serial.m_RecvData[29];
nCount[7] = m_SO7_Serial.m_RecvData[30] << 24 | m_SO7_Serial.m_RecvData[31] << 16 | m_SO7_Serial.m_RecvData[32] << 8 | m_SO7_Serial.m_RecvData[33];
nCount[8] = m_SO7_Serial.m_RecvData[34] << 24 | m_SO7_Serial.m_RecvData[35] << 16 | m_SO7_Serial.m_RecvData[36] << 8 | m_SO7_Serial.m_RecvData[37];
nCount[9] = m_SO7_Serial.m_RecvData[38] << 24 | m_SO7_Serial.m_RecvData[39] << 16 | m_SO7_Serial.m_RecvData[40] << 8 | m_SO7_Serial.m_RecvData[41];
g_pLogger->SendAndFlushWithTime(L"[GetTriggleCount] nArea = %d\n", nArea);
}
}
return rStatus;
}
//===========================================================================
//点胶
//===========================================================================
HSI_STATUS HSI_Motion::GluePPSpnts(int* ppsDir, double* gluePPSPos, int* delayLightBefore, int* lightTime, double* lightData, int num)
{
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
unsigned char send_gluePPS_data[64] = { 0 };
send_gluePPS_data[0] = CT_GLUEDISPENSER;
send_gluePPS_data[1] = 0x01; //擦除指令
m_WriteByte = Send_Command(0, (const char*)send_gluePPS_data, m_SendDataLength);
Sleep(200);
int posIndex = 0;
int loadFeet[4];
int j = 0;
int c = 0; //触发计数
int dirType = 0;
int saveDir = 0;
int duanNum = 0;
double triggerPnt[4];
double savePPSPnt[4];
GlueDispenserindexNum = 0;
#pragma region 拍照点
for (size_t i = 0; i < num; i++)
{
if (i == 0)
{
dirType = ppsDir[0];
saveDir = dirType;
j = 7;
}
else
{
dirType = ppsDir[i];
}
if (dirType != saveDir)
{
//->
send_gluePPS_data[0] = CT_GLUEDISPENSER;
send_gluePPS_data[1] = 0x02;
send_gluePPS_data[2] = GlueDispenserindexNum;
send_gluePPS_data[3] = duanNum;
send_gluePPS_data[4] = c;
int axisNum = 0;
int dirnum = 0;
switch (saveDir)
{
case 0:
{
axisNum = 1;
dirnum = 2;
break;
}
case 1:
{
axisNum = 1;
dirnum = 1;
break;
}
case 2:
{
axisNum = 2;
dirnum = 2;
break;
}
case 3:
{
axisNum = 2;
dirnum = 1;
break;
}
default:
break;
}
send_gluePPS_data[5] = axisNum;
send_gluePPS_data[6] = dirnum;
m_WriteByte = Send_Command(0, (const char*)send_gluePPS_data, m_SendDataLength);
Sleep(20);
GlueDispenserindexNum++;
duanNum = 0;
saveDir = dirType;
c = 0;
j = 7;
}
switch (dirType)
{
case 0:
{
c++;
loadFeet[0] = (int)(gluePPSPos[posIndex++] / m_Resolution[1]);
loadFeet[1] = (int)(gluePPSPos[posIndex++] / m_Resolution[1]);
loadFeet[2] = (int)(gluePPSPos[posIndex++] / m_Resolution[1]);
send_gluePPS_data[j++] = (loadFeet[0] & 0xff);
send_gluePPS_data[j++] = ((loadFeet[0] >> 8) & 0xff);
send_gluePPS_data[j++] = ((loadFeet[0] >> 16) & 0xff);
send_gluePPS_data[j++] = ((loadFeet[0] >> 24) & 0xff);
break;
}
case 1:
{
c++;
loadFeet[0] = (int)(gluePPSPos[posIndex++] / m_Resolution[1]);
loadFeet[1] = (int)(gluePPSPos[posIndex++] / m_Resolution[1]);
loadFeet[2] = (int)(gluePPSPos[posIndex++] / m_Resolution[1]);
send_gluePPS_data[j++] = (loadFeet[0] & 0xff);
send_gluePPS_data[j++] = ((loadFeet[0] >> 8) & 0xff);
send_gluePPS_data[j++] = ((loadFeet[0] >> 16) & 0xff);
send_gluePPS_data[j++] = ((loadFeet[0] >> 24) & 0xff);
break;
}
case 2:
{
c++;
loadFeet[0] = (int)(gluePPSPos[posIndex++] / m_Resolution[1]);
loadFeet[1] = (int)(gluePPSPos[posIndex++] / m_Resolution[1]);
loadFeet[2] = (int)(gluePPSPos[posIndex++] / m_Resolution[1]);
send_gluePPS_data[j++] = (loadFeet[1] & 0xff);
send_gluePPS_data[j++] = ((loadFeet[1] >> 8) & 0xff);
send_gluePPS_data[j++] = ((loadFeet[1] >> 16) & 0xff);
send_gluePPS_data[j++] = ((loadFeet[1] >> 24) & 0xff);
break;
}
case 3:
{
c++;
loadFeet[0] = (int)(gluePPSPos[posIndex++] / m_Resolution[1]);
loadFeet[1] = (int)(gluePPSPos[posIndex++] / m_Resolution[1]);
loadFeet[2] = (int)(gluePPSPos[posIndex++] / m_Resolution[1]);
send_gluePPS_data[j++] = (loadFeet[1] & 0xff);
send_gluePPS_data[j++] = ((loadFeet[1] >> 8) & 0xff);
send_gluePPS_data[j++] = ((loadFeet[1] >> 16) & 0xff);
send_gluePPS_data[j++] = ((loadFeet[1] >> 24) & 0xff);
break;
}
default:
break;
}
if (c == 12)
{
//->
send_gluePPS_data[0] = CT_GLUEDISPENSER;
send_gluePPS_data[1] = 0x02;
send_gluePPS_data[2] = GlueDispenserindexNum;
send_gluePPS_data[3] = duanNum;
send_gluePPS_data[4] = c;
int axisNum = 0;
int dirnum = 0;
switch (saveDir)
{
case 0:
{
axisNum = 1;
dirnum = 2;
break;
}
case 1:
{
axisNum = 1;
dirnum = 1;
break;
}
case 2:
{
axisNum = 2;
dirnum = 2;
break;
}
case 3:
{
axisNum = 2;
dirnum = 1;
break;
}
default:
break;
}
send_gluePPS_data[5] = axisNum;
send_gluePPS_data[6] = dirnum;
m_WriteByte = Send_Command(0, (const char*)send_gluePPS_data, m_SendDataLength);
Sleep(20);
duanNum++;
saveDir = dirType;
c = 0;
j = 7;
}
}
if (c != 0)
{
send_gluePPS_data[0] = CT_GLUEDISPENSER;
send_gluePPS_data[1] = 0x02;
send_gluePPS_data[2] = GlueDispenserindexNum;
send_gluePPS_data[3] = duanNum;
send_gluePPS_data[4] = c;
int axisNum = 0;
int dirnum = 0;
switch (saveDir)
{
case 0:
{
axisNum = 1;
dirnum = 2;
break;
}
case 1:
{
axisNum = 1;
dirnum = 1;
break;
}
case 2:
{
axisNum = 2;
dirnum = 2;
break;
}
case 3:
{
axisNum = 2;
dirnum = 1;
break;
}
default:
break;
}
send_gluePPS_data[5] = axisNum;
send_gluePPS_data[6] = dirnum;
m_WriteByte = Send_Command(0, (const char*)send_gluePPS_data, m_SendDataLength);
Sleep(20);
}
#pragma endregion
unsigned char send_data_light2[64] = { 0 };
unsigned char send_data_delay[64] = { 0 };
c = 0;
GlueDispenserindexNum = 0;
int ldIndex = 0;
int light1 = 0;
int light2 = 0;
int indexCount = 0;
dirType = 0;
saveDir = 0;
j = 3;
int k = 4;
int d = 4;
for (size_t i = 0; i < num; i++)
{
if (i == 0)
{
dirType = ppsDir[0];
saveDir = dirType;
j = 4;
}
else
{
dirType = ppsDir[i];
}
if (dirType != saveDir)
{
send_gluePPS_data[0] = CT_GLUEDISPENSER;
send_gluePPS_data[1] = 0x03;
send_gluePPS_data[2] = GlueDispenserindexNum;
send_gluePPS_data[3] = 0;
m_WriteByte = Send_Command(0, (const char*)send_gluePPS_data, m_SendDataLength);
Sleep(20);
send_data_light2[0] = CT_GLUEDISPENSER;
send_data_light2[1] = 0x03;
send_data_light2[2] = GlueDispenserindexNum;
send_data_light2[3] = 1;
m_WriteByte = Send_Command(0, (const char*)send_data_light2, m_SendDataLength);
Sleep(20);
send_data_delay[0] = CT_GLUEDISPENSER;
send_data_delay[1] = 0x03;
send_data_delay[2] = GlueDispenserindexNum;
send_data_delay[3] = 2;
m_WriteByte = Send_Command(0, (const char*)send_data_delay, m_SendDataLength);
Sleep(20);
c = 0;
j = 4;
k = 4;
d = 4;
indexCount = 0;
GlueDispenserindexNum++;
}
light1 = (int)(lightData[ldIndex++] * 1120);
light2 = (int)(lightData[ldIndex++] * 1120);
//send_gluePPS_data[j++] = (delayLightBefore[i] & 0xff);
//send_gluePPS_data[j++] = ((delayLightBefore[i] >> 8) & 0xff);
send_gluePPS_data[j++] = (light1 & 0xff);
send_gluePPS_data[j++] = ((light1 >> 8) & 0xff);
send_data_light2[k++] = (light2 & 0xff);
send_data_light2[k++] = ((light2 >> 8) & 0xff);
send_data_delay[d++] = (lightTime[i] & 0xff);
c++;
if (c == 30)
{
send_gluePPS_data[0] = CT_GLUEDISPENSER;
send_gluePPS_data[1] = 0x03;
send_gluePPS_data[2] = GlueDispenserindexNum;
send_gluePPS_data[3] = 0;
m_WriteByte = Send_Command(0, (const char*)send_gluePPS_data, m_SendDataLength);
Sleep(20);
send_data_light2[0] = CT_GLUEDISPENSER;
send_data_light2[1] = 0x03;
send_data_light2[2] = GlueDispenserindexNum;
send_data_light2[3] = 1;
m_WriteByte = Send_Command(0, (const char*)send_data_light2, m_SendDataLength);
Sleep(20);
send_data_delay[0] = CT_GLUEDISPENSER;
send_data_delay[1] = 0x03;
send_data_delay[2] = GlueDispenserindexNum;
send_data_delay[3] = 2;
m_WriteByte = Send_Command(0, (const char*)send_data_delay, m_SendDataLength);
Sleep(20);
c = 0;
j = 4;
k = 4;
d = 4;
indexCount++;
GlueDispenserindexNum++;
}
}
if (c != 0)
{
send_gluePPS_data[0] = CT_GLUEDISPENSER;
send_gluePPS_data[1] = 0x03;
send_gluePPS_data[2] = GlueDispenserindexNum;
send_gluePPS_data[3] = 0;
m_WriteByte = Send_Command(0, (const char*)send_gluePPS_data, m_SendDataLength);
Sleep(20);
send_data_light2[0] = CT_GLUEDISPENSER;
send_data_light2[1] = 0x03;
send_data_light2[2] = GlueDispenserindexNum;
send_data_light2[3] = 1;
m_WriteByte = Send_Command(0, (const char*)send_data_light2, m_SendDataLength);
Sleep(20);
send_data_delay[0] = CT_GLUEDISPENSER;
send_data_delay[1] = 0x03;
send_data_delay[2] = GlueDispenserindexNum;
send_data_delay[3] = 2;
m_WriteByte = Send_Command(0, (const char*)send_data_delay, m_SendDataLength);
Sleep(20);
GlueDispenserindexNum++;
}
}
return rStatus;
}
HSI_STATUS HSI_Motion::GlueDispenser(int* index, int* cirdirection, double* gluePos, int num)
{
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
GluerunCount = 0;
unsigned char send_glue_data[64] = { 0 };
send_glue_data[0] = CT_ORDER;
send_glue_data[1] = CT_GLUEDISPENSER_CLEAR;
send_glue_data[2] = 0x02;
m_WriteByte = Send_Command(0, (const char*)send_glue_data, m_SendDataLength);
Sleep(200);
int posIndex = 0;
int loadFeet[4];
double startGluepos[4];
double endGluepos[4];
double saveGluepos[4];
int circlepnt[4];
unsigned char glueaxis = 0;
for (size_t i = 0; i < num; i++)
{
switch (index[i])
{
case 0:
{
glueaxis = 0;
if (i == 0)
{
GluerunCount++;
endGluepos[0] = gluePos[posIndex++];
endGluepos[1] = gluePos[posIndex++];
endGluepos[2] = gluePos[posIndex++];
endGluepos[3] = gluePos[posIndex++];
saveGluepos[0] = endGluepos[0];
saveGluepos[1] = endGluepos[1];
saveGluepos[2] = endGluepos[2];
saveGluepos[3] = endGluepos[3];
loadFeet[0] = (int)(endGluepos[0] / m_Resolution[1]);
loadFeet[1] = (int)(endGluepos[1] / m_Resolution[2]);
loadFeet[2] = (int)(endGluepos[2] / m_Resolution[3]);
loadFeet[3] = (int)(endGluepos[3] / m_Resolution[4]);
if (abs(loadFeet[0]) > 20) glueaxis |= 0x01;
if (abs(loadFeet[1]) > 20) glueaxis |= 0x02;
if (abs(loadFeet[2]) > 20) glueaxis |= 0x04;
if (abs(loadFeet[3]) > 20) glueaxis |= 0x08;
send_glue_data[3] = glueaxis;
send_glue_data[4] = 0x01;
send_glue_data[5] = 0x00;
int j = 6;
for (size_t k = 0; k < 4; k++)
{
send_glue_data[j++] = (loadFeet[k] & 0xff);
send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff);
send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff);
send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff);
}
send_glue_data[0] = CT_MOTOR;
send_glue_data[1] = CT_GLUEDISPENSER_SET;
int len = j - 3;
send_glue_data[2] = len;
m_WriteByte = Send_Command(0, (const char*)send_glue_data, m_SendDataLength);
Sleep(20);
}
else
{
GluerunCount++;
endGluepos[0] = gluePos[posIndex++];
endGluepos[1] = gluePos[posIndex++];
endGluepos[2] = gluePos[posIndex++];
endGluepos[3] = gluePos[posIndex++];
startGluepos[0] = saveGluepos[0];
startGluepos[1] = saveGluepos[1];
startGluepos[2] = saveGluepos[2];
startGluepos[3] = saveGluepos[3];
saveGluepos[0] = endGluepos[0];
saveGluepos[1] = endGluepos[1];
saveGluepos[2] = endGluepos[2];
saveGluepos[3] = endGluepos[3];
loadFeet[0] = (int)((endGluepos[0] - startGluepos[0]) / m_Resolution[1]);
loadFeet[1] = (int)((endGluepos[1] - startGluepos[1]) / m_Resolution[2]);
loadFeet[2] = (int)((endGluepos[2] - startGluepos[2]) / m_Resolution[3]);
loadFeet[3] = (int)((endGluepos[3] - startGluepos[3]) / m_Resolution[4]);
if (abs(loadFeet[0]) > 20) glueaxis |= 0x01;
if (abs(loadFeet[1]) > 20) glueaxis |= 0x02;
if (abs(loadFeet[2]) > 20) glueaxis |= 0x04;
if (abs(loadFeet[3]) > 20) glueaxis |= 0x08;
send_glue_data[3] = glueaxis;
send_glue_data[4] = 0x01;
send_glue_data[5] = 0x00;
int j = 6;
for (size_t k = 0; k < 4; k++)
{
send_glue_data[j++] = (loadFeet[k] & 0xff);
send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff);
send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff);
send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff);
}
send_glue_data[0] = CT_MOTOR;
send_glue_data[1] = CT_GLUEDISPENSER_SET;
int len = j - 3;
send_glue_data[2] = len;
m_WriteByte = Send_Command(0, (const char*)send_glue_data, m_SendDataLength);
Sleep(20);
}
break;
}
case 1:
{
if (i == 0)
{
GluerunCount++;
endGluepos[0] = gluePos[posIndex++];
endGluepos[1] = gluePos[posIndex++];
endGluepos[2] = gluePos[posIndex++];
endGluepos[3] = gluePos[posIndex++];
saveGluepos[0] = endGluepos[0];
saveGluepos[1] = endGluepos[1];
saveGluepos[2] = endGluepos[2];
saveGluepos[3] = endGluepos[3];
loadFeet[0] = (int)(endGluepos[0] / m_Resolution[1]);
loadFeet[1] = (int)(endGluepos[1] / m_Resolution[2]);
loadFeet[2] = (int)(endGluepos[2] / m_Resolution[3]);
loadFeet[3] = (int)(endGluepos[3] / m_Resolution[4]);
if (abs(loadFeet[0]) > 20) glueaxis |= 0x01;
if (abs(loadFeet[1]) > 20) glueaxis |= 0x02;
if (abs(loadFeet[2]) > 20) glueaxis |= 0x04;
if (abs(loadFeet[3]) > 20) glueaxis |= 0x08;
send_glue_data[3] = glueaxis;
send_glue_data[4] = 0x01;
send_glue_data[5] = 0x00;
int j = 6;
for (size_t k = 0; k < 4; k++)
{
send_glue_data[j++] = (loadFeet[k] & 0xff);
send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff);
send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff);
send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff);
}
GluerunCount++;
glueaxis = 0;
endGluepos[0] = gluePos[posIndex++];
endGluepos[1] = gluePos[posIndex++];
endGluepos[2] = gluePos[posIndex++];
endGluepos[3] = gluePos[posIndex++];
startGluepos[0] = saveGluepos[0];
startGluepos[1] = saveGluepos[1];
startGluepos[2] = saveGluepos[2];
startGluepos[3] = saveGluepos[3];
saveGluepos[0] = endGluepos[0];
saveGluepos[1] = endGluepos[1];
saveGluepos[2] = endGluepos[2];
saveGluepos[3] = endGluepos[3];
loadFeet[0] = (int)((endGluepos[0] - startGluepos[0]) / m_Resolution[1]);
loadFeet[1] = (int)((endGluepos[1] - startGluepos[1]) / m_Resolution[2]);
loadFeet[2] = (int)((endGluepos[2] - startGluepos[2]) / m_Resolution[3]);
loadFeet[3] = (int)((endGluepos[3] - startGluepos[3]) / m_Resolution[4]);
if (abs(loadFeet[0]) > 20) glueaxis |= 0x01;
if (abs(loadFeet[1]) > 20) glueaxis |= 0x02;
if (abs(loadFeet[2]) > 20) glueaxis |= 0x04;
if (abs(loadFeet[3]) > 20) glueaxis |= 0x08;
send_glue_data[j++] = glueaxis;
send_glue_data[j++] = 0x02;
send_glue_data[j++] = 0x01;
for (size_t k = 0; k < 4; k++)
{
send_glue_data[j++] = (loadFeet[k] & 0xff);
send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff);
send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff);
send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff);
}
send_glue_data[0] = CT_MOTOR;
send_glue_data[1] = CT_GLUEDISPENSER_SET;
int len = j - 3;
send_glue_data[2] = len;
m_WriteByte = Send_Command(0, (const char*)send_glue_data, m_SendDataLength);
Sleep(20);
}
else
{
GluerunCount++;
glueaxis = 0;
endGluepos[0] = gluePos[posIndex++];
endGluepos[1] = gluePos[posIndex++];
endGluepos[2] = gluePos[posIndex++];
endGluepos[3] = gluePos[posIndex++];
startGluepos[0] = saveGluepos[0];
startGluepos[1] = saveGluepos[1];
startGluepos[2] = saveGluepos[2];
startGluepos[3] = saveGluepos[3];
saveGluepos[0] = endGluepos[0];
saveGluepos[1] = endGluepos[1];
saveGluepos[2] = endGluepos[2];
saveGluepos[3] = endGluepos[3];
loadFeet[0] = (int)((endGluepos[0] - startGluepos[0]) / m_Resolution[1]);
loadFeet[1] = (int)((endGluepos[1] - startGluepos[1]) / m_Resolution[2]);
loadFeet[2] = (int)((endGluepos[2] - startGluepos[2]) / m_Resolution[3]);
loadFeet[3] = (int)((endGluepos[3] - startGluepos[3]) / m_Resolution[4]);
if (abs(loadFeet[0]) > 20) glueaxis |= 0x01;
if (abs(loadFeet[1]) > 20) glueaxis |= 0x02;
if (abs(loadFeet[2]) > 20) glueaxis |= 0x04;
if (abs(loadFeet[3]) > 20) glueaxis |= 0x08;
send_glue_data[3] = glueaxis;
send_glue_data[4] = 0x02;
send_glue_data[5] = 0x00;
int j = 6;
for (size_t k = 0; k < 4; k++)
{
send_glue_data[j++] = (loadFeet[k] & 0xff);
send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff);
send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff);
send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff);
}
GluerunCount++;
glueaxis = 0;
endGluepos[0] = gluePos[posIndex++];
endGluepos[1] = gluePos[posIndex++];
endGluepos[2] = gluePos[posIndex++];
endGluepos[3] = gluePos[posIndex++];
startGluepos[0] = saveGluepos[0];
startGluepos[1] = saveGluepos[1];
startGluepos[2] = saveGluepos[2];
startGluepos[3] = saveGluepos[3];
saveGluepos[0] = endGluepos[0];
saveGluepos[1] = endGluepos[1];
saveGluepos[2] = endGluepos[2];
saveGluepos[3] = endGluepos[3];
loadFeet[0] = (int)((endGluepos[0] - startGluepos[0]) / m_Resolution[1]);
loadFeet[1] = (int)((endGluepos[1] - startGluepos[1]) / m_Resolution[2]);
loadFeet[2] = (int)((endGluepos[2] - startGluepos[2]) / m_Resolution[3]);
loadFeet[3] = (int)((endGluepos[3] - startGluepos[3]) / m_Resolution[4]);
if (abs(loadFeet[0]) > 20) glueaxis |= 0x01;
if (abs(loadFeet[1]) > 20) glueaxis |= 0x02;
if (abs(loadFeet[2]) > 20) glueaxis |= 0x04;
if (abs(loadFeet[3]) > 20) glueaxis |= 0x08;
send_glue_data[j++] = glueaxis;
send_glue_data[j++] = 0x02;
send_glue_data[j++] = 0x01;
for (size_t k = 0; k < 4; k++)
{
send_glue_data[j++] = (loadFeet[k] & 0xff);
send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff);
send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff);
send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff);
}
send_glue_data[0] = CT_MOTOR;
send_glue_data[1] = CT_GLUEDISPENSER_SET;
int len = j - 3;
send_glue_data[2] = len;
m_WriteByte = Send_Command(0, (const char*)send_glue_data, m_SendDataLength);
Sleep(20);
}
break;
}
case 3:
{
if (i == 0)
{
GluerunCount++;
endGluepos[0] = gluePos[posIndex++];
endGluepos[1] = gluePos[posIndex++];
endGluepos[2] = gluePos[posIndex++];
endGluepos[3] = gluePos[posIndex++];
saveGluepos[0] = endGluepos[0];
saveGluepos[1] = endGluepos[1];
saveGluepos[2] = endGluepos[2];
saveGluepos[3] = endGluepos[3];
loadFeet[0] = (int)(endGluepos[0] / m_Resolution[1]);
loadFeet[1] = (int)(endGluepos[1] / m_Resolution[2]);
loadFeet[2] = (int)(endGluepos[2] / m_Resolution[3]);
loadFeet[3] = (int)(endGluepos[3] / m_Resolution[4]);
if (abs(loadFeet[0]) > 20) glueaxis |= 0x01;
if (abs(loadFeet[1]) > 20) glueaxis |= 0x02;
if (abs(loadFeet[2]) > 20) glueaxis |= 0x04;
if (abs(loadFeet[3]) > 20) glueaxis |= 0x08;
send_glue_data[3] = glueaxis;
send_glue_data[4] = 0x01;
send_glue_data[5] = 0x00;
int j = 6;
for (size_t k = 0; k < 4; k++)
{
send_glue_data[j++] = (loadFeet[k] & 0xff);
send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff);
send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff);
send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff);
}
GluerunCount++;
glueaxis = 0;
endGluepos[0] = gluePos[posIndex++];
endGluepos[1] = gluePos[posIndex++];
endGluepos[2] = gluePos[posIndex++];
endGluepos[3] = gluePos[posIndex++];
startGluepos[0] = saveGluepos[0];
startGluepos[1] = saveGluepos[1];
startGluepos[2] = saveGluepos[2];
startGluepos[3] = saveGluepos[3];
circlepnt[0] = (int)((gluePos[posIndex++] - startGluepos[0]) / m_Resolution[1]);
circlepnt[1] = (int)((gluePos[posIndex++] - startGluepos[1]) / m_Resolution[2]);
circlepnt[2] = (int)((gluePos[posIndex++] - startGluepos[2]) / m_Resolution[3]);
circlepnt[3] = (int)((gluePos[posIndex++] - startGluepos[3]) / m_Resolution[4]);
saveGluepos[0] = endGluepos[0];
saveGluepos[1] = endGluepos[1];
saveGluepos[2] = endGluepos[2];
saveGluepos[3] = endGluepos[3];
loadFeet[0] = (int)((endGluepos[0] - startGluepos[0]) / m_Resolution[1]);
loadFeet[1] = (int)((endGluepos[1] - startGluepos[1]) / m_Resolution[2]);
loadFeet[2] = (int)((endGluepos[2] - startGluepos[2]) / m_Resolution[3]);
loadFeet[3] = (int)((endGluepos[3] - startGluepos[3]) / m_Resolution[4]);
if (abs(loadFeet[0]) > 20) glueaxis |= 0x01;
if (abs(loadFeet[1]) > 20) glueaxis |= 0x02;
if (abs(loadFeet[2]) > 20) glueaxis |= 0x04;
if (abs(loadFeet[3]) > 20) glueaxis |= 0x08;
send_glue_data[j++] = 0x03;
if (cirdirection[i] == 0)
{
send_glue_data[j++] = 0x04;
}
else
{
send_glue_data[j++] = 0x03;
}
send_glue_data[j++] = 0x01;
for (size_t k = 0; k < 4; k++)
{
send_glue_data[j++] = (loadFeet[k] & 0xff);
send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff);
send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff);
send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff);
}
for (size_t k = 0; k < 4; k++)
{
send_glue_data[j++] = (circlepnt[k] & 0xff);
send_glue_data[j++] = ((circlepnt[k] >> 8) & 0xff);
send_glue_data[j++] = ((circlepnt[k] >> 16) & 0xff);
send_glue_data[j++] = ((circlepnt[k] >> 24) & 0xff);
}
send_glue_data[0] = CT_MOTOR;
send_glue_data[1] = CT_GLUEDISPENSER_SET;
int len = j - 3;
send_glue_data[2] = len;
m_WriteByte = Send_Command(0, (const char*)send_glue_data, m_SendDataLength);
Sleep(20);
}
else
{
GluerunCount++;
glueaxis = 0;
endGluepos[0] = gluePos[posIndex++];
endGluepos[1] = gluePos[posIndex++];
endGluepos[2] = gluePos[posIndex++];
endGluepos[3] = gluePos[posIndex++];
startGluepos[0] = saveGluepos[0];
startGluepos[1] = saveGluepos[1];
startGluepos[2] = saveGluepos[2];
startGluepos[3] = saveGluepos[3];
saveGluepos[0] = endGluepos[0];
saveGluepos[1] = endGluepos[1];
saveGluepos[2] = endGluepos[2];
saveGluepos[3] = endGluepos[3];
loadFeet[0] = (int)((endGluepos[0] - startGluepos[0]) / m_Resolution[1]);
loadFeet[1] = (int)((endGluepos[1] - startGluepos[1]) / m_Resolution[2]);
loadFeet[2] = (int)((endGluepos[2] - startGluepos[2]) / m_Resolution[3]);
loadFeet[3] = (int)((endGluepos[3] - startGluepos[3]) / m_Resolution[4]);
if (abs(loadFeet[0]) > 20) glueaxis |= 0x01;
if (abs(loadFeet[1]) > 20) glueaxis |= 0x02;
if (abs(loadFeet[2]) > 20) glueaxis |= 0x04;
if (abs(loadFeet[3]) > 20) glueaxis |= 0x08;
send_glue_data[3] = glueaxis;
send_glue_data[4] = 0x02;
send_glue_data[5] = 0x00;
int j = 6;
for (size_t k = 0; k < 4; k++)
{
send_glue_data[j++] = (loadFeet[k] & 0xff);
send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff);
send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff);
send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff);
}
GluerunCount++;
glueaxis = 0;
endGluepos[0] = gluePos[posIndex++];
endGluepos[1] = gluePos[posIndex++];
endGluepos[2] = gluePos[posIndex++];
endGluepos[3] = gluePos[posIndex++];
startGluepos[0] = saveGluepos[0];
startGluepos[1] = saveGluepos[1];
startGluepos[2] = saveGluepos[2];
startGluepos[3] = saveGluepos[3];
circlepnt[0] = (int)((gluePos[posIndex++] - startGluepos[0]) / m_Resolution[1]);
circlepnt[1] = (int)((gluePos[posIndex++] - startGluepos[1]) / m_Resolution[2]);
circlepnt[2] = (int)((gluePos[posIndex++] - startGluepos[2]) / m_Resolution[3]);
circlepnt[3] = (int)((gluePos[posIndex++] - startGluepos[3]) / m_Resolution[4]);
saveGluepos[0] = endGluepos[0];
saveGluepos[1] = endGluepos[1];
saveGluepos[2] = endGluepos[2];
saveGluepos[3] = endGluepos[3];
loadFeet[0] = (int)((endGluepos[0] - startGluepos[0]) / m_Resolution[1]);
loadFeet[1] = (int)((endGluepos[1] - startGluepos[1]) / m_Resolution[2]);
loadFeet[2] = (int)((endGluepos[2] - startGluepos[2]) / m_Resolution[3]);
loadFeet[3] = (int)((endGluepos[3] - startGluepos[3]) / m_Resolution[4]);
if (abs(loadFeet[0]) > 20) glueaxis |= 0x01;
if (abs(loadFeet[1]) > 20) glueaxis |= 0x02;
if (abs(loadFeet[2]) > 20) glueaxis |= 0x04;
if (abs(loadFeet[3]) > 20) glueaxis |= 0x08;
send_glue_data[j++] = 0x03;
if (cirdirection[i] == 0)
{
send_glue_data[j++] = 0x04;
}
else
{
send_glue_data[j++] = 0x03;
}
send_glue_data[j++] = 0x01;
for (size_t k = 0; k < 4; k++)
{
send_glue_data[j++] = (loadFeet[k] & 0xff);
send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff);
send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff);
send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff);
}
for (size_t k = 0; k < 4; k++)
{
send_glue_data[j++] = (circlepnt[k] & 0xff);
send_glue_data[j++] = ((circlepnt[k] >> 8) & 0xff);
send_glue_data[j++] = ((circlepnt[k] >> 16) & 0xff);
send_glue_data[j++] = ((circlepnt[k] >> 24) & 0xff);
}
send_glue_data[0] = CT_MOTOR;
send_glue_data[1] = CT_GLUEDISPENSER_SET;
int len = j - 3;
send_glue_data[2] = len;
m_WriteByte = Send_Command(0, (const char*)send_glue_data, m_SendDataLength);
Sleep(20);
}
break;
}
default:
break;
}
}
/*send_glue_data[0] = CT_MOTOR;
send_glue_data[1] = CT_GLUEDISPENSER_START;
send_glue_data[2] = runCount;*/
}
return rStatus;
}
HSI_STATUS HSI_Motion::GlueDispenserStart(double xOffset, double yOffset, double qOffset)
{
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
int xSkew = xOffset / m_Resolution[1];
int ySkew = yOffset / m_Resolution[2];
int qSkew = qOffset / m_Resolution[1];
unsigned char send_glue_data[64] = { 0 };
send_glue_data[0] = CT_MOTOR;
send_glue_data[1] = CT_GLUEDISPENSER_START;
send_glue_data[2] = GlueDispenserindexNum + 1;
send_glue_data[3] = GluerunCount;
send_glue_data[4] = (m_SetPotion_StartSpeed[1] & 0xff);
send_glue_data[5] = ((m_SetPotion_StartSpeed[1] >> 8) & 0xff);
send_glue_data[6] = (m_SetPotion_DriveSpeed[1] & 0xff);
send_glue_data[7] = ((m_SetPotion_DriveSpeed[1] >> 8) & 0xff);
send_glue_data[8] = (m_SetPotion_Line[1] & 0xff);
send_glue_data[9] = ((m_SetPotion_Line[1] >> 8) & 0xff);
send_glue_data[10] = (xSkew & 0xff);
send_glue_data[11] = ((xSkew >> 8) & 0xff);
send_glue_data[12] = (ySkew & 0xff);
send_glue_data[13] = ((ySkew >> 8) & 0xff);
send_glue_data[14] = (qSkew & 0xff);
send_glue_data[15] = ((qSkew >> 8) & 0xff);
g_pLogger->SendAndFlushWithTime(L"[GlueDispenser] m_SetPotion_StartSpeed = %d, m_SetPotion_DriveSpeed = %d,m_SetPotion_Line = %d \n", m_SetPotion_StartSpeed[1], m_SetPotion_DriveSpeed[1], m_SetPotion_Line[1]);
m_WriteByte = Send_Command(0, (const char*)send_glue_data, m_SendDataLength);
Sleep(5);
g_IsClose = false;
bUseGlueDispenser = true;
m_Thread_State = HSI_THREAD_RUNNING;
bRunGlueDispenser = HSI_THREAD_RUNNING;
SetEvent(m_hTriggerEvent);
}
return rStatus;
}
void HSI_Motion::GluedispenserDone()//涂胶机完成
{
if (bRunGlueDispenser == HSI_THREAD_RUNNING)
{
Sleep(100);
int timeStart = GetTickCount();
do
{
if (g_IsClose)
{
g_IsClose = false;
break;
}
if (m_SO7_Serial.m_RecvData[0] == 2)
{
if ((m_SO7_Serial.m_RecvData[60] & 0x0f))
{
g_pLogger->SendAndFlushWithTime(L"[GluedispenserDone] Run End Normal\n");//涂胶机完成
m_SO7_Serial.m_RecvData[60] = 0;
break;
}
}
Sleep(1);
int timeEnd = GetTickCount();
if (timeStart - timeEnd > 10 * 1000)
{
g_pLogger->SendAndFlushWithTime(L"[GluedispenserDone] Run End Abnormal\n");
break;
}
} while (true);
bRunGlueDispenser = HSI_THREAD_PAUSED;
sEvenProp.Init();
sEvenProp.EventType = HSI_EVENT_FUNCTION;
sEvenProp.EventID = HSI_EVENT_MOTION_DISPENSER;
sEvenProp.EventResponse = HSI_EVENT_FUNCTION_OK;
EventCallback(sEvenProp);
}
}
//===========================================================================
HSI_STATUS HSI_Motion::GetPntsDistance(double& pDistance, int& spTimeCount)
{
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
if (SetPotionRunEnd)
{
g_pLogger->SendAndFlushWithTime(L"[GetPntsDistance] PTPDistance = %.4f,spTimeCount = %d\n", PntToPntDistance, SpTimeCount);
SetPotionRunEnd = false;
pDistance = PntToPntDistance;
spTimeCount = SpTimeCount;
}
else
{
pDistance = -100;
spTimeCount = -1;
}
}
return rStatus;
}
//========================运动控制参数读取及设置===================================================
int HSI_Motion::SpeedPercent(int AxisNum, double &Speed, int &DirveSpeed, int &StartSpeed, int &AccLine, int &DecLine, int &AccCurve, int &DecCurve)
{
int MovetoSpeedGear = 0;
int flag = 1;
if (fabs(Speed) > 1.01)
{
MovetoSpeedGear = 0;
}
else if (fabs(Speed) > 0.81)
{
DirveSpeed = m_JogDriveSpeed[AxisNum][0];
StartSpeed = m_JogStartSpeed[AxisNum][0];
AccLine = m_JogAccLine[AxisNum][0];
DecLine = m_JogDecLine[AxisNum][0];
AccCurve = m_JogAccCurve[AxisNum][0];
DecCurve = m_JogDecCurve[AxisNum][0];
}
else if (fabs(Speed) > 0.61)
{
DirveSpeed = m_JogDriveSpeed[AxisNum][1];
StartSpeed = m_JogStartSpeed[AxisNum][1];
AccLine = m_JogAccLine[AxisNum][1];
DecLine = m_JogDecLine[AxisNum][1];
AccCurve = m_JogAccCurve[AxisNum][1];
DecCurve = m_JogDecCurve[AxisNum][1];
}
else if (fabs(Speed) > 0.41)
{
DirveSpeed = m_JogDriveSpeed[AxisNum][2];
StartSpeed = m_JogStartSpeed[AxisNum][2];
AccLine = m_JogAccLine[AxisNum][2];
DecLine = m_JogDecLine[AxisNum][2];
AccCurve = m_JogAccCurve[AxisNum][2];
DecCurve = m_JogDecCurve[AxisNum][2];
}
else if (fabs(Speed) > 0.21)
{
DirveSpeed = m_JogDriveSpeed[AxisNum][3];
StartSpeed = m_JogStartSpeed[AxisNum][3];
AccLine = m_JogAccLine[AxisNum][3];
DecLine = m_JogDecLine[AxisNum][3];
AccCurve = m_JogAccCurve[AxisNum][3];
DecCurve = m_JogDecCurve[AxisNum][3];
}
else if (fabs(Speed) > 0.01)
{
DirveSpeed = m_JogDriveSpeed[AxisNum][4];
StartSpeed = m_JogStartSpeed[AxisNum][4];
AccLine = m_JogAccLine[AxisNum][4];
DecLine = m_JogDecLine[AxisNum][4];
AccCurve = m_JogAccCurve[AxisNum][4];
DecCurve = m_JogDecCurve[AxisNum][4];
}
else
{
MovetoSpeedGear = 0;
}
Speed = MovetoSpeedGear * flag;
return (int)Speed;
}
//==========================JoyStick运动控制参数读取及设置=================================================
bool HSI_Motion::SpeedPercentJoyStick(int AxisNum, long &Speed, int &DirveSpeed, int &StartSpeed, int &AccLine, int &DecLine, int &AccCurve, int &DecCurve)
{
Speed = Speed > 1000 ? 1000 : Speed;
double dSpeedRate = (double)Speed / 1000;
DirveSpeed = m_JogDriveSpeed[AxisNum][0] * (dSpeedRate*dSpeedRate);
if (DirveSpeed >m_JogDriveSpeed[AxisNum][1])
{
DirveSpeed = m_JogDriveSpeed[AxisNum][0];
StartSpeed = m_JogStartSpeed[AxisNum][0];
AccLine = m_JogAccLine[AxisNum][0];
DecLine = m_JogDecLine[AxisNum][0];
AccCurve = m_JogAccCurve[AxisNum][0];
DecCurve = m_JogDecCurve[AxisNum][0];
}
else if (DirveSpeed >m_JogDriveSpeed[AxisNum][2])
{
StartSpeed = m_JogStartSpeed[AxisNum][1];
AccLine = m_JogAccLine[AxisNum][1];
DecLine = m_JogDecLine[AxisNum][1];
AccCurve = m_JogAccCurve[AxisNum][1];
DecCurve = m_JogDecCurve[AxisNum][1];
}
else if (DirveSpeed >m_JogDriveSpeed[AxisNum][3])
{
StartSpeed = m_JogStartSpeed[AxisNum][2];
AccLine = m_JogAccLine[AxisNum][2];
DecLine = m_JogDecLine[AxisNum][2];
AccCurve = m_JogAccCurve[AxisNum][2];
DecCurve = m_JogDecCurve[AxisNum][2];
}
else if (DirveSpeed >m_JogDriveSpeed[AxisNum][4])
{
StartSpeed = m_JogStartSpeed[AxisNum][3];
AccLine = m_JogAccLine[AxisNum][3];
DecLine = m_JogDecLine[AxisNum][3];
AccCurve = m_JogAccCurve[AxisNum][3];
DecCurve = m_JogDecCurve[AxisNum][3];
}
else if (DirveSpeed == 0)
{
return false;
}
else
{
StartSpeed = m_JogStartSpeed[AxisNum][4];
AccLine = m_JogAccLine[AxisNum][4];
DecLine = m_JogDecLine[AxisNum][4];
AccCurve = m_JogAccCurve[AxisNum][4];
DecCurve = m_JogDecCurve[AxisNum][4];
}
return true;
}
//===========================================================================
HSI_STATUS HSI_Motion::GetSpeedXyz(int AxisNum, double &Speed)
{
g_pLogger->SendAndFlushWithTime(L"[GetSpeedXyz] In\n");
auto rStatus = HSI_STATUS_NORMAL;
short AxisNumber = AxisConvertIndex(AxisNum);
if (1==m_iSpeedType)
{
Speed = m_SetPotion_DriveSpeed[AxisNumber] * (m_Resolution[AxisNumber] * 50);
}
else
{
Speed = m_SetPotion_DriveSpeed[AxisNumber];
}
g_pLogger->SendAndFlushWithTime(L"[GetSpeedXyz] Out\n");
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::SetSpeedXyz(double Speed)
{
auto rStatus = HSI_STATUS_NORMAL;
g_pLogger->SendAndFlushWithTime(L"[SetSpeedXyz] In\n");
if (Speed >= -1 && Speed <= 0)
{
g_pLogger->SendAndFlushWithTime(L"[SetSpeedXyz] Speed = %f, m_Jog_Auto_Focus = %f\n", Speed, m_Jog_Auto_Focus);
m_JogDriveSpeed[3][4] = (int)fabs(Speed * m_Jog_Auto_Focus);
g_pLogger->SendAndFlushWithTime(L"[SetSpeedXyz] m_Jog_SpeedZ[3][4] = %f\n", m_JogDriveSpeed[3][4]);
}
else
{
g_pLogger->SendAndFlushWithTime(L"[SetSpeedXyz] Speed = %f\n", Speed);
if (1 == m_iSpeedType)
{
m_SetPotion_DriveSpeed[1] = (int)(Speed / (m_Resolution[1] * 50));
if (m_SetPotion_DriveSpeed[1] > 1000 / (m_Resolution[1] * 50))
{
m_SetPotion_DriveSpeed[1] = 1000 / (m_Resolution[1] * 50);
}
if (m_SetPotion_DriveSpeed[1] < 0)
{
m_SetPotion_DriveSpeed[1] = 0;
}
}
else
{
m_SetPotion_DriveSpeed[1] = (int)Speed;
if (m_SetPotion_DriveSpeed[1] > 1000 / (m_Resolution[1] * 50))
{
m_SetPotion_DriveSpeed[1] = 1000 / (m_Resolution[1] * 50);
}
if (m_SetPotion_DriveSpeed[1] < 0)
{
m_SetPotion_DriveSpeed[1] = 0;
}
}
}
g_pLogger->SendAndFlushWithTime(L"[SetSpeedXyz] Out\n");
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::GetFocusSpeed(double &Speed)
{
g_pLogger->SendAndFlushWithTime(L"[GetFocusSpeed] In\n");
auto rStatus = HSI_STATUS_NORMAL;
if (1==m_iSpeedType)
{
Speed = m_JogDriveSpeed[3][4] * (m_Resolution[3] * 50);
}
else
{
Speed = m_JogDriveSpeed[3][4];
}
g_pLogger->SendAndFlushWithTime(L"[GetFocusSpeed] Out\n");
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::SetFocusSpeed(double Speed)
{
auto rStatus = HSI_STATUS_NORMAL;
g_pLogger->SendAndFlushWithTime(L"[SetFocusSpeed] In\n");
g_pLogger->SendAndFlushWithTime(L"[SetFocusSpeed] Speed = %f, m_Jog_Auto_Focus = %f\n", Speed, m_Jog_Auto_Focus);
if (1==m_iSpeedType)
{
m_JogDriveSpeed[3][4] = Speed / (m_Resolution[3] * 50);
}
else
{
m_JogDriveSpeed[3][4] = Speed;
}
g_pLogger->SendAndFlushWithTime(L"[SetFocusSpeed] m_Jog_SpeedZ[3][4] = %f\n", m_JogDriveSpeed[4][3]);
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::GetAccelerationXyz(double &AccelX, double &AccelY, double &AccelZ)
{
auto rStatus = HSI_STATUS_NORMAL;
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::SetAccelerationXyz(double AccelX, double AccelY, double AccelZ)
{
auto rStatus = HSI_STATUS_NORMAL;
return rStatus;
}
//===========================================================================
double HSI_Motion::LimitOver(UINT AxisTypes, double &LimitPos)
{
short AxisNumber = AxisConvertIndex(AxisTypes);
if (g_pHSI_Motion)
{
switch (AxisNumber)
{
case 1: //轴1
{
if (LimitPos >= m_P_Work_Limit[1])
{
LimitPos = m_P_Work_Limit[1] - 5 * m_Resolution[1];
}
if (LimitPos <= m_N_Work_Limit[1])
{
LimitPos = m_N_Work_Limit[1] + 5 * m_Resolution[1];
}
break;
}
case 2:
{
if (LimitPos >= m_P_Work_Limit[2])
{
LimitPos = m_P_Work_Limit[2] - 5 * m_Resolution[2];
}
if (LimitPos <= m_N_Work_Limit[2])
{
LimitPos = m_N_Work_Limit[2] + 5 * m_Resolution[2];
}
break;
}
case 3:
{
if (LimitPos >= m_P_Work_Limit[3])
{
LimitPos = m_P_Work_Limit[3] - 5 * m_Resolution[3];
}
if (LimitPos <= m_N_Work_Limit[3])
{
LimitPos = m_N_Work_Limit[3] + 5 * m_Resolution[3];
}
break;
}
case 4:
{
if (LimitPos >= m_P_Work_Limit[4])
{
LimitPos = m_P_Work_Limit[4] - 5 * m_Resolution[4];
}
if (LimitPos <= m_N_Work_Limit[4])
{
LimitPos = m_N_Work_Limit[4] + 5 * m_Resolution[4];
}
break;
}
default:
break;
}
}
return LimitPos;
}
//===========================================================================
short HSI_Motion::AxisConvertIndex(UINT AxisTypes)
{
short AxisNumber(0);
switch (AxisTypes)
{
case HSI_MOTION_AXIS_X:
{
AxisNumber = 0x01;
break;
}
case HSI_MOTION_AXIS_Y:
{
AxisNumber = 0x02;
break;
}
case HSI_MOTION_AXIS_Z:
{
AxisNumber = 0x03;
break;
}
case HSI_MOTION_AXIS_R:
{
AxisNumber = 0x04;
break;
}
default:
{
g_pLogger->SendAndFlushWithTime(L"AxisConvertIndex failed,AxisTypes = %d,AxisNumber = %d\n", AxisTypes, AxisNumber);
break;
}
}
return AxisNumber;
}
//===========================================================================
short HSI_Motion::IndexConvertAxis(int Index)
{
short AxisNumber(0);
switch (Index)
{
case 1:
{
AxisNumber = AXIS_X;
break;
}
case 2:
{
AxisNumber = AXIS_Y;
break;
}
case 3:
{
AxisNumber = AXIS_Z;
break;
}
case 4:
{
AxisNumber = AXIS_U;
break;
}
default:
{
g_pLogger->SendAndFlushWithTime(L"IndexConvertAxis failed,Index = %d,AxisTypes = %d\n", Index, AxisNumber);
break;
}
}
return AxisNumber;
}
//===========================================================================
HSI_STATUS HSI_Motion::IsSupportedEx(UINT AxisTypes, UINT &Types)
{
auto rStatus = HSI_STATUS_NORMAL;
Types = 1;
short AxisNumber = AxisConvertIndex(AxisTypes);
if (AxisNumber < 1 && AxisNumber > 4)
{
rStatus = HSI_STATUS_NOT_SUPPORTED;
}
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::StartupEx(UINT AxisTypes, bool bHome)
{
auto rStatus = HSI_STATUS_NORMAL;
g_pLogger->SendAndFlushWithTime(L"[StartupEx] In\n");
g_pLogger->SendAndFlushWithTime(L"[StartupEx] Out\n");
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::GetScaleResolutionEx(UINT AxisTypes, double &Scale)
{
auto rStatus = HSI_STATUS_NORMAL;
short AxisNumber = AxisConvertIndex(AxisTypes);
if (g_pHSI_Motion)
{
Scale = m_Resolution[AxisNumber];
}
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::SetScaleResolutionEx(UINT AxisTypes, double Scale)
{
auto rStatus = HSI_STATUS_NORMAL;
short AxisNumber = AxisConvertIndex(AxisTypes);
if (g_pHSI_Motion)
{
m_Resolution[AxisNumber] = (int)(Scale * 10000);
}
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::GetPositionEx(UINT AxisTypes, double &Position, double &Time)
{
auto rStatus = HSI_STATUS_NORMAL;
UNREFERENCED_PARAMETER(AxisTypes);
if (g_pHSI_Motion)
{
double position[4] = { 0.0 };
short AxisNumber = AxisConvertIndex(AxisTypes);
Position = m_PosForAllAxis[AxisNumber];
}
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::SetPositionEx(UINT AxisTypes, double Position, HSI_MOTION_MOVE_TYPE eType, double dSpeedGear)
{
auto rStatus = HSI_STATUS_NORMAL;
UNREFERENCED_PARAMETER(AxisTypes);
if (g_pHSI_Motion)
{
g_pLogger->SendAndFlushWithTime(L"[SetPositionEx] In\n");
short AxisNumber = AxisConvertIndex(AxisTypes);
double Time = 0.0;
double position[5] = { 0.0 };
for (size_t i = 0; i < 5; i++)
{
position[i] = m_PosForAllAxis[i];
}
position[AxisNumber] = Position;
SetPositionXyza(AxisTypes, position[1], position[2], position[3], position[4], eType, dSpeedGear);
g_pLogger->SendAndFlushWithTime(L"[SetPositionEx] Out\n");
}
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::SetPositionStep(UINT AxisTypes, double Position, HSI_MOTION_MOVE_TYPE eType, double dSpeedGear)
{
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
}
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::GetSpeedEx(UINT AxisTypes, double &Speed)
{
auto rStatus = HSI_STATUS_NORMAL;
g_pLogger->SendAndFlushWithTime(L"[GetSpeedEx] In\n");
short AxisNumber = AxisConvertIndex(AxisTypes);
if (AxisNumber >= 1 && AxisNumber <= 4)
{
if (m_motorType == 1)
{
if (1==m_iSpeedType)
{
Speed = m_SetPotion_DriveSpeed[1] * (m_Resolution[1] * 50);
}
else
{
Speed = m_SetPotion_DriveSpeed[1];
}
}
else
{
if (1==m_iSpeedType)
{
Speed = m_SetPotion_DriveSpeed[AxisNumber] * (m_Resolution[AxisNumber] * 50);
}
else
{
Speed = m_SetPotion_DriveSpeed[AxisNumber];
}
}
}
g_pLogger->SendAndFlushWithTime(L"[GetSpeedEx] Out\n");
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::SetSpeedEx(UINT AxisTypes, double Speed)
{
auto rStatus = HSI_STATUS_NORMAL;
g_pLogger->SendAndFlushWithTime(L"[SetSpeedEx] In\n");
short AxisNumber = AxisConvertIndex(AxisTypes);
if (Speed <= 0)
{
m_SetPotion_DriveSpeed[AxisNumber] = 0;
}
if (1==m_iSpeedType)
{
if (Speed >= 65000 * (m_Resolution[AxisNumber] * 50))
{
m_SetPotion_DriveSpeed[AxisNumber] = 65000;
}
}
else
{
if (Speed >= 65000)
{
m_SetPotion_DriveSpeed[AxisNumber] = 65000;
}
}
if (AxisNumber >= 1 && AxisNumber <= 4)
{
if (m_ForSoft == 1)
{
if (m_motorType == 1)
{
if (1==m_iSpeedType)
{
m_SetPotion_DriveSpeed[1] = (int)Speed/(m_Resolution[1] * 50);
}
else
{
m_SetPotion_DriveSpeed[1] = (int)Speed;
}
}
else
{
g_pLogger->SendAndFlushWithTime(L"[SetSpeedEx] AxisNumber = %d,Speed = %f \n", AxisNumber, Speed);
//if (!bSaveSpeedFlag)
//{
// bSaveSpeedFlag = true;
// m_SaveAxisNum = AxisNumber;
// m_SaveAxisSpeed = m_SetPotion_DriveSpeed[AxisNumber];
// g_pLogger->SendAndFlushWithTime(L"[SetSpeedEx] m_SaveAxisNum = %d,m_SaveAxisSpeed = %d \n", m_SaveAxisNum, m_SaveAxisSpeed);
//}
//if (bSaveSpeedFlag && ((int)Speed == m_SetPotion_DriveSpeed[1] * (m_Resolution[1] * 50)))
//{
// bSaveSpeedFlag = false;
// m_SetPotion_DriveSpeed[m_SaveAxisNum] = m_SaveAxisSpeed;
// return rStatus;
//}
if (1==m_iSpeedType)
{
m_SetPotion_DriveSpeed[AxisNumber] = (int)(Speed / (m_Resolution[AxisNumber] * 50));
}
else
{
m_SetPotion_DriveSpeed[AxisNumber] = (int)Speed;
}
g_pLogger->SendAndFlushWithTime(L"[SetSpeedEx] AxisNumber = %d\n", AxisNumber);
}
}
else
{
if (m_motorType == 1)
{
m_SetPotion_DriveSpeed[AxisNumber] = (int)(Speed / (m_Resolution[AxisNumber] * 50));
}
else
{
m_SetPotion_DriveSpeed[AxisNumber] = (int)(Speed / (m_Resolution[AxisNumber] * 50));
}
}
}
g_pLogger->SendAndFlushWithTime(L"[SetSpeedEx] Out\n");
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::GetAccelerationEx(UINT AxisTypes, double &Accel)
{
auto rStatus = HSI_STATUS_NORMAL;
short AxisNumber = AxisConvertIndex(AxisTypes);
if (AxisNumber >= 1 && AxisNumber <= 4)
{
if (m_motorType == 1)
{
Accel = m_SetPotion_StartSpeed[1] * (m_Resolution[1] * 500);
}
else
{
Accel = m_SetPotion_StartSpeed[AxisNumber] * (m_Resolution[AxisNumber] * 500);
}
}
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::SetAccelerationEx(UINT AxisTypes, double Accel)
{
auto rStatus = HSI_STATUS_NORMAL;
short AxisNumber = AxisConvertIndex(AxisTypes);
if (Accel <= 0)
{
m_SetPotion_StartSpeed[AxisNumber] = 1;
}
if (AxisNumber >= 1 && AxisNumber <= 4)
{
if (m_motorType == 1)
{
m_SetPotion_StartSpeed[AxisNumber] = (int)(Accel / (m_Resolution[AxisNumber] * 500));
}
else
{
m_SetPotion_StartSpeed[AxisNumber] = (int)(Accel / (m_Resolution[AxisNumber] * 500));
}
}
return rStatus;
}
//===========================================================================
VOID HSI_Motion::EventCallback(sHSIEventProperties& sEventProp)
{
WaitForSingleObject(m_Thread_Mutex, INFINITE);
if (g_pHSI_Sevenocean_EF3)
{
g_pHSI_Sevenocean_EF3->EventCallback(sEventProp);
}
ReleaseMutex(m_Thread_Mutex);
}
//===========================================================================
void HSI_Motion::CreateThread()
{
if (m_Thread_Id == NULL)
{
m_Thread_State = HSI_THREAD_RUNNING;
m_hTriggerEvent = CreateEvent(NULL, FALSE, NULL, NULL);
m_Thread_Id = ::CreateThread(
(LPSECURITY_ATTRIBUTES)NULL,
0,
(LPTHREAD_START_ROUTINE)m_Thread,
(LPVOID)this,
0,
NULL);
m_Thread_Mutex = CreateMutex(NULL, FALSE, NULL);
TRACE("CreateThread!\n");
}
}
//===========================================================================
void HSI_Motion::CloseThread()
{
m_Thread_State = HSI_THREAD_EXIT;
Sleep(20);
SetEvent(m_hTriggerEvent);
if (m_Thread_Id)
{
DWORD dwCode = STILL_ACTIVE;
while (dwCode == STILL_ACTIVE)
{
GetExitCodeThread(m_Thread_Id, &dwCode);
Sleep(1);
}
}
SetEvent(m_hTriggerEvent);
CloseHandle(m_hTriggerEvent);
m_Thread_State = HSI_THREAD_EXIT;
ReleaseMutex(m_Thread_Mutex);
CloseHandle(m_Thread_Mutex);
m_Thread_Id = NULL;
m_hTriggerEvent = NULL;
m_Thread_Mutex = NULL;
TRACE("CloseThread!\n");
}
//===========================================================================
unsigned __stdcall HSI_Motion::m_Thread(LPVOID pThis)
{
HSI_Motion* _This = (HSI_Motion*)pThis;
for (;;)
{
TRACE("m_Thread!\n");
if (m_Thread_State == THREAD_EXIT)
ExitThread(0);
WaitForSingleObject(m_hTriggerEvent, INFINITE);
switch (m_Thread_State)
{
case HSI_THREAD_RUNNING:
{
TRACE("HSI_THREAD_RUNNING.\r\n");
if (bRunGlueDispenser == HSI_THREAD_RUNNING)
_This->GluedispenserDone();
else
{
_This->UpdateMotionState();
_This->UpdateMotionStateEx();
}
break;
}
case HSI_THREAD_PAUSED:
{
TRACE("HSI_THREAD_PAUSED.\r\n");
break;
}
case HSI_THREAD_EXIT:
{
TRACE("HSI_THREAD_EXIT.\r\n");
ExitThread(0);
break;
}
default:
break;
}
}
}
//===========================================================================
void HSI_Motion::CreateThreadProbe()
{
if (m_Thread_IdProbe == NULL)
{
m_Thread_StateProbe = HSI_THREAD_RUNNING;
m_hTriggerEventProbe = CreateEvent(NULL, FALSE, NULL, NULL);
m_Thread_IdProbe = ::CreateThread(
(LPSECURITY_ATTRIBUTES)NULL,
0,
(LPTHREAD_START_ROUTINE)m_ThreadProbe,
(LPVOID)this,
0,
NULL);
m_Thread_MutexProbe = CreateMutex(NULL, FALSE, NULL);
TRACE("CreateThread!\n");
}
}
//===========================================================================
void HSI_Motion::CloseThreadProbe()
{
m_Thread_StateProbe = HSI_THREAD_EXIT;
Sleep(20);
SetEvent(m_hTriggerEventProbe);
if (m_Thread_IdProbe)
{
DWORD dwCode = STILL_ACTIVE;
while (dwCode == STILL_ACTIVE)
{
GetExitCodeThread(m_Thread_IdProbe, &dwCode);
Sleep(1);
}
}
SetEvent(m_hTriggerEventProbe);
CloseHandle(m_hTriggerEventProbe);
m_Thread_StateProbe = HSI_THREAD_EXIT;
ReleaseMutex(m_Thread_MutexProbe);
CloseHandle(m_Thread_MutexProbe);
m_Thread_IdProbe = NULL;
m_hTriggerEventProbe = NULL;
m_Thread_MutexProbe = NULL;
TRACE("CloseThread!\n");
}
//===========================================================================
unsigned __stdcall HSI_Motion::m_ThreadProbe(LPVOID pThis)
{
HSI_Motion* _This = (HSI_Motion*)pThis;
for (;;)
{
TRACE("m_Thread!\n");
if (m_Thread_StateProbe == THREAD_EXIT)
ExitThread(0);
WaitForSingleObject(m_hTriggerEventProbe, INFINITE);
switch (m_Thread_StateProbe)
{
case HSI_THREAD_RUNNING:
{
TRACE("HSI_THREAD_RUNNING.\r\n");
_This->UpdateMotionStateProbe();
break;
}
case HSI_THREAD_PAUSED:
{
TRACE("HSI_THREAD_PAUSED.\r\n");
break;
}
case HSI_THREAD_EXIT:
{
TRACE("HSI_THREAD_EXIT.\r\n");
ExitThread(0);
break;
}
default:
break;
}
}
}
//===========================================================================
void HSI_Motion::CreateThreadIO()
{
if (m_Thread_IdIO == NULL)
{
m_Thread_StateIO = HSI_THREAD_RUNNING;
m_hTriggerEventIO = CreateEvent(NULL, FALSE, NULL, NULL);
m_Thread_IdIO = ::CreateThread(
(LPSECURITY_ATTRIBUTES)NULL,
0,
(LPTHREAD_START_ROUTINE)m_ThreadIO,
(LPVOID)this,
0,
NULL);
m_Thread_MutexIO = CreateMutex(NULL, FALSE, NULL);
TRACE("CreateThreadIO!\n");
}
}
//===========================================================================
void HSI_Motion::CloseThreadIO()
{
m_Thread_StateIO = HSI_THREAD_EXIT;
Sleep(20);
SetEvent(m_hTriggerEventIO);
if (m_Thread_IdIO)
{
DWORD dwCode = STILL_ACTIVE;
while (dwCode == STILL_ACTIVE)
{
GetExitCodeThread(m_Thread_IdIO, &dwCode);
Sleep(1);
}
}
SetEvent(m_hTriggerEventIO);
CloseHandle(m_hTriggerEventIO);
m_Thread_StateIO = HSI_THREAD_EXIT;
ReleaseMutex(m_Thread_MutexIO);
CloseHandle(m_Thread_MutexIO);
m_Thread_IdIO = NULL;
m_hTriggerEventIO = NULL;
m_Thread_MutexIO = NULL;
TRACE("CloseThreadIO!\n");
}
//===========================================================================
unsigned __stdcall HSI_Motion::m_ThreadIO(LPVOID pThis)
{
HSI_Motion* _This = (HSI_Motion*)pThis;
for (;;)
{
TRACE("m_ThreadIO!\n");
if (m_Thread_StateIO == THREAD_EXIT)
ExitThread(0);
WaitForSingleObject(m_hTriggerEventIO, INFINITE);
switch (m_Thread_StateIO)
{
case HSI_THREAD_RUNNING:
{
TRACE("HSI_THREAD_RUNNING.\r\n");
_This->UpdateMotionStateIO();
break;
}
case HSI_THREAD_PAUSED:
{
TRACE("HSI_THREAD_PAUSED.\r\n");
break;
}
case HSI_THREAD_EXIT:
{
TRACE("HSI_THREAD_EXIT.\r\n");
ExitThread(0);
break;
}
default:
break;
}
}
}
//===========================================================================
void HSI_Motion::CreateThreadData()
{
if (m_Thread_IdData == NULL)
{
m_Thread_StateData = HSI_THREAD_RUNNING;
m_hTriggerEventData = CreateEvent(NULL, FALSE, NULL, NULL);
m_Thread_IdData = ::CreateThread(
(LPSECURITY_ATTRIBUTES)NULL,
0,
(LPTHREAD_START_ROUTINE)m_ThreadData,
(LPVOID)this,
0,
NULL);
m_Thread_MutexData = CreateMutex(NULL, FALSE, NULL);
TRACE("CreateThreadData!\n");
}
}
//===========================================================================
void HSI_Motion::CloseThreadData()
{
m_Thread_StateData = HSI_THREAD_EXIT;
Sleep(20);
SetEvent(m_hTriggerEventData);
if (m_Thread_IdData)
{
DWORD dwCode = STILL_ACTIVE;
while (dwCode == STILL_ACTIVE)
{
GetExitCodeThread(m_Thread_IdData, &dwCode);
Sleep(1);
}
}
SetEvent(m_hTriggerEventData);
CloseHandle(m_hTriggerEventData);
m_Thread_StateData = HSI_THREAD_EXIT;
ReleaseMutex(m_Thread_MutexData);
CloseHandle(m_Thread_MutexData);
m_Thread_IdData = NULL;
m_hTriggerEventData = NULL;
m_Thread_MutexData = NULL;
TRACE("CloseThreadData!\n");
}
//===========================================================================
unsigned __stdcall HSI_Motion::m_ThreadData(LPVOID pThis)
{
HSI_Motion* _This = (HSI_Motion*)pThis;
for (;;)
{
TRACE("m_ThreadData!\n");
if (m_Thread_StateData == THREAD_EXIT)
ExitThread(0);
WaitForSingleObject(m_hTriggerEventData, INFINITE);
switch (m_Thread_StateData)
{
case HSI_THREAD_RUNNING:
{
TRACE("HSI_THREAD_RUNNING.\r\n");
_This->UpdateMotionStateData();//获取最新位置命令
break;
}
case HSI_THREAD_PAUSED:
{
TRACE("HSI_THREAD_PAUSED.\r\n");
break;
}
case HSI_THREAD_EXIT:
{
TRACE("HSI_THREAD_EXIT.\r\n");
ExitThread(0);
break;
}
default:
break;
}
}
}
//===========================================================================
//JOG运行到软限位的运动调节
//===========================================================================
void HSI_Motion::UpdateMotionStateJOGStop()
{
unsigned char send_JogLimt_data[64] = { 0 };
UINT axis = 0;
double posx = 0.0;
double posy = 0.0;
double posz = 0.0;
double post = 0.0;
int slowdowndistance = 0;
while (m_Thread_StateJOGStop == HSI_THREAD_RUNNING)
{
while (jogMoving)
{
slowdowndistance = jogspeed * 13;
Sleep(3);
GetPositionXyz(axis, posx, posy, posz, post);
switch (jogAxisNum)
{
case 1:
{
if (jogDirFlag)
{
double limitx = m_P_Work_Limit[1];
if ((limitx - posx) / m_Resolution[1] < slowdowndistance)
{
send_JogLimt_data[0] = 0x01;
send_JogLimt_data[1] = 0x14;
send_JogLimt_data[2] = 0x01;
m_WriteByte = Send_Command(0, (const char*)send_JogLimt_data, m_SendDataLength);
Sleep(5);
jogMoving = false;
}
}
else
{
double limitx = m_N_Work_Limit[1];
if ((posx - limitx) / m_Resolution[1] < slowdowndistance)
{
send_JogLimt_data[0] = 0x01;
send_JogLimt_data[1] = 0x14;
send_JogLimt_data[2] = 0x01;
m_WriteByte = Send_Command(0, (const char*)send_JogLimt_data, m_SendDataLength);
Sleep(5);
jogMoving = false;
}
}
} break;
case 2:
{
if (jogDirFlag)
{
double limity = m_P_Work_Limit[2];
if ((limity - posy) / m_Resolution[2] < slowdowndistance)
{
send_JogLimt_data[0] = 0x01;
send_JogLimt_data[1] = 0x14;
send_JogLimt_data[2] = 0x02;
m_WriteByte = Send_Command(0, (const char*)send_JogLimt_data, m_SendDataLength);
Sleep(5);
jogMoving = false;
}
}
else
{
double limity = m_N_Work_Limit[2];
if ((posy - limity) / m_Resolution[2] < slowdowndistance)
{
send_JogLimt_data[0] = 0x01;
send_JogLimt_data[1] = 0x14;
send_JogLimt_data[2] = 0x02;
m_WriteByte = Send_Command(0, (const char*)send_JogLimt_data, m_SendDataLength);
Sleep(5);
jogMoving = false;
}
}
} break;
case 3:
{
if (jogDirFlag)
{
double limitz = m_P_Work_Limit[3];
if ((limitz - posz) / m_Resolution[3] < slowdowndistance)
{
send_JogLimt_data[0] = 0x01;
send_JogLimt_data[1] = 0x14;
send_JogLimt_data[2] = 0x04;
m_WriteByte = Send_Command(0, (const char*)send_JogLimt_data, m_SendDataLength);
Sleep(5);
jogMoving = false;
}
}
else
{
double limitz = m_N_Work_Limit[3];
if ((posz - limitz) / m_Resolution[3] < slowdowndistance)
{
send_JogLimt_data[0] = 0x01;
send_JogLimt_data[1] = 0x14;
send_JogLimt_data[2] = 0x04;
m_WriteByte = Send_Command(0, (const char*)send_JogLimt_data, m_SendDataLength);
Sleep(5);
jogMoving = false;
}
}
} break;
case 4:
{
//第四轴未添加
} break;
default:
break;
}
}
Sleep(2);
}
}
//===========================================================================
void HSI_Motion::CreateThreadJOGStop() //JOG运行到软限位的运动调节
{
if (m_Thread_IdJOGStop == NULL)
{
m_Thread_StateJOGStop = HSI_THREAD_RUNNING;
m_hTriggerEventJOGStop = CreateEvent(NULL, FALSE, NULL, NULL);
m_Thread_IdJOGStop = ::CreateThread(
(LPSECURITY_ATTRIBUTES)NULL,
0,
(LPTHREAD_START_ROUTINE)m_ThreadJOGStop,
(LPVOID)this,
0,
NULL);
m_Thread_MutexJOGStop = CreateMutex(NULL, FALSE, NULL);
TRACE("CreateThreadJOGStop!\n");
}
}
//===========================================================================
void HSI_Motion::CloseThreadJOGStop()
{
m_Thread_StateJOGStop = HSI_THREAD_EXIT;
Sleep(20);
SetEvent(m_hTriggerEventJOGStop);
if (m_Thread_IdData)
{
DWORD dwCode = STILL_ACTIVE;
while (dwCode == STILL_ACTIVE)
{
GetExitCodeThread(m_Thread_IdJOGStop, &dwCode);
Sleep(1);
}
}
SetEvent(m_hTriggerEventJOGStop);
CloseHandle(m_hTriggerEventJOGStop);
m_Thread_StateJOGStop = HSI_THREAD_EXIT;
ReleaseMutex(m_Thread_MutexJOGStop);
CloseHandle(m_Thread_MutexJOGStop);
m_Thread_IdJOGStop = NULL;
m_hTriggerEventJOGStop = NULL;
m_Thread_MutexJOGStop = NULL;
TRACE("CloseThreadJOGStop!\n");
}
//===========================================================================
unsigned __stdcall HSI_Motion::m_ThreadJOGStop(LPVOID pThis) //JOG运行到软限位的运动调节
{
HSI_Motion* _This = (HSI_Motion*)pThis;
for (;;)
{
TRACE("m_ThreadJOGStop!\n");
if (m_Thread_StateData == THREAD_EXIT)
ExitThread(0);
WaitForSingleObject(m_hTriggerEventJOGStop, INFINITE);
switch (m_Thread_StateJOGStop)
{
case HSI_THREAD_RUNNING:
{
TRACE("HSI_THREAD_RUNNING.\r\n");
_This->UpdateMotionStateJOGStop();
break;
}
case HSI_THREAD_PAUSED:
{
TRACE("HSI_THREAD_PAUSED.\r\n");
break;
}
case HSI_THREAD_EXIT:
{
TRACE("HSI_THREAD_EXIT.\r\n");
ExitThread(0);
break;
}
default:
break;
}
}
}
//===========================================================================
//无用
HSI_STATUS HSI_Motion::IOStep(bool RunSts)
{
auto rStatus = HSI_STATUS_NORMAL;
m_MSTRunFlag = RunSts;
if (g_pHSI_Motion)
{
}
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::IOprogram(byte* SendData, int length)
{
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
Send_Command(0, (const char*)SendData, m_SendDataLength);
}
return rStatus;
}
HSI_STATUS HSI_Motion::FindOriginTest(bool type)
{
auto rStatus = HSI_STATUS_NORMAL;
byte findorigdata[64] = { 0 };
findorigdata[0] = 0x04;
if (type == true)
{
findorigdata[1] = 0x18;
findorigdata[2] = (byte)(m_Resolution[1] * 10000);
Send_Command(0, (const char*)findorigdata, 64);
}
else
{
findorigdata[1] = 0x19;
findorigdata[2] = (byte)(m_Resolution[1] * 10000);
Send_Command(0, (const char*)findorigdata, 64);
}
return rStatus;
}
//===========================================================================
BOOL HSI_Motion::Send_Command(int com, const char* _SendData, DWORD SendDataLength)
{
BOOL rStatus = TRUE;
WaitForSingleObject(g_RW_Data_Mutex, INFINITE);
if (m_bConnected && (m_IsUseEF3 == 1) && (com == 0))
{
int iWriteByte = m_SO7_Serial.Send(_SendData, 64);
if (iWriteByte == 0)
{
int iError = GetLastError();
g_pLogger->SendAndFlushWithTime(L"[Send_Command] GetLastError = %d\n", iError);
if (iError == 1167)
{
g_pLogger->SendAndFlushWithTime(L"[Send_Command] GetLastError is 1167,EF3 device is no connect\n");
m_Thread_StateData = THREAD_PAUSED;
if (m_bConnected == true && g_IsClose == false)
{
g_pLogger->SendAndFlushWithTime(L"[Send_Command] GetLastError is 1167,EF3 device is no connect\n");
sEvenProp.Init();
sEvenProp.EventType = HSI_EVENT_ERROR;
sEvenProp.EventID = HSI_EVENT_MOTION;
sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK;
strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "EF3控制器已断开连接");
EventCallback(sEvenProp);
}
}
g_pLogger->SendAndFlushWithTime(L"[Send_Command] m_SO7_Serial[%d].Send failed\n", com);
rStatus = FALSE;
}
// Sleep(5);
// m_SO7_Serial.OnReceive();
}
ReleaseMutex(g_RW_Data_Mutex);
return rStatus;
};
/////////////////////////////////////////////////////////////////////////
#pragma region //网口通信8路 26路光源板通信
#pragma warning(disable:4996)
TCPIP_RETURN_CODE HSI_Motion::TCPConnect(int index, char* Address, u_short port)
{
TCPIP_RETURN_CODE rCode = TCPIP_CONNECT_OK;
if (!m_tcpCntFlag[index])
{
int retVal(0), errorCode(0);
first = true;
retVal = Init_Winsock();
if (!retVal)
{
return TCPIP_INIT_WINSOCK_ERROR;
}
m_socket[index] = socket(AF_INET, SOCK_STREAM, 0);
if (m_socket[index] == INVALID_SOCKET)
{
rCode = TCPIP_INVAILD_SOCKET;
}
else
{
int iMode = 1;
int i = 0;
int retVal = ioctlsocket(m_socket[index], FIONBIO, (u_long FAR*)&iMode);//非阻塞连接
if (retVal == SOCKET_ERROR)
{
closesocket(m_socket[index]);
WSACleanup();
return TCPIP_INIT_WINSOCK_ERROR;
}
SOCKADDR_IN addSrv;
addSrv.sin_addr.S_un.S_addr = inet_addr(Address);
addSrv.sin_family = AF_INET;
addSrv.sin_port = htons(port);
while (true)
{
retVal = connect(m_socket[index], (SOCKADDR*)&addSrv, sizeof(SOCKADDR));
if (retVal)
{
errorCode = WSAGetLastError();
if (errorCode == WSAEWOULDBLOCK)
{
i++;
if (i > 400)
break;
Sleep(5);
continue;
}
else if (errorCode == WSAEISCONN)
{
break;
}
else
{
first = true;
closesocket(m_socket[index]);
WSACleanup();
rCode = (TCPIP_RETURN_CODE)errorCode;
return rCode;
}
}
}
iMode = 0;
retVal = ioctlsocket(m_socket[index], FIONBIO, (u_long FAR*)&iMode);//设置阻塞
if (retVal == SOCKET_ERROR)
{
closesocket(m_socket[index]);
WSACleanup();
return TCPIP_INIT_WINSOCK_ERROR;
}
}
}
return rCode;
}
void HSI_Motion::DisConnect()
{
if (m_socket)
{
if (LightSend != 0 || IOSend != 0 || OrderSend != 0)
{
Sleep(20);
}
first = true;
Exit_Thread();
WSACleanup();
for (size_t i = 0; i < m_bISUseMoreLights; i++)
{
closesocket(m_socket[i]);
m_socket[i] = NULL;
}
}
}
unsigned __stdcall HSI_Motion::m_ThreadSendTCP(LPVOID pThis)
{
HSI_Motion* _This = (HSI_Motion*)pThis;
for (;;)
{
TRACE("m_Thread!\n");
Sleep(2);
if (m_ThreadTCP_State == TCPIP_THREAD_EXIT)
ExitThread(0);
switch (m_ThreadTCP_State)
{
case TCPIP_THREAD_RUNNING:
{
TRACE("TCPIP_THREAD_RUNNING.\r\n");
_This->TCPSend();
break;
}
case TCPIP_THREAD_PAUSED:
{
break;
}
case TCPIP_THREAD_EXIT:
{
ExitThread(0);
break;
}
default:
break;
}
};
}
TCPIP_RETURN_CODE HSI_Motion::TCPSend()
{
if (m_tcpCntFlag[m_selectedIndex])
{
bool recvFlag = false;
int bytesSend = 0;
if (LightSend > 0)
{
g_pLogger->SendAndFlushWithTime(L"[TCPSend] LightSend = %d,m_selectedIndex = %d\n", LightSend, m_selectedIndex);
LightSend--;
bytesSend = send(m_socket[m_selectedIndex], (const char*)lightdata, 64, 0);
for (size_t i = 0; i < 64; i++)
{
TempData[i] = lightdata[i];
}
}
else if (IOSend > 0)
{
IOSend--;
bytesSend = send(m_socket[m_selectedIndex], (const char*)IOdata, 64, 0);
for (size_t i = 0; i < 64; i++)
{
TempData[i] = IOdata[i];
}
}
else if (OrderSend > 0)
{
OrderSend--;
bytesSend = send(m_socket[m_selectedIndex], (const char*)Orderdata, 64, 0);
for (size_t i = 0; i < 64; i++)
{
TempData[i] = Orderdata[i];
}
}
else
{
if (m_Led8MotionFlag[m_selectedIndex])
{
//g_pLogger->SendAndFlushWithTime(L"[TCPSend] IOCheck8\n");
IOCheck[0] = 0x01;
IOCheck[1] = 0x04;
}
else
{
//g_pLogger->SendAndFlushWithTime(L"[TCPSend] IOCheck26\n");
IOCheck[0] = 0x01;
IOCheck[1] = 0x01;
}
bytesSend = send(m_socket[m_selectedIndex], (const char*)IOCheck, 64, 0);
for (size_t i = 0; i < 64; i++)
{
TempData[i] = IOCheck[i];
}
}
if (bytesSend == SOCKET_ERROR)
{
bytesSend = WSAGetLastError();
if (bytesSend == WSAEWOULDBLOCK || bytesSend == WSAENOTSOCK)
{
Sleep(2);
bytesSend = send(m_socket[m_selectedIndex], (const char*)TempData, 64, 0);
//have to wait for the next receive event
}
else
{
g_pLogger->SendAndFlushWithTime(L"[TCPSend] ERROR1 bytesSend = %d\n", bytesSend);
sEvenProp.Init();
sEvenProp.EventType = HSI_EVENT_ERROR;
sEvenProp.EventID = HSI_EVENT_MOTION;
sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK;
strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "光源板已断开连接");
EventCallback(sEvenProp);
m_ThreadTCP_State = TCPIP_THREAD_EXIT;
closesocket(m_socket[m_selectedIndex]);
WSACleanup();
return TCPIP_CONNECT_STATUS_UNKNOWN;
}
}
//ZeroMemory(tReciveData, TCPIP_MAX_DAT_SIZE);
int bytesReceived = 0;
int num = 0;
bytesReceived = recv(m_socket[m_selectedIndex], (char *)tReciveData, TCPIP_MAX_DAT_SIZE, 0);
if (bytesReceived == SOCKET_ERROR)
{
bytesReceived = WSAGetLastError();
if (bytesReceived == WSAEWOULDBLOCK || bytesReceived == WSAENOTSOCK)
{
//ZeroMemory(tReciveData, TCPIP_MAX_DAT_SIZE);
bytesReceived = recv(m_socket[m_selectedIndex], (char *)tReciveData, TCPIP_MAX_DAT_SIZE, 0);
if (bytesReceived == SOCKET_ERROR)
{
recvFlag = false;
}
else
{
recvFlag = true;
}
//if (num > 5)
//{
// break;
//}
//Sleep(1);
//num++;
////have to wait for the next receive event
//continue;
}
else
{
g_pLogger->SendAndFlushWithTime(L"[TCPReceived] ERROR1 bytesReceived = %d\n", bytesReceived);
sEvenProp.Init();
sEvenProp.EventType = HSI_EVENT_ERROR;
sEvenProp.EventID = HSI_EVENT_MOTION;
sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK;
strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "光源板已断开连接");
EventCallback(sEvenProp);
m_ThreadTCP_State = TCPIP_THREAD_EXIT;
closesocket(m_socket[m_selectedIndex]);
WSACleanup();
return TCPIP_CONNECT_STATUS_UNKNOWN;
}
}
else
{
recvFlag = true;
}
if (!recvFlag)
{
bytesSend = send(m_socket[m_selectedIndex], (const char*)TempData, 64, 0);
if (bytesSend == SOCKET_ERROR)
{
bytesSend = WSAGetLastError();
if (bytesSend == WSAEWOULDBLOCK)
{
bytesSend = send(m_socket[m_selectedIndex], (const char*)TempData, 64, 0);
if (bytesSend == SOCKET_ERROR)
{
g_pLogger->SendAndFlushWithTime(L"[TCPSend] ERROR2\n");
}
//have to wait for the next receive event
}
}
bytesReceived = recv(m_socket[m_selectedIndex], (char *)tReciveData, TCPIP_MAX_DAT_SIZE, 0);
if (bytesReceived == SOCKET_ERROR)
{
bytesReceived = WSAGetLastError();
if (bytesReceived == WSAEWOULDBLOCK)
{
//ZeroMemory(tReciveData, TCPIP_MAX_DAT_SIZE);
bytesReceived = recv(m_socket[m_selectedIndex], (char *)tReciveData, TCPIP_MAX_DAT_SIZE, 0);
if (bytesReceived == SOCKET_ERROR)
{
g_pLogger->SendAndFlushWithTime(L"[TCPReceived] ERROR2\n");
}
}
}
}
Sleep(5);
}
return TCPIP_CONNECT_OK;
}
void HSI_Motion::Create_Thread()
{
if (!m_ThreadTCP_Id)
{
m_ThreadTCP_State = TCPIP_THREAD_RUNNING;
m_ThreadTCP_Id = ::CreateThread((LPSECURITY_ATTRIBUTES)NULL, 0, (LPTHREAD_START_ROUTINE)m_ThreadSendTCP, (LPVOID)this, 0, NULL);
}
}
void HSI_Motion::Exit_Thread()
{
m_ThreadTCP_State = TCPIP_THREAD_EXIT;
if (m_ThreadTCP_Id)
{
DWORD dwCode = STILL_ACTIVE;
while (dwCode == STILL_ACTIVE)
{
GetExitCodeThread(m_ThreadTCP_Id, &dwCode);
Sleep(1);
}
}
m_ThreadTCP_State = TCPIP_THREAD_EXIT;
m_ThreadTCP_Id = NULL;
}
int HSI_Motion::Init_Winsock()
{
int nRet;
if (first)
{
WSADATA stWSAData; /* WinSock DLL Info */
nRet = WSAStartup(WSA_VERSION, &stWSAData);
if (nRet)
{
return FALSE;
}
first = false;
}
return TRUE;
}
#pragma endregion
//////////////////////////////////////////////////////////////////////////
//===========================================================================
HSI_STATUS HSI_Motion::CollectPos(bool isEnable, MOTOR_AXISCHOOES_CMD axis, short cycle)
{
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
unsigned char send_glue_data[64] = { 0 };
send_glue_data[0] = 0x01;
send_glue_data[1] = 0x1E;
send_glue_data[2] = isEnable ? 1 : 2;
send_glue_data[3] = ((cycle >> 8) & 0xff);
send_glue_data[4] = (cycle & 0xff);
send_glue_data[5] = axis;
Send_Command(0, (const char*)send_glue_data, 64);
}
return rStatus;
}
//===========================================================================
HSI_STATUS HSI_Motion::SetAllGears()
{
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
for (size_t i = 1; i < 5; i++)
{
unsigned char send_glue_data[64] = { 0 };
send_glue_data[0] = 0x01;
send_glue_data[1] = 0x01;
send_glue_data[2] = 0x01 << (i - 1); //轴号
send_glue_data[3] = 0x08;
for (size_t j = 0; j < 5; j++)
{
send_glue_data[4 + 6 * j] = ((m_JogStartSpeed[i][4 - j] >> 8) & 0xff);
send_glue_data[5 + 6 * j] = (m_JogStartSpeed[i][4 - j] & 0xff);
send_glue_data[6 + 6 * j] = ((m_JogDriveSpeed[i][4 - j] >> 8) & 0xff);
send_glue_data[7 + 6 * j] = (m_JogDriveSpeed[i][4 - j] & 0xff);
send_glue_data[8 + 6 * j] = ((m_JogAccLine[i][4 - j] >> 8) & 0xff);
send_glue_data[9 + 6 * j] = (m_JogAccLine[i][4 - j] & 0xff);
}
Send_Command(0, (const char*)send_glue_data, 64);
Sleep(10);
}
}
return rStatus;
}
//===========================================================================
Reference in New Issue View Git Blame Copy Permalink