From 36cff662a5fafd5b60b9ac88284e62de959a1aee Mon Sep 17 00:00:00 2001 From: "zhengxuan.zhang" Date: Mon, 23 Dec 2024 13:55:43 +0800 Subject: [PATCH] =?UTF-8?q?#0016:=20=E6=96=B0=E5=A2=9E=E5=8F=98=E7=84=A6?= =?UTF-8?q?=E5=8A=9F=E8=83=BD=E7=9A=84=E9=80=82=E9=85=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- HSI_HexagonMI_EF3/HSI.cpp | 18 +- HSI_HexagonMI_EF3/HSI_HexagonMI_EF3.vcxproj | 3 + .../HSI_HexagonMI_EF3.vcxproj.filters | 3 + HSI_HexagonMI_EF3/HSI_Motion.cpp | 4874 +++++++++-------- HSI_HexagonMI_EF3/HSI_Motion.h | 161 +- HSI_HexagonMI_EF3/version.h | 4 +- 6 files changed, 2543 insertions(+), 2520 deletions(-) diff --git a/HSI_HexagonMI_EF3/HSI.cpp b/HSI_HexagonMI_EF3/HSI.cpp index 6fc61a9..c164a2e 100644 --- a/HSI_HexagonMI_EF3/HSI.cpp +++ b/HSI_HexagonMI_EF3/HSI.cpp @@ -145,9 +145,8 @@ HSI_API HSI_STATUS WINAPI HSI_SHUTDOWM() //////////////////////////////////////////////////////////////////////////////// // MOTION API 运动相关API /////////////////////////////////////////////////////////////////////////////// -#pragma region Motion API +#pragma region Motion API Phase 1 -#pragma region 第一阶段实现的API功能 //=========================================================================== HSI_API HSI_STATUS WINAPI HSI_MOTION_STARTUP(bool bHome) { @@ -378,12 +377,6 @@ HSI_API HSI_STATUS WINAPI HSI_MOTION_IS_SUPPORTED(UINT& Types) } return rStatus; } -#pragma endregion - -#define USE_Motion_API -#ifdef USE_Motion_API - -#pragma region 后续考虑 //=========================================================================== HSI_API HSI_STATUS WINAPI HSI_MOTION_GET_FOCUSSPEED_XYZ(double& Speed) @@ -415,6 +408,15 @@ HSI_API HSI_STATUS WINAPI HSI_MOTION_SET_FOCUSSPEED_XYZ(double Speed) return rStatus; } + +#pragma endregion + +#define USE_Motion_API +#ifdef USE_Motion_API + +#pragma region 后续考虑 + + //=========================================================================== HSI_API HSI_STATUS WINAPI HSI_MOTION_SET_SPEED_XYZ(double Speed) { diff --git a/HSI_HexagonMI_EF3/HSI_HexagonMI_EF3.vcxproj b/HSI_HexagonMI_EF3/HSI_HexagonMI_EF3.vcxproj index e8d4191..1d4b2f5 100644 --- a/HSI_HexagonMI_EF3/HSI_HexagonMI_EF3.vcxproj +++ b/HSI_HexagonMI_EF3/HSI_HexagonMI_EF3.vcxproj @@ -122,6 +122,9 @@ copy "$(TargetDir)$(ProjectName).dll" "D:\HSI_Sevenocean_EF3.dll" Create + + + diff --git a/HSI_HexagonMI_EF3/HSI_HexagonMI_EF3.vcxproj.filters b/HSI_HexagonMI_EF3/HSI_HexagonMI_EF3.vcxproj.filters index 207a525..790b4b0 100644 --- a/HSI_HexagonMI_EF3/HSI_HexagonMI_EF3.vcxproj.filters +++ b/HSI_HexagonMI_EF3/HSI_HexagonMI_EF3.vcxproj.filters @@ -22,4 +22,7 @@ + + + \ No newline at end of file diff --git a/HSI_HexagonMI_EF3/HSI_Motion.cpp b/HSI_HexagonMI_EF3/HSI_Motion.cpp index a04d278..6820100 100644 --- a/HSI_HexagonMI_EF3/HSI_Motion.cpp +++ b/HSI_HexagonMI_EF3/HSI_Motion.cpp @@ -64,10 +64,9 @@ int HSI_Motion::bRunGlueDispenser = THREAD_PAUSED; //=========================================================================== HANDLE HSI_Motion::m_ThreadTCP_Id = nullptr; int HSI_Motion::m_ThreadTCP_State = TCPIP_THREAD_PAUSED; -SOCKET m_socket[4] = {0}; +SOCKET m_socket[4] = { 0 }; - - void HSI_Motion::ErrorsHandler() +void HSI_Motion::ErrorsHandler() { if (handleACS != ACSC_INVALID) { @@ -76,17 +75,16 @@ SOCKET m_socket[4] = {0}; int Received = 0; ErrorCode = GetLastError(); if (acsc_GetErrorString(handleACS, // communication handle - ErrorCode, // error code - ErrorStr, // buffer for the error explanation - 255, // available buffer length - &Received // number of actually received bytes + ErrorCode, // error code + ErrorStr, // buffer for the error explanation + 255, // available buffer length + &Received // number of actually received bytes )) { ErrorStr[Received] = '\0'; printf("Motion Error: %d [%s]\n", ErrorCode, ErrorStr); g_pLogger->SendAndFlushWithTime(L"[ACS Motion] Motion Error,Code: %d, %S\n", ErrorCode, ErrorStr); - //向上层发送错误信息 sEvenProp.Init(); sEvenProp.EventType = HSI_EVENT_FUNCTION; @@ -126,7 +124,7 @@ HSI_Motion::HSI_Motion() iMotionDirection = 1; m_InputStatus = 0; m_ForStatus = 0; - m_setPositionDelay = 0; //设置定位超时 + m_setPositionDelay = 0; //设置定位超时 m_setPositionPrecision = 0; //设置定位精度 m_IsUse_HSICompensation = 0; //是否启用HSI进行定位补偿 0为不启用 1为启用 默认为0 m_Compensation_Pluse = 20; //补偿脉冲数 @@ -161,7 +159,7 @@ HSI_Motion::HSI_Motion() m_IsUseJerk = 0; //是否启用急停 0为不启用 1为启用 t_start = 0; //获取jog运行的开始时间 t_end = 0; //获取jog运行的结束时间 - m_isOKGlint = 0; //是否开启ok/ng 闪烁 + m_isOKGlint = 0; //是否开启ok/ng 闪烁 m_selectedIndex = 0; m_setPositionNum = 5; m_axisStatus = 0; //运动各轴的状态 @@ -189,7 +187,7 @@ HSI_Motion::HSI_Motion() m_axisDirZ = 0; //探针运动时Z轴的运动方向 m_probeSeekSpeed = 0; - bUseGlueDispenser = false; //是否开启点胶 + bUseGlueDispenser = false; //是否开启点胶 m_iGlueStartSpeed = 1; m_iGlueDriveSpeed = 1; m_iGlueAccSpeed = 1; @@ -215,7 +213,7 @@ HSI_Motion::HSI_Motion() for (int i = 0; i < 4; i++) { - m_rockerHStartSpeed[i] = 5; //摇杆XYZ轴高初始速度 + m_rockerHStartSpeed[i] = 5; //摇杆XYZ轴高初始速度 m_rockerHDriveSpeed[i] = 20; //摇杆XYZ轴高驱动速度 m_rockerLStartSpeed[i] = 2; //摇杆XYZ轴低初始速度 m_rockerLDriveSpeed[i] = 10; //摇杆XYZ轴低驱动速度 @@ -271,7 +269,7 @@ HSI_Motion::HSI_Motion() m_Home_AddJogGears[i] = 4; m_Home_DecJogGears[i] = 3; m_SetPotion_StartSpeed[i] = 20; - + m_SetPotion_Line[i] = 150; m_SetPotion_Buffer[i] = 40; m_SetPotion_AccCurve[i] = 0; @@ -362,12 +360,12 @@ bool HSI_Motion::PortInit(int iSerialComPort, int iBuadRate) sprintf_s(buf, "COM%d", iSerialComPort); CString comName(buf); hCom = CreateFile(comName, - GENERIC_READ | GENERIC_WRITE, //允许读和写 - 0, //独占方式,串口必须为0 - nullptr, - OPEN_EXISTING, //打开而不是创建，串口必须为打开 - 0, //同步方式,同步执行时，函数直到操作完成后才返回 - nullptr); //串口必须为NULL + GENERIC_READ | GENERIC_WRITE, //允许读和写 + 0, //独占方式,串口必须为0 + nullptr, + OPEN_EXISTING, //打开而不是创建，串口必须为打开 + 0, //同步方式,同步执行时，函数直到操作完成后才返回 + nullptr); //串口必须为NULL if (hCom != (HANDLE)-1) { PurgeComm(hCom, PURGE_TXCLEAR | PURGE_RXCLEAR); @@ -451,7 +449,6 @@ HSI_STATUS HSI_Motion::Startup(HWND _hWnd, bool _bOfflineOnly) } #endif // OFFLINE - m_SO7_Serial.SetTimeouts(1000, 1000); m_bConnected = true; g_pLogger->SendAndFlushWithTime(L"[Startup] Serial: [COM%d] is open success\n", m_EF3COMPort); @@ -495,7 +492,7 @@ HSI_STATUS HSI_Motion::Startup(HWND _hWnd, bool _bOfflineOnly) } #endif //使能电机 - int Axes[] = {ACSC_AXIS_1, ACSC_AXIS_0, ACSC_AXIS_8, -1}; //根据电气层面定义，这里的0对应X轴，1对应Y轴，8对应Z轴 + int Axes[] = { ACSC_AXIS_1, ACSC_AXIS_0, ACSC_AXIS_8, -1 }; //根据电气层面定义，这里的0对应X轴，1对应Y轴，8对应Z轴 if (!acsc_EnableM(handleACS, Axes, nullptr)) { g_pLogger->SendAndFlushWithTime(L"[ACS Motion] ACS Motors Enable Failed!\n"); @@ -509,7 +506,6 @@ HSI_STATUS HSI_Motion::Startup(HWND _hWnd, bool _bOfflineOnly) m_bACSConnected = true; g_pLogger->SendAndFlushWithTime(L"[ACS Motion] ACS Initialization Success!\n"); - //获取ACS 控制器版本号, 待测试，2.70 char Firmware[256]; int Received; @@ -528,11 +524,11 @@ HSI_STATUS HSI_Motion::Startup(HWND _hWnd, bool _bOfflineOnly) //获取SPiiPlus C Library version unsigned int Ver = acsc_GetLibraryVersion(); /* printf("SPiiPlus C Library version is %d.%d.%d.%d\n", HIBYTE(HIWORD(Ver)), LOBYTE(HIWORD(Ver)), - HIBYTE(LOWORD(Ver)), - LOBYTE(LOWORD(Ver)));*/ + HIBYTE(LOWORD(Ver)), + LOBYTE(LOWORD(Ver)));*/ g_pLogger->SendAndFlushWithTime(L"[ACS Motion] SPiiPlus C Library version is %d.%d.%d.%d\n", - HIBYTE(HIWORD(Ver)), LOBYTE(HIWORD(Ver)), HIBYTE(LOWORD(Ver)), - LOBYTE(LOWORD(Ver))); + HIBYTE(HIWORD(Ver)), LOBYTE(HIWORD(Ver)), HIBYTE(LOWORD(Ver)), + LOBYTE(LOWORD(Ver))); } else { @@ -545,13 +541,13 @@ HSI_STATUS HSI_Motion::Startup(HWND _hWnd, bool _bOfflineOnly) } // 04 05 0F 4A 04 00 04 00 00 00 00 00 00 00 00 00 - // + // //01 01 0F 04 00 00 00 00 00 00 00 00 00 00 00 00 //01 01 0F 04 00 00 00 00 00 00 00 00 00 00 00 00 - // + // //01 01 01 07 02 01 01 00 00 00 00 00 00 00 00 00 //01 09 01 01 01 07 07 00 00 00 07 04 00 00 00 00 - // + // //01 01 01 05 01 07 07 00 00 00 07 04 00 00 00 00 //01 01 02 05 01 07 07 00 00 00 07 04 00 00 00 00 //01 01 04 05 01 07 07 00 00 00 07 04 00 00 00 00 @@ -649,11 +645,11 @@ HSI_STATUS HSI_Motion::Startup(HWND _hWnd, bool _bOfflineOnly) //m_cSendData[12] = m_IsCollectPos; //是否从串口打印位置 //m_cSendData[16] = m_IsLightDebug; //是否不回家也能调试灯光 - //新摇杆,并启动脚踏功能 - //if (m_IsStartInput == 1 && m_IsUseRocker == 2) + //新摇杆,并启动脚踏功能 + //if (m_IsStartInput == 1 && m_IsUseRocker == 2) //{ // m_cSendData[14] = m_StartInputPort >> 8 & 0xff; //外部启动端口号 H - // m_cSendData[15] = m_StartInputPort & 0xff; //外部启动端口号 L + // m_cSendData[15] = m_StartInputPort & 0xff; //外部启动端口号 L //} //m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength); //Sleep(10); @@ -762,7 +758,7 @@ HSI_STATUS HSI_Motion::Startup(HWND _hWnd, bool _bOfflineOnly) g_pLogger->SendAndFlushWithTime(L"[Startup] SetpositionXyz Enable\n"); //读取EF3数据状态线程 - //CreateThreadData(); + //CreateThreadData(); //SetEvent(m_hTriggerEventData); //m_Thread_StateData = HSI_THREAD_RUNNING; //g_pLogger->SendAndFlushWithTime(L"[Startup] Read EF3 Status Run\n"); @@ -770,7 +766,7 @@ HSI_STATUS HSI_Motion::Startup(HWND _hWnd, bool _bOfflineOnly) //JOG运行到软限位的运动调节线程 //if (m_DeviceType != 3) //{ - // CreateThreadJOGStop(); + // CreateThreadJOGStop(); // SetEvent(m_hTriggerEventJOGStop); // m_Thread_StateJOGStop = HSI_THREAD_PAUSED; //} @@ -796,7 +792,7 @@ HSI_STATUS HSI_Motion::Startup(HWND _hWnd, bool _bOfflineOnly) g_pLogger->SendAndFlushWithTime(L"[Startup] Out\n"); //启用EF3锁存模式设置，定时模式 - if (m_IsUseEF3 ) + if (m_IsUseEF3) { //设置锁存频率 1秒钟（） m_cSendData[0] = 0x01; @@ -828,15 +824,15 @@ HSI_STATUS HSI_Motion::Startup(HWND _hWnd, bool _bOfflineOnly) //=========================================================================== /** * \brief 获取EF3固件版本 - * \param version - * \return + * \param version + * \return */ HSI_STATUS HSI_Motion::GetFirmwareVersion(byte* version) { m_Thread_StateData = HSI_THREAD_PAUSED; Sleep(3); int waite_count = 0; - unsigned char senddata[8] = {0}; + unsigned char senddata[8] = { 0 }; //senddata[0] = 0x04; //senddata[1] = 0x03; @@ -898,8 +894,8 @@ HSI_STATUS HSI_Motion::GetFirmwareVersion(byte* version) //=========================================================================== /** * \brief 回家 - * \param bHomed - * \return + * \param bHomed + * \return */ HSI_STATUS HSI_Motion::HomeMachineOld(bool bHomed) { @@ -943,7 +939,7 @@ HSI_STATUS HSI_Motion::HomeMachineOld(bool bHomed) //根据当前位置，保存位置，求解相对运动 //回家后，设置正负限位 CurrentHomeMachineState = E_EF3_HOME_ING; - int GetHomePos[5] = {0}; + int GetHomePos[5] = { 0 }; if (m_SO7_Serial.m_RecvData[0] == 2) { if (m_IsHavePattern & 0x01 == 0x01) @@ -982,7 +978,7 @@ HSI_STATUS HSI_Motion::HomeMachineOld(bool bHomed) return HSI_STATUS_FAILED; } - int SavePos[5] = {0}; + 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. @@ -994,7 +990,7 @@ HSI_STATUS HSI_Motion::HomeMachineOld(bool bHomed) 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}; + 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]; @@ -1007,7 +1003,7 @@ HSI_STATUS HSI_Motion::HomeMachineOld(bool bHomed) 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}; + 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[ @@ -1021,7 +1017,7 @@ HSI_STATUS HSI_Motion::HomeMachineOld(bool bHomed) } m_Thread_State = HSI_THREAD_RUNNING; g_pLogger->SendAndFlushWithTime(L"[HomeMachine] SetPositionXyz\n"); - double PrinfMovePos[5] = {0}; + 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]; @@ -1040,7 +1036,7 @@ HSI_STATUS HSI_Motion::HomeMachineOld(bool bHomed) } Sleep(20); SetPositionXyza(0, PrinfMovePos[1], PrinfMovePos[2], PrinfMovePos[3], PrinfMovePos[4], HSI_MOTION_MOVE_WAIT, - 0); + 0); } CurrentHomeMachineState = E_EF3_HOME_FINISHED; for (int AxisTypes = 1; AxisTypes <= 8;) @@ -1133,21 +1129,20 @@ HSI_STATUS HSI_Motion::HomeMachine(bool bHomed) //等待运动结束 ，方式1 /* acsc_WaitMotionEnd(handleACS, ACSC_AXIS_1, INFINITE); g_pLogger->SendAndFlushWithTime(L"[HomeMachine] X homed\n"); - + acsc_WaitMotionEnd(handleACS, ACSC_AXIS_0, INFINITE); g_pLogger->SendAndFlushWithTime(L"[HomeMachine] Y homed\n"); - + acsc_WaitMotionEnd(handleACS, ACSC_AXIS_4, INFINITE); g_pLogger->SendAndFlushWithTime(L"[HomeMachine] Z homed\n");*/ - //等待运动结束 ，方式2 + //等待运动结束 ，方式2 do { //再次读取回家标志位，或者上个动作完成回调 IsHomed(home); Sleep(200); - } - while (!home); + } while (!home); if (m_IsUseEF3) //启用EF3锁存功能，需要再回家完成后，设置锁存板的位置 { @@ -1163,7 +1158,6 @@ HSI_STATUS HSI_Motion::HomeMachine(bool bHomed) m_cSendData[1] = 0x04; m_WriteByte = Send_Command(0, (const char*)m_cSendData, 2); Sleep(5); - } //回家表示改为1 @@ -1196,7 +1190,7 @@ HSI_STATUS HSI_Motion::HomeJog(short AxisNumber, short Dir, bool Wait) //=========================================================================== /** * \brief 取消限位，设置初始速度，加减速等参数 - * \return + * \return */ HSI_STATUS HSI_Motion::HomeFindIndex() { @@ -1241,7 +1235,7 @@ HSI_STATUS HSI_Motion::HomeFindIndex() m_cSendData[2] = AxisTypes; m_cSendData[3] = HOME_LIMIT_SPEED_ACC_DEC; HomeJogGearsChoice(i, m_Home_AddJogGears[i], DriveSpeed, StartSpeed, AccLine, DecLine, AccCurve, - DecCurve); + DecCurve); m_cSendData[4] = (StartSpeed & 0xff); m_cSendData[5] = ((StartSpeed >> 8) & 0xff); @@ -1280,7 +1274,7 @@ HSI_STATUS HSI_Motion::HomeFindIndex() m_cSendData[2] = AxisTypes; m_cSendData[3] = HOME_ORG_SPEED_ACC_DEC; HomeJogGearsChoice(i, m_Home_DecJogGears[i], DriveSpeed, StartSpeed, AccLine, DecLine, AccCurve, - DecCurve); + DecCurve); m_cSendData[4] = (StartSpeed & 0xff); m_cSendData[5] = ((StartSpeed >> 8) & 0xff); m_cSendData[6] = ((StartSpeed >> 16) & 0xff); @@ -1366,30 +1360,29 @@ HSI_STATUS HSI_Motion::HomeFindIndex() } Count++; g_pLogger->SendAndFlushWithTime(L"[HomeFindIndex] Going Home Count=%d\n", Count); - } - while (true); + } while (true); char MessageHome[100] = ""; bool bHomed = true; if ((m_SO7_Serial.m_RecvData[38] & 0x01) == 0 && m_Home_Machine_Axis[1] == 1) { /* strcat_s 是系统的安全函数，微软在 2005 后建议用一系统所谓安全的函数，这中间就有 strcat_s 取代了 strcat ，原来 strcat 函数，没有方法来保证有效的缓冲区尺寸，所以它只能假定缓冲足够大来容纳要拷贝的字符串, 容易产生程序崩溃。而strcat_s函数能很好的规避这个问题*/ - strcat_s(MessageHome, 30, "1、"); + 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、"); + 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、"); + 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、"); + strcat_s(MessageHome, 30, "4"); bHomed = false; } if (!bHomed) @@ -1412,7 +1405,7 @@ HSI_STATUS HSI_Motion::HomeFindIndex() //=========================================================================== void HSI_Motion::HomeJogGearsChoice(int AxisTypes, int JogGears, int& DriveSpeed, int& StartSpeed, int& AccLine, - int& DecLine, int& AccCurve, int& DecCurve) + int& DecLine, int& AccCurve, int& DecCurve) { DriveSpeed = m_JogDriveSpeed[AxisTypes][4 - JogGears]; StartSpeed = m_JogStartSpeed[AxisTypes][4 - JogGears]; @@ -1450,7 +1443,7 @@ HSI_STATUS HSI_Motion::IsHomedOld(bool& bHomed) if (g_pHSI_Motion) { g_pLogger->SendAndFlushWithTime(L"[IsHomed] In\n"); - short isHomed[5] = {1, 1, 1, 1, 1}; + short isHomed[5] = { 1, 1, 1, 1, 1 }; //int Count = 0; //if (bHomed == true)//定位是增大判断精度 //{ @@ -1521,8 +1514,7 @@ HSI_STATUS HSI_Motion::IsHomed(bool& bHomed) if (g_pHSI_Motion && handleACS != ACSC_INVALID) { g_pLogger->SendAndFlushWithTime(L"[IsHomed] In\n"); - int isHomed[5] = {0, 1, 1, 1, 1}; //暂定只有一个回家标志位，即全部回家完成，没有按单个轴回家来看 - + int isHomed[5] = { 0, 1, 1, 1, 1 }; //暂定只有一个回家标志位，即全部回家完成，没有按单个轴回家来看 //所有轴都不需要回家 if (m_Home_Machine_Axis[1] == 0 && m_Home_Machine_Axis[2] == 0 && m_Home_Machine_Axis[3] == 0 && @@ -1536,8 +1528,8 @@ HSI_STATUS HSI_Motion::IsHomed(bool& bHomed) // 判断是否需要回家，读取ACS控制器回家标志位，来判断本次上电是否已经回过家 ， 1：已经回过家，0：未回过家 if (!acsc_ReadInteger(handleACS, ACSC_NONE, "YAW_HOME_DONE", ACSC_NONE, ACSC_NONE, ACSC_NONE, ACSC_NONE, - isHomed, - nullptr)) + isHomed, + nullptr)) { g_pLogger->SendAndFlushWithTime(L"[IsHomed] ACS Read ISHOMED Flag Error\n"); ErrorsHandler(); @@ -1548,7 +1540,7 @@ HSI_STATUS HSI_Motion::IsHomed(bool& bHomed) } g_pLogger->SendAndFlushWithTime(L"[IsHomed] ACS Read YAW_HOME_DONE X:[%d] Y:[%d] Z:[%d]\n", isHomed[0], - isHomed[1], isHomed[2]); + isHomed[1], isHomed[2]); //如果各个轴标志位 已经回过家 if (isHomed[0] == 1 && isHomed[1] == 1 && isHomed[2] == 1 && isHomed[3] == 1) @@ -1562,7 +1554,6 @@ HSI_STATUS HSI_Motion::IsHomed(bool& bHomed) 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"); } @@ -1627,7 +1618,7 @@ HSI_STATUS HSI_Motion::ZeroPos(bool bZeroPos) * \brief JOG模式 * \param AxisTypes 单轴 * \param Speed 速度，Speed > 0 正移动 - * \return + * \return */ HSI_STATUS HSI_Motion::JogOld(UINT AxisTypes, double Speed) { @@ -1694,9 +1685,9 @@ HSI_STATUS HSI_Motion::JogOld(UINT AxisTypes, double Speed) { return rStatus; } - double now_pos[5] = {0}; - double Prf_pos[5] = {0}; - double limitpos[4] = {0}; + double now_pos[5] = { 0 }; + double Prf_pos[5] = { 0 }; + double limitpos[4] = { 0 }; int RemainPul = 0; int limitSDPul = 0; double time; @@ -1936,7 +1927,6 @@ HSI_STATUS HSI_Motion::Jog(UINT AxisTypes, double Speed) int DecCurve(1); int JogSpeed(1); bool bJOGDir = Speed > 0 ? true : false; //运动方向 - jogDirFlag = bJOGDir; m_Thread_State = HSI_THREAD_PAUSED; @@ -1946,7 +1936,7 @@ HSI_STATUS HSI_Motion::Jog(UINT AxisTypes, double Speed) //软限位 g_pLogger->SendAndFlushWithTime( L"[Jog] Limit Enable, Axis = %d, m_P_Work_Limit = %f,m_N_Work_Limit = %f\n", - AxisTypes,m_P_Work_Limit[AxisTypes], m_N_Work_Limit[AxisTypes]); + AxisTypes, m_P_Work_Limit[AxisTypes], m_N_Work_Limit[AxisTypes]); } else { @@ -1962,9 +1952,9 @@ HSI_STATUS HSI_Motion::Jog(UINT AxisTypes, double Speed) } //设置 JOG运动参数 加减速 JOG_SPEED_ACC_DEC - double now_pos[5] = {0}; - double Prf_pos[5] = {0}; - double limitpos[4] = {0}; + double now_pos[5] = { 0 }; + double Prf_pos[5] = { 0 }; + double limitpos[4] = { 0 }; int RemainPul = 0; int limitSDPul = 0; double time; @@ -1974,13 +1964,13 @@ HSI_STATUS HSI_Motion::Jog(UINT AxisTypes, double Speed) g_pLogger->SendAndFlushWithTime( L"[Jog] Speed: [%d], DriveSpeed: [%d],AccLine: [%d], DecLine: [%d] AccCurve: [%d], DecCurve: [%d],\n", - Speed, DriveSpeed, AccLine, DecLine,AccCurve, DecCurve ); + Speed, DriveSpeed, AccLine, DecLine, AccCurve, DecCurve); //转到真实ACS平台轴号，并开始执行 byte AxisNumber = static_cast(AxisConvertIndex(AxisTypes)); //Jog - double motionParam[5] = { DriveSpeed,AccLine , DecLine, AccCurve,DecCurve }; //速度，加速度，减速度，Kill, jerk + double motionParam[5] = { DriveSpeed,AccLine , DecLine, AccCurve,DecCurve }; //速度，加速度，减速度，Kill, jerk SetSingleAxisMotionParams(AxisNumber, motionParam); - + // 急停判断 if ((StartSpeed < 250) && (DriveSpeed < 6)) { @@ -2011,19 +2001,15 @@ HSI_STATUS HSI_Motion::Jog(UINT AxisTypes, double Speed) DriveSpeed = DriveSpeed * (-1); // Negative direction : Using - (minus) velocity //正方向，或 负方向 } - if (!acsc_Jog(handleACS, 0, AxisNumber, DriveSpeed, nullptr)) { - printf("[Jog] 轴[%d] [%s] 方向移动失败", AxisTypes, bJOGDir ? "正" : "负"); - g_pLogger->SendAndFlushWithTime(L"[Jog] failed, Aixs:[%d] JOGDir:[%S]\n", AxisTypes, - bJOGDir ? "Positive" : "Negative"); + g_pLogger->SendAndFlushWithTime(L"[Jog] 方向移动失败, 轴:[%d] JOGDir:[%S]\n", AxisTypes, bJOGDir ? "Positive" : "Negative"); ErrorsHandler(); } - jogMoving = true; g_pLogger->SendAndFlushWithTime(L"[Jog] Out, AxisNumber = %d, DriveSpeed = %d AccCurve:[%d] DecCurve:[%d]\n", AxisNumber, DriveSpeed, - AccLine, AccLine); + AccLine, AccLine); } return rStatus; } @@ -2095,9 +2081,9 @@ HSI_STATUS HSI_Motion::JoyStick(UINT AxisTypes, long Speed) { return rStatus; } - double now_pos[5] = {0}; - double Prf_pos[5] = {0}; - double limitpos[4] = {0}; + double now_pos[5] = { 0 }; + double Prf_pos[5] = { 0 }; + double limitpos[4] = { 0 }; int RemainPul = 0; int limitSDPul = 0; double time; @@ -2105,7 +2091,7 @@ HSI_STATUS HSI_Motion::JoyStick(UINT AxisTypes, long Speed) GetPositionEncPrfMulti(1, now_pos, Prf_pos, 1); long lSpeed = abs(Speed); if (!abs(SpeedPercentJoyStick(AxisNumber, lSpeed, DriveSpeed, StartSpeed, AccLine, DecLine, AccCurve, - DecCurve))) + DecCurve))) { return rStatus; } @@ -2326,7 +2312,7 @@ HSI_STATUS HSI_Motion::JoyStick(UINT AxisTypes, long Speed) //=========================================================================== /** * \brief 停止Jog运动 - * \return + * \return */ HSI_STATUS HSI_Motion::StopJogOld() { @@ -2344,7 +2330,7 @@ HSI_STATUS HSI_Motion::StopJogOld() g_pLogger->SendAndFlushWithTime(L"[StopJog] PushButtonTime = %d\n", t_use); Sleep(t_use); } - unsigned char m_SendJogData[64] = {0}; + unsigned char m_SendJogData[64] = { 0 }; if (m_IsUseJerk == 0) { m_SendJogData[0] = CT_MOTOR; @@ -2390,7 +2376,7 @@ HSI_STATUS HSI_Motion::StopJog() g_pLogger->SendAndFlushWithTime(L"[StopJog] PushButtonTime = %d\n", t_use); Sleep(t_use); } - unsigned char m_SendJogData[64] = {0}; + unsigned char m_SendJogData[64] = { 0 }; //是否启用急停 /*if (m_IsUseJerk == 0) @@ -2416,7 +2402,7 @@ HSI_STATUS HSI_Motion::StopJog() m_WriteByte = Send_Command(0, (const char*)m_SendJogData, m_SendDataLength); }*/ - int Axes[] = {ACSC_AXIS_1, ACSC_AXIS_0, ACSC_AXIS_8, -1}; + int Axes[] = { ACSC_AXIS_1, ACSC_AXIS_0, ACSC_AXIS_8, -1 }; if (handleACS != ACSC_INVALID) { if (!acsc_HaltM(handleACS, Axes, nullptr)) //停止JOG运动 @@ -2435,7 +2421,6 @@ HSI_STATUS HSI_Motion::StopJog() return rStatus; } - //=========================================================================== HSI_STATUS HSI_Motion::StopJogEx(UINT AxisTypes) { @@ -2454,7 +2439,7 @@ HSI_STATUS HSI_Motion::StopJogEx(UINT AxisTypes) g_pLogger->SendAndFlushWithTime(L"[StopJog] PushButtonTime = %d\n", t_use); Sleep(t_use); } - unsigned char m_SendJogData[64] = {0}; + unsigned char m_SendJogData[64] = { 0 }; if (m_IsUseJerk == 0) { m_SendJogData[0] = CT_MOTOR; @@ -2506,11 +2491,11 @@ int HSI_Motion::P2P(short AxisNumber, long Pos, double Speed, double Acc) //=========================================================================== /** * \brief 对比现在编码器位置和期望位置, position给上层用的, HSI内部用的是EncPrf - * \param AxisTypes - * \param EncPos - * \param PrfPos - * \param Count - * \return + * \param AxisTypes + * \param EncPos + * \param PrfPos + * \param Count + * \return */ HSI_STATUS HSI_Motion::GetPositionEncPrfMultiOld(UINT AxisTypes, double* EncPos, double* PrfPos, int Count) { @@ -2524,48 +2509,48 @@ HSI_STATUS HSI_Motion::GetPositionEncPrfMultiOld(UINT AxisTypes, double* EncPos, { 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]; + 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]; + 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]; + 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]; + 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]; + //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 { @@ -2596,48 +2581,48 @@ HSI_STATUS HSI_Motion::GetPositionEncPrfMulti(UINT AxisTypes, double* EncPos, do { 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]; + 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]; + 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]; + 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]; + 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]; + //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 { @@ -2659,15 +2644,15 @@ HSI_STATUS HSI_Motion::GetPositionEncPrfMulti(UINT AxisTypes, double* EncPos, do //=========================================================================== /** * \brief 获取轴当前运动位置 - * \param AxisTypes - * \param PositionX - * \param PositionY - * \param PositionZ + * \param AxisTypes + * \param PositionX + * \param PositionY + * \param PositionZ * \param Time 耗时 - * \return + * \return */ HSI_STATUS HSI_Motion::GetPositionXyzOld(UINT AxisTypes, double& PositionX, double& PositionY, double& PositionZ, - double& Time) + double& Time) { auto rStatus = HSI_STATUS_NORMAL; UNREFERENCED_PARAMETER(AxisTypes); @@ -2741,7 +2726,7 @@ HSI_STATUS HSI_Motion::GetPositionXyzOld(UINT AxisTypes, double& PositionX, doub } HSI_STATUS HSI_Motion::GetPositionXyz(UINT AxisTypes, double& PositionX, double& PositionY, double& PositionZ, - double& Time) + double& Time) { auto rStatus = HSI_STATUS_NORMAL; //UNREFERENCED_PARAMETER(AxisTypes)的意思就是告诉编译器， @@ -2822,7 +2807,7 @@ HSI_STATUS HSI_Motion::GetEncoderXyzOld(long* lEncoderVal)//原读取编码器 if (m_DeviceType != 1) { if (m_IsHavePattern & 0x01) - { + { lEncoderVal[0] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial .m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]); g_pLogger->SendAndFlushWithTime(L"[GetEncoderXyz] lEncoderVal[0] %ld \n", lEncoderVal[0]); @@ -2840,7 +2825,7 @@ HSI_STATUS HSI_Motion::GetEncoderXyzOld(long* lEncoderVal)//原读取编码器 g_pLogger->SendAndFlushWithTime(L"[GetEncoderXyz] lEncoderVal[1] %ld \n", lEncoderVal[1]); } else - { + { lEncoderVal[1] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial.m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]); g_pLogger->SendAndFlushWithTime(L"[GetEncoderXyz] lEncoderVal[01] %ld \n", lEncoderVal[1]); @@ -2852,7 +2837,7 @@ HSI_STATUS HSI_Motion::GetEncoderXyzOld(long* lEncoderVal)//原读取编码器 g_pLogger->SendAndFlushWithTime(L"[GetEncoderXyz] lEncoderVal[2] %ld \n", lEncoderVal[2]); } else - { + { lEncoderVal[2] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial.m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]); g_pLogger->SendAndFlushWithTime(L"[GetEncoderXyz] lEncoderVal[02] %ld \n", lEncoderVal[2]); @@ -2890,8 +2875,8 @@ HSI_STATUS HSI_Motion::GetEncoderXyz(long* lEncoderVal)//原读取编码器值 { lEncoderVal[0] = m_EncPos[1] / m_Resolution[1];//PositionX lEncoderVal[1] = m_EncPos[2] / m_Resolution[1];// PositionY; - lEncoderVal[2] = m_EncPos[3]/ m_Resolution[1]; // PositionZ; - + lEncoderVal[2] = m_EncPos[3] / m_Resolution[1]; // PositionZ; + g_pLogger->SendAndFlushWithTime(L"[GetEncoderXyz] lEncoderVal[0]= %ld,EncoderVal[1]= %ld,,EncoderVal[2]= %ld, \n", lEncoderVal[0], lEncoderVal[1], lEncoderVal[2]); } //else @@ -2906,7 +2891,7 @@ HSI_STATUS HSI_Motion::GetEncoderXyz(long* lEncoderVal)//原读取编码器值 //=========================================================================== HSI_STATUS HSI_Motion::GetPositionXyzaProbe(UINT AxisTypes, double& PositionX, double& PositionY, double& PositionZ, - double& PositionA) + double& PositionA) { auto rStatus = HSI_STATUS_NORMAL; UNREFERENCED_PARAMETER(AxisTypes); @@ -2923,590 +2908,586 @@ HSI_STATUS HSI_Motion::GetPositionXyzaProbe(UINT AxisTypes, double& PositionX, d //=========================================================================== /** * \brief 设置多轴运动到指定位置 - * \param AxisTypes - * \param PositionX - * \param PositionY - * \param PositionZ - * \param eType - * \param dFlyRadius - * \return + * \param AxisTypes + * \param PositionX + * \param PositionY + * \param PositionZ + * \param eType + * \param dFlyRadius + * \return */ -HSI_STATUS HSI_Motion::SetPositionXyzOld(UINT AxisTypes, double PositionX, double PositionY, double PositionZ, - HSI_MOTION_MOVE_TYPE eType, double dFlyRadius) -{ - WaitForSingleObject(g_WR_ToMove_Mutex, INFINITE); - auto rStatus = HSI_STATUS_NORMAL; - if (g_pHSI_Motion) - { - g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] In\n"); - g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Pos[1] = %.4f,Pos[2] = %.4f,Pos[3] = %.4f\n", PositionX, - PositionY, PositionZ); - unsigned char send_pos_data[64] = {0}; - axis_start = 0; - unsigned char direct_pos = 0; - unsigned char xyzAxis = 0; - //如果状态非 运动中，设置为运动中 - if (CurrentMotionState != E_SO7_MOTION_MOVETO) - { - CurrentMotionState = E_SO7_MOTION_MOVETO; - LimitOver(HSI_MOTION_AXIS_X, PositionX); - LimitOver(HSI_MOTION_AXIS_Y, PositionY); - LimitOver(HSI_MOTION_AXIS_Z, PositionZ); - LimitOver(HSI_MOTION_AXIS_R, m_PositionA); - - m_PosThread[1] = PositionX; //SetpositionXyz的目标位置 - m_PosThread[2] = PositionY; - m_PosThread[3] = PositionZ; - m_PosThread[4] = m_PositionA; - - targetpos_n[1] = PositionX; - targetpos_n[2] = PositionY; - targetpos_n[3] = PositionZ; - targetpos_n[4] = m_PositionA; - - int Pos_t[5] = {0}; - int Pos_n[5] = {0}; - int Pos[5] = {0}; - int NowPos[5] = {0}; - int target_pos[5] = {0}; - - if (m_SO7_Serial.m_RecvData[0] == 2) - { - //根据设备类型获取现在位置 - if (m_DeviceType != 1) //设备类型非三激光 - { - if (m_IsHavePattern & 0x01 == 0x01) - NowPos[1] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial. - m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]); - else - NowPos[1] = (m_SO7_Serial.m_RecvData[17] << 24 | m_SO7_Serial.m_RecvData[18] << 16 | - m_SO7_Serial.m_RecvData[19] << 8 | m_SO7_Serial.m_RecvData[20]); - if (m_IsHavePattern & 0x02) - NowPos[2] = (m_SO7_Serial.m_RecvData[5] << 24 | m_SO7_Serial.m_RecvData[6] << 16 | m_SO7_Serial. - m_RecvData[7] << 8 | m_SO7_Serial.m_RecvData[8]); - else - NowPos[2] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | - m_SO7_Serial.m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]); - if (m_IsHavePattern & 0x04) - NowPos[3] = (m_SO7_Serial.m_RecvData[9] << 24 | m_SO7_Serial.m_RecvData[10] << 16 | m_SO7_Serial - .m_RecvData[11] << 8 | m_SO7_Serial.m_RecvData[12]); - else - NowPos[3] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | - m_SO7_Serial.m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]); - if (m_IsHavePattern & 0x08) - NowPos[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | - m_SO7_Serial.m_RecvData[15] << 8 | m_SO7_Serial.m_RecvData[16]); - else - NowPos[4] = (m_SO7_Serial.m_RecvData[29] << 24 | m_SO7_Serial.m_RecvData[30] << 16 | - m_SO7_Serial.m_RecvData[31] << 8 | m_SO7_Serial.m_RecvData[32]); - } - else - { - NowPos[1] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial. - m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]); - NowPos[2] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial. - m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]); - NowPos[3] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial. - m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]); - NowPos[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | m_SO7_Serial. - m_RecvData[15] << 8 | m_SO7_Serial.m_RecvData[16]); - } - //NowPos[1] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial.m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]); - //NowPos[2] = (m_SO7_Serial.m_RecvData[5] << 24 | m_SO7_Serial.m_RecvData[6] << 16 | m_SO7_Serial.m_RecvData[7] << 8 | m_SO7_Serial.m_RecvData[8]); - //NowPos[3] = (m_SO7_Serial.m_RecvData[9] << 24 | m_SO7_Serial.m_RecvData[10] << 16 | m_SO7_Serial.m_RecvData[11] << 8 | m_SO7_Serial.m_RecvData[12]); - //NowPos[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | m_SO7_Serial.m_RecvData[15] << 8 | m_SO7_Serial.m_RecvData[16]); - - Pos_t[1] = (m_SO7_Serial.m_RecvData[17] << 24 | m_SO7_Serial.m_RecvData[18] << 16 | m_SO7_Serial. - m_RecvData[19] << 8 | m_SO7_Serial.m_RecvData[20]); - Pos_t[2] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial. - m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]); - Pos_t[3] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial. - m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]); - Pos_t[4] = (m_SO7_Serial.m_RecvData[29] << 24 | m_SO7_Serial.m_RecvData[30] << 16 | m_SO7_Serial. - m_RecvData[31] << 8 | m_SO7_Serial.m_RecvData[32]); - } - else - { - /* 编译器隐式执行的任何类型转换都可以由static_cast显式完成。 - static_cast可以用来将枚举类型转换成整型，或者整型转换成浮点型*/ - Pos_t[1] = NowPos[1] = static_cast(m_EncPos[1] / m_Resolution[1]); - Pos_t[2] = NowPos[2] = static_cast(m_EncPos[2] / m_Resolution[2]); - Pos_t[3] = NowPos[3] = static_cast(m_EncPos[3] / m_Resolution[3]); - Pos_t[4] = NowPos[4] = static_cast(m_EncPos[4] / m_Resolution[4]); - } - - if (m_motorType & 0x01) //步进电机 - Pos[1] = static_cast(PositionX / m_Resolution[1]) - NowPos[1]; - else - Pos[1] = static_cast(PositionX / m_Resolution[1]) - Pos_t[1]; - if (m_motorType & 0x02) //步进电机 - Pos[2] = static_cast(PositionX / m_Resolution[2]) - NowPos[2]; - else - Pos[2] = static_cast(PositionY / m_Resolution[2]) - Pos_t[2]; - if (m_motorType & 0x04) //步进电机 - Pos[3] = static_cast(PositionX / m_Resolution[3]) - NowPos[3]; - else - Pos[3] = static_cast(PositionZ / m_Resolution[3]) - Pos_t[3]; - if (m_motorType & 0x08) //步进电机 - Pos[4] = static_cast(PositionX / m_Resolution[4]) - NowPos[4]; - else - Pos[4] = static_cast(m_PositionA / m_Resolution[4]) - Pos_t[4]; - - /*if (m_motorType==1) - { - if (m_IsUse_HSICompensation) - { - - } - else - { - Pos[1] = (int)(PositionX / m_Resolution[1]) - NowPos[1]; - Pos[2] = (int)(PositionY / m_Resolution[2]) - NowPos[2]; - Pos[3] = (int)(PositionZ / m_Resolution[3]) - NowPos[3]; - Pos[4] = (int)(m_PositionA / m_Resolution[4]) - NowPos[4]; - } - } - else - { - Pos_n[1] = (int)(targetpos_n[1] / m_Resolution[1]) - NowPos[1]; - Pos_n[2] = (int)(targetpos_n[2] / m_Resolution[2]) - NowPos[2]; - Pos_n[3] = (int)(targetpos_n[3] / m_Resolution[3]) - NowPos[3]; - - Pos_t[1] = (int)(targetpos_n[1] / m_Resolution[1]) - (int)(targetpos_l[1] / m_Resolution[1]); - Pos_t[2] = (int)(targetpos_n[2] / m_Resolution[2]) - (int)(targetpos_l[2] / m_Resolution[1]); - Pos_t[3] = (int)(targetpos_n[3] / m_Resolution[3]) - (int)(targetpos_l[3] / m_Resolution[1]); - - if (m_IsUse_HSICompensation) - { - for (int k = 1; k < 4; k++) - { - if (abs(Pos_n[k] - Pos_t[k]) > m_Compensation_Pluse) - { - Pos[k] = Pos_n[k]; - } - else - { - Pos[k] = Pos_t[k]; - } - if (abs(Pos[k]) < m_Compensation_Pluse) - { - Pos[k] = 0; - } - } - } - else - { - Pos[1] = (int)(PositionX / m_Resolution[1]) - NowPos[1]; - Pos[2] = (int)(PositionY / m_Resolution[2]) - NowPos[2]; - Pos[3] = (int)(PositionZ / m_Resolution[3]) - NowPos[3]; - Pos[4] = (int)(m_PositionA / m_Resolution[4]) - NowPos[4]; - } - }*/ - target_pos[1] = static_cast(PositionX / m_Resolution[1]); //计算到目标位置 - target_pos[2] = static_cast(PositionY / m_Resolution[2]); - target_pos[3] = static_cast(PositionZ / m_Resolution[3]); - target_pos[4] = static_cast(m_PositionA / m_Resolution[4]); - - begin_position[1] = target_pos[1]; //将目标位置设置为 开始位置 - begin_position[2] = target_pos[2]; - begin_position[3] = target_pos[3]; - begin_position[4] = target_pos[4]; - - float scale[4] = {0}; - int Stepdriverspeed[5] = {0}; - int StepStartspeed[5] = {0}; - int StepAcc[5] = {0}; - //if (Pos[1] > 0) direct_pos |= 0x01; - //if (Pos[2] > 0) direct_pos |= 0x02; - //if (Pos[3] > 0) direct_pos |= 0x04; - if (abs(Pos[1]) > 2) axis_start |= 0x01; - if (abs(Pos[2]) > 2) axis_start |= 0x02; - if (abs(Pos[3]) > 2) axis_start |= 0x04; - int MaxPos = abs(Pos[1]); - for (int i = 2; i < 5; i++) - { - if (MaxPos < abs(Pos[i])) - { - MaxPos = abs(Pos[i]); - } - } - //MaxPos = abs(MaxPos); - scale[0] = abs(Pos[1]) / static_cast(MaxPos); - scale[1] = abs(Pos[2]) / static_cast(MaxPos); - scale[2] = abs(Pos[3]) / static_cast(MaxPos); - scale[3] = abs(Pos[4]) / static_cast(MaxPos); - - g_pLogger->SendAndFlushWithTime( - L"[SetPositionXyzNowPos] Pos[1] = %.4f,Pos[2] = %.4f,Pos[3] = %.4f,Pos[4] = %.4f\n", - NowPos[1] * m_Resolution[1], NowPos[2] * m_Resolution[2], NowPos[3] * m_Resolution[3], - NowPos[4] * m_Resolution[4]); - g_pLogger->SendAndFlushWithTime( - L"[SetPositionXyzTagPos] Pos[1] = %.4f,Pos[2] = %.4f,Pos[3] = %.4f,Pos[4] = %.4f\n", PositionX, - PositionY, PositionZ, m_PositionA); - - - int stepinterpolation = 0x03; - //if (!m_motorType) - //{ - // /*if ((stepinterpolation & 0x01) && m_IsUseManualRunin) - // { - // Stepdriverspeed[1] = m_SetPotion_DriveSpeed[1] * scale[0]; - // if (Stepdriverspeed[1] < m_SetPotion_StartSpeed[1]) - // { - // Stepdriverspeed[1] = m_SetPotion_StartSpeed[1]; - // } - // } - // else - // { - // Stepdriverspeed[1] = m_SetPotion_DriveSpeed[1]; - // } - // if ((stepinterpolation & 0x02) && m_IsUseManualRunin) - // { - // Stepdriverspeed[2] = m_SetPotion_DriveSpeed[2] * scale[1]; - // if (Stepdriverspeed[2] < m_SetPotion_StartSpeed[2]) - // { - // Stepdriverspeed[2] = m_SetPotion_StartSpeed[2]; - // } - // } - // else - // { - // Stepdriverspeed[2] = m_SetPotion_DriveSpeed[2]; - // } - // if ((stepinterpolation & 0x04) && m_IsUseManualRunin) - // { - // Stepdriverspeed[3] = m_SetPotion_DriveSpeed[3] * scale[2]; - // if (Stepdriverspeed[3] < m_SetPotion_StartSpeed[3]) - // { - // Stepdriverspeed[3] = m_SetPotion_DriveSpeed[3]; - // } - // } - // else - // { - // Stepdriverspeed[3] = m_SetPotion_DriveSpeed[3]; - // } - // if ((stepinterpolation & 0x08) && m_IsUseManualRunin) - // { - // Stepdriverspeed[4] = m_SetPotion_DriveSpeed[4] * scale[3]; - // if (Stepdriverspeed[4] < m_SetPotion_StartSpeed[4]) - // { - // Stepdriverspeed[4] = m_SetPotion_DriveSpeed[4]; - // } - // } - // else - // { - // Stepdriverspeed[4] = m_SetPotion_DriveSpeed[4]; - // }*/ - // for (int i = 1; i < 3; i++) - // { - // if (abs(Pos[i]) < m_stepPosition_Load[0]) - // { - // StepStartspeed[i] = m_stepPosition_L_speed[0]; - // Stepdriverspeed[i] = m_stepPosition_H_speed[0]; - // StepAcc[i] = m_stepPosition_acc[0]; - // } - // else if (abs(Pos[i]) < m_stepPosition_Load[1]) - // { - // StepStartspeed[i] = m_stepPosition_L_speed[1]; - // Stepdriverspeed[i] = m_stepPosition_H_speed[1]; - // StepAcc[i] = m_stepPosition_acc[1]; - // } - // else if (abs(Pos[i]) < m_stepPosition_Load[2]) - // { - // StepStartspeed[i] = m_stepPosition_L_speed[2]; - // Stepdriverspeed[i] = m_stepPosition_H_speed[2]; - // StepAcc[i] = m_stepPosition_acc[2]; - // } - // else if (abs(Pos[i]) < m_stepPosition_Load[3]) - // { - // StepStartspeed[i] = m_stepPosition_L_speed[3]; - // Stepdriverspeed[i] = m_stepPosition_H_speed[3]; - // StepAcc[i] = m_stepPosition_acc[3]; - // } - // else if (abs(Pos[i]) < m_stepPosition_Load[4]) - // { - // StepStartspeed[i] = m_stepPosition_L_speed[4]; - // Stepdriverspeed[i] = m_stepPosition_H_speed[4]; - // StepAcc[i] = m_stepPosition_acc[4]; - // } - // else if (abs(Pos[i]) < m_stepPosition_Load[5]) - // { - // StepStartspeed[i] = m_stepPosition_L_speed[5]; - // Stepdriverspeed[i] = m_stepPosition_H_speed[5]; - // StepAcc[i] = m_stepPosition_acc[5]; - // } - // else if (abs(Pos[i]) < m_stepPosition_Load[6]) - // { - // StepStartspeed[i] = m_stepPosition_L_speed[6]; - // Stepdriverspeed[i] = m_stepPosition_H_speed[6]; - // StepAcc[i] = m_stepPosition_acc[6]; - // } - // else if (abs(Pos[i]) < m_stepPosition_Load[7]) - // { - // StepStartspeed[i] = m_stepPosition_L_speed[7]; - // Stepdriverspeed[i] = m_stepPosition_H_speed[7]; - // StepAcc[i] = m_stepPosition_acc[7]; - // } - // else if (abs(Pos[i]) < m_stepPosition_Load[8]) - // { - // StepStartspeed[i] = m_stepPosition_L_speed[8]; - // Stepdriverspeed[i] = m_stepPosition_H_speed[8]; - // StepAcc[i] = m_stepPosition_acc[8]; - // } - // else if (abs(Pos[i]) < m_stepPosition_Load[9]) - // { - // StepStartspeed[i] = m_stepPosition_L_speed[9]; - // Stepdriverspeed[i] = m_stepPosition_H_speed[9]; - // StepAcc[i] = m_stepPosition_acc[9]; - // } - // else - // { - // StepStartspeed[i] = m_SetPotion_StartSpeed[i]; - // Stepdriverspeed[i] = m_SetPotion_DriveSpeed[i]; - // StepAcc[i] = m_SetPotion_Line[i]; - // } - // } - // StepStartspeed[3] = m_SetPotion_StartSpeed[3]; - // StepAcc[3] = m_SetPotion_Line[3]; - // Stepdriverspeed[3] = m_SetPotion_DriveSpeed[3]; - //} - - int axisCount = 4; - if (fourthAxisFlag) //第4轴 - { - fourthAxisFlag = false; - if (Pos[4] > 0) direct_pos |= 0x08; - if (abs(Pos[4]) > 20) axis_start |= 0x08; - axisCount = 5; - xyzAxis = AXIS_XYZU; - } - else - xyzAxis = AXIS_XYZ; - - //设置运动参数 - for (int i = 1; i < axisCount; i++) - { - int time_out_send = 0; - - send_pos_data[0] = CT_MOTOR; - send_pos_data[1] = CT_MOTOR_SET; - send_pos_data[2] = 1 << (i - 1); - send_pos_data[3] = POSITION_SPEED_ACC_DEC_POS; - /*if (m_motorType == 1) - {*/ - send_pos_data[4] = (m_SetPotion_StartSpeed[i] & 0xff); - send_pos_data[5] = ((m_SetPotion_StartSpeed[i] >> 8) & 0xff); - send_pos_data[6] = ((m_SetPotion_StartSpeed[i] >> 16) & 0xff); - send_pos_data[7] = ((m_SetPotion_StartSpeed[i] >> 24) & 0xff); - if ((xyzAxis == AXIS_XYZU) && (m_IsUseFourthSpeed == 1)) - { - send_pos_data[8] = (m_SetPotion_DriveSpeed[4] & 0xff); - send_pos_data[9] = ((m_SetPotion_DriveSpeed[4] >> 8) & 0xff); - send_pos_data[10] = ((m_SetPotion_DriveSpeed[4] >> 16) & 0xff); - send_pos_data[11] = ((m_SetPotion_DriveSpeed[4] >> 24) & 0xff); - } - else - { - send_pos_data[8] = (m_SetPotion_DriveSpeed[i] & 0xff); - send_pos_data[9] = ((m_SetPotion_DriveSpeed[i] >> 8) & 0xff); - send_pos_data[10] = ((m_SetPotion_DriveSpeed[i] >> 16) & 0xff); - send_pos_data[11] = ((m_SetPotion_DriveSpeed[i] >> 24) & 0xff); - } - send_pos_data[12] = (m_SetPotion_Line[i] & 0xff); - send_pos_data[13] = ((m_SetPotion_Line[i] >> 8) & 0xff); - send_pos_data[14] = ((m_SetPotion_Line[i] >> 16) & 0xff); - send_pos_data[15] = ((m_SetPotion_Line[i] >> 24) & 0xff); - - send_pos_data[16] = (m_SetPotion_Buffer[i] & 0xff); - send_pos_data[17] = ((m_SetPotion_Buffer[i] >> 8) & 0xff); - send_pos_data[18] = ((m_SetPotion_Buffer[i] >> 16) & 0xff); - send_pos_data[19] = ((m_SetPotion_Buffer[i] >> 24) & 0xff); - - send_pos_data[28] = (target_pos[i] & 0xff); - send_pos_data[29] = ((target_pos[i] >> 8) & 0xff); - send_pos_data[30] = ((target_pos[i] >> 16) & 0xff); - send_pos_data[31] = ((target_pos[i] >> 24) & 0xff); - - send_pos_data[32] = (Pos[i] & 0xff); - send_pos_data[33] = ((Pos[i] >> 8) & 0xff); - send_pos_data[34] = ((Pos[i] >> 16) & 0xff); - send_pos_data[35] = ((Pos[i] >> 24) & 0xff); - - g_pLogger->SendAndFlushWithTime( - L"[SetPositionXyz] m_SetPotion_StartSpeed[%d] = %d,m_SetPotion_DriveSpeed[%d] = %d\n", i, - m_SetPotion_StartSpeed[i], i, m_SetPotion_DriveSpeed[i]); - //send_pos_data[32] = 0x21; - /*}*/ - - /*else - { - send_pos_data[4] = (StepStartspeed[i] & 0xff); - send_pos_data[5] = ((StepStartspeed[i] >> 8) & 0xff); - send_pos_data[6] = ((StepStartspeed[i] >> 16) & 0xff); - send_pos_data[7] = ((StepStartspeed[i] >> 24) & 0xff); - - - send_pos_data[8] = (Stepdriverspeed[i] & 0xff); - send_pos_data[9] = ((Stepdriverspeed[i] >> 8) & 0xff); - send_pos_data[10] = ((Stepdriverspeed[i] >> 16) & 0xff); - send_pos_data[11] = ((Stepdriverspeed[i] >> 24) & 0xff); - - send_pos_data[12] = (StepAcc[i] & 0xff); - send_pos_data[13] = ((StepAcc[i] >> 8) & 0xff); - send_pos_data[14] = ((StepAcc[i] >> 16) & 0xff); - send_pos_data[15] = ((StepAcc[i] >> 24) & 0xff); - - send_pos_data[16] = (m_SetPotion_Buffer[i] & 0xff); - send_pos_data[17] = ((m_SetPotion_Buffer[i] >> 8) & 0xff); - send_pos_data[18] = ((m_SetPotion_Buffer[i] >> 16) & 0xff); - send_pos_data[19] = ((m_SetPotion_Buffer[i] >> 24) & 0xff); - - send_pos_data[28] = (target_pos[i] & 0xff); - send_pos_data[29] = ((target_pos[i] >> 8) & 0xff); - send_pos_data[30] = ((target_pos[i] >> 16) & 0xff); - send_pos_data[31] = ((target_pos[i] >> 24) & 0xff); - - send_pos_data[32] = (Pos[i] & 0xff); - send_pos_data[33] = ((Pos[i] >> 8) & 0xff); - send_pos_data[34] = ((Pos[i] >> 16) & 0xff); - send_pos_data[35] = ((Pos[i] >> 24) & 0xff); - - g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] m_SetPotion_StartSpeed[%d] = %d,m_SetPotion_DriveSpeed[%d] = %d\n", i, m_SetPotion_StartSpeed[i], i, m_SetPotion_DriveSpeed[i]); - - }*/ - m_WriteByte = Send_Command(0, (const char*)send_pos_data, m_SendDataLength); - Sleep(6); - } - - //清除缓存位置 - while (m_SO7_Serial.m_RecvData[39]) - { - send_pos_data[0] = CT_ORDER; - send_pos_data[1] = CT_POSFLAG_CLEAR; - m_WriteByte = Send_Command(0, (const char*)send_pos_data, m_SendDataLength); - Sleep(10); - } - - Sleep(10); - //启动插补和定位功能 - /* if (m_motorType == 1) - { - send_pos_data[0] = CT_MOTOR; - send_pos_data[1] = CT_START_POSITION; - send_pos_data[2] = xyzAxis; - send_pos_data[3] = 0x53; - send_pos_data[4] = INTERPOLATION; - } - else - {*/ - send_pos_data[0] = CT_MOTOR; - send_pos_data[1] = CT_START_POSITION; - send_pos_data[2] = axis_start; - send_pos_data[3] = 0x53; - send_pos_data[4] = m_motorType & 0xff; - /* }*/ - for (int j = 1; j < axisCount; j++) - { - send_pos_data[9 + 4 * (j - 1)] = (target_pos[j] & 0xff); - send_pos_data[10 + 4 * (j - 1)] = ((target_pos[j] >> 8) & 0xff); - send_pos_data[11 + 4 * (j - 1)] = ((target_pos[j] >> 16) & 0xff); - send_pos_data[12 + 4 * (j - 1)] = ((target_pos[j] >> 24) & 0xff); - } - //send_pos_data[25] = direct_pos; - int stepmotoracc = 0; - stepmotoracc = CaculateStepMotorACC(Pos[1], m_SetPotion_Line[1], 10); - send_pos_data[25] = stepmotoracc; - stepmotoracc = CaculateStepMotorACC(Pos[2], m_SetPotion_Line[2], 10); - send_pos_data[26] = stepmotoracc; - stepmotoracc = CaculateStepMotorACC(Pos[3], m_SetPotion_Line[3], 10); - send_pos_data[27] = stepmotoracc; - stepmotoracc = CaculateStepMotorACC(Pos[4], m_SetPotion_Line[4], 10); - /*send_pos_data[25] = m_SetPotion_Line[1]; - send_pos_data[26] = m_SetPotion_Line[2]; - send_pos_data[27] = m_SetPotion_Line[3]; - send_pos_data[28] = m_SetPotion_Line[4];*/ - - send_pos_data[29] = m_SpeedAdjustPeriod[1]; - send_pos_data[30] = m_SpeedAdjustPeriod[2]; - send_pos_data[31] = m_SpeedAdjustPeriod[3]; - send_pos_data[32] = m_SpeedAdjustPeriod[4]; - - for (size_t i = 1; i < 5; i++) - { - Stepdriverspeed[i] = m_SetPotion_DriveSpeed[i] * m_Resolution[i] * 50; - } - - send_pos_data[33] = (Stepdriverspeed[1] >> 8) & 0xff; - send_pos_data[34] = Stepdriverspeed[1] & 0xff; - send_pos_data[35] = (Stepdriverspeed[2] >> 8) & 0xff; - send_pos_data[36] = Stepdriverspeed[2] & 0xff; - send_pos_data[37] = (Stepdriverspeed[3] >> 8) & 0xff; - send_pos_data[38] = Stepdriverspeed[3] & 0xff; - send_pos_data[39] = (Stepdriverspeed[4] >> 8) & 0xff; - send_pos_data[40] = Stepdriverspeed[4] & 0xff; - - - if (bCircleRun) //圆弧插补 - { - bCircleRun = false; - send_pos_data[1] = CT_CIRCLERUN_POSITION; - send_pos_data[2] = 0x03; - send_pos_data[4] = CIRCLER; - for (size_t i = 1; i < 3; i++) - { - iCircleRunPnt[i] = iCircleRunPnt[i] - NowPos[i]; - send_pos_data[26 + 4 * (i - 1)] = (iCircleRunPnt[i] & 0xff); - send_pos_data[27 + 4 * (i - 1)] = ((iCircleRunPnt[i] >> 8) & 0xff); - send_pos_data[28 + 4 * (i - 1)] = ((iCircleRunPnt[i] >> 16) & 0xff); - send_pos_data[29 + 4 * (i - 1)] = ((iCircleRunPnt[i] >> 24) & 0xff); - } - } - g_IsClose = false; - m_WriteByte = Send_Command(0, (const char*)send_pos_data, m_SendDataLength); - set_start = GetTickCount(); - Sleep(3); // - if (eType == HSI_MOTION_MOVE_NOWAIT) - { - g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Nowait SetEvent\n"); - g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Nowait move!\n"); - g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Out Success Nowait\n"); - m_IsExMotion = 0; - bRunGlueDispenser = HSI_THREAD_PAUSED; - m_Thread_State = HSI_THREAD_RUNNING; - SetEvent(m_hTriggerEvent); - } - if (eType == HSI_MOTION_MOVE_WAIT) - { - g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Out Success Wait Mode\n"); - m_Thread_State = HSI_THREAD_RUNNING; - m_IsExMotion = 0; - UpdateMotionState(); - m_Thread_State = HSI_THREAD_PAUSED; - } - } - else - { - g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] HSI_STATUS_MOTION_MOVING\n"); - g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Out\n"); - rStatus = HSI_STATUS_MOTION_MOVING; - } - g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Out\n"); - targetpos_l[1] = PositionX; - targetpos_l[2] = PositionY; - targetpos_l[3] = PositionZ; - } - ReleaseMutex(g_WR_ToMove_Mutex); - return rStatus; -} +//HSI_STATUS HSI_Motion::SetPositionXyzOld(UINT AxisTypes, double PositionX, double PositionY, double PositionZ, +// HSI_MOTION_MOVE_TYPE eType, double dFlyRadius) +//{ +// WaitForSingleObject(g_WR_ToMove_Mutex, INFINITE); +// auto rStatus = HSI_STATUS_NORMAL; +// if (g_pHSI_Motion) +// { +// g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] In\n"); +// g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Pos[1] = %.4f,Pos[2] = %.4f,Pos[3] = %.4f\n", PositionX,PositionY, PositionZ); +// unsigned char send_pos_data[64] = { 0 }; +// axis_start = 0; +// unsigned char direct_pos = 0; +// unsigned char xyzAxis = 0; +// //如果状态非 运动中，设置为运动中 +// if (CurrentMotionState != E_SO7_MOTION_MOVETO) +// { +// CurrentMotionState = E_SO7_MOTION_MOVETO; +// LimitOver(HSI_MOTION_AXIS_X, PositionX); +// LimitOver(HSI_MOTION_AXIS_Y, PositionY); +// LimitOver(HSI_MOTION_AXIS_Z, PositionZ); +// LimitOver(HSI_MOTION_AXIS_R, m_PositionA); +// +// m_PosThread[1] = PositionX; //SetpositionXyz的目标位置 +// m_PosThread[2] = PositionY; +// m_PosThread[3] = PositionZ; +// m_PosThread[4] = m_PositionA; +// +// targetpos_n[1] = PositionX; +// targetpos_n[2] = PositionY; +// targetpos_n[3] = PositionZ; +// targetpos_n[4] = m_PositionA; +// +// int Pos_t[5] = { 0 }; +// int Pos_n[5] = { 0 }; +// int Pos[5] = { 0 }; +// int NowPos[5] = { 0 }; +// int target_pos[5] = { 0 }; +// +// if (m_SO7_Serial.m_RecvData[0] == 2) +// { +// //根据设备类型获取现在位置 +// if (m_DeviceType != 1) //设备类型非三激光 +// { +// if (m_IsHavePattern & 0x01 == 0x01) +// NowPos[1] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial. +// m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]); +// else +// NowPos[1] = (m_SO7_Serial.m_RecvData[17] << 24 | m_SO7_Serial.m_RecvData[18] << 16 | +// m_SO7_Serial.m_RecvData[19] << 8 | m_SO7_Serial.m_RecvData[20]); +// if (m_IsHavePattern & 0x02) +// NowPos[2] = (m_SO7_Serial.m_RecvData[5] << 24 | m_SO7_Serial.m_RecvData[6] << 16 | m_SO7_Serial. +// m_RecvData[7] << 8 | m_SO7_Serial.m_RecvData[8]); +// else +// NowPos[2] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | +// m_SO7_Serial.m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]); +// if (m_IsHavePattern & 0x04) +// NowPos[3] = (m_SO7_Serial.m_RecvData[9] << 24 | m_SO7_Serial.m_RecvData[10] << 16 | m_SO7_Serial +// .m_RecvData[11] << 8 | m_SO7_Serial.m_RecvData[12]); +// else +// NowPos[3] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | +// m_SO7_Serial.m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]); +// if (m_IsHavePattern & 0x08) +// NowPos[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | +// m_SO7_Serial.m_RecvData[15] << 8 | m_SO7_Serial.m_RecvData[16]); +// else +// NowPos[4] = (m_SO7_Serial.m_RecvData[29] << 24 | m_SO7_Serial.m_RecvData[30] << 16 | +// m_SO7_Serial.m_RecvData[31] << 8 | m_SO7_Serial.m_RecvData[32]); +// } +// else +// { +// NowPos[1] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial. +// m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]); +// NowPos[2] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial. +// m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]); +// NowPos[3] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial. +// m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]); +// NowPos[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | m_SO7_Serial. +// m_RecvData[15] << 8 | m_SO7_Serial.m_RecvData[16]); +// } +// //NowPos[1] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial.m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]); +// //NowPos[2] = (m_SO7_Serial.m_RecvData[5] << 24 | m_SO7_Serial.m_RecvData[6] << 16 | m_SO7_Serial.m_RecvData[7] << 8 | m_SO7_Serial.m_RecvData[8]); +// //NowPos[3] = (m_SO7_Serial.m_RecvData[9] << 24 | m_SO7_Serial.m_RecvData[10] << 16 | m_SO7_Serial.m_RecvData[11] << 8 | m_SO7_Serial.m_RecvData[12]); +// //NowPos[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | m_SO7_Serial.m_RecvData[15] << 8 | m_SO7_Serial.m_RecvData[16]); +// +// Pos_t[1] = (m_SO7_Serial.m_RecvData[17] << 24 | m_SO7_Serial.m_RecvData[18] << 16 | m_SO7_Serial. +// m_RecvData[19] << 8 | m_SO7_Serial.m_RecvData[20]); +// Pos_t[2] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial. +// m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]); +// Pos_t[3] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial. +// m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]); +// Pos_t[4] = (m_SO7_Serial.m_RecvData[29] << 24 | m_SO7_Serial.m_RecvData[30] << 16 | m_SO7_Serial. +// m_RecvData[31] << 8 | m_SO7_Serial.m_RecvData[32]); +// } +// else +// { +// /* 编译器隐式执行的任何类型转换都可以由static_cast显式完成。 +// static_cast可以用来将枚举类型转换成整型，或者整型转换成浮点型*/ +// Pos_t[1] = NowPos[1] = static_cast(m_EncPos[1] / m_Resolution[1]); +// Pos_t[2] = NowPos[2] = static_cast(m_EncPos[2] / m_Resolution[2]); +// Pos_t[3] = NowPos[3] = static_cast(m_EncPos[3] / m_Resolution[3]); +// Pos_t[4] = NowPos[4] = static_cast(m_EncPos[4] / m_Resolution[4]); +// } +// +// if (m_motorType & 0x01) //步进电机 +// Pos[1] = static_cast(PositionX / m_Resolution[1]) - NowPos[1]; +// else +// Pos[1] = static_cast(PositionX / m_Resolution[1]) - Pos_t[1]; +// if (m_motorType & 0x02) //步进电机 +// Pos[2] = static_cast(PositionX / m_Resolution[2]) - NowPos[2]; +// else +// Pos[2] = static_cast(PositionY / m_Resolution[2]) - Pos_t[2]; +// if (m_motorType & 0x04) //步进电机 +// Pos[3] = static_cast(PositionX / m_Resolution[3]) - NowPos[3]; +// else +// Pos[3] = static_cast(PositionZ / m_Resolution[3]) - Pos_t[3]; +// if (m_motorType & 0x08) //步进电机 +// Pos[4] = static_cast(PositionX / m_Resolution[4]) - NowPos[4]; +// else +// Pos[4] = static_cast(m_PositionA / m_Resolution[4]) - Pos_t[4]; +// +// /*if (m_motorType==1) +// { +// if (m_IsUse_HSICompensation) +// { +// } +// else +// { +// Pos[1] = (int)(PositionX / m_Resolution[1]) - NowPos[1]; +// Pos[2] = (int)(PositionY / m_Resolution[2]) - NowPos[2]; +// Pos[3] = (int)(PositionZ / m_Resolution[3]) - NowPos[3]; +// Pos[4] = (int)(m_PositionA / m_Resolution[4]) - NowPos[4]; +// } +// } +// else +// { +// Pos_n[1] = (int)(targetpos_n[1] / m_Resolution[1]) - NowPos[1]; +// Pos_n[2] = (int)(targetpos_n[2] / m_Resolution[2]) - NowPos[2]; +// Pos_n[3] = (int)(targetpos_n[3] / m_Resolution[3]) - NowPos[3]; +// +// Pos_t[1] = (int)(targetpos_n[1] / m_Resolution[1]) - (int)(targetpos_l[1] / m_Resolution[1]); +// Pos_t[2] = (int)(targetpos_n[2] / m_Resolution[2]) - (int)(targetpos_l[2] / m_Resolution[1]); +// Pos_t[3] = (int)(targetpos_n[3] / m_Resolution[3]) - (int)(targetpos_l[3] / m_Resolution[1]); +// +// if (m_IsUse_HSICompensation) +// { +// for (int k = 1; k < 4; k++) +// { +// if (abs(Pos_n[k] - Pos_t[k]) > m_Compensation_Pluse) +// { +// Pos[k] = Pos_n[k]; +// } +// else +// { +// Pos[k] = Pos_t[k]; +// } +// if (abs(Pos[k]) < m_Compensation_Pluse) +// { +// Pos[k] = 0; +// } +// } +// } +// else +// { +// Pos[1] = (int)(PositionX / m_Resolution[1]) - NowPos[1]; +// Pos[2] = (int)(PositionY / m_Resolution[2]) - NowPos[2]; +// Pos[3] = (int)(PositionZ / m_Resolution[3]) - NowPos[3]; +// Pos[4] = (int)(m_PositionA / m_Resolution[4]) - NowPos[4]; +// } +// }*/ +// target_pos[1] = static_cast(PositionX / m_Resolution[1]); //计算到目标位置 +// target_pos[2] = static_cast(PositionY / m_Resolution[2]); +// target_pos[3] = static_cast(PositionZ / m_Resolution[3]); +// target_pos[4] = static_cast(m_PositionA / m_Resolution[4]); +// +// begin_position[1] = target_pos[1]; //将目标位置设置为 开始位置 +// begin_position[2] = target_pos[2]; +// begin_position[3] = target_pos[3]; +// begin_position[4] = target_pos[4]; +// +// float scale[4] = { 0 }; +// int Stepdriverspeed[5] = { 0 }; +// int StepStartspeed[5] = { 0 }; +// int StepAcc[5] = { 0 }; +// //if (Pos[1] > 0) direct_pos |= 0x01; +// //if (Pos[2] > 0) direct_pos |= 0x02; +// //if (Pos[3] > 0) direct_pos |= 0x04; +// if (abs(Pos[1]) > 2) axis_start |= 0x01; +// if (abs(Pos[2]) > 2) axis_start |= 0x02; +// if (abs(Pos[3]) > 2) axis_start |= 0x04; +// int MaxPos = abs(Pos[1]); +// for (int i = 2; i < 5; i++) +// { +// if (MaxPos < abs(Pos[i])) +// { +// MaxPos = abs(Pos[i]); +// } +// } +// //MaxPos = abs(MaxPos); +// scale[0] = abs(Pos[1]) / static_cast(MaxPos); +// scale[1] = abs(Pos[2]) / static_cast(MaxPos); +// scale[2] = abs(Pos[3]) / static_cast(MaxPos); +// scale[3] = abs(Pos[4]) / static_cast(MaxPos); +// +// g_pLogger->SendAndFlushWithTime( +// L"[SetPositionXyzNowPos] Pos[1] = %.4f,Pos[2] = %.4f,Pos[3] = %.4f,Pos[4] = %.4f\n", +// NowPos[1] * m_Resolution[1], NowPos[2] * m_Resolution[2], NowPos[3] * m_Resolution[3], +// NowPos[4] * m_Resolution[4]); +// g_pLogger->SendAndFlushWithTime( +// L"[SetPositionXyzTagPos] Pos[1] = %.4f,Pos[2] = %.4f,Pos[3] = %.4f,Pos[4] = %.4f\n", PositionX, +// PositionY, PositionZ, m_PositionA); +// +// int stepinterpolation = 0x03; +// //if (!m_motorType) +// //{ +// // /*if ((stepinterpolation & 0x01) && m_IsUseManualRunin) +// // { +// // Stepdriverspeed[1] = m_SetPotion_DriveSpeed[1] * scale[0]; +// // if (Stepdriverspeed[1] < m_SetPotion_StartSpeed[1]) +// // { +// // Stepdriverspeed[1] = m_SetPotion_StartSpeed[1]; +// // } +// // } +// // else +// // { +// // Stepdriverspeed[1] = m_SetPotion_DriveSpeed[1]; +// // } +// // if ((stepinterpolation & 0x02) && m_IsUseManualRunin) +// // { +// // Stepdriverspeed[2] = m_SetPotion_DriveSpeed[2] * scale[1]; +// // if (Stepdriverspeed[2] < m_SetPotion_StartSpeed[2]) +// // { +// // Stepdriverspeed[2] = m_SetPotion_StartSpeed[2]; +// // } +// // } +// // else +// // { +// // Stepdriverspeed[2] = m_SetPotion_DriveSpeed[2]; +// // } +// // if ((stepinterpolation & 0x04) && m_IsUseManualRunin) +// // { +// // Stepdriverspeed[3] = m_SetPotion_DriveSpeed[3] * scale[2]; +// // if (Stepdriverspeed[3] < m_SetPotion_StartSpeed[3]) +// // { +// // Stepdriverspeed[3] = m_SetPotion_DriveSpeed[3]; +// // } +// // } +// // else +// // { +// // Stepdriverspeed[3] = m_SetPotion_DriveSpeed[3]; +// // } +// // if ((stepinterpolation & 0x08) && m_IsUseManualRunin) +// // { +// // Stepdriverspeed[4] = m_SetPotion_DriveSpeed[4] * scale[3]; +// // if (Stepdriverspeed[4] < m_SetPotion_StartSpeed[4]) +// // { +// // Stepdriverspeed[4] = m_SetPotion_DriveSpeed[4]; +// // } +// // } +// // else +// // { +// // Stepdriverspeed[4] = m_SetPotion_DriveSpeed[4]; +// // }*/ +// // for (int i = 1; i < 3; i++) +// // { +// // if (abs(Pos[i]) < m_stepPosition_Load[0]) +// // { +// // StepStartspeed[i] = m_stepPosition_L_speed[0]; +// // Stepdriverspeed[i] = m_stepPosition_H_speed[0]; +// // StepAcc[i] = m_stepPosition_acc[0]; +// // } +// // else if (abs(Pos[i]) < m_stepPosition_Load[1]) +// // { +// // StepStartspeed[i] = m_stepPosition_L_speed[1]; +// // Stepdriverspeed[i] = m_stepPosition_H_speed[1]; +// // StepAcc[i] = m_stepPosition_acc[1]; +// // } +// // else if (abs(Pos[i]) < m_stepPosition_Load[2]) +// // { +// // StepStartspeed[i] = m_stepPosition_L_speed[2]; +// // Stepdriverspeed[i] = m_stepPosition_H_speed[2]; +// // StepAcc[i] = m_stepPosition_acc[2]; +// // } +// // else if (abs(Pos[i]) < m_stepPosition_Load[3]) +// // { +// // StepStartspeed[i] = m_stepPosition_L_speed[3]; +// // Stepdriverspeed[i] = m_stepPosition_H_speed[3]; +// // StepAcc[i] = m_stepPosition_acc[3]; +// // } +// // else if (abs(Pos[i]) < m_stepPosition_Load[4]) +// // { +// // StepStartspeed[i] = m_stepPosition_L_speed[4]; +// // Stepdriverspeed[i] = m_stepPosition_H_speed[4]; +// // StepAcc[i] = m_stepPosition_acc[4]; +// // } +// // else if (abs(Pos[i]) < m_stepPosition_Load[5]) +// // { +// // StepStartspeed[i] = m_stepPosition_L_speed[5]; +// // Stepdriverspeed[i] = m_stepPosition_H_speed[5]; +// // StepAcc[i] = m_stepPosition_acc[5]; +// // } +// // else if (abs(Pos[i]) < m_stepPosition_Load[6]) +// // { +// // StepStartspeed[i] = m_stepPosition_L_speed[6]; +// // Stepdriverspeed[i] = m_stepPosition_H_speed[6]; +// // StepAcc[i] = m_stepPosition_acc[6]; +// // } +// // else if (abs(Pos[i]) < m_stepPosition_Load[7]) +// // { +// // StepStartspeed[i] = m_stepPosition_L_speed[7]; +// // Stepdriverspeed[i] = m_stepPosition_H_speed[7]; +// // StepAcc[i] = m_stepPosition_acc[7]; +// // } +// // else if (abs(Pos[i]) < m_stepPosition_Load[8]) +// // { +// // StepStartspeed[i] = m_stepPosition_L_speed[8]; +// // Stepdriverspeed[i] = m_stepPosition_H_speed[8]; +// // StepAcc[i] = m_stepPosition_acc[8]; +// // } +// // else if (abs(Pos[i]) < m_stepPosition_Load[9]) +// // { +// // StepStartspeed[i] = m_stepPosition_L_speed[9]; +// // Stepdriverspeed[i] = m_stepPosition_H_speed[9]; +// // StepAcc[i] = m_stepPosition_acc[9]; +// // } +// // else +// // { +// // StepStartspeed[i] = m_SetPotion_StartSpeed[i]; +// // Stepdriverspeed[i] = m_SetPotion_DriveSpeed[i]; +// // StepAcc[i] = m_SetPotion_Line[i]; +// // } +// // } +// // StepStartspeed[3] = m_SetPotion_StartSpeed[3]; +// // StepAcc[3] = m_SetPotion_Line[3]; +// // Stepdriverspeed[3] = m_SetPotion_DriveSpeed[3]; +// //} +// +// int axisCount = 4; +// if (fourthAxisFlag) //第4轴 +// { +// fourthAxisFlag = false; +// if (Pos[4] > 0) direct_pos |= 0x08; +// if (abs(Pos[4]) > 20) axis_start |= 0x08; +// axisCount = 5; +// xyzAxis = AXIS_XYZU; +// } +// else +// { +// xyzAxis = AXIS_XYZ; +// } +// +// //设置运动参数 +// for (int i = 1; i < axisCount; i++) +// { +// int time_out_send = 0; +// +// send_pos_data[0] = CT_MOTOR; +// send_pos_data[1] = CT_MOTOR_SET; +// send_pos_data[2] = 1 << (i - 1); +// send_pos_data[3] = POSITION_SPEED_ACC_DEC_POS; +// /*if (m_motorType == 1) +// {*/ +// send_pos_data[4] = (m_SetPotion_StartSpeed[i] & 0xff); +// send_pos_data[5] = ((m_SetPotion_StartSpeed[i] >> 8) & 0xff); +// send_pos_data[6] = ((m_SetPotion_StartSpeed[i] >> 16) & 0xff); +// send_pos_data[7] = ((m_SetPotion_StartSpeed[i] >> 24) & 0xff); +// if ((xyzAxis == AXIS_XYZU) && (m_IsUseFourthSpeed == 1)) +// { +// send_pos_data[8] = (m_SetPotion_DriveSpeed[4] & 0xff); +// send_pos_data[9] = ((m_SetPotion_DriveSpeed[4] >> 8) & 0xff); +// send_pos_data[10] = ((m_SetPotion_DriveSpeed[4] >> 16) & 0xff); +// send_pos_data[11] = ((m_SetPotion_DriveSpeed[4] >> 24) & 0xff); +// } +// else +// { +// send_pos_data[8] = (m_SetPotion_DriveSpeed[i] & 0xff); +// send_pos_data[9] = ((m_SetPotion_DriveSpeed[i] >> 8) & 0xff); +// send_pos_data[10] = ((m_SetPotion_DriveSpeed[i] >> 16) & 0xff); +// send_pos_data[11] = ((m_SetPotion_DriveSpeed[i] >> 24) & 0xff); +// } +// send_pos_data[12] = (m_SetPotion_Line[i] & 0xff); +// send_pos_data[13] = ((m_SetPotion_Line[i] >> 8) & 0xff); +// send_pos_data[14] = ((m_SetPotion_Line[i] >> 16) & 0xff); +// send_pos_data[15] = ((m_SetPotion_Line[i] >> 24) & 0xff); +// +// send_pos_data[16] = (m_SetPotion_Buffer[i] & 0xff); +// send_pos_data[17] = ((m_SetPotion_Buffer[i] >> 8) & 0xff); +// send_pos_data[18] = ((m_SetPotion_Buffer[i] >> 16) & 0xff); +// send_pos_data[19] = ((m_SetPotion_Buffer[i] >> 24) & 0xff); +// +// send_pos_data[28] = (target_pos[i] & 0xff); +// send_pos_data[29] = ((target_pos[i] >> 8) & 0xff); +// send_pos_data[30] = ((target_pos[i] >> 16) & 0xff); +// send_pos_data[31] = ((target_pos[i] >> 24) & 0xff); +// +// send_pos_data[32] = (Pos[i] & 0xff); +// send_pos_data[33] = ((Pos[i] >> 8) & 0xff); +// send_pos_data[34] = ((Pos[i] >> 16) & 0xff); +// send_pos_data[35] = ((Pos[i] >> 24) & 0xff); +// +// g_pLogger->SendAndFlushWithTime( +// L"[SetPositionXyz] m_SetPotion_StartSpeed[%d] = %d,m_SetPotion_DriveSpeed[%d] = %d\n", i, +// m_SetPotion_StartSpeed[i], i, m_SetPotion_DriveSpeed[i]); +// //send_pos_data[32] = 0x21; +// /*}*/ +// +// /*else +// { +// send_pos_data[4] = (StepStartspeed[i] & 0xff); +// send_pos_data[5] = ((StepStartspeed[i] >> 8) & 0xff); +// send_pos_data[6] = ((StepStartspeed[i] >> 16) & 0xff); +// send_pos_data[7] = ((StepStartspeed[i] >> 24) & 0xff); +// +// send_pos_data[8] = (Stepdriverspeed[i] & 0xff); +// send_pos_data[9] = ((Stepdriverspeed[i] >> 8) & 0xff); +// send_pos_data[10] = ((Stepdriverspeed[i] >> 16) & 0xff); +// send_pos_data[11] = ((Stepdriverspeed[i] >> 24) & 0xff); +// +// send_pos_data[12] = (StepAcc[i] & 0xff); +// send_pos_data[13] = ((StepAcc[i] >> 8) & 0xff); +// send_pos_data[14] = ((StepAcc[i] >> 16) & 0xff); +// send_pos_data[15] = ((StepAcc[i] >> 24) & 0xff); +// +// send_pos_data[16] = (m_SetPotion_Buffer[i] & 0xff); +// send_pos_data[17] = ((m_SetPotion_Buffer[i] >> 8) & 0xff); +// send_pos_data[18] = ((m_SetPotion_Buffer[i] >> 16) & 0xff); +// send_pos_data[19] = ((m_SetPotion_Buffer[i] >> 24) & 0xff); +// +// send_pos_data[28] = (target_pos[i] & 0xff); +// send_pos_data[29] = ((target_pos[i] >> 8) & 0xff); +// send_pos_data[30] = ((target_pos[i] >> 16) & 0xff); +// send_pos_data[31] = ((target_pos[i] >> 24) & 0xff); +// +// send_pos_data[32] = (Pos[i] & 0xff); +// send_pos_data[33] = ((Pos[i] >> 8) & 0xff); +// send_pos_data[34] = ((Pos[i] >> 16) & 0xff); +// send_pos_data[35] = ((Pos[i] >> 24) & 0xff); +// +// g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] m_SetPotion_StartSpeed[%d] = %d,m_SetPotion_DriveSpeed[%d] = %d\n", i, m_SetPotion_StartSpeed[i], i, m_SetPotion_DriveSpeed[i]); +// }*/ +// m_WriteByte = Send_Command(0, (const char*)send_pos_data, m_SendDataLength); +// Sleep(6); +// } +// +// //清除缓存位置 +// while (m_SO7_Serial.m_RecvData[39]) +// { +// send_pos_data[0] = CT_ORDER; +// send_pos_data[1] = CT_POSFLAG_CLEAR; +// m_WriteByte = Send_Command(0, (const char*)send_pos_data, m_SendDataLength); +// Sleep(10); +// } +// +// Sleep(10); +// //启动插补和定位功能 +// /* if (m_motorType == 1) +// { +// send_pos_data[0] = CT_MOTOR; +// send_pos_data[1] = CT_START_POSITION; +// send_pos_data[2] = xyzAxis; +// send_pos_data[3] = 0x53; +// send_pos_data[4] = INTERPOLATION; +// } +// else +// {*/ +// send_pos_data[0] = CT_MOTOR; +// send_pos_data[1] = CT_START_POSITION; +// send_pos_data[2] = axis_start; +// send_pos_data[3] = 0x53; +// send_pos_data[4] = m_motorType & 0xff; +// /* }*/ +// for (int j = 1; j < axisCount; j++) +// { +// send_pos_data[9 + 4 * (j - 1)] = (target_pos[j] & 0xff); +// send_pos_data[10 + 4 * (j - 1)] = ((target_pos[j] >> 8) & 0xff); +// send_pos_data[11 + 4 * (j - 1)] = ((target_pos[j] >> 16) & 0xff); +// send_pos_data[12 + 4 * (j - 1)] = ((target_pos[j] >> 24) & 0xff); +// } +// //send_pos_data[25] = direct_pos; +// int stepmotoracc = 0; +// stepmotoracc = CaculateStepMotorACC(Pos[1], m_SetPotion_Line[1], 10); +// send_pos_data[25] = stepmotoracc; +// stepmotoracc = CaculateStepMotorACC(Pos[2], m_SetPotion_Line[2], 10); +// send_pos_data[26] = stepmotoracc; +// stepmotoracc = CaculateStepMotorACC(Pos[3], m_SetPotion_Line[3], 10); +// send_pos_data[27] = stepmotoracc; +// stepmotoracc = CaculateStepMotorACC(Pos[4], m_SetPotion_Line[4], 10); +// /*send_pos_data[25] = m_SetPotion_Line[1]; +// send_pos_data[26] = m_SetPotion_Line[2]; +// send_pos_data[27] = m_SetPotion_Line[3]; +// send_pos_data[28] = m_SetPotion_Line[4];*/ +// +// send_pos_data[29] = m_SpeedAdjustPeriod[1]; +// send_pos_data[30] = m_SpeedAdjustPeriod[2]; +// send_pos_data[31] = m_SpeedAdjustPeriod[3]; +// send_pos_data[32] = m_SpeedAdjustPeriod[4]; +// +// for (size_t i = 1; i < 5; i++) +// { +// Stepdriverspeed[i] = m_SetPotion_DriveSpeed[i] * m_Resolution[i] * 50; +// } +// +// send_pos_data[33] = (Stepdriverspeed[1] >> 8) & 0xff; +// send_pos_data[34] = Stepdriverspeed[1] & 0xff; +// send_pos_data[35] = (Stepdriverspeed[2] >> 8) & 0xff; +// send_pos_data[36] = Stepdriverspeed[2] & 0xff; +// send_pos_data[37] = (Stepdriverspeed[3] >> 8) & 0xff; +// send_pos_data[38] = Stepdriverspeed[3] & 0xff; +// send_pos_data[39] = (Stepdriverspeed[4] >> 8) & 0xff; +// send_pos_data[40] = Stepdriverspeed[4] & 0xff; +// +// if (bCircleRun) //圆弧插补 +// { +// bCircleRun = false; +// send_pos_data[1] = CT_CIRCLERUN_POSITION; +// send_pos_data[2] = 0x03; +// send_pos_data[4] = CIRCLER; +// for (size_t i = 1; i < 3; i++) +// { +// iCircleRunPnt[i] = iCircleRunPnt[i] - NowPos[i]; +// send_pos_data[26 + 4 * (i - 1)] = (iCircleRunPnt[i] & 0xff); +// send_pos_data[27 + 4 * (i - 1)] = ((iCircleRunPnt[i] >> 8) & 0xff); +// send_pos_data[28 + 4 * (i - 1)] = ((iCircleRunPnt[i] >> 16) & 0xff); +// send_pos_data[29 + 4 * (i - 1)] = ((iCircleRunPnt[i] >> 24) & 0xff); +// } +// } +// g_IsClose = false; +// m_WriteByte = Send_Command(0, (const char*)send_pos_data, m_SendDataLength); +// set_start = GetTickCount(); +// Sleep(3); // +// if (eType == HSI_MOTION_MOVE_NOWAIT) +// { +// g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Nowait SetEvent\n"); +// g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Nowait move!\n"); +// g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Out Success Nowait\n"); +// m_IsExMotion = 0; +// bRunGlueDispenser = HSI_THREAD_PAUSED; +// m_Thread_State = HSI_THREAD_RUNNING; +// SetEvent(m_hTriggerEvent); +// } +// if (eType == HSI_MOTION_MOVE_WAIT) +// { +// g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Out Success Wait Mode\n"); +// m_Thread_State = HSI_THREAD_RUNNING; +// m_IsExMotion = 0; +// UpdateMotionState(); +// m_Thread_State = HSI_THREAD_PAUSED; +// } +// } +// else +// { +// g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] HSI_STATUS_MOTION_MOVING\n"); +// g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Out\n"); +// rStatus = HSI_STATUS_MOTION_MOVING; +// } +// g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Out\n"); +// targetpos_l[1] = PositionX; +// targetpos_l[2] = PositionY; +// targetpos_l[3] = PositionZ; +// } +// ReleaseMutex(g_WR_ToMove_Mutex); +// return rStatus; +//} HSI_STATUS HSI_Motion::SetPositionXyz(UINT AxisTypes, double PositionX, double PositionY, double PositionZ, - HSI_MOTION_MOVE_TYPE eType, double dFlyRadius) + HSI_MOTION_MOVE_TYPE eType, double dFlyRadius) { WaitForSingleObject(g_WR_ToMove_Mutex, INFINITE); auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] In\n"); - g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] AxisTypes = %d, PositionX = %.4f,PositionY = %.4f,PositionZ = %.4f\n", AxisTypes,PositionX, PositionY, PositionZ); + g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] AxisTypes = %d, PositionX = %.4f,PositionY = %.4f,PositionZ = %.4f\n", AxisTypes, PositionX, PositionY, PositionZ); axis_start = 0; unsigned char direct_pos = 0; @@ -3514,7 +3495,7 @@ HSI_STATUS HSI_Motion::SetPositionXyz(UINT AxisTypes, double PositionX, double P if (CurrentMotionState != E_SO7_MOTION_MOVETO) //当前运动状态 { CurrentMotionState = E_SO7_MOTION_MOVETO; - + //限位功能 LimitOver(HSI_MOTION_AXIS_X, PositionX); LimitOver(HSI_MOTION_AXIS_Y, PositionY); @@ -3531,9 +3512,8 @@ HSI_STATUS HSI_Motion::SetPositionXyz(UINT AxisTypes, double PositionX, double P m_PosThread[4] = m_PositionA; //目标位置 - g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] PositionX = %.4f, PositionY = %.4f, PositionZ = %.4f\n", PositionX,PositionY, PositionZ); + g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] PositionX = %.4f, PositionY = %.4f, PositionZ = %.4f\n", PositionX, PositionY, PositionZ); - //设置速度，对应配置文件中定位合成速度 SET_POTION_DRIVESPEED_1 g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] DriveSpeed[0] = %d, DriveSpeed[1] = %d, DriveSpeed[2] = %d, DriveSpeed[3] = %d, DriveSpeed[4] = %d\n", m_SetPotion_DriveSpeed[0], m_SetPotion_DriveSpeed[1], m_SetPotion_DriveSpeed[2], m_SetPotion_DriveSpeed[3], m_SetPotion_DriveSpeed[4]); @@ -3546,25 +3526,24 @@ HSI_STATUS HSI_Motion::SetPositionXyz(UINT AxisTypes, double PositionX, double P m_SetPotion_DriveSpeed[1] * 100, m_SetPotion_DriveSpeed[1] * 100 }; - + SetSingleAxisMotionParams(ACSC_AXIS_1, X_SetmotionParam);//设置X轴定位速度 m_SetPotion_DriveSpeed[ACSC_AXIS_1] = X_SetmotionParam[0];// 记录X轴速度 g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] 设置Y轴定位速度 %d\n", m_SetPotion_DriveSpeed[2]); - double Y_SetmotionParam[5] = { + double Y_SetmotionParam[5] = { m_SetPotion_DriveSpeed[2], - m_SetPotion_DriveSpeed[2] * 10 , - m_SetPotion_DriveSpeed[2] * 10, + m_SetPotion_DriveSpeed[2] * 10 , + m_SetPotion_DriveSpeed[2] * 10, m_SetPotion_DriveSpeed[2] * 100, m_SetPotion_DriveSpeed[2] * 100 }; - SetSingleAxisMotionParams(ACSC_AXIS_0, Y_SetmotionParam);//设置Y轴定位速度 m_SetPotion_DriveSpeed[ACSC_AXIS_0] = Y_SetmotionParam[0];// 记录Y轴速度 g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] 设置Z轴定位速度 %d\n", m_SetPotion_DriveSpeed[3]); - double Z_SetmotionParam[5] = { + double Z_SetmotionParam[5] = { m_SetPotion_DriveSpeed[3], //速度 m_SetPotion_DriveSpeed[3] * 10 ,//加速度 m_SetPotion_DriveSpeed[3] * 10, //减速度 @@ -3588,14 +3567,13 @@ HSI_STATUS HSI_Motion::SetPositionXyz(UINT AxisTypes, double PositionX, double P } //开始运动到指定位置，多轴运动 - double Points[] = {PositionX, PositionY, PositionZ}; //目标位置点 + double Points[] = { PositionX, PositionY, PositionZ }; //目标位置点 if (!acsc_ToPointM(handleACS, 0, Axes, Points, nullptr)) //移动到绝对位置 { g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] ACS Multi Motion Error\n"); ErrorsHandler(); } - //状态更新 if (eType == HSI_MOTION_MOVE_NOWAIT) //非等待 { @@ -3657,16 +3635,16 @@ int HSI_Motion::CaculateStepMotorACC(int pos, int maxacc, int minacc) //版权声明：本文为CSDN博主「nick__huang」的原创文章，遵循CC 4.0 BY - SA版权协议，转载请附上原文出处链接及本声明。 //原文链接：https ://blog.csdn.net/u012610237/article/details/58599083 -//void func3(void); //利用全局变量返回数组 -//void func2(uchar* s); //利用指针返回数组 -//uchar* func1(); //利用指针函数返回数组 +//void func3(void); //利用全局变量返回数组 +//void func2(uchar* s); //利用指针返回数组 +//uchar* func1(); //利用指针函数返回数组 //void func0(uchar*& r); //利用引用返回数组 /** * \brief 获取单轴运动参数 - * \param AXIS - * \param motionParam - * \return + * \param AXIS + * \param motionParam + * \return */ HSI_STATUS HSI_Motion::GetSingleAxisParam(int AXIS, double motionParam[5]) { @@ -3713,7 +3691,7 @@ HSI_STATUS HSI_Motion::GetSingleAxisParam(int AXIS, double motionParam[5]) //打印数组 motionParam[5] /* g_pLogger->SendAndFlushWithTime(L"[GetSingleAxisParam] AXIS = %d, Vel = %.4f, ACC = %.4f, DCC = %.4f, KillDec = %.4f, Jerk = %.4f\n", AXIS,motionParam[0], motionParam[1], motionParam[2], motionParam[3], motionParam[4]);*/ - //g_pLogger->SendAndFlushWithTime(L"[GetSingleAxisParam] Out\n"); + //g_pLogger->SendAndFlushWithTime(L"[GetSingleAxisParam] Out\n"); } return rStatus; @@ -3766,7 +3744,7 @@ HSI_STATUS HSI_Motion::SetSingleAxisParam(int AXIS, double motionParam[5]) //设 //打印 motionParam[5] g_pLogger->SendAndFlushWithTime(L"[SetSingleAxisParam] AXIS = %d, Vel = %.4f, ACC = %.4f, DCC = %.4f, KillDec = %.4f, Jerk = %.4f\n", - AXIS,motionParam[0], motionParam[1], motionParam[2], motionParam[3], motionParam[4]); + AXIS, motionParam[0], motionParam[1], motionParam[2], motionParam[3], motionParam[4]); //g_pLogger->SendAndFlushWithTime(L"[SetSingleAxisParam] Out\n"); } @@ -3776,7 +3754,7 @@ HSI_STATUS HSI_Motion::SetSingleAxisParam(int AXIS, double motionParam[5]) //设 //=========================================================================== HSI_STATUS HSI_Motion::SetPositionXyza(UINT AxisTypes, double PositionX, double PositionY, double PositionZ, - double PositionA, HSI_MOTION_MOVE_TYPE eType, double dFlyRadius) + double PositionA, HSI_MOTION_MOVE_TYPE eType, double dFlyRadius) { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) @@ -3805,9 +3783,9 @@ HSI_STATUS HSI_Motion::SetPositionXyzCache(UINT AxisTypes, HSI_MOTION_MOVE_TYPE //=========================================================================== /** * \brief 获取缓存点 - * \param CacheData - * \param DataCount - * \return + * \param CacheData + * \param DataCount + * \return */ HSI_STATUS HSI_Motion::GetPositionXyzCache(unsigned char* CacheData, int& DataCount) { @@ -3819,7 +3797,7 @@ HSI_STATUS HSI_Motion::GetPositionXyzCache(unsigned char* CacheData, int& DataCo if (m_SO7_Serial.IsOpen()) { //发送获取点数量命令 0x01 0x05 - unsigned char m_cSendData[8] = {0}; + unsigned char m_cSendData[8] = { 0 }; m_cSendData[0] = 0x01; m_cSendData[1] = 0x05; m_WriteByte = Send_Command(0, (const char*)m_cSendData, 2, 8); //期望回复8个字节，其中 4-7字节为点数量 @@ -3836,12 +3814,12 @@ HSI_STATUS HSI_Motion::GetPositionXyzCache(unsigned char* CacheData, int& DataCo //详细 https://blog.csdn.net/hebbely/article/details/79577880 g_pLogger->SendAndFlushWithTime(L"[GetPositionXyzCache] DataCount：%d, %s\n", DataCount, - m_SO7_Serial.HexToStr((const char*)m_SO7_Serial.m_RecvData, 8)); + m_SO7_Serial.HexToStr((const char*)m_SO7_Serial.m_RecvData, 8)); } else //查询失败 { g_pLogger->SendAndFlushWithTime(L"[GetPositionXyzCache] DataCount：%d, %s\n", DataCount, - m_SO7_Serial.HexToStr((const char*)m_SO7_Serial.m_RecvData, 8)); + m_SO7_Serial.HexToStr((const char*)m_SO7_Serial.m_RecvData, 8)); } } @@ -3865,9 +3843,9 @@ HSI_STATUS HSI_Motion::GetPositionXyzCache(unsigned char* CacheData, int& DataCo m_SO7_Serial.m_RecvData[0] = 0xff; memcpy(CacheData, m_SO7_Serial.m_RecvData, m_SO7_Serial.m_iRecvBytes); //返回内容 - DataCount = m_SO7_Serial.m_iRecvBytes ; //返回点数 + DataCount = m_SO7_Serial.m_iRecvBytes; //返回点数 g_pLogger->SendAndFlushWithTime(L"[GetPositionXyzCache] m_iRecvBytes ok, Points: %d\n", - m_SO7_Serial.m_iRecvBytes); + m_SO7_Serial.m_iRecvBytes); m_SO7_Serial.m_iRecvState = FALSE; //将接收标志重置 rStatus = HSI_STATUS_NORMAL; } @@ -3886,10 +3864,10 @@ HSI_STATUS HSI_Motion::GetPositionXyzCache(unsigned char* CacheData, int& DataCo //=========================================================================== /** * \brief 圆弧插补 - * \param PositionX - * \param PositionY - * \param PositionZ - * \return + * \param PositionX + * \param PositionY + * \param PositionZ + * \return */ HSI_STATUS HSI_Motion::SetCircleInterpolate(double PositionX, double PositionY, double PositionZ) { @@ -3909,7 +3887,7 @@ HSI_STATUS HSI_Motion::SetCircleInterpolate(double PositionX, double PositionY, //=========================================================================== /** * \brief 探针接口 - * \param RetractManDist + * \param RetractManDist */ void HSI_Motion::ProbeRetractManDist(int RetractManDist) { @@ -3940,7 +3918,7 @@ HSI_STATUS HSI_Motion::JogProbe(UINT AxisTypes, double Speed) m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength); Sleep(10); } - unsigned char send_probe_data[64] = {0}; + unsigned char send_probe_data[64] = { 0 }; unsigned char motionAxis = 0; unsigned char motionDir = 0; for (size_t i = 0; i < 3; i++) @@ -3986,7 +3964,7 @@ HSI_STATUS HSI_Motion::JogProbe(UINT AxisTypes, double Speed) m_Thread_State = HSI_THREAD_PAUSED; JogSpeed = abs(SpeedPercent(AxisNumber, Speed, DriveSpeed, StartSpeed, AccLine, DecLine, AccCurve, - DecCurve)); + DecCurve)); send_probe_data[0] = CT_MOTOR; send_probe_data[1] = CT_MOTOR_SET; send_probe_data[2] = AxisNumber; @@ -4045,8 +4023,8 @@ HSI_STATUS HSI_Motion::JogProbe(UINT AxisTypes, double Speed) //=========================================================================== /** * \brief 读取配置 - * \param GoogolIniFile - * \return + * \param GoogolIniFile + * \return */ HSI_STATUS HSI_Motion::Load_EF3_Motion_Inifile(CString GoogolIniFile) { @@ -4056,8 +4034,8 @@ HSI_STATUS HSI_Motion::Load_EF3_Motion_Inifile(CString GoogolIniFile) 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"}; + CString strGear[5] = { L"GEAR0_", L"GEAR1_", L"GEAR2_", L"GEAR3_", L"GEAR4_" }; + CString axisNum[5] = { L"0", L"1", L"2", L"3", L"4" }; //判断Log目录是否存在，不存在就创建 if (CreateDirectory(m_AppPath + L"\\Log", nullptr)) { @@ -4081,78 +4059,79 @@ HSI_STATUS HSI_Motion::Load_EF3_Motion_Inifile(CString GoogolIniFile) m_precisionCount[i] = GetPrivateProfileInt(L"PRECISION", L"PRECISION_COUNT_" + axisNum[i], 14000, csAppPath); //回家误差脉冲个数 m_precisionTime[i] = static_cast(GetPrivateProfileInt(L"PRECISION", L"PRECISION_TIME_" + axisNum[i], - 14000, - csAppPath)); //超时时间(0.1ms) + 14000, + csAppPath)); //超时时间(0.1ms) GetPrivateProfileString(L"RESOLUTION", L"SCALE_RESOLUTION_" + axisNum[i], L"0.0004", - temp.GetBufferSetLength(50), 50, csAppPath); + 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); + 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); + 50, csAppPath); m_P_Work_Limit[i] = (atof(T2A(temp)));//各轴正限位 //打印正负限位 g_pLogger->SendAndFlushWithTime(L"[Load_EF3_Motion_Inifile] m_N_Work_Limit[%d]: %.2f, m_P_Work_Limit[%d]: %.2f\n", - i, m_N_Work_Limit[i], i, m_P_Work_Limit[i]); + i, m_N_Work_Limit[i], i, m_P_Work_Limit[i]); m_Home_Time[i] = static_cast(GetPrivateProfileInt(L"HOME", L"HOME_TIME_" + axisNum[i], 1500, - csAppPath)); //回家超时时间(0.1ms) + csAppPath)); //回家超时时间(0.1ms) m_Home_AddJogGears[i] = GetPrivateProfileInt(L"HOME", L"HOME_ADD_JOGCHOICE_" + axisNum[i], 100, csAppPath); m_Home_DecJogGears[i] = GetPrivateProfileInt(L"HOME", L"HOME_DEC_JOGCHOICE_" + axisNum[i], 100, csAppPath); m_SetPotion_Count[i] = GetPrivateProfileInt(L"SET_POSITION_SPEED", L"SET_POSITION_COUNT_" + axisNum[i], 10, - csAppPath); + csAppPath); m_SetPotion_DriveSpeed[i] = GetPrivateProfileInt(L"SET_POSITION_SPEED", - L"SET_POTION_DRIVESPEED_" + axisNum[i], 100, csAppPath); + L"SET_POTION_DRIVESPEED_" + axisNum[i], 100, csAppPath); //m_SetPotion_DriveSpeed[i] = m_SetPotion_DriveSpeed[i] / (m_Resolution[i] * 50); //打印定位合成速度 g_pLogger->SendAndFlushWithTime(L"[Load_EF3_Motion_Inifile] m_SetPotion_DriveSpeed[%d]: %d\n", i, - m_SetPotion_DriveSpeed[i]); + m_SetPotion_DriveSpeed[i]); m_SetPotion_Line[i] = GetPrivateProfileInt(L"SET_POSITION_SPEED", L"SET_POTION_LINE_" + axisNum[i], 100, - csAppPath); + csAppPath); m_SetPotion_StartSpeed[i] = GetPrivateProfileInt(L"SET_POSITION_SPEED", - L"SET_POTION_STARTSPEED_" + axisNum[i], 100, csAppPath); + 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); + 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); + csAppPath); m_SpeedMax[i] = GetPrivateProfileInt(L"SET_POSITION_SPEED", L"SPEED_RUNMAX_" + axisNum[i], 150, csAppPath); } - for (int i = 0; i < 5; i++) // i 轴 + for (int i = 0; i < 5; i++) // i 轴 { for (int j = 1; j < 5; j++) //j 档位 { GetPrivateProfileString(L"JOG_SPEED", L"JOG_DRIVESPEED_" + strGear[i] + axisNum[j], L"10", - temp.GetBufferSetLength(50), 10, csAppPath); + temp.GetBufferSetLength(50), 10, csAppPath); float speed = (atof(T2A(temp))); //printf("spedd:%.2F %S\n",speed, L"JOG_DRIVESPEED_" + strGear[i] + axisNum[j]); //调试打印 /*m_JogDriveSpeed[j][i] = GetPrivateProfileInt(L"JOG_SPEED", L"JOG_DRIVESPEED_" , 10, csAppPath);*/ //m_JogDriveSpeed[j][i] = speed / (m_Resolution[j] * 50);//速度转换为脉冲 m_JogDriveSpeed[j][i] = speed; //直接读取速度 g_pLogger->SendAndFlushWithTime(L"[Load_EF3_Motion_Inifile] m_JogDriveSpeed[%d][%d]: %ld %ld\n", j, i, - speed, m_JogDriveSpeed[j][i]); //打印配置文件 档位速度 + speed, m_JogDriveSpeed[j][i]); //打印配置文件 档位速度 GetPrivateProfileString(L"JOG_SPEED", L"JOG_STARTSPEED_" + strGear[i] + axisNum[j], L"10", - temp.GetBufferSetLength(50), 10, csAppPath); + 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); + 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); + csAppPath); m_JogDecLine[j][i] = GetPrivateProfileInt(L"JOG_SPEED", L"JOG_LINE_" + strGear[i] + axisNum[j], 10, - csAppPath); + csAppPath); } } m_Jog_Auto_Focus = m_JogDriveSpeed[3][4]; + g_pLogger->SendAndFlushWithTime(L"[Load_EF3_Motion_Inifile] m_Jog_Auto_Focus: %f\n", m_Jog_Auto_Focus); //记录自动对焦速度 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); @@ -4181,52 +4160,52 @@ HSI_STATUS HSI_Motion::Load_EF3_Motion_Inifile(CString GoogolIniFile) m_iSpeedType = GetPrivateProfileInt(L"SET_SPEED", L"SPEEDTYPE", 0, csAppPath); - CString comNum[5] = {L"0", L"1", L"2", L"3"}; + 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); + 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); + 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); + 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); + 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); + 10, csAppPath); m_rockerASpeed[i] = (atoi(T2A(temp))); GetPrivateProfileString(L"ROCKER_SPEED", L"ROCKER_DSPEED_" + comNum[i], L"100", temp.GetBufferSetLength(50), - 10, csAppPath); + 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"}; + 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); + 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); + 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); + 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); + csAppPath); } - CString SubArea[8] = {L"0", L"1", L"2", L"3", L"4", L"5", L"6", L"7"}; + 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); + csAppPath); } } return rStatus; @@ -4315,11 +4294,11 @@ HSI_STATUS HSI_Motion::Load_EF3_Config_Inifile(CString GoogolIniFile) 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"}; + 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); + temp.GetBufferSetLength(50), 50, csAppPath); m_IsOpenTCPIP[i] = temp; temp = temp.Trim(); int iNext = temp.Find(':'); @@ -4339,10 +4318,10 @@ HSI_STATUS HSI_Motion::Load_EF3_Config_Inifile(CString GoogolIniFile) //=========================================================================== /** * \brief 读取/设置光栅尺精度 - * \param _ScaleX - * \param _ScaleY - * \param _ScaleZ - * \return + * \param _ScaleX + * \param _ScaleY + * \param _ScaleZ + * \return */ HSI_STATUS HSI_Motion::GetScaleResolution(double& _ScaleX, double& _ScaleY, double& _ScaleZ) { @@ -4460,7 +4439,6 @@ void HSI_Motion::UpdateMotionState() acsc_WaitMotionEnd(handleACS, ACSC_AXIS_8, INFINITE); printf("\nMotion end\n"); - //回家误差 unsigned int tempPrecision[5] = { 0, m_precisionCount[1], m_precisionCount[2], m_precisionCount[3], m_precisionCount[4] @@ -4472,15 +4450,14 @@ void HSI_Motion::UpdateMotionState() 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}; - + double prfpos[5] = { 0 }; interpolationflag = true; if (interpolationflag && m_motorType) { while (Count < m_SetPotion_Count[1]) //到位次数判断, 读配置文件240 { - Sleep(2); //延时2毫秒，1秒 =1000 毫秒 + Sleep(2); //延时2毫秒，1秒 =1000 毫秒 //获取当前位置 GetPositionXyz(HSI_MOTION_AXIS_ALL, prfpos[1], prfpos[2], prfpos[3], prfpos[0]); @@ -4494,8 +4471,8 @@ void HSI_Motion::UpdateMotionState() 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])) + 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) @@ -4573,7 +4550,7 @@ void HSI_Motion::UpdateMotionState() } switch (CurrentMotionState) { - case E_SO7_MOTION_MOVETO: + case E_SO7_MOTION_MOVETO: { g_pLogger->SendAndFlushWithTime( L"[UpdateMotionState] Nowait CurrentMotionState E_SO7_MOTION_MOVETO\n"); @@ -4583,7 +4560,7 @@ void HSI_Motion::UpdateMotionState() SendMsgMotionFinished(); //定位完成 break; } - default: + default: { g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Nowait CurrentMotionState default\n"); break; @@ -4647,8 +4624,7 @@ void HSI_Motion::UpdateMotionStateOld() timeoutflag = true; break; } - } - while (true); + } 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] @@ -4659,7 +4635,7 @@ void HSI_Motion::UpdateMotionStateOld() 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}; + double prfpos[5] = { 0 }; //GetPositionEncPrfMulti(1, m_EncPos, m_PrfPos, 4); //double EncPos[5] = { 0.0 }; @@ -4673,8 +4649,8 @@ void HSI_Motion::UpdateMotionStateOld() //目标位置 和当前位置 小于回家误差脉冲数 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])) + 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) @@ -4735,7 +4711,7 @@ void HSI_Motion::UpdateMotionStateOld() 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]); + prfpos[1], prfpos[2], prfpos[3]); if (timeoutflag) //定位超时 { g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Nowait Timeout\n"); @@ -4749,20 +4725,20 @@ void HSI_Motion::UpdateMotionStateOld() 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; - } + { + 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] Nowait CurrentMotionState default\n"); + break; + } } } g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Out\n"); @@ -4928,9 +4904,9 @@ void HSI_Motion::UpdateMotionStateProbe() { Sleep(200); hitFlag = false; - double ProbeHitCompletePos[4] = {0}; + double ProbeHitCompletePos[4] = { 0 }; GetPositionXyz(0, ProbeHitCompletePos[0], ProbeHitCompletePos[1], ProbeHitCompletePos[2], - ProbeHitCompletePos[3]); + ProbeHitCompletePos[3]); m_ijk[0] = ProbeHitCompletePos[0]; m_ijk[1] = ProbeHitCompletePos[1]; m_ijk[2] = ProbeHitCompletePos[2]; @@ -5029,10 +5005,10 @@ HSI_STATUS HSI_Motion::SpecialMotorMove(short AxisNumber, double Position) //========================IO=================================================== /** - * \brief - * \param IOChannel - * \param _Status - * \return + * \brief + * \param IOChannel + * \param _Status + * \return */ HSI_STATUS HSI_Motion::GetDIOOld(UINT IOChannel, UINT& _Status) { @@ -5142,17 +5118,16 @@ HSI_STATUS HSI_Motion::GetDIO(UINT IOChannel, UINT& _Status) //-----------TEST Begin------------------ _Status = 0; - //-----------TEST End------------------ return rStatus; } //=========================================================================== /** - * \brief - * \param IOChannel - * \param _Status - * \return + * \brief + * \param IOChannel + * \param _Status + * \return */ HSI_STATUS HSI_Motion::SetDIOOld(UINT IOChannel, UINT _Status) { @@ -5283,7 +5258,7 @@ HSI_STATUS HSI_Motion::GetAxisStatus(int* _Status) //=========================================================================== /** * \brief 暂停和关闭 - * \return + * \return */ HSI_STATUS HSI_Motion::AbortMotionOld() //需要运动实现 { @@ -5332,7 +5307,7 @@ HSI_STATUS HSI_Motion::AbortMotion() //需要运动实现 g_IsClose = true; g_pLogger->SendAndFlushWithTime(L"[AbortMotion] In\n"); - int Axes[] = {ACSC_AXIS_1, ACSC_AXIS_0, ACSC_AXIS_8, -1}; + int Axes[] = { ACSC_AXIS_1, ACSC_AXIS_0, ACSC_AXIS_8, -1 }; if (handleACS != ACSC_INVALID) { if (!acsc_HaltM(handleACS, Axes, nullptr)) //停止JOG运动 @@ -5349,7 +5324,7 @@ HSI_STATUS HSI_Motion::AbortMotion() //需要运动实现 //=========================================================================== /** * \brief 关闭 - * \return + * \return */ HSI_STATUS HSI_Motion::Shutdown() { @@ -5417,21 +5392,21 @@ HSI_STATUS HSI_Motion::Shutdown() //=========================================================================== /** * \brief 触发灯光 - * \param triggleNum - * \param delayLighting - * \param delayLightBefor - * \param triggleMode - * \param Intensities - * \return + * \param triggleNum + * \param delayLighting + * \param delayLightBefor + * \param triggleMode + * \param Intensities + * \return */ HSI_STATUS HSI_Motion::SetTriggerLight(int triggleNum, int delayLighting, int delayLightBefor, int triggleMode, - double* Intensities) + double* Intensities) { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { setLightFlag = true; - unsigned char m_SetTriggerLightData[64] = {0}; + unsigned char m_SetTriggerLightData[64] = { 0 }; m_SetTriggerLightData[0] = CT_LIGHT; m_SetTriggerLightData[1] = 0x03; m_SetTriggerLightData[2] = triggleNum; @@ -5489,8 +5464,8 @@ HSI_STATUS HSI_Motion::SetTriggerLight(int triggleNum, int delayLighting, int de //=========================================================================== /** * \brief 硬件触发拍照 - * \param startPoint - * \return + * \param startPoint + * \return */ HSI_STATUS HSI_Motion::DCCPPStartPoint(double* startPoint) { @@ -5529,7 +5504,6 @@ HSI_STATUS HSI_Motion::DCCScanSetDataWithPoints(UINT AxisTypes, double speed, Po // 其他处理逻辑 // ... - return rStatus; } @@ -5543,7 +5517,7 @@ HSI_STATUS HSI_Motion::DCCScanSetDataOld(UINT AxisTypes, HSI_SCAN_MOTION_TYPE eT //触发的位置为相对位置,用法一般是移动到起点位置，再开始设置触发位置(相对位置)，最终设置终点位置 g_pLogger->SendAndFlushWithTime(L"[DCCScanSetData] In\n"); int axisNum; - unsigned char m_SendDCCData[64] = {0}; + unsigned char m_SendDCCData[64] = { 0 }; m_SendDCCData[0] = CT_MOTOR; m_SendDCCData[1] = CT_TRIGGER_DATA; m_SendDCCData[2] = 0x01; @@ -5573,379 +5547,379 @@ HSI_STATUS HSI_Motion::DCCScanSetDataOld(UINT AxisTypes, HSI_SCAN_MOTION_TYPE eT switch (eType) { case HSI_SCAN_MOTION_SPEC_LOCA: + { + if (dTrigDis[0] > 0.0001) { - if (dTrigDis[0] > 0.0001) + iMotionDirection = 0; + } + else + { + iMotionDirection = 1; + } + + switch (iaxisNum) + { + case AXIS_X: + { + if (!m_IsUsePPS) { - iMotionDirection = 0; + begin_position[1] = begin_position[1] + dTrigDis[0] / m_Resolution[axisNum]; } else { - iMotionDirection = 1; + if (iMotionDirection == 0) + { + begin_position[1] = begin_position[1] - 20; + } + else + { + begin_position[1] = begin_position[1] + 20; + } } - - switch (iaxisNum) + if (iMotionDirection == 0) { - 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; + limit = 1000.0; } - - iScanMotionType = HSI_SCAN_MOTION_SPEC_LOCA; - if (lTrigNumber > 100) + else { - return HSI_STATUS_FAILED; - } - int num = lTrigNumber / 14; - int i = 0; - for (i = 0; i < num; i++) - { - m_SendDCCData[3] = i; - for (size_t j = 14 * i, z = 4; j < 14 * (i + 1); ++j) - { - m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) & 0xff; - m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 8) & 0xff; - m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 16) & 0xff; - m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 24; - } - m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength); - Sleep(1); - } - if ((num > 0) && (lTrigNumber % 14 != 0)) - { - m_SendDCCData[3] = i; - int j = 0, z = 0; - for (j = 14 * i, z = 4; j < lTrigNumber; ++j) - { - m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) & 0xff; - m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 8) & 0xff; - m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 16) & 0xff; - m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 24; - } - m_SendDCCData[z++] = static_cast(limit / m_Resolution[axisNum]) & 0xff; - m_SendDCCData[z++] = (static_cast(limit / m_Resolution[axisNum]) >> 8) & 0xff; - m_SendDCCData[z++] = (static_cast(limit / m_Resolution[axisNum]) >> 16) & 0xff; - m_SendDCCData[z++] = static_cast(limit / m_Resolution[axisNum]) >> 24; - m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength); - Sleep(10); - } - else if (num == 0) - { - m_SendDCCData[3] = 0; - int j = 0, z = 0; - for (j = 0, z = 4; j < lTrigNumber; ++j) - { - m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) & 0xff; - m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 8) & 0xff; - m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 16) & 0xff; - m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 24; - } - m_SendDCCData[z++] = static_cast(limit / m_Resolution[axisNum]) & 0xff; - m_SendDCCData[z++] = (static_cast(limit / m_Resolution[axisNum]) >> 8) & 0xff; - m_SendDCCData[z++] = (static_cast(limit / m_Resolution[axisNum]) >> 16) & 0xff; - m_SendDCCData[z++] = static_cast(limit / m_Resolution[axisNum]) >> 24; - m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength); - Sleep(10); + limit = -1000.0; } break; } + case AXIS_Y: + { + if (!m_IsUsePPS) + { + begin_position[2] = begin_position[2] + dTrigDis[0] / m_Resolution[axisNum]; + } + else + { + if (iMotionDirection == 0) + { + begin_position[2] = begin_position[2] - 20; + } + else + { + begin_position[2] = begin_position[2] + 20; + } + } + if (iMotionDirection == 0) + { + limit = 1000.0; + } + else + { + limit = -1000.0; + } + break; + } + case AXIS_Z: + { + if (!m_IsUsePPS) + { + begin_position[3] = begin_position[3] + dTrigDis[0] / m_Resolution[axisNum]; + } + else + { + if (iMotionDirection == 0) + { + begin_position[3] = begin_position[3] - 20; + } + else + { + begin_position[3] = begin_position[3] + 20; + } + } + if (iMotionDirection == 0) + { + limit = 1000.0; + } + else + { + limit = -1000.0; + } + break; + } + default: + break; + } + + iScanMotionType = HSI_SCAN_MOTION_SPEC_LOCA; + if (lTrigNumber > 100) + { + return HSI_STATUS_FAILED; + } + int num = lTrigNumber / 14; + int i = 0; + for (i = 0; i < num; i++) + { + m_SendDCCData[3] = i; + for (size_t j = 14 * i, z = 4; j < 14 * (i + 1); ++j) + { + m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) & 0xff; + m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 8) & 0xff; + m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 16) & 0xff; + m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 24; + } + m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength); + Sleep(1); + } + if ((num > 0) && (lTrigNumber % 14 != 0)) + { + m_SendDCCData[3] = i; + int j = 0, z = 0; + for (j = 14 * i, z = 4; j < lTrigNumber; ++j) + { + m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) & 0xff; + m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 8) & 0xff; + m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 16) & 0xff; + m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 24; + } + m_SendDCCData[z++] = static_cast(limit / m_Resolution[axisNum]) & 0xff; + m_SendDCCData[z++] = (static_cast(limit / m_Resolution[axisNum]) >> 8) & 0xff; + m_SendDCCData[z++] = (static_cast(limit / m_Resolution[axisNum]) >> 16) & 0xff; + m_SendDCCData[z++] = static_cast(limit / m_Resolution[axisNum]) >> 24; + m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength); + Sleep(10); + } + else if (num == 0) + { + m_SendDCCData[3] = 0; + int j = 0, z = 0; + for (j = 0, z = 4; j < lTrigNumber; ++j) + { + m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) & 0xff; + m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 8) & 0xff; + m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 16) & 0xff; + m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 24; + } + m_SendDCCData[z++] = static_cast(limit / m_Resolution[axisNum]) & 0xff; + m_SendDCCData[z++] = (static_cast(limit / m_Resolution[axisNum]) >> 8) & 0xff; + m_SendDCCData[z++] = (static_cast(limit / m_Resolution[axisNum]) >> 16) & 0xff; + m_SendDCCData[z++] = static_cast(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) { - if (dTrigDis[0] > 0.0001) + iMotionDirection = 0; + } + else + { + iMotionDirection = 1; + } + switch (iaxisNum) + { + case AXIS_X: + { + if (!m_IsUsePPS) { - iMotionDirection = 0; + begin_position[1] = begin_position[1] + dTrigDis[0] / m_Resolution[axisNum]; } else { - iMotionDirection = 1; + if (iMotionDirection == 0) + { + begin_position[1] = begin_position[1] - 20; + } + else + { + begin_position[1] = begin_position[1] + 20; + } } - switch (iaxisNum) + if (iMotionDirection == 0) { - 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; + limit = 1000; + } + else + { + limit = -1000; } - iScanMotionType = HSI_SCAN_MOTION_EQ_DIS; - m_SendDCCData[3] = 0x00; - m_SendDCCData[4] = static_cast(dTrigDis[0] / m_Resolution[axisNum]) & 0xff; - m_SendDCCData[5] = (static_cast(dTrigDis[0] / m_Resolution[axisNum]) >> 8) & 0xff; - m_SendDCCData[6] = (static_cast(dTrigDis[0] / m_Resolution[axisNum]) >> 16) & 0xff; - m_SendDCCData[7] = static_cast(dTrigDis[0] / m_Resolution[axisNum]) >> 24; - m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength); - Sleep(10); break; } + case AXIS_Y: + { + if (!m_IsUsePPS) + { + begin_position[2] = begin_position[2] + dTrigDis[0] / m_Resolution[axisNum]; + } + else + { + if (iMotionDirection == 0) + { + begin_position[2] = begin_position[2] - 20; + } + else + { + begin_position[2] = begin_position[2] + 20; + } + } + if (iMotionDirection == 0) + { + limit = 1000; + } + else + { + limit = -1000; + } + break; + } + case AXIS_Z: + { + if (!m_IsUsePPS) + { + begin_position[3] = begin_position[3] + dTrigDis[0] / m_Resolution[axisNum]; + } + else + { + if (iMotionDirection == 0) + { + begin_position[3] = begin_position[3] - 20; + } + else + { + begin_position[3] = begin_position[3] + 20; + } + } + if (iMotionDirection == 0) + { + limit = 1000; + } + else + { + limit = -1000; + } + break; + } + default: + break; + } + iScanMotionType = HSI_SCAN_MOTION_EQ_DIS; + m_SendDCCData[3] = 0x00; + m_SendDCCData[4] = static_cast(dTrigDis[0] / m_Resolution[axisNum]) & 0xff; + m_SendDCCData[5] = (static_cast(dTrigDis[0] / m_Resolution[axisNum]) >> 8) & 0xff; + m_SendDCCData[6] = (static_cast(dTrigDis[0] / m_Resolution[axisNum]) >> 16) & 0xff; + m_SendDCCData[7] = static_cast(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) { - if (dTrigDis[1] > 0.0001) + iMotionDirection = 0; + } + else + { + iMotionDirection = 1; + } + + switch (iaxisNum) + { + case AXIS_X: + { + if (!m_IsUsePPS) { - iMotionDirection = 0; + begin_position[1] = begin_position[1] + dTrigDis[0] / m_Resolution[axisNum]; } else { - iMotionDirection = 1; + if (iMotionDirection == 0) + { + begin_position[1] = begin_position[1] - 20; + } + else + { + begin_position[1] = begin_position[1] + 20; + } } - - switch (iaxisNum) + if (iMotionDirection == 0) { - 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; + limit = 1000; } - - iScanMotionType = HSI_SCAN_MOTION_EQ_DIS_II; - m_SendDCCData[3] = 0x00; - int i = 0, j = 0; - for (i = 0, j = 4; i < 2; ++i) + else { - m_SendDCCData[j++] = static_cast(dTrigDis[i] / m_Resolution[axisNum]) & 0xff; - m_SendDCCData[j++] = (static_cast(dTrigDis[i] / m_Resolution[axisNum]) >> 8) & 0xff; - m_SendDCCData[j++] = (static_cast(dTrigDis[i] / m_Resolution[axisNum]) >> 16) & 0xff; - m_SendDCCData[j++] = static_cast(dTrigDis[i] / m_Resolution[axisNum]) >> 24; + limit = -1000; } - m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength); - Sleep(10); break; } + case AXIS_Y: + { + if (!m_IsUsePPS) + { + begin_position[2] = begin_position[2] + dTrigDis[0] / m_Resolution[axisNum]; + } + else + { + if (iMotionDirection == 0) + { + begin_position[2] = begin_position[2] - 20; + } + else + { + begin_position[2] = begin_position[2] + 20; + } + } + if (iMotionDirection == 0) + { + limit = 1000; + } + else + { + limit = -1000; + } + break; + } + case AXIS_Z: + { + if (!m_IsUsePPS) + { + begin_position[3] = begin_position[3] + dTrigDis[0] / m_Resolution[axisNum]; + } + else + { + if (iMotionDirection == 0) + { + begin_position[3] = begin_position[3] - 20; + } + else + { + begin_position[3] = begin_position[3] + 20; + } + } + if (iMotionDirection == 0) + { + limit = 1000; + } + else + { + limit = -1000; + } + break; + } + default: + break; + } + + iScanMotionType = HSI_SCAN_MOTION_EQ_DIS_II; + m_SendDCCData[3] = 0x00; + int i = 0, j = 0; + for (i = 0, j = 4; i < 2; ++i) + { + m_SendDCCData[j++] = static_cast(dTrigDis[i] / m_Resolution[axisNum]) & 0xff; + m_SendDCCData[j++] = (static_cast(dTrigDis[i] / m_Resolution[axisNum]) >> 8) & 0xff; + m_SendDCCData[j++] = (static_cast(dTrigDis[i] / m_Resolution[axisNum]) >> 16) & 0xff; + m_SendDCCData[j++] = static_cast(dTrigDis[i] / m_Resolution[axisNum]) >> 24; + } + m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength); + Sleep(10); + break; + } default: break; } @@ -5963,9 +5937,9 @@ HSI_STATUS HSI_Motion::DCCScanSetData(UINT AxisTypes, HSI_SCAN_MOTION_TYPE eType { //触发的位置为相对位置,用法一般是移动到起点位置，再开始设置触发位置(相对位置)，最终设置终点位置 g_pLogger->SendAndFlushWithTime(L"[DCCScanSetData] In AxisTypes:%d eType:%d lTrigNumber:%d dTrigDis:ld\n", - AxisTypes, eType, lTrigNumber, dTrigDis); + AxisTypes, eType, lTrigNumber, dTrigDis); int axisNum; - unsigned char m_SendDCCData[64] = {0}; + unsigned char m_SendDCCData[64] = { 0 }; m_SendDCCData[0] = CT_MOTOR; m_SendDCCData[1] = CT_TRIGGER_DATA; m_SendDCCData[2] = 0x01; @@ -5995,387 +5969,387 @@ HSI_STATUS HSI_Motion::DCCScanSetData(UINT AxisTypes, HSI_SCAN_MOTION_TYPE eType switch (eType) { case HSI_SCAN_MOTION_SPEC_LOCA: + { + if (dTrigDis[0] > 0.0001) { - if (dTrigDis[0] > 0.0001) + iMotionDirection = 0; + } + else + { + iMotionDirection = 1; + } + + switch (iaxisNum) + { + case AXIS_X: + { + if (!m_IsUsePPS) { - iMotionDirection = 0; + begin_position[1] = begin_position[1] + dTrigDis[0] / m_Resolution[axisNum]; } else { - iMotionDirection = 1; + if (iMotionDirection == 0) + { + begin_position[1] = begin_position[1] - 20; + } + else + { + begin_position[1] = begin_position[1] + 20; + } } - - switch (iaxisNum) + if (iMotionDirection == 0) { - 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; + limit = 1000.0; } - - iScanMotionType = HSI_SCAN_MOTION_SPEC_LOCA; - if (lTrigNumber > 100) + else { - return HSI_STATUS_FAILED; - } - int num = lTrigNumber / 14; - int i = 0; - for (i = 0; i < num; i++) - { - m_SendDCCData[3] = i; - for (size_t j = 14 * i, z = 4; j < 14 * (i + 1); ++j) - { - m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) & 0xff; - m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 8) & 0xff; - m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 16) & 0xff; - m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 24; - } - //m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength); - Sleep(1); - } - if ((num > 0) && (lTrigNumber % 14 != 0)) - { - m_SendDCCData[3] = i; - int j = 0, z = 0; - for (j = 14 * i, z = 4; j < lTrigNumber; ++j) - { - m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) & 0xff; - m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 8) & 0xff; - m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 16) & 0xff; - m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 24; - } - m_SendDCCData[z++] = static_cast(limit / m_Resolution[axisNum]) & 0xff; - m_SendDCCData[z++] = (static_cast(limit / m_Resolution[axisNum]) >> 8) & 0xff; - m_SendDCCData[z++] = (static_cast(limit / m_Resolution[axisNum]) >> 16) & 0xff; - m_SendDCCData[z++] = static_cast(limit / m_Resolution[axisNum]) >> 24; - //m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength); - Sleep(10); - } - else if (num == 0) - { - m_SendDCCData[3] = 0; - int j = 0, z = 0; - for (j = 0, z = 4; j < lTrigNumber; ++j) - { - m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) & 0xff; - m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 8) & 0xff; - m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 16) & 0xff; - m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 24; - } - m_SendDCCData[z++] = static_cast(limit / m_Resolution[axisNum]) & 0xff; - m_SendDCCData[z++] = (static_cast(limit / m_Resolution[axisNum]) >> 8) & 0xff; - m_SendDCCData[z++] = (static_cast(limit / m_Resolution[axisNum]) >> 16) & 0xff; - m_SendDCCData[z++] = static_cast(limit / m_Resolution[axisNum]) >> 24; - //m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength); - Sleep(10); + limit = -1000.0; } break; } + case AXIS_Y: + { + if (!m_IsUsePPS) + { + begin_position[2] = begin_position[2] + dTrigDis[0] / m_Resolution[axisNum]; + } + else + { + if (iMotionDirection == 0) + { + begin_position[2] = begin_position[2] - 20; + } + else + { + begin_position[2] = begin_position[2] + 20; + } + } + if (iMotionDirection == 0) + { + limit = 1000.0; + } + else + { + limit = -1000.0; + } + break; + } + case AXIS_Z: + { + if (!m_IsUsePPS) + { + begin_position[3] = begin_position[3] + dTrigDis[0] / m_Resolution[axisNum]; + } + else + { + if (iMotionDirection == 0) + { + begin_position[3] = begin_position[3] - 20; + } + else + { + begin_position[3] = begin_position[3] + 20; + } + } + if (iMotionDirection == 0) + { + limit = 1000.0; + } + else + { + limit = -1000.0; + } + break; + } + default: + break; + } + + iScanMotionType = HSI_SCAN_MOTION_SPEC_LOCA; + if (lTrigNumber > 100) + { + return HSI_STATUS_FAILED; + } + int num = lTrigNumber / 14; + int i = 0; + for (i = 0; i < num; i++) + { + m_SendDCCData[3] = i; + for (size_t j = 14 * i, z = 4; j < 14 * (i + 1); ++j) + { + m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) & 0xff; + m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 8) & 0xff; + m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 16) & 0xff; + m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 24; + } + //m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength); + Sleep(1); + } + if ((num > 0) && (lTrigNumber % 14 != 0)) + { + m_SendDCCData[3] = i; + int j = 0, z = 0; + for (j = 14 * i, z = 4; j < lTrigNumber; ++j) + { + m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) & 0xff; + m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 8) & 0xff; + m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 16) & 0xff; + m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 24; + } + m_SendDCCData[z++] = static_cast(limit / m_Resolution[axisNum]) & 0xff; + m_SendDCCData[z++] = (static_cast(limit / m_Resolution[axisNum]) >> 8) & 0xff; + m_SendDCCData[z++] = (static_cast(limit / m_Resolution[axisNum]) >> 16) & 0xff; + m_SendDCCData[z++] = static_cast(limit / m_Resolution[axisNum]) >> 24; + //m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength); + Sleep(10); + } + else if (num == 0) + { + m_SendDCCData[3] = 0; + int j = 0, z = 0; + for (j = 0, z = 4; j < lTrigNumber; ++j) + { + m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) & 0xff; + m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 8) & 0xff; + m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 16) & 0xff; + m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 24; + } + m_SendDCCData[z++] = static_cast(limit / m_Resolution[axisNum]) & 0xff; + m_SendDCCData[z++] = (static_cast(limit / m_Resolution[axisNum]) >> 8) & 0xff; + m_SendDCCData[z++] = (static_cast(limit / m_Resolution[axisNum]) >> 16) & 0xff; + m_SendDCCData[z++] = static_cast(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) { - if (dTrigDis[0] > 0.0001) + iMotionDirection = 0; + } + else + { + iMotionDirection = 1; + } + switch (iaxisNum) + { + case AXIS_X: + { + if (!m_IsUsePPS) { - iMotionDirection = 0; + begin_position[1] = begin_position[1] + dTrigDis[0] / m_Resolution[axisNum]; } else { - iMotionDirection = 1; + if (iMotionDirection == 0) + { + begin_position[1] = begin_position[1] - 20; + } + else + { + begin_position[1] = begin_position[1] + 20; + } } - switch (iaxisNum) + if (iMotionDirection == 0) { - 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; + limit = 1000; + } + else + { + limit = -1000; } - iScanMotionType = HSI_SCAN_MOTION_EQ_DIS; - m_SendDCCData[3] = 0x00; - m_SendDCCData[4] = static_cast(dTrigDis[0] / m_Resolution[axisNum]) & 0xff; - m_SendDCCData[5] = (static_cast(dTrigDis[0] / m_Resolution[axisNum]) >> 8) & 0xff; - m_SendDCCData[6] = (static_cast(dTrigDis[0] / m_Resolution[axisNum]) >> 16) & 0xff; - m_SendDCCData[7] = static_cast(dTrigDis[0] / m_Resolution[axisNum]) >> 24; - //m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength); - Sleep(10); break; } + case AXIS_Y: + { + if (!m_IsUsePPS) + { + begin_position[2] = begin_position[2] + dTrigDis[0] / m_Resolution[axisNum]; + } + else + { + if (iMotionDirection == 0) + { + begin_position[2] = begin_position[2] - 20; + } + else + { + begin_position[2] = begin_position[2] + 20; + } + } + if (iMotionDirection == 0) + { + limit = 1000; + } + else + { + limit = -1000; + } + break; + } + case AXIS_Z: + { + if (!m_IsUsePPS) + { + begin_position[3] = begin_position[3] + dTrigDis[0] / m_Resolution[axisNum]; + } + else + { + if (iMotionDirection == 0) + { + begin_position[3] = begin_position[3] - 20; + } + else + { + begin_position[3] = begin_position[3] + 20; + } + } + if (iMotionDirection == 0) + { + limit = 1000; + } + else + { + limit = -1000; + } + break; + } + default: + break; + } + iScanMotionType = HSI_SCAN_MOTION_EQ_DIS; + m_SendDCCData[3] = 0x00; + m_SendDCCData[4] = static_cast(dTrigDis[0] / m_Resolution[axisNum]) & 0xff; + m_SendDCCData[5] = (static_cast(dTrigDis[0] / m_Resolution[axisNum]) >> 8) & 0xff; + m_SendDCCData[6] = (static_cast(dTrigDis[0] / m_Resolution[axisNum]) >> 16) & 0xff; + m_SendDCCData[7] = static_cast(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) { - if (dTrigDis[1] > 0.0001) + iMotionDirection = 0; + } + else + { + iMotionDirection = 1; + } + + switch (iaxisNum) + { + case AXIS_X: + { + if (!m_IsUsePPS) { - iMotionDirection = 0; + begin_position[1] = begin_position[1] + dTrigDis[0] / m_Resolution[axisNum]; } else { - iMotionDirection = 1; + if (iMotionDirection == 0) + { + begin_position[1] = begin_position[1] - 20; + } + else + { + begin_position[1] = begin_position[1] + 20; + } } - - switch (iaxisNum) + if (iMotionDirection == 0) { - 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; + limit = 1000; } - - iScanMotionType = HSI_SCAN_MOTION_EQ_DIS_II; - m_SendDCCData[3] = 0x00; - int i = 0, j = 0; - for (i = 0, j = 4; i < 2; ++i) + else { - m_SendDCCData[j++] = static_cast(dTrigDis[i] / m_Resolution[axisNum]) & 0xff; - m_SendDCCData[j++] = (static_cast(dTrigDis[i] / m_Resolution[axisNum]) >> 8) & 0xff; - m_SendDCCData[j++] = (static_cast(dTrigDis[i] / m_Resolution[axisNum]) >> 16) & 0xff; - m_SendDCCData[j++] = static_cast(dTrigDis[i] / m_Resolution[axisNum]) >> 24; + limit = -1000; } - //m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength); - Sleep(10); break; } + case AXIS_Y: + { + if (!m_IsUsePPS) + { + begin_position[2] = begin_position[2] + dTrigDis[0] / m_Resolution[axisNum]; + } + else + { + if (iMotionDirection == 0) + { + begin_position[2] = begin_position[2] - 20; + } + else + { + begin_position[2] = begin_position[2] + 20; + } + } + if (iMotionDirection == 0) + { + limit = 1000; + } + else + { + limit = -1000; + } + break; + } + case AXIS_Z: + { + if (!m_IsUsePPS) + { + begin_position[3] = begin_position[3] + dTrigDis[0] / m_Resolution[axisNum]; + } + else + { + if (iMotionDirection == 0) + { + begin_position[3] = begin_position[3] - 20; + } + else + { + begin_position[3] = begin_position[3] + 20; + } + } + if (iMotionDirection == 0) + { + limit = 1000; + } + else + { + limit = -1000; + } + break; + } + default: + break; + } + + iScanMotionType = HSI_SCAN_MOTION_EQ_DIS_II; + m_SendDCCData[3] = 0x00; + int i = 0, j = 0; + for (i = 0, j = 4; i < 2; ++i) + { + m_SendDCCData[j++] = static_cast(dTrigDis[i] / m_Resolution[axisNum]) & 0xff; + m_SendDCCData[j++] = (static_cast(dTrigDis[i] / m_Resolution[axisNum]) >> 8) & 0xff; + m_SendDCCData[j++] = (static_cast(dTrigDis[i] / m_Resolution[axisNum]) >> 16) & 0xff; + m_SendDCCData[j++] = static_cast(dTrigDis[i] / m_Resolution[axisNum]) >> 24; + } + //m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength); + Sleep(10); + break; + } default: break; } //启动定时锁存的同时，启动扫描 外部IO - unsigned char m_cSendData[8] = {0}; + unsigned char m_cSendData[8] = { 0 }; - //清空EF3缓存Flash + //清空EF3缓存Flash m_cSendData[0] = 0x01; m_cSendData[1] = 0x04; m_WriteByte = Send_Command(0, (const char*)m_cSendData, 2); @@ -6395,7 +6369,7 @@ HSI_STATUS HSI_Motion::DCCScanStartOld() { if (!setLightFlag) { - unsigned char m_SetTriggerLightData[64] = {0}; + unsigned char m_SetTriggerLightData[64] = { 0 }; m_SetTriggerLightData[0] = CT_LIGHT; m_SetTriggerLightData[1] = 0x03; m_SetTriggerLightData[53] = 0; @@ -6404,7 +6378,7 @@ HSI_STATUS HSI_Motion::DCCScanStartOld() } setLightFlag = false; g_pLogger->SendAndFlushWithTime(L"[DCCScanStart] In\n"); - unsigned char m_SendDCCData[64] = {0}; + unsigned char m_SendDCCData[64] = { 0 }; m_SendDCCData[0] = CT_MOTOR; m_SendDCCData[1] = CT_TRIGGER_DATA; m_SendDCCData[2] = 0x00; @@ -6457,7 +6431,7 @@ HSI_STATUS HSI_Motion::DCCScanStart() { if (!setLightFlag) { - unsigned char m_SetTriggerLightData[64] = {0}; + unsigned char m_SetTriggerLightData[64] = { 0 }; m_SetTriggerLightData[0] = CT_LIGHT; m_SetTriggerLightData[1] = 0x03; m_SetTriggerLightData[53] = 0; @@ -6466,7 +6440,7 @@ HSI_STATUS HSI_Motion::DCCScanStart() } setLightFlag = false; g_pLogger->SendAndFlushWithTime(L"[DCCScanStart] In\n"); - unsigned char m_SendDCCData[64] = {0}; + unsigned char m_SendDCCData[64] = { 0 }; m_SendDCCData[0] = CT_MOTOR; m_SendDCCData[1] = CT_TRIGGER_DATA; m_SendDCCData[2] = 0x00; @@ -6508,7 +6482,7 @@ HSI_STATUS HSI_Motion::DCCScanStart() //m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength); //启动定时扫描，此处清空旧缓存值 - unsigned char m_cSendData[8] = {0}; + unsigned char m_cSendData[8] = { 0 }; m_cSendData[0] = 0x01; m_cSendData[1] = 0x04; m_WriteByte = Send_Command(0, (const char*)m_cSendData, 2, 2); @@ -6519,7 +6493,6 @@ HSI_STATUS HSI_Motion::DCCScanStart() m_cSendData[1] = 0x02; m_WriteByte = Send_Command(0, (const char*)m_cSendData, 2, 2); - g_pLogger->SendAndFlushWithTime(L"[DCCScanStart] Out\n"); m_IsUsePPS = false; } @@ -6535,7 +6508,7 @@ HSI_STATUS HSI_Motion::DCCScanStopOld() if (g_pHSI_Motion) { g_pLogger->SendAndFlushWithTime(L"[DCCScanStop] In\n"); - unsigned char m_SendDCCData[64] = {0}; + unsigned char m_SendDCCData[64] = { 0 }; m_SendDCCData[0] = CT_MOTOR; m_SendDCCData[1] = CT_TRIGGER_DATA; m_SendDCCData[2] = 0x02; @@ -6554,7 +6527,7 @@ HSI_STATUS HSI_Motion::DCCScanStop() if (g_pHSI_Motion) { g_pLogger->SendAndFlushWithTime(L"[DCCScanStop] In\n"); - unsigned char m_SendDCCData[64] = {0}; + unsigned char m_SendDCCData[64] = { 0 }; m_SendDCCData[0] = 0x01; m_SendDCCData[1] = 0x03; m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, 2, 2); @@ -6603,16 +6576,16 @@ HSI_STATUS HSI_Motion::DCCForLightPlate() //=========================================================================== /** * \brief 转盘 - * \param CamerasDis - * \param BinsDis - * \param SubArea - * \param filterTime1 - * \param filterTime2 - * \param pluseSumDis - * \return + * \param CamerasDis + * \param BinsDis + * \param SubArea + * \param filterTime1 + * \param filterTime2 + * \param pluseSumDis + * \return */ HSI_STATUS HSI_Motion::StartPlcJob(int* CamerasDis, int* BinsDis, int SubArea, int filterTime1, int filterTime2, - int pluseSumDis) + int pluseSumDis) { g_pLogger->SendAndFlushWithTime(L"[StartPlcJob] In\n"); auto rStatus = HSI_STATUS_NORMAL; @@ -6625,7 +6598,7 @@ HSI_STATUS HSI_Motion::StartPlcJob(int* CamerasDis, int* BinsDis, int SubArea, i sEvenProp.EventType = HSI_EVENT_ERROR; sEvenProp.EventID = HSI_EVENT_MOTION; sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK; - strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "开始前请确认当前位置大于零！"); + strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "开始前请确认当前位置大于零"); EventCallback(sEvenProp); return HSI_STATUS_FAILED; } @@ -6722,21 +6695,21 @@ HSI_STATUS HSI_Motion::GetTriggleCount(int* nCount, int& nArea) << 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); + 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; @@ -6746,12 +6719,12 @@ HSI_STATUS HSI_Motion::GetTriggleCount(int* nCount, int& nArea) //点胶 //=========================================================================== HSI_STATUS HSI_Motion::GluePPSpnts(int* ppsDir, double* gluePPSPos, int* delayLightBefore, int* lightTime, - double* lightData, int num) + double* lightData, int num) { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { - unsigned char send_gluePPS_data[64] = {0}; + 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); @@ -6792,29 +6765,29 @@ HSI_STATUS HSI_Motion::GluePPSpnts(int* ppsDir, double* gluePPSPos, int* delayLi switch (saveDir) { case 0: - { - axisNum = 1; - dirnum = 2; - break; - } + { + axisNum = 1; + dirnum = 2; + break; + } case 1: - { - axisNum = 1; - dirnum = 1; - break; - } + { + axisNum = 1; + dirnum = 1; + break; + } case 2: - { - axisNum = 2; - dirnum = 2; - break; - } + { + axisNum = 2; + dirnum = 2; + break; + } case 3: - { - axisNum = 2; - dirnum = 1; - break; - } + { + axisNum = 2; + dirnum = 1; + break; + } default: break; } @@ -6831,53 +6804,53 @@ HSI_STATUS HSI_Motion::GluePPSpnts(int* ppsDir, double* gluePPSPos, int* delayLi switch (dirType) { case 0: - { - c++; - loadFeet[0] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]); - loadFeet[1] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]); - loadFeet[2] = static_cast(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; - } + { + c++; + loadFeet[0] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]); + loadFeet[1] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]); + loadFeet[2] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]); + send_gluePPS_data[j++] = (loadFeet[0] & 0xff); + send_gluePPS_data[j++] = ((loadFeet[0] >> 8) & 0xff); + send_gluePPS_data[j++] = ((loadFeet[0] >> 16) & 0xff); + send_gluePPS_data[j++] = ((loadFeet[0] >> 24) & 0xff); + break; + } case 1: - { - c++; - loadFeet[0] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]); - loadFeet[1] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]); - loadFeet[2] = static_cast(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; - } + { + c++; + loadFeet[0] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]); + loadFeet[1] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]); + loadFeet[2] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]); + send_gluePPS_data[j++] = (loadFeet[0] & 0xff); + send_gluePPS_data[j++] = ((loadFeet[0] >> 8) & 0xff); + send_gluePPS_data[j++] = ((loadFeet[0] >> 16) & 0xff); + send_gluePPS_data[j++] = ((loadFeet[0] >> 24) & 0xff); + break; + } case 2: - { - c++; - loadFeet[0] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]); - loadFeet[1] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]); - loadFeet[2] = static_cast(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; - } + { + c++; + loadFeet[0] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]); + loadFeet[1] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]); + loadFeet[2] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]); + send_gluePPS_data[j++] = (loadFeet[1] & 0xff); + send_gluePPS_data[j++] = ((loadFeet[1] >> 8) & 0xff); + send_gluePPS_data[j++] = ((loadFeet[1] >> 16) & 0xff); + send_gluePPS_data[j++] = ((loadFeet[1] >> 24) & 0xff); + break; + } case 3: - { - c++; - loadFeet[0] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]); - loadFeet[1] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]); - loadFeet[2] = static_cast(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; - } + { + c++; + loadFeet[0] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]); + loadFeet[1] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]); + loadFeet[2] = static_cast(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; } @@ -6894,29 +6867,29 @@ HSI_STATUS HSI_Motion::GluePPSpnts(int* ppsDir, double* gluePPSPos, int* delayLi switch (saveDir) { case 0: - { - axisNum = 1; - dirnum = 2; - break; - } + { + axisNum = 1; + dirnum = 2; + break; + } case 1: - { - axisNum = 1; - dirnum = 1; - break; - } + { + axisNum = 1; + dirnum = 1; + break; + } case 2: - { - axisNum = 2; - dirnum = 2; - break; - } + { + axisNum = 2; + dirnum = 2; + break; + } case 3: - { - axisNum = 2; - dirnum = 1; - break; - } + { + axisNum = 2; + dirnum = 1; + break; + } default: break; } @@ -6942,29 +6915,29 @@ HSI_STATUS HSI_Motion::GluePPSpnts(int* ppsDir, double* gluePPSPos, int* delayLi switch (saveDir) { case 0: - { - axisNum = 1; - dirnum = 2; - break; - } + { + axisNum = 1; + dirnum = 2; + break; + } case 1: - { - axisNum = 1; - dirnum = 1; - break; - } + { + axisNum = 1; + dirnum = 1; + break; + } case 2: - { - axisNum = 2; - dirnum = 2; - break; - } + { + axisNum = 2; + dirnum = 2; + break; + } case 3: - { - axisNum = 2; - dirnum = 1; - break; - } + { + axisNum = 2; + dirnum = 1; + break; + } default: break; } @@ -6975,8 +6948,8 @@ HSI_STATUS HSI_Motion::GluePPSpnts(int* ppsDir, double* gluePPSPos, int* delayLi } #pragma endregion - unsigned char send_data_light2[64] = {0}; - unsigned char send_data_delay[64] = {0}; + unsigned char send_data_light2[64] = { 0 }; + unsigned char send_data_delay[64] = { 0 }; c = 0; GlueDispenserindexNum = 0; int ldIndex = 0; @@ -7097,7 +7070,7 @@ HSI_STATUS HSI_Motion::GlueDispenser(int* index, int* cirdirection, double* glue if (g_pHSI_Motion) { GluerunCount = 0; - unsigned char send_glue_data[64] = {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; @@ -7115,418 +7088,418 @@ HSI_STATUS HSI_Motion::GlueDispenser(int* index, int* cirdirection, double* glue switch (index[i]) { case 0: + { + glueaxis = 0; + if (i == 0) { - glueaxis = 0; - if (i == 0) + GluerunCount++; + endGluepos[0] = gluePos[posIndex++]; + endGluepos[1] = gluePos[posIndex++]; + endGluepos[2] = gluePos[posIndex++]; + endGluepos[3] = gluePos[posIndex++]; + saveGluepos[0] = endGluepos[0]; + saveGluepos[1] = endGluepos[1]; + saveGluepos[2] = endGluepos[2]; + saveGluepos[3] = endGluepos[3]; + loadFeet[0] = static_cast(endGluepos[0] / m_Resolution[1]); + loadFeet[1] = static_cast(endGluepos[1] / m_Resolution[2]); + loadFeet[2] = static_cast(endGluepos[2] / m_Resolution[3]); + loadFeet[3] = static_cast(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++) { - GluerunCount++; - endGluepos[0] = gluePos[posIndex++]; - endGluepos[1] = gluePos[posIndex++]; - endGluepos[2] = gluePos[posIndex++]; - endGluepos[3] = gluePos[posIndex++]; - saveGluepos[0] = endGluepos[0]; - saveGluepos[1] = endGluepos[1]; - saveGluepos[2] = endGluepos[2]; - saveGluepos[3] = endGluepos[3]; - loadFeet[0] = static_cast(endGluepos[0] / m_Resolution[1]); - loadFeet[1] = static_cast(endGluepos[1] / m_Resolution[2]); - loadFeet[2] = static_cast(endGluepos[2] / m_Resolution[3]); - loadFeet[3] = static_cast(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); + 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); } - else - { - GluerunCount++; - endGluepos[0] = gluePos[posIndex++]; - endGluepos[1] = gluePos[posIndex++]; - endGluepos[2] = gluePos[posIndex++]; - endGluepos[3] = gluePos[posIndex++]; - startGluepos[0] = saveGluepos[0]; - startGluepos[1] = saveGluepos[1]; - startGluepos[2] = saveGluepos[2]; - startGluepos[3] = saveGluepos[3]; - saveGluepos[0] = endGluepos[0]; - saveGluepos[1] = endGluepos[1]; - saveGluepos[2] = endGluepos[2]; - saveGluepos[3] = endGluepos[3]; - loadFeet[0] = static_cast((endGluepos[0] - startGluepos[0]) / m_Resolution[1]); - loadFeet[1] = static_cast((endGluepos[1] - startGluepos[1]) / m_Resolution[2]); - loadFeet[2] = static_cast((endGluepos[2] - startGluepos[2]) / m_Resolution[3]); - loadFeet[3] = static_cast((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; + send_glue_data[0] = CT_MOTOR; + send_glue_data[1] = CT_GLUEDISPENSER_SET; + int len = j - 3; + send_glue_data[2] = len; + m_WriteByte = Send_Command(0, (const char*)send_glue_data, m_SendDataLength); + Sleep(20); } + else + { + GluerunCount++; + endGluepos[0] = gluePos[posIndex++]; + endGluepos[1] = gluePos[posIndex++]; + endGluepos[2] = gluePos[posIndex++]; + endGluepos[3] = gluePos[posIndex++]; + startGluepos[0] = saveGluepos[0]; + startGluepos[1] = saveGluepos[1]; + startGluepos[2] = saveGluepos[2]; + startGluepos[3] = saveGluepos[3]; + saveGluepos[0] = endGluepos[0]; + saveGluepos[1] = endGluepos[1]; + saveGluepos[2] = endGluepos[2]; + saveGluepos[3] = endGluepos[3]; + loadFeet[0] = static_cast((endGluepos[0] - startGluepos[0]) / m_Resolution[1]); + loadFeet[1] = static_cast((endGluepos[1] - startGluepos[1]) / m_Resolution[2]); + loadFeet[2] = static_cast((endGluepos[2] - startGluepos[2]) / m_Resolution[3]); + loadFeet[3] = static_cast((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) { - if (i == 0) + GluerunCount++; + endGluepos[0] = gluePos[posIndex++]; + endGluepos[1] = gluePos[posIndex++]; + endGluepos[2] = gluePos[posIndex++]; + endGluepos[3] = gluePos[posIndex++]; + saveGluepos[0] = endGluepos[0]; + saveGluepos[1] = endGluepos[1]; + saveGluepos[2] = endGluepos[2]; + saveGluepos[3] = endGluepos[3]; + loadFeet[0] = static_cast(endGluepos[0] / m_Resolution[1]); + loadFeet[1] = static_cast(endGluepos[1] / m_Resolution[2]); + loadFeet[2] = static_cast(endGluepos[2] / m_Resolution[3]); + loadFeet[3] = static_cast(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++) { - GluerunCount++; - endGluepos[0] = gluePos[posIndex++]; - endGluepos[1] = gluePos[posIndex++]; - endGluepos[2] = gluePos[posIndex++]; - endGluepos[3] = gluePos[posIndex++]; - saveGluepos[0] = endGluepos[0]; - saveGluepos[1] = endGluepos[1]; - saveGluepos[2] = endGluepos[2]; - saveGluepos[3] = endGluepos[3]; - loadFeet[0] = static_cast(endGluepos[0] / m_Resolution[1]); - loadFeet[1] = static_cast(endGluepos[1] / m_Resolution[2]); - loadFeet[2] = static_cast(endGluepos[2] / m_Resolution[3]); - loadFeet[3] = static_cast(endGluepos[3] / m_Resolution[4]); - if (abs(loadFeet[0]) > 20) glueaxis |= 0x01; - if (abs(loadFeet[1]) > 20) glueaxis |= 0x02; - if (abs(loadFeet[2]) > 20) glueaxis |= 0x04; - if (abs(loadFeet[3]) > 20) glueaxis |= 0x08; - send_glue_data[3] = glueaxis; - send_glue_data[4] = 0x01; - send_glue_data[5] = 0x00; - int j = 6; - for (size_t k = 0; k < 4; k++) - { - send_glue_data[j++] = (loadFeet[k] & 0xff); - send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff); - send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff); - send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff); - } - GluerunCount++; - glueaxis = 0; - endGluepos[0] = gluePos[posIndex++]; - endGluepos[1] = gluePos[posIndex++]; - endGluepos[2] = gluePos[posIndex++]; - endGluepos[3] = gluePos[posIndex++]; - startGluepos[0] = saveGluepos[0]; - startGluepos[1] = saveGluepos[1]; - startGluepos[2] = saveGluepos[2]; - startGluepos[3] = saveGluepos[3]; - saveGluepos[0] = endGluepos[0]; - saveGluepos[1] = endGluepos[1]; - saveGluepos[2] = endGluepos[2]; - saveGluepos[3] = endGluepos[3]; - loadFeet[0] = static_cast((endGluepos[0] - startGluepos[0]) / m_Resolution[1]); - loadFeet[1] = static_cast((endGluepos[1] - startGluepos[1]) / m_Resolution[2]); - loadFeet[2] = static_cast((endGluepos[2] - startGluepos[2]) / m_Resolution[3]); - loadFeet[3] = static_cast((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); + 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); } - else + GluerunCount++; + glueaxis = 0; + endGluepos[0] = gluePos[posIndex++]; + endGluepos[1] = gluePos[posIndex++]; + endGluepos[2] = gluePos[posIndex++]; + endGluepos[3] = gluePos[posIndex++]; + startGluepos[0] = saveGluepos[0]; + startGluepos[1] = saveGluepos[1]; + startGluepos[2] = saveGluepos[2]; + startGluepos[3] = saveGluepos[3]; + saveGluepos[0] = endGluepos[0]; + saveGluepos[1] = endGluepos[1]; + saveGluepos[2] = endGluepos[2]; + saveGluepos[3] = endGluepos[3]; + loadFeet[0] = static_cast((endGluepos[0] - startGluepos[0]) / m_Resolution[1]); + loadFeet[1] = static_cast((endGluepos[1] - startGluepos[1]) / m_Resolution[2]); + loadFeet[2] = static_cast((endGluepos[2] - startGluepos[2]) / m_Resolution[3]); + loadFeet[3] = static_cast((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++) { - GluerunCount++; - glueaxis = 0; - endGluepos[0] = gluePos[posIndex++]; - endGluepos[1] = gluePos[posIndex++]; - endGluepos[2] = gluePos[posIndex++]; - endGluepos[3] = gluePos[posIndex++]; - startGluepos[0] = saveGluepos[0]; - startGluepos[1] = saveGluepos[1]; - startGluepos[2] = saveGluepos[2]; - startGluepos[3] = saveGluepos[3]; - saveGluepos[0] = endGluepos[0]; - saveGluepos[1] = endGluepos[1]; - saveGluepos[2] = endGluepos[2]; - saveGluepos[3] = endGluepos[3]; - loadFeet[0] = static_cast((endGluepos[0] - startGluepos[0]) / m_Resolution[1]); - loadFeet[1] = static_cast((endGluepos[1] - startGluepos[1]) / m_Resolution[2]); - loadFeet[2] = static_cast((endGluepos[2] - startGluepos[2]) / m_Resolution[3]); - loadFeet[3] = static_cast((endGluepos[3] - startGluepos[3]) / m_Resolution[4]); - if (abs(loadFeet[0]) > 20) glueaxis |= 0x01; - if (abs(loadFeet[1]) > 20) glueaxis |= 0x02; - if (abs(loadFeet[2]) > 20) glueaxis |= 0x04; - if (abs(loadFeet[3]) > 20) glueaxis |= 0x08; - send_glue_data[3] = glueaxis; - send_glue_data[4] = 0x02; - send_glue_data[5] = 0x00; - int j = 6; - for (size_t k = 0; k < 4; k++) - { - send_glue_data[j++] = (loadFeet[k] & 0xff); - send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff); - send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff); - send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff); - } - GluerunCount++; - glueaxis = 0; - endGluepos[0] = gluePos[posIndex++]; - endGluepos[1] = gluePos[posIndex++]; - endGluepos[2] = gluePos[posIndex++]; - endGluepos[3] = gluePos[posIndex++]; - startGluepos[0] = saveGluepos[0]; - startGluepos[1] = saveGluepos[1]; - startGluepos[2] = saveGluepos[2]; - startGluepos[3] = saveGluepos[3]; - saveGluepos[0] = endGluepos[0]; - saveGluepos[1] = endGluepos[1]; - saveGluepos[2] = endGluepos[2]; - saveGluepos[3] = endGluepos[3]; - loadFeet[0] = static_cast((endGluepos[0] - startGluepos[0]) / m_Resolution[1]); - loadFeet[1] = static_cast((endGluepos[1] - startGluepos[1]) / m_Resolution[2]); - loadFeet[2] = static_cast((endGluepos[2] - startGluepos[2]) / m_Resolution[3]); - loadFeet[3] = static_cast((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); + 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); } - break; + send_glue_data[0] = CT_MOTOR; + send_glue_data[1] = CT_GLUEDISPENSER_SET; + int len = j - 3; + send_glue_data[2] = len; + m_WriteByte = Send_Command(0, (const char*)send_glue_data, m_SendDataLength); + Sleep(20); } + else + { + GluerunCount++; + glueaxis = 0; + endGluepos[0] = gluePos[posIndex++]; + endGluepos[1] = gluePos[posIndex++]; + endGluepos[2] = gluePos[posIndex++]; + endGluepos[3] = gluePos[posIndex++]; + startGluepos[0] = saveGluepos[0]; + startGluepos[1] = saveGluepos[1]; + startGluepos[2] = saveGluepos[2]; + startGluepos[3] = saveGluepos[3]; + saveGluepos[0] = endGluepos[0]; + saveGluepos[1] = endGluepos[1]; + saveGluepos[2] = endGluepos[2]; + saveGluepos[3] = endGluepos[3]; + loadFeet[0] = static_cast((endGluepos[0] - startGluepos[0]) / m_Resolution[1]); + loadFeet[1] = static_cast((endGluepos[1] - startGluepos[1]) / m_Resolution[2]); + loadFeet[2] = static_cast((endGluepos[2] - startGluepos[2]) / m_Resolution[3]); + loadFeet[3] = static_cast((endGluepos[3] - startGluepos[3]) / m_Resolution[4]); + if (abs(loadFeet[0]) > 20) glueaxis |= 0x01; + if (abs(loadFeet[1]) > 20) glueaxis |= 0x02; + if (abs(loadFeet[2]) > 20) glueaxis |= 0x04; + if (abs(loadFeet[3]) > 20) glueaxis |= 0x08; + send_glue_data[3] = glueaxis; + send_glue_data[4] = 0x02; + send_glue_data[5] = 0x00; + int j = 6; + for (size_t k = 0; k < 4; k++) + { + send_glue_data[j++] = (loadFeet[k] & 0xff); + send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff); + send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff); + send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff); + } + GluerunCount++; + glueaxis = 0; + endGluepos[0] = gluePos[posIndex++]; + endGluepos[1] = gluePos[posIndex++]; + endGluepos[2] = gluePos[posIndex++]; + endGluepos[3] = gluePos[posIndex++]; + startGluepos[0] = saveGluepos[0]; + startGluepos[1] = saveGluepos[1]; + startGluepos[2] = saveGluepos[2]; + startGluepos[3] = saveGluepos[3]; + saveGluepos[0] = endGluepos[0]; + saveGluepos[1] = endGluepos[1]; + saveGluepos[2] = endGluepos[2]; + saveGluepos[3] = endGluepos[3]; + loadFeet[0] = static_cast((endGluepos[0] - startGluepos[0]) / m_Resolution[1]); + loadFeet[1] = static_cast((endGluepos[1] - startGluepos[1]) / m_Resolution[2]); + loadFeet[2] = static_cast((endGluepos[2] - startGluepos[2]) / m_Resolution[3]); + loadFeet[3] = static_cast((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) { - if (i == 0) + GluerunCount++; + endGluepos[0] = gluePos[posIndex++]; + endGluepos[1] = gluePos[posIndex++]; + endGluepos[2] = gluePos[posIndex++]; + endGluepos[3] = gluePos[posIndex++]; + saveGluepos[0] = endGluepos[0]; + saveGluepos[1] = endGluepos[1]; + saveGluepos[2] = endGluepos[2]; + saveGluepos[3] = endGluepos[3]; + loadFeet[0] = static_cast(endGluepos[0] / m_Resolution[1]); + loadFeet[1] = static_cast(endGluepos[1] / m_Resolution[2]); + loadFeet[2] = static_cast(endGluepos[2] / m_Resolution[3]); + loadFeet[3] = static_cast(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++) { - GluerunCount++; - endGluepos[0] = gluePos[posIndex++]; - endGluepos[1] = gluePos[posIndex++]; - endGluepos[2] = gluePos[posIndex++]; - endGluepos[3] = gluePos[posIndex++]; - saveGluepos[0] = endGluepos[0]; - saveGluepos[1] = endGluepos[1]; - saveGluepos[2] = endGluepos[2]; - saveGluepos[3] = endGluepos[3]; - loadFeet[0] = static_cast(endGluepos[0] / m_Resolution[1]); - loadFeet[1] = static_cast(endGluepos[1] / m_Resolution[2]); - loadFeet[2] = static_cast(endGluepos[2] / m_Resolution[3]); - loadFeet[3] = static_cast(endGluepos[3] / m_Resolution[4]); - if (abs(loadFeet[0]) > 20) glueaxis |= 0x01; - if (abs(loadFeet[1]) > 20) glueaxis |= 0x02; - if (abs(loadFeet[2]) > 20) glueaxis |= 0x04; - if (abs(loadFeet[3]) > 20) glueaxis |= 0x08; - send_glue_data[3] = glueaxis; - send_glue_data[4] = 0x01; - send_glue_data[5] = 0x00; - int j = 6; - for (size_t k = 0; k < 4; k++) - { - send_glue_data[j++] = (loadFeet[k] & 0xff); - send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff); - send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff); - send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff); - } - GluerunCount++; - glueaxis = 0; - endGluepos[0] = gluePos[posIndex++]; - endGluepos[1] = gluePos[posIndex++]; - endGluepos[2] = gluePos[posIndex++]; - endGluepos[3] = gluePos[posIndex++]; - startGluepos[0] = saveGluepos[0]; - startGluepos[1] = saveGluepos[1]; - startGluepos[2] = saveGluepos[2]; - startGluepos[3] = saveGluepos[3]; - circlepnt[0] = static_cast((gluePos[posIndex++] - startGluepos[0]) / m_Resolution[1]); - circlepnt[1] = static_cast((gluePos[posIndex++] - startGluepos[1]) / m_Resolution[2]); - circlepnt[2] = static_cast((gluePos[posIndex++] - startGluepos[2]) / m_Resolution[3]); - circlepnt[3] = static_cast((gluePos[posIndex++] - startGluepos[3]) / m_Resolution[4]); - saveGluepos[0] = endGluepos[0]; - saveGluepos[1] = endGluepos[1]; - saveGluepos[2] = endGluepos[2]; - saveGluepos[3] = endGluepos[3]; - loadFeet[0] = static_cast((endGluepos[0] - startGluepos[0]) / m_Resolution[1]); - loadFeet[1] = static_cast((endGluepos[1] - startGluepos[1]) / m_Resolution[2]); - loadFeet[2] = static_cast((endGluepos[2] - startGluepos[2]) / m_Resolution[3]); - loadFeet[3] = static_cast((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); + send_glue_data[j++] = (loadFeet[k] & 0xff); + send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff); + send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff); + send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff); + } + GluerunCount++; + glueaxis = 0; + endGluepos[0] = gluePos[posIndex++]; + endGluepos[1] = gluePos[posIndex++]; + endGluepos[2] = gluePos[posIndex++]; + endGluepos[3] = gluePos[posIndex++]; + startGluepos[0] = saveGluepos[0]; + startGluepos[1] = saveGluepos[1]; + startGluepos[2] = saveGluepos[2]; + startGluepos[3] = saveGluepos[3]; + circlepnt[0] = static_cast((gluePos[posIndex++] - startGluepos[0]) / m_Resolution[1]); + circlepnt[1] = static_cast((gluePos[posIndex++] - startGluepos[1]) / m_Resolution[2]); + circlepnt[2] = static_cast((gluePos[posIndex++] - startGluepos[2]) / m_Resolution[3]); + circlepnt[3] = static_cast((gluePos[posIndex++] - startGluepos[3]) / m_Resolution[4]); + saveGluepos[0] = endGluepos[0]; + saveGluepos[1] = endGluepos[1]; + saveGluepos[2] = endGluepos[2]; + saveGluepos[3] = endGluepos[3]; + loadFeet[0] = static_cast((endGluepos[0] - startGluepos[0]) / m_Resolution[1]); + loadFeet[1] = static_cast((endGluepos[1] - startGluepos[1]) / m_Resolution[2]); + loadFeet[2] = static_cast((endGluepos[2] - startGluepos[2]) / m_Resolution[3]); + loadFeet[3] = static_cast((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 { - GluerunCount++; - glueaxis = 0; - endGluepos[0] = gluePos[posIndex++]; - endGluepos[1] = gluePos[posIndex++]; - endGluepos[2] = gluePos[posIndex++]; - endGluepos[3] = gluePos[posIndex++]; - startGluepos[0] = saveGluepos[0]; - startGluepos[1] = saveGluepos[1]; - startGluepos[2] = saveGluepos[2]; - startGluepos[3] = saveGluepos[3]; - saveGluepos[0] = endGluepos[0]; - saveGluepos[1] = endGluepos[1]; - saveGluepos[2] = endGluepos[2]; - saveGluepos[3] = endGluepos[3]; - loadFeet[0] = static_cast((endGluepos[0] - startGluepos[0]) / m_Resolution[1]); - loadFeet[1] = static_cast((endGluepos[1] - startGluepos[1]) / m_Resolution[2]); - loadFeet[2] = static_cast((endGluepos[2] - startGluepos[2]) / m_Resolution[3]); - loadFeet[3] = static_cast((endGluepos[3] - startGluepos[3]) / m_Resolution[4]); - if (abs(loadFeet[0]) > 20) glueaxis |= 0x01; - if (abs(loadFeet[1]) > 20) glueaxis |= 0x02; - if (abs(loadFeet[2]) > 20) glueaxis |= 0x04; - if (abs(loadFeet[3]) > 20) glueaxis |= 0x08; - send_glue_data[3] = glueaxis; - send_glue_data[4] = 0x02; - send_glue_data[5] = 0x00; - int j = 6; - for (size_t k = 0; k < 4; k++) - { - send_glue_data[j++] = (loadFeet[k] & 0xff); - send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff); - send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff); - send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff); - } - GluerunCount++; - glueaxis = 0; - endGluepos[0] = gluePos[posIndex++]; - endGluepos[1] = gluePos[posIndex++]; - endGluepos[2] = gluePos[posIndex++]; - endGluepos[3] = gluePos[posIndex++]; - startGluepos[0] = saveGluepos[0]; - startGluepos[1] = saveGluepos[1]; - startGluepos[2] = saveGluepos[2]; - startGluepos[3] = saveGluepos[3]; - circlepnt[0] = static_cast((gluePos[posIndex++] - startGluepos[0]) / m_Resolution[1]); - circlepnt[1] = static_cast((gluePos[posIndex++] - startGluepos[1]) / m_Resolution[2]); - circlepnt[2] = static_cast((gluePos[posIndex++] - startGluepos[2]) / m_Resolution[3]); - circlepnt[3] = static_cast((gluePos[posIndex++] - startGluepos[3]) / m_Resolution[4]); - saveGluepos[0] = endGluepos[0]; - saveGluepos[1] = endGluepos[1]; - saveGluepos[2] = endGluepos[2]; - saveGluepos[3] = endGluepos[3]; - loadFeet[0] = static_cast((endGluepos[0] - startGluepos[0]) / m_Resolution[1]); - loadFeet[1] = static_cast((endGluepos[1] - startGluepos[1]) / m_Resolution[2]); - loadFeet[2] = static_cast((endGluepos[2] - startGluepos[2]) / m_Resolution[3]); - loadFeet[3] = static_cast((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; + send_glue_data[j++] = 0x01; + for (size_t k = 0; k < 4; k++) + { + send_glue_data[j++] = (loadFeet[k] & 0xff); + send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff); + send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff); + send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff); + } + for (size_t k = 0; k < 4; k++) + { + send_glue_data[j++] = (circlepnt[k] & 0xff); + send_glue_data[j++] = ((circlepnt[k] >> 8) & 0xff); + send_glue_data[j++] = ((circlepnt[k] >> 16) & 0xff); + send_glue_data[j++] = ((circlepnt[k] >> 24) & 0xff); + } + send_glue_data[0] = CT_MOTOR; + send_glue_data[1] = CT_GLUEDISPENSER_SET; + int len = j - 3; + send_glue_data[2] = len; + m_WriteByte = Send_Command(0, (const char*)send_glue_data, m_SendDataLength); + Sleep(20); } + else + { + GluerunCount++; + glueaxis = 0; + endGluepos[0] = gluePos[posIndex++]; + endGluepos[1] = gluePos[posIndex++]; + endGluepos[2] = gluePos[posIndex++]; + endGluepos[3] = gluePos[posIndex++]; + startGluepos[0] = saveGluepos[0]; + startGluepos[1] = saveGluepos[1]; + startGluepos[2] = saveGluepos[2]; + startGluepos[3] = saveGluepos[3]; + saveGluepos[0] = endGluepos[0]; + saveGluepos[1] = endGluepos[1]; + saveGluepos[2] = endGluepos[2]; + saveGluepos[3] = endGluepos[3]; + loadFeet[0] = static_cast((endGluepos[0] - startGluepos[0]) / m_Resolution[1]); + loadFeet[1] = static_cast((endGluepos[1] - startGluepos[1]) / m_Resolution[2]); + loadFeet[2] = static_cast((endGluepos[2] - startGluepos[2]) / m_Resolution[3]); + loadFeet[3] = static_cast((endGluepos[3] - startGluepos[3]) / m_Resolution[4]); + if (abs(loadFeet[0]) > 20) glueaxis |= 0x01; + if (abs(loadFeet[1]) > 20) glueaxis |= 0x02; + if (abs(loadFeet[2]) > 20) glueaxis |= 0x04; + if (abs(loadFeet[3]) > 20) glueaxis |= 0x08; + send_glue_data[3] = glueaxis; + send_glue_data[4] = 0x02; + send_glue_data[5] = 0x00; + int j = 6; + for (size_t k = 0; k < 4; k++) + { + send_glue_data[j++] = (loadFeet[k] & 0xff); + send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff); + send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff); + send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff); + } + GluerunCount++; + glueaxis = 0; + endGluepos[0] = gluePos[posIndex++]; + endGluepos[1] = gluePos[posIndex++]; + endGluepos[2] = gluePos[posIndex++]; + endGluepos[3] = gluePos[posIndex++]; + startGluepos[0] = saveGluepos[0]; + startGluepos[1] = saveGluepos[1]; + startGluepos[2] = saveGluepos[2]; + startGluepos[3] = saveGluepos[3]; + circlepnt[0] = static_cast((gluePos[posIndex++] - startGluepos[0]) / m_Resolution[1]); + circlepnt[1] = static_cast((gluePos[posIndex++] - startGluepos[1]) / m_Resolution[2]); + circlepnt[2] = static_cast((gluePos[posIndex++] - startGluepos[2]) / m_Resolution[3]); + circlepnt[3] = static_cast((gluePos[posIndex++] - startGluepos[3]) / m_Resolution[4]); + saveGluepos[0] = endGluepos[0]; + saveGluepos[1] = endGluepos[1]; + saveGluepos[2] = endGluepos[2]; + saveGluepos[3] = endGluepos[3]; + loadFeet[0] = static_cast((endGluepos[0] - startGluepos[0]) / m_Resolution[1]); + loadFeet[1] = static_cast((endGluepos[1] - startGluepos[1]) / m_Resolution[2]); + loadFeet[2] = static_cast((endGluepos[2] - startGluepos[2]) / m_Resolution[3]); + loadFeet[3] = static_cast((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; } @@ -7546,7 +7519,7 @@ HSI_STATUS HSI_Motion::GlueDispenserStart(double xOffset, double yOffset, double 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}; + 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; @@ -7606,8 +7579,7 @@ void HSI_Motion::GluedispenserDone() //涂胶机完成 g_pLogger->SendAndFlushWithTime(L"[GluedispenserDone] Run End Abnormal\n"); break; } - } - while (true); + } while (true); bRunGlueDispenser = HSI_THREAD_PAUSED; sEvenProp.Init(); sEvenProp.EventType = HSI_EVENT_FUNCTION; @@ -7626,7 +7598,7 @@ HSI_STATUS HSI_Motion::GetPntsDistance(double& pDistance, int& spTimeCount) if (SetPotionRunEnd) { g_pLogger->SendAndFlushWithTime(L"[GetPntsDistance] PTPDistance = %.4f,spTimeCount = %d\n", - PntToPntDistance, SpTimeCount); + PntToPntDistance, SpTimeCount); SetPotionRunEnd = false; pDistance = PntToPntDistance; spTimeCount = SpTimeCount; @@ -7643,18 +7615,18 @@ HSI_STATUS HSI_Motion::GetPntsDistance(double& pDistance, int& spTimeCount) //=========================================================================== /** * \brief 界面挡位获取速度 - * \param AxisNum - * \param Speed - * \param DirveSpeed - * \param StartSpeed - * \param AccLine - * \param DecLine - * \param AccCurve - * \param DecCurve - * \return + * \param AxisNum + * \param Speed + * \param DirveSpeed + * \param StartSpeed + * \param AccLine + * \param DecLine + * \param AccCurve + * \param DecCurve + * \return */ int HSI_Motion::SpeedPercent(int AxisNum, double& Speed, int& DirveSpeed, int& StartSpeed, int& AccLine, int& DecLine, - int& AccCurve, int& DecCurve) + int& AccCurve, int& DecCurve) { g_pLogger->SendAndFlushWithTime(L"[SpeedPercent] AxisNum: [%d]\n", AxisNum); int MovetoSpeedGear = 0; @@ -7669,8 +7641,8 @@ int HSI_Motion::SpeedPercent(int AxisNum, double& Speed, int& DirveSpeed, int& S StartSpeed = m_JogStartSpeed[AxisNum][0]; AccLine = m_JogAccLine[AxisNum][0]; DecLine = m_JogDecLine[AxisNum][0]; - AccCurve = m_JogAccLine[AxisNum][0]*10; - DecCurve = m_JogDecLine[AxisNum][0]*10; + AccCurve = m_JogAccLine[AxisNum][0] * 10; + DecCurve = m_JogDecLine[AxisNum][0] * 10; g_pLogger->SendAndFlushWithTime( L"[SpeedPercent] 0.81 DriveSpeed: [%d], StartSpeed: [%d], AccLine: [%d], DecLine: [%d], AccCurve: [%d], DecCurve: [%d]\n", DirveSpeed, StartSpeed, AccLine, DecLine, AccCurve, DecCurve); @@ -7681,8 +7653,8 @@ int HSI_Motion::SpeedPercent(int AxisNum, double& Speed, int& DirveSpeed, int& S StartSpeed = m_JogStartSpeed[AxisNum][1]; AccLine = m_JogAccLine[AxisNum][1]; DecLine = m_JogDecLine[AxisNum][1]; - AccCurve = m_JogAccLine[AxisNum][1]*10; - DecCurve = m_JogDecLine[AxisNum][1]*10; + AccCurve = m_JogAccLine[AxisNum][1] * 10; + DecCurve = m_JogDecLine[AxisNum][1] * 10; g_pLogger->SendAndFlushWithTime( L"[SpeedPercent] 0.61 DriveSpeed: [%d], StartSpeed: [%d], AccLine: [%d], DecLine: [%d], AccCurve: [%d], DecCurve: [%d]\n", DirveSpeed, StartSpeed, AccLine, DecLine, AccCurve, DecCurve); @@ -7693,8 +7665,8 @@ int HSI_Motion::SpeedPercent(int AxisNum, double& Speed, int& DirveSpeed, int& S StartSpeed = m_JogStartSpeed[AxisNum][2]; AccLine = m_JogAccLine[AxisNum][2]; DecLine = m_JogDecLine[AxisNum][2]; - AccCurve = m_JogAccLine[AxisNum][2]*10; - DecCurve = m_JogDecLine[AxisNum][2]*10; + AccCurve = m_JogAccLine[AxisNum][2] * 10; + DecCurve = m_JogDecLine[AxisNum][2] * 10; g_pLogger->SendAndFlushWithTime( L"[SpeedPercent] 0.41 DriveSpeed: [%d], StartSpeed: [%d], AccLine: [%d], DecLine: [%d], AccCurve: [%d], DecCurve: [%d]\n", DirveSpeed, StartSpeed, AccLine, DecLine, AccCurve, DecCurve); @@ -7705,8 +7677,8 @@ int HSI_Motion::SpeedPercent(int AxisNum, double& Speed, int& DirveSpeed, int& S StartSpeed = m_JogStartSpeed[AxisNum][3]; AccLine = m_JogAccLine[AxisNum][3]; DecLine = m_JogDecLine[AxisNum][3]; - AccCurve = m_JogAccLine[AxisNum][3]*10; - DecCurve = m_JogDecLine[AxisNum][3]*10; + AccCurve = m_JogAccLine[AxisNum][3] * 10; + DecCurve = m_JogDecLine[AxisNum][3] * 10; g_pLogger->SendAndFlushWithTime( L"[SpeedPercent] 0.21 DriveSpeed: [%d], StartSpeed: [%d], AccLine: [%d], DecLine: [%d], AccCurve: [%d], DecCurve: [%d]\n", DirveSpeed, StartSpeed, AccLine, DecLine, AccCurve, DecCurve); @@ -7717,8 +7689,8 @@ int HSI_Motion::SpeedPercent(int AxisNum, double& Speed, int& DirveSpeed, int& S StartSpeed = m_JogStartSpeed[AxisNum][4]; AccLine = m_JogAccLine[AxisNum][4]; DecLine = m_JogDecLine[AxisNum][4]; - AccCurve = m_JogAccLine[AxisNum][4]*10; - DecCurve = m_JogDecLine[AxisNum][4]*10; + AccCurve = m_JogAccLine[AxisNum][4] * 10; + DecCurve = m_JogDecLine[AxisNum][4] * 10; g_pLogger->SendAndFlushWithTime( L"[SpeedPercent] 0.01 DriveSpeed: [%d], StartSpeed: [%d], AccLine: [%d], DecLine: [%d], AccCurve: [%d], DecCurve: [%d]\n", DirveSpeed, StartSpeed, AccLine, DecLine, AccCurve, DecCurve); @@ -7734,18 +7706,18 @@ int HSI_Motion::SpeedPercent(int AxisNum, double& Speed, int& DirveSpeed, int& S //=========================================================================== /** * \brief JoyStick运动控制参数读取及设置 - * \param AxisNum - * \param Speed - * \param DirveSpeed - * \param StartSpeed - * \param AccLine - * \param DecLine - * \param AccCurve - * \param DecCurve - * \return + * \param AxisNum + * \param Speed + * \param DirveSpeed + * \param StartSpeed + * \param AccLine + * \param DecLine + * \param AccCurve + * \param DecCurve + * \return */ bool HSI_Motion::SpeedPercentJoyStick(int AxisNum, long& Speed, int& DirveSpeed, int& StartSpeed, int& AccLine, - int& DecLine, int& AccCurve, int& DecCurve) + int& DecLine, int& AccCurve, int& DecCurve) { Speed = Speed > 1000 ? 1000 : Speed; double dSpeedRate = static_cast(Speed) / 1000; @@ -7801,9 +7773,9 @@ bool HSI_Motion::SpeedPercentJoyStick(int AxisNum, long& Speed, int& DirveSpeed, //=========================================================================== /** * \brief 获取单轴设置的速度 - * \param AxisNum - * \param Speed - * \return + * \param AxisNum + * \param Speed + * \return */ HSI_STATUS HSI_Motion::GetSpeedXyzOld(int AxisNum, double& Speed) { @@ -7830,7 +7802,7 @@ HSI_STATUS HSI_Motion::GetSpeedXyz(int AxisNum, double& Speed) Speed = m_SetPotion_DriveSpeed[AxisNumber]; //从m_SetPotion_DriveSpeed[AxisNumber]中获取速度 g_pLogger->SendAndFlushWithTime(L"[GetSpeedXyz] AxisNum = %d, Speed=%d\n", AxisNumber, Speed); - + //g_pLogger->SendAndFlushWithTime(L"[GetSpeedXyz] Out\n"); return rStatus; } @@ -7848,7 +7820,6 @@ HSI_STATUS HSI_Motion::SetSpeedXyz(double Speed) } else { - if (1 == m_iSpeedType) { //m_SetPotion_DriveSpeed[1] = static_cast(Speed / (m_Resolution[1] * 50)); @@ -7884,6 +7855,22 @@ HSI_STATUS HSI_Motion::SetSpeedXyz(double Speed) } //=========================================================================== +HSI_STATUS HSI_Motion::GetFocusSpeedOld(double& Speed) +{ + g_pLogger->SendAndFlushWithTime(L"[GetFocusSpeed] In\n"); + auto rStatus = HSI_STATUS_NORMAL; + if (1 == m_iSpeedType) + { + Speed = m_JogDriveSpeed[3][4] * (m_Resolution[3] * 50); + } + else + { + Speed = m_JogDriveSpeed[3][4]; + } + g_pLogger->SendAndFlushWithTime(L"[GetFocusSpeed] Out\n"); + return rStatus; +} + HSI_STATUS HSI_Motion::GetFocusSpeed(double& Speed) { g_pLogger->SendAndFlushWithTime(L"[GetFocusSpeed] In\n"); @@ -7901,7 +7888,7 @@ HSI_STATUS HSI_Motion::GetFocusSpeed(double& Speed) } //=========================================================================== -HSI_STATUS HSI_Motion::SetFocusSpeed(double Speed) +HSI_STATUS HSI_Motion::SetFocusSpeedOld(double Speed) { auto rStatus = HSI_STATUS_NORMAL; g_pLogger->SendAndFlushWithTime(L"[SetFocusSpeed] In\n"); @@ -7919,6 +7906,36 @@ HSI_STATUS HSI_Motion::SetFocusSpeed(double Speed) return rStatus; } +HSI_STATUS HSI_Motion::SetFocusSpeed(double Speed) +{ + auto rStatus = HSI_STATUS_NORMAL; + g_pLogger->SendAndFlushWithTime(L"[SetFocusSpeed] In\n"); + + if (Speed >= -1 && Speed <= 0) + { + g_pLogger->SendAndFlushWithTime(L"[SetFocusSpeed] Speed = %f, m_Jog_Auto_Focus = %f\n", Speed, m_Jog_Auto_Focus); + m_JogDriveSpeed[3][4] = static_cast(fabs(Speed * m_Jog_Auto_Focus)); + g_pLogger->SendAndFlushWithTime(L"[SetFocusSpeed] m_Jog_SpeedZ[3][4] = %f\n", m_JogDriveSpeed[3][4]); + } + else + { + if (1 == m_iSpeedType) + { + m_JogDriveSpeed[3][4] = Speed / (m_Resolution[3] * 50); + + g_pLogger->SendAndFlushWithTime(L"[SetFocusSpeed] m_iSpeedType=1, m_Jog_SpeedZ[3][4] = %f\n", m_JogDriveSpeed[4][3]); + } + else + { + m_JogDriveSpeed[3][4] = Speed; + + g_pLogger->SendAndFlushWithTime(L"[SetFocusSpeed] m_SetPotion_DriveSpeed[1] = %d\n", m_JogDriveSpeed[4][3]); + } + } + g_pLogger->SendAndFlushWithTime(L"[SetFocusSpeed] Out\n"); + return rStatus; +} + //=========================================================================== HSI_STATUS HSI_Motion::GetAccelerationXyz(double& AccelX, double& AccelY, double& AccelZ) { @@ -7942,53 +7959,53 @@ double HSI_Motion::LimitOver(UINT AxisTypes, double& LimitPos) //单轴软限位 switch (AxisNumber) { case HSI_MOTION_AXIS_X: // X 轴 + { + if (LimitPos >= m_P_Work_Limit[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; + LimitPos = m_P_Work_Limit[1] - 5 * m_Resolution[1]; } + if (LimitPos <= m_N_Work_Limit[1]) + { + LimitPos = m_N_Work_Limit[1] + 5 * m_Resolution[1]; + } + break; + } case HSI_MOTION_AXIS_Y: //Y 轴 + { + if (LimitPos >= m_P_Work_Limit[2]) { - 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; + LimitPos = m_P_Work_Limit[2] - 5 * m_Resolution[2]; } + if (LimitPos <= m_N_Work_Limit[2]) + { + LimitPos = m_N_Work_Limit[2] + 5 * m_Resolution[2]; + } + break; + } case HSI_MOTION_AXIS_Z: + { + if (LimitPos >= m_P_Work_Limit[3]) { - 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; + LimitPos = m_P_Work_Limit[3] - 5 * m_Resolution[3]; } - case HSI_MOTION_AXIS_R: + if (LimitPos <= m_N_Work_Limit[3]) { - 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; + LimitPos = m_N_Work_Limit[3] + 5 * m_Resolution[3]; } + break; + } + case HSI_MOTION_AXIS_R: + { + if (LimitPos >= m_P_Work_Limit[4]) + { + LimitPos = m_P_Work_Limit[4] - 5 * m_Resolution[4]; + } + if (LimitPos <= m_N_Work_Limit[4]) + { + LimitPos = m_N_Work_Limit[4] + 5 * m_Resolution[4]; + } + break; + } default: break; } @@ -8003,31 +8020,31 @@ short HSI_Motion::AxisConvertIndex(UINT AxisTypes) switch (AxisTypes) { case HSI_MOTION_AXIS_X: - { - AxisNumber = 0x01; //对应ACS 平台 1轴 - break; - } + { + AxisNumber = 0x01; //对应ACS 平台 1轴 + break; + } case HSI_MOTION_AXIS_Y: - { + { AxisNumber = 0x00; //对应 ACS 平台 0轴 (龙门轴) - break; - } + break; + } case HSI_MOTION_AXIS_Z: - { - AxisNumber = 0x08;//对应 ACS平台 8轴 （ecat通讯） - break; - } + { + AxisNumber = 0x08;//对应 ACS平台 8轴 （ecat通讯） + break; + } case HSI_MOTION_AXIS_R: - { - AxisNumber = 0x04; - break; - } + { + AxisNumber = 0x04; + break; + } default: - { - g_pLogger->SendAndFlushWithTime(L"[AxisConvertIndex] failed, AxisTypes = %d,AxisNumber = %d\n", AxisTypes, - AxisNumber); - break; - } + { + g_pLogger->SendAndFlushWithTime(L"[AxisConvertIndex] failed, AxisTypes = %d,AxisNumber = %d\n", AxisTypes, + AxisNumber); + break; + } } return AxisNumber; } @@ -8037,7 +8054,7 @@ void HSI_Motion::SetSingleAxisMotionParams(UINT AxisTypes, double SetMotionParam { g_pLogger->SendAndFlushWithTime(L"[SetSingleAxisMotionParams] In >>>>>>>>>>>>>>>>>>>>>> \n"); double getMotionParam[5] = { 0 }; - + //打印 AxisTypes g_pLogger->SendAndFlushWithTime(L"[SetSingleAxisMotionParams] AxisTypes = %d\n", AxisTypes); @@ -8056,7 +8073,6 @@ void HSI_Motion::SetSingleAxisMotionParams(UINT AxisTypes, double SetMotionParam AxisTypes, getMotionParam[0], getMotionParam[1], getMotionParam[2], getMotionParam[3], getMotionParam[4]); g_pLogger->SendAndFlushWithTime(L"[SetSingleAxisMotionParams] Out >>>>>>>>>>>>>>>>>>>>>>\n"); - } //=========================================================================== @@ -8066,30 +8082,30 @@ short HSI_Motion::IndexConvertAxis(int Index) switch (Index) { case 1: - { - AxisNumber = AXIS_X; - break; - } + { + AxisNumber = AXIS_X; + break; + } case 2: - { - AxisNumber = AXIS_Y; - break; - } + { + AxisNumber = AXIS_Y; + break; + } case 3: - { - AxisNumber = AXIS_Z; - break; - } + { + AxisNumber = AXIS_Z; + break; + } case 4: - { - AxisNumber = AXIS_U; - break; - } + { + AxisNumber = AXIS_U; + break; + } default: - { - g_pLogger->SendAndFlushWithTime(L"IndexConvertAxis failed,Index = %d,AxisTypes = %d\n", Index, AxisNumber); - break; - } + { + g_pLogger->SendAndFlushWithTime(L"IndexConvertAxis failed,Index = %d,AxisTypes = %d\n", Index, AxisNumber); + break; + } } return AxisNumber; } @@ -8154,7 +8170,7 @@ HSI_STATUS HSI_Motion::GetPositionEx(UINT AxisTypes, double& Position, double& T UNREFERENCED_PARAMETER(AxisTypes); if (g_pHSI_Motion) { - double position[4] = {0.0}; + double position[4] = { 0.0 }; short AxisNumber = AxisConvertIndex(AxisTypes); Position = m_PosForAllAxis[AxisNumber]; g_pLogger->SendAndFlushWithTime(L"[GetPositionEx] Position: %d\n", Position); @@ -8172,7 +8188,7 @@ HSI_STATUS HSI_Motion::SetPositionEx(UINT AxisTypes, double Position, HSI_MOTION g_pLogger->SendAndFlushWithTime(L"[SetPositionEx] In\n"); short AxisNumber = AxisConvertIndex(AxisTypes); double Time = 0.0; - double position[5] = {0.0}; + double position[5] = { 0.0 }; for (size_t i = 0; i < 5; i++) { position[i] = m_PosForAllAxis[i]; @@ -8204,17 +8220,14 @@ HSI_STATUS HSI_Motion::GetSpeedEx(UINT AxisTypes, double& Speed) { if (m_motorType == 1) { - - - Speed = m_SetPotion_DriveSpeed[1]; - + Speed = m_SetPotion_DriveSpeed[1]; + g_pLogger->SendAndFlushWithTime(L"[GetSpeedEx] m_motorType =1， Speed=%d\n", Speed); } else { - - Speed = m_SetPotion_DriveSpeed[AxisNumber]; - + Speed = m_SetPotion_DriveSpeed[AxisNumber]; + g_pLogger->SendAndFlushWithTime(L"[GetSpeedEx] m_iSpeedType=1, Speed=%d\n", Speed); } } @@ -8225,9 +8238,9 @@ HSI_STATUS HSI_Motion::GetSpeedEx(UINT AxisTypes, double& Speed) //=========================================================================== /** * \brief 设置单轴运动速度 - * \param AxisTypes - * \param Speed - * \return + * \param AxisTypes + * \param Speed + * \return */ HSI_STATUS HSI_Motion::SetSpeedExOld(UINT AxisTypes, double Speed) { @@ -8320,7 +8333,7 @@ HSI_STATUS HSI_Motion::SetSpeedEx(UINT AxisTypes, double Speed) m_SetPotion_DriveSpeed[AxisNumber] = 0; g_pLogger->SendAndFlushWithTime(L"[SetSpeedEx] Speed <= 0 \n"); } - + if (AxisNumber >= 0 && AxisNumber <= 8) { m_SetPotion_DriveSpeed[1] = static_cast(Speed); @@ -8448,28 +8461,28 @@ unsigned __stdcall HSI_Motion::m_Thread(LPVOID pThis) switch (m_Thread_State) { case HSI_THREAD_RUNNING: + { + TRACE("HSI_THREAD_RUNNING.\r\n"); + if (bRunGlueDispenser == HSI_THREAD_RUNNING) + _This->GluedispenserDone(); + else { - TRACE("HSI_THREAD_RUNNING.\r\n"); - if (bRunGlueDispenser == HSI_THREAD_RUNNING) - _This->GluedispenserDone(); - else - { - _This->UpdateMotionState(); - _This->UpdateMotionStateEx(); - } - break; + _This->UpdateMotionState(); + _This->UpdateMotionStateEx(); } + break; + } case HSI_THREAD_PAUSED: - { - TRACE("HSI_THREAD_PAUSED.\r\n"); - break; - } + { + TRACE("HSI_THREAD_PAUSED.\r\n"); + break; + } case HSI_THREAD_EXIT: - { - TRACE("HSI_THREAD_EXIT.\r\n"); - ExitThread(0); - break; - } + { + TRACE("HSI_THREAD_EXIT.\r\n"); + ExitThread(0); + break; + } default: break; } @@ -8534,22 +8547,22 @@ unsigned __stdcall HSI_Motion::m_ThreadProbe(LPVOID pThis) switch (m_Thread_StateProbe) { case HSI_THREAD_RUNNING: - { - TRACE("HSI_THREAD_RUNNING.\r\n"); - _This->UpdateMotionStateProbe(); - break; - } + { + TRACE("HSI_THREAD_RUNNING.\r\n"); + _This->UpdateMotionStateProbe(); + break; + } case HSI_THREAD_PAUSED: - { - TRACE("HSI_THREAD_PAUSED.\r\n"); - break; - } + { + TRACE("HSI_THREAD_PAUSED.\r\n"); + break; + } case HSI_THREAD_EXIT: - { - TRACE("HSI_THREAD_EXIT.\r\n"); - ExitThread(0); - break; - } + { + TRACE("HSI_THREAD_EXIT.\r\n"); + ExitThread(0); + break; + } default: break; } @@ -8614,22 +8627,22 @@ unsigned __stdcall HSI_Motion::m_ThreadIO(LPVOID pThis) switch (m_Thread_StateIO) { case HSI_THREAD_RUNNING: - { - TRACE("HSI_THREAD_RUNNING.\r\n"); - _This->UpdateMotionStateIO(); - break; - } + { + TRACE("HSI_THREAD_RUNNING.\r\n"); + _This->UpdateMotionStateIO(); + break; + } case HSI_THREAD_PAUSED: - { - TRACE("HSI_THREAD_PAUSED.\r\n"); - break; - } + { + TRACE("HSI_THREAD_PAUSED.\r\n"); + break; + } case HSI_THREAD_EXIT: - { - TRACE("HSI_THREAD_EXIT.\r\n"); - ExitThread(0); - break; - } + { + TRACE("HSI_THREAD_EXIT.\r\n"); + ExitThread(0); + break; + } default: break; } @@ -8698,36 +8711,35 @@ unsigned __stdcall HSI_Motion::m_ThreadData(LPVOID pThis) switch (m_Thread_StateData) { case HSI_THREAD_RUNNING: - { - TRACE("HSI_THREAD_RUNNING.\r\n"); - _This->UpdateMotionStateData(); //获取运动状态命令 - break; - } + { + TRACE("HSI_THREAD_RUNNING.\r\n"); + _This->UpdateMotionStateData(); //获取运动状态命令 + break; + } case HSI_THREAD_PAUSED: - { - TRACE("HSI_THREAD_PAUSED.\r\n"); - break; - } + { + TRACE("HSI_THREAD_PAUSED.\r\n"); + break; + } case HSI_THREAD_EXIT: - { - TRACE("HSI_THREAD_EXIT.\r\n"); - ExitThread(0); - break; - } + { + TRACE("HSI_THREAD_EXIT.\r\n"); + ExitThread(0); + break; + } default: break; } } } - //=========================================================================== /** * \brief JOG运行到软限位的运动调节 */ void HSI_Motion::UpdateMotionStateJOGStop() { - unsigned char send_JogLimt_data[64] = {0}; + unsigned char send_JogLimt_data[64] = { 0 }; UINT axis = 0; double posx = 0.0; double posy = 0.0; @@ -8744,100 +8756,100 @@ void HSI_Motion::UpdateMotionStateJOGStop() switch (jogAxisNum) { case 1: + { + if (jogDirFlag) { - if (jogDirFlag) + double limitx = m_P_Work_Limit[1]; + if ((limitx - posx) / m_Resolution[1] < slowdowndistance) { - 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; - } + 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; + 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) { - if (jogDirFlag) + double limity = m_P_Work_Limit[2]; + if ((limity - posy) / m_Resolution[2] < slowdowndistance) { - 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; - } + 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; + 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) { - if (jogDirFlag) + double limitz = m_P_Work_Limit[3]; + if ((limitz - posz) / m_Resolution[3] < slowdowndistance) { - 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; - } + 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; + 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; + { + //第四轴未添加 + } + break; default: break; } @@ -8904,22 +8916,22 @@ unsigned __stdcall HSI_Motion::m_ThreadJOGStop(LPVOID pThis) //JOG运行到软 switch (m_Thread_StateJOGStop) { case HSI_THREAD_RUNNING: - { - TRACE("HSI_THREAD_RUNNING.\r\n"); - _This->UpdateMotionStateJOGStop(); - break; - } + { + TRACE("HSI_THREAD_RUNNING.\r\n"); + _This->UpdateMotionStateJOGStop(); + break; + } case HSI_THREAD_PAUSED: - { - TRACE("HSI_THREAD_PAUSED.\r\n"); - break; - } + { + TRACE("HSI_THREAD_PAUSED.\r\n"); + break; + } case HSI_THREAD_EXIT: - { - TRACE("HSI_THREAD_EXIT.\r\n"); - ExitThread(0); - break; - } + { + TRACE("HSI_THREAD_EXIT.\r\n"); + ExitThread(0); + break; + } default: break; } @@ -8952,7 +8964,7 @@ HSI_STATUS HSI_Motion::IOprogram(byte* SendData, int length) HSI_STATUS HSI_Motion::FindOriginTest(bool type) { auto rStatus = HSI_STATUS_NORMAL; - byte findorigdata[64] = {0}; + byte findorigdata[64] = { 0 }; findorigdata[0] = 0x04; if (type == true) { @@ -8972,7 +8984,7 @@ HSI_STATUS HSI_Motion::FindOriginTest(bool type) //=========================================================================== /** * \brief 串口发送命令 - * \param com + * \param com * \param _SendData 发送内容指针 * \param SendDataLength 发送字节长度 * \param expectType 期待返回长度，2：返回ok，8：返回8个字节，N:返回N字节，如果本次返回内容不是期待字节，等待或重发该命令。 @@ -8985,7 +8997,7 @@ BOOL HSI_Motion::Send_Command(int com, const char* _SendData, DWORD SendDataLeng if (m_bConnected && (m_IsUseEF3 == 1) && (com == 0)) { g_pLogger->SendAndFlushWithTime(L"[Send_Command] lenth:%d, %s\n", SendDataLength, - m_SO7_Serial.HexToStr(_SendData, SendDataLength)); + m_SO7_Serial.HexToStr(_SendData, SendDataLength)); int iWriteByte = m_SO7_Serial.Send(_SendData, SendDataLength); if (iWriteByte == 0) { @@ -9041,7 +9053,7 @@ BOOL HSI_Motion::Send_Command(int com, const char* _SendData, DWORD SendDataLeng //------------------------------调试区-----------------------------------// g_pLogger->SendAndFlushWithTime(L"[Send_Command] lenth:%d, expectType:%d, %s\n", SendDataLength, expectType, - m_SO7_Serial.HexToStr(_SendData, SendDataLength)); + m_SO7_Serial.HexToStr(_SendData, SendDataLength)); int iWriteByte = m_SO7_Serial.Send(_SendData, SendDataLength); if (iWriteByte == 0) @@ -9071,12 +9083,12 @@ BOOL HSI_Motion::Send_Command(int com, const char* _SendData, DWORD SendDataLeng { rStatus = FALSE; g_pLogger->SendAndFlushWithTime(L"[Send_Command] WriteByte:%d,!= SendDataLength:%d\n", iWriteByte, - SendDataLength); + SendDataLength); } m_SO7_Serial.m_iRecvState = false; //发送完毕，接收状态置为false，并开始等待EF3回复 m_SO7_Serial.m_iExpectBytes = expectType; //给出期望结果，传递到下层，由下层进行拼包 - //判断回复值，是否为ok 6F 6B + //判断回复值，是否为ok 6F 6B int iRetrys = 0; while (!m_SO7_Serial.m_iRecvState && iRetrys < 100) //再次接收等待 { @@ -9090,11 +9102,11 @@ BOOL HSI_Motion::Send_Command(int com, const char* _SendData, DWORD SendDataLeng { rStatus = TRUE; g_pLogger->SendAndFlushWithTime(L"[Send_Command] EF3 Reply OK, RecvBytes:%d, %S\n", - m_SO7_Serial.m_iRecvBytes, - m_SO7_Serial.HexToStr((const char*)m_SO7_Serial.m_RecvData, - m_SO7_Serial.m_iRecvBytes)); - /* g_pLogger->SendAndFlushWithTime(L"[Send_Command] EF3 Reply OK, RecvBytes:%d\n", - m_SO7_Serial.m_iRecvBytes);*/ + m_SO7_Serial.m_iRecvBytes, + m_SO7_Serial.HexToStr((const char*)m_SO7_Serial.m_RecvData, + m_SO7_Serial.m_iRecvBytes)); + /* g_pLogger->SendAndFlushWithTime(L"[Send_Command] EF3 Reply OK, RecvBytes:%d\n", + m_SO7_Serial.m_iRecvBytes);*/ } else if (iRetrys > 100) //获取超时，重发 { @@ -9119,9 +9131,9 @@ BOOL HSI_Motion::Send_Command(int com, const char* _SendData, DWORD SendDataLeng { rStatus = HSI_STATUS_NORMAL; g_pLogger->SendAndFlushWithTime(L"[Send_Command] EF3 Reply OK, RecvBytes:%d, %s\n", - m_SO7_Serial.m_iRecvBytes, - m_SO7_Serial.HexToStr((const char*)m_SO7_Serial.m_RecvData, - m_SO7_Serial.m_iRecvBytes)); + m_SO7_Serial.m_iRecvBytes, + m_SO7_Serial.HexToStr((const char*)m_SO7_Serial.m_RecvData, + m_SO7_Serial.m_iRecvBytes)); } } @@ -9176,13 +9188,11 @@ BOOL HSI_Motion::Send_Command(int com, const char* _SendData, DWORD SendDataLeng // m_SO7_Serial.m_iRecvBytes)); //} } - ReleaseMutex(g_RW_Data_Mutex); return rStatus; }; - ///////////////////////////////////////////////////////////////////////// #pragma region //网口通信8路 26路光源板通信 @@ -9208,7 +9218,7 @@ TCPIP_RETURN_CODE HSI_Motion::TCPConnect(int index, char* Address, u_short port) { int iMode = 1; int i = 0; - int retVal = ioctlsocket(m_socket[index], FIONBIO, (u_long FAR*)&iMode); //非阻塞连接 + int retVal = ioctlsocket(m_socket[index], FIONBIO, (u_long FAR*) & iMode); //非阻塞连接 if (retVal == SOCKET_ERROR) { closesocket(m_socket[index]); @@ -9245,7 +9255,7 @@ TCPIP_RETURN_CODE HSI_Motion::TCPConnect(int index, char* Address, u_short port) } } iMode = 0; - retVal = ioctlsocket(m_socket[index], FIONBIO, (u_long FAR*)&iMode); //设置阻塞 + retVal = ioctlsocket(m_socket[index], FIONBIO, (u_long FAR*) & iMode); //设置阻塞 if (retVal == SOCKET_ERROR) { closesocket(m_socket[index]); @@ -9288,20 +9298,20 @@ unsigned __stdcall HSI_Motion::m_ThreadSendTCP(LPVOID pThis) switch (m_ThreadTCP_State) { case TCPIP_THREAD_RUNNING: - { - TRACE("TCPIP_THREAD_RUNNING.\r\n"); - _This->TCPSend(); - break; - } + { + TRACE("TCPIP_THREAD_RUNNING.\r\n"); + _This->TCPSend(); + break; + } case TCPIP_THREAD_PAUSED: - { - break; - } + { + break; + } case TCPIP_THREAD_EXIT: - { - ExitThread(0); - break; - } + { + ExitThread(0); + break; + } default: break; } @@ -9317,7 +9327,7 @@ TCPIP_RETURN_CODE HSI_Motion::TCPSend() if (LightSend > 0) { g_pLogger->SendAndFlushWithTime(L"[TCPSend] LightSend = %d,m_selectedIndex = %d\n", LightSend, - m_selectedIndex); + m_selectedIndex); LightSend--; bytesSend = send(m_socket[m_selectedIndex], (const char*)lightdata, 64, 0); for (size_t i = 0; i < 64; i++) @@ -9476,7 +9486,7 @@ void HSI_Motion::Create_Thread() { m_ThreadTCP_State = TCPIP_THREAD_RUNNING; m_ThreadTCP_Id = ::CreateThread(nullptr, 0, (LPTHREAD_START_ROUTINE)m_ThreadSendTCP, - this, 0, nullptr); + this, 0, nullptr); } } @@ -9519,7 +9529,7 @@ HSI_STATUS HSI_Motion::CollectPos(bool isEnable, MOTOR_AXISCHOOES_CMD axis, shor auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { - unsigned char send_glue_data[64] = {0}; + unsigned char send_glue_data[64] = { 0 }; send_glue_data[0] = 0x01; send_glue_data[1] = 0x1E; send_glue_data[2] = isEnable ? 1 : 2; @@ -9540,7 +9550,7 @@ HSI_STATUS HSI_Motion::SetAllGears() { for (size_t i = 1; i < 5; i++) { - unsigned char send_glue_data[64] = {0}; + unsigned char send_glue_data[64] = { 0 }; send_glue_data[0] = 0x01; send_glue_data[1] = 0x01; send_glue_data[2] = 0x01 << (i - 1); //轴号 @@ -9562,4 +9572,4 @@ HSI_STATUS HSI_Motion::SetAllGears() return rStatus; } -//=========================================================================== +//=========================================================================== \ No newline at end of file diff --git a/HSI_HexagonMI_EF3/HSI_Motion.h b/HSI_HexagonMI_EF3/HSI_Motion.h index 6ea475c..d637193 100644 --- a/HSI_HexagonMI_EF3/HSI_Motion.h +++ b/HSI_HexagonMI_EF3/HSI_Motion.h @@ -70,7 +70,7 @@ enum MOTOR_CMD //第二级指令 CT_MOTOR_UPDATE, // up date all the aixs }; -enum MOTOR_START_POSOTION // +enum MOTOR_START_POSOTION // { INTERPOLATION = 0x61, //插补 @@ -154,14 +154,14 @@ public: HSI_STATUS Startup(HWND _hWnd, bool _bOfflineOnly) override; /** * \brief 获取EF3固件版本 - * \param version - * \return + * \param version + * \return */ HSI_STATUS GetFirmwareVersion(byte* version); /** * \brief 判断机台是否回家，如果没有，则执行回家动作 - * \param bHomed - * \return + * \param bHomed + * \return */ HSI_STATUS HomeMachine(bool bHomed); HSI_STATUS HomeMachineOld(bool bHomed); @@ -172,15 +172,15 @@ public: /** * \brief 判断机台是否回家，仅获取是否回家的状态 * \param bHomed 是否回家标志位 - * \return + * \return */ HSI_STATUS IsHomed(bool& bHomed); HSI_STATUS IsHomedOld(bool& bHomed); /** * \brief 获取单轴运动速度 - * \param AxisNum - * \param Speed - * \return + * \param AxisNum + * \param Speed + * \return */ HSI_STATUS GetSpeedXyz(int AxisNum, double& Speed); HSI_STATUS GetSpeedXyzOld(int AxisNum, double& Speed); @@ -191,8 +191,13 @@ public: * \return */ HSI_STATUS SetSpeedXyz(double Speed); + // 获取变焦速度 + HSI_STATUS GetFocusSpeedOld(double& Speed); HSI_STATUS GetFocusSpeed(double& Speed); + // 设置变焦速度 + HSI_STATUS SetFocusSpeedOld(double Speed); HSI_STATUS SetFocusSpeed(double Speed); + HSI_STATUS GetAccelerationXyz(double& AccelX, double& AccelY, double& AccelZ); HSI_STATUS SetAccelerationXyz(double AccelX, double AccelY, double AccelZ); HSI_STATUS GetSpeedR(double& Speed); @@ -203,27 +208,27 @@ public: HSI_STATUS GetRefreshDeadBand(double& Deadband); /** * \brief Jog运动,挡位只有在JOG才有，CNC运行的速度是合成速度 - * \param AxisTypes - * \param Speed - * \return + * \param AxisTypes + * \param Speed + * \return */ HSI_STATUS Jog(UINT AxisTypes, double Speed); HSI_STATUS JogOld(UINT AxisTypes, double Speed); HSI_STATUS JoyStick(UINT AxisTypes, long Speed); /** - * \brief - * \return + * \brief + * \return */ HSI_STATUS StopJog(); HSI_STATUS StopJogOld(); HSI_STATUS StopJogEx(UINT AxisTypes); /** - * \brief - * \param AxisTypes - * \param EncPos - * \param PrfPos - * \param Count - * \return + * \brief + * \param AxisTypes + * \param EncPos + * \param PrfPos + * \param Count + * \return */ HSI_STATUS GetPositionEncPrfMulti(UINT AxisTypes, double* EncPos, double* PrfPos, int Count); HSI_STATUS GetPositionEncPrfMultiOld(UINT AxisTypes, double* EncPos, double* PrfPos, int Count); @@ -233,13 +238,13 @@ public: * \param PositionX X坐标 * \param PositionY Y坐标 * \param PositionZ Z坐标 - * \param Time - * \return + * \param Time + * \return */ HSI_STATUS GetPositionXyz(UINT AxisTypes, double& PositionX, double& PositionY, double& PositionZ, double& Time); HSI_STATUS GetPositionXyzOld(UINT AxisTypes, double& PositionX, double& PositionY, double& PositionZ, double& Time); HSI_STATUS GetPositionXyzaProbe(UINT AxisTypes, double& PositionX, double& PositionY, double& PositionZ, - double& PositionA); + double& PositionA); HSI_STATUS GetEncoderXyz(long* lEncoderVal);//获取编码器位置 HSI_STATUS GetEncoderXyzOld(long* lEncoderVal); HSI_STATUS JogProbe(UINT AxisTypes, double Speed); @@ -247,12 +252,12 @@ public: int CaculateStepMotorACC(int pos, int maxacc, int minacc); /** * \brief 求各轴运动配置参数 - * \param AXIS - * \param motionParam - * \return + * \param AXIS + * \param motionParam + * \return */ - HSI_STATUS HSI_Motion::GetSingleAxisParam(int AXIS, double motionParam[5]); //获取单轴运动参数 - HSI_STATUS HSI_Motion::SetSingleAxisParam(int AXIS, double motionParam[5]); //设置单轴运动参数 + HSI_STATUS GetSingleAxisParam(int AXIS, double motionParam[5]); //获取单轴运动参数 + HSI_STATUS SetSingleAxisParam(int AXIS, double motionParam[5]); //设置单轴运动参数 /** * \brief 运行到指定位置 * \param AxisTypes 轴号 @@ -260,15 +265,15 @@ public: * \param PositionY Y坐标 * \param PositionZ Z坐标 * \param eType 运动模式， - * \param dFlyRadius - * \return + * \param dFlyRadius + * \return */ HSI_STATUS SetPositionXyz(UINT AxisTypes, double PositionX, double PositionY, double PositionZ, - HSI_MOTION_MOVE_TYPE eType, double dFlyRadius); + HSI_MOTION_MOVE_TYPE eType, double dFlyRadius); HSI_STATUS SetPositionXyzOld(UINT AxisTypes, double PositionX, double PositionY, double PositionZ, - HSI_MOTION_MOVE_TYPE eType, double dFlyRadius); + HSI_MOTION_MOVE_TYPE eType, double dFlyRadius); HSI_STATUS SetPositionXyza(UINT AxisTypes, double PositionX, double PositionY, double PositionZ, double PositionA, - HSI_MOTION_MOVE_TYPE eType, double dFlyRadius); + HSI_MOTION_MOVE_TYPE eType, double dFlyRadius); HSI_STATUS SetPositionXyzCache(UINT AxisTypes, HSI_MOTION_MOVE_TYPE eType, int DataCount, Point* CacheData); HSI_STATUS GetPositionXyzCache(unsigned char* CacheData, int& DataCount); HSI_STATUS GetPositionR(UINT AxisTypes, double& PositionR, double& Time); @@ -277,32 +282,32 @@ public: HSI_STATUS Load_EF3_Motion_Inifile(CString GoogolIniFile); HSI_STATUS Load_EF3_Config_Inifile(CString GoogolIniFile); /** - * \brief - * \return + * \brief + * \return */ HSI_STATUS AbortMotion(); HSI_STATUS AbortMotionOld(); /** - * \brief - * \param IOChannel - * \param _Status - * \return + * \brief + * \param IOChannel + * \param _Status + * \return */ HSI_STATUS GetDIO(UINT IOChannel, UINT& _Status); HSI_STATUS GetDIOOld(UINT IOChannel, UINT& _Status); /** - * \brief - * \param IOChannel - * \param _Status - * \return + * \brief + * \param IOChannel + * \param _Status + * \return */ HSI_STATUS SetDIO(UINT IOChannel, UINT _Status); HSI_STATUS SetDIOOld(UINT IOChannel, UINT _Status); HSI_STATUS GetAxisStatus(int* _Status); HSI_STATUS GetAppPath(CString& Path); /** - * \brief - * \return + * \brief + * \return */ HSI_STATUS Shutdown() override; HSI_STATUS IsSupportedEx(UINT AxisTypes, UINT& Types); @@ -319,7 +324,7 @@ public: HSI_STATUS GetAccelerationEx(UINT AxisTypes, double& Accel); HSI_STATUS SetAccelerationEx(UINT AxisTypes, double Accel); HSI_STATUS SetTriggerLight(int triggleNum, int delayLighting, int delayLightBefor, int triggleMode, - double* Intensities); + double* Intensities); HSI_STATUS DCCPPStartPoint(double* startPoint); HSI_STATUS DCCScanSetData(UINT AxisTypes, HSI_SCAN_MOTION_TYPE eType, UINT lTrigNumber, double* dTrigDis);// 下发触发间距,轴、等间距或者等时间、触发数量、间距 @@ -334,16 +339,16 @@ public: HSI_STATUS DCCScanSetDataWithPoints(UINT AxisTypes, double speed, Point3D* points, UINT numPoints); // 直线电机平台新增，三轴轨迹规划 HSI_STATUS DCCForLightPlate(); HSI_STATUS IOStep(bool RunSts); - + HSI_STATUS IOprogram(byte* SendData, int length); HSI_STATUS FindOriginTest(bool state); - + HSI_STATUS StartPlcJob(int* CamerasDis, int* BinsDis, int SubArea, int filterTime1, int filterTime2, - int pluseSumDis); + int pluseSumDis); HSI_STATUS SendBinResult(int* BinResult); HSI_STATUS GetTriggleCount(int* nCount, int& nArea); HSI_STATUS GluePPSpnts(int* ppsDir, double* gluePPSPos, int* delayLightBefore, int* lightTime, double* lightData, - int num); + int num); HSI_STATUS GlueDispenser(int* index, int* cirdirection, double* gluePos, int num); HSI_STATUS GlueDispenserStart(double xOffset, double yOffset, double qOffset); HSI_STATUS GetPntsDistance(double& ptpDistance, int& spTimeCount); @@ -441,7 +446,7 @@ public: UINT m_ForStatus; int m_Set_XYZA_Reserve; //XYZA轴方向 int m_motorType; //电机类型 1为伺服电机 0为步进电机 - int m_setPositionDelay; //设置定位超时 + int m_setPositionDelay; //设置定位超时 int m_setPositionPrecision; //设置定位精度 int m_setPositionNum; CString m_AppPath;//MST软件运行标志，trueMST软件已经启动，falseMST软件停止 @@ -482,7 +487,7 @@ public: long m_ProbeCapturePos[5]; //锁存各轴的位置 double m_ProbeReturnPos; //探针触发时，调试时返回的距离mm，点击启动按钮时不起作用,默认10.0mm int m_ProbeReturnSpeed; //探针触发后，轴的回退速度 - int m_isOKGlint; //是否开启ok/ng 闪烁 + int m_isOKGlint; //是否开启ok/ng 闪烁 int m_ETIPort; //外部触发拍照输入端口号 int m_axisStatus; //运动各轴的状态 int m_axisAlarmStatus; //轴报警状态 @@ -494,7 +499,7 @@ public: int m_IsCollectPos; //是否通过串口打印位置，与摇杆2互斥 int m_IsCloseRocker; int m_IsLightDebug; //是否不回家也能调试灯光 0为不启用 1为启用 默认为0 - int m_rockerHStartSpeed[4]; //摇杆XYZ轴高初始速度 + int m_rockerHStartSpeed[4]; //摇杆XYZ轴高初始速度 int m_rockerHDriveSpeed[4]; //摇杆XYZ轴高驱动速度 int m_rockerLStartSpeed[4]; //摇杆XYZ轴低初始速度 int m_rockerLDriveSpeed[4]; //摇杆XYZ轴低驱动速度 @@ -511,7 +516,7 @@ public: bool jogMoving; bool jogDirFlag; bool m_bEmergencyState; - bool bUseGlueDispenser; //是否开启点胶 + bool bUseGlueDispenser; //是否开启点胶 int m_iGlueStartSpeed; int m_iGlueDriveSpeed; int m_iGlueAccSpeed; @@ -598,38 +603,38 @@ public: VOID EventCallback(sHSIEventProperties& sEventProp); /** * \brief 界面挡位获取速度 - * \param AxisNum - * \param Speed - * \param DriveSpeed - * \param StartSpeed - * \param AccLine - * \param DecLine - * \param AccCurve - * \param DecCurve - * \return + * \param AxisNum + * \param Speed + * \param DriveSpeed + * \param StartSpeed + * \param AccLine + * \param DecLine + * \param AccCurve + * \param DecCurve + * \return */ int SpeedPercent(int AxisNum, double& Speed, int& DriveSpeed, int& StartSpeed, int& AccLine, int& DecLine, - int& AccCurve, int& DecCurve); + int& AccCurve, int& DecCurve); /** * \brief 摇杆切换的运动速度, 与上面那个互锁，动了界面的 摇杆就会失能; - * \param AxisNum - * \param Speed - * \param DriveSpeed - * \param StartSpeed - * \param AccLine - * \param DecLine - * \param AccCurve - * \param DecCurve - * \return + * \param AxisNum + * \param Speed + * \param DriveSpeed + * \param StartSpeed + * \param AccLine + * \param DecLine + * \param AccCurve + * \param DecCurve + * \return */ bool SpeedPercentJoyStick(int AxisNum, long& Speed, int& DriveSpeed, int& StartSpeed, int& AccLine, int& DecLine, - int& AccCurve, int& DecCurve); + int& AccCurve, int& DecCurve); void HomeJogGearsChoice(int AxisType, int JogGears, int& DriveSpeed, int& StartSpeed, int& AccLine, int& DecLine, - int& AccCurve, int& DecCurve); + int& AccCurve, int& DecCurve); /** - * \brief - * \param AxisTypes - * \return + * \brief + * \param AxisTypes + * \return */ short AxisConvertIndex(UINT AxisTypes); short IndexConvertAxis(int Index); diff --git a/HSI_HexagonMI_EF3/version.h b/HSI_HexagonMI_EF3/version.h index 64ea463..5178d5e 100644 --- a/HSI_HexagonMI_EF3/version.h +++ b/HSI_HexagonMI_EF3/version.h @@ -12,5 +12,5 @@ #define HSI_VERSION_REVNUM #define HSI_VERSION_BUILD_DATE _T(__DATE__ ) #define HSI_VERSION_BUILD_TIME _T(__TIME__ ) -#define HSI_FILE_DESCRIPTION "周四 2.24.08 / 13:09 " -#define HSI_FILE_CSDESCRIPTION _T("周四 2.24.08 / 13:09 ") +#define HSI_FILE_DESCRIPTION "2024.12.23 / 13:41 " +#define HSI_FILE_CSDESCRIPTION _T("2024.12.23 / 13:41 ")