diff --git a/HSI_HexagonMI_EF3/HSI.cpp b/HSI_HexagonMI_EF3/HSI.cpp index d248fd7..222d788 100644 --- a/HSI_HexagonMI_EF3/HSI.cpp +++ b/HSI_HexagonMI_EF3/HSI.cpp @@ -163,7 +163,7 @@ HSI_API HSI_STATUS WINAPI HSI_MOTION_STARTUP(bool bHome) return HSI_STATUS_FAILED; } rStatus = g_pHSI_Motion->HomeMachine(bHome); - switch (g_pHSI_Motion->m_iJoyStick) + switch (g_pHSI_Motion->m_iJoyStick) //摇杆设置 { case 0: break; @@ -266,7 +266,7 @@ HSI_API HSI_STATUS WINAPI HSI_MOTION_GET_POSITION_XYZ(UINT AxisTypes, double& Po //=========================================================================== HSI_API HSI_STATUS WINAPI HSI_MOTION_SET_POSITION_XYZ(UINT AxisTypes, double PositionX, double PositionY, - double PositionZ, HSI_MOTION_MOVE_TYPE eType, double dSpeedGear) + double PositionZ, HSI_MOTION_MOVE_TYPE eType, double dSpeedGear) { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) diff --git a/HSI_HexagonMI_EF3/HSI.h b/HSI_HexagonMI_EF3/HSI.h index aad3d5c..84d564f 100644 --- a/HSI_HexagonMI_EF3/HSI.h +++ b/HSI_HexagonMI_EF3/HSI.h @@ -22,11 +22,11 @@ const int HSI_APIVersionMajor = 0; const int HSI_APIVersionMinor = 0; - const int HSI_MaxStringLength = 255; // Maximum string length (buffer size - 1) /////////////////////////////////////////////////////////////////////////////// // Interface API /////////////////////////////////////////////////////////////////////////////// + enum HSI_STATUS { HSI_STATUS_NOT_SUPPORTED = -1, @@ -77,7 +77,10 @@ enum HSI_STATUS HSI_STATUS_LP_EXCEED_LIMIT = 301, HSI_STATUS_VP_TIMEOUT = 350, - HSI_STATUS_VP_IMAGEPROCESS_FAIL + HSI_STATUS_VP_IMAGEPROCESS_FAIL, + + HSI_ACS_OK=400,// ACS命令成功 + HSI_ACS_ERROR }; enum HSI_MACHINE_TYPE @@ -356,23 +359,16 @@ enum HSI_MOTION_IO_TYPE { HSI_MOTION_INPUT = 0x0001, HSI_MOTION_INPUT_LIMIT_SWITCH, - HSI_MOTION_INPUT_CH1, - //固高、众为兴、EF1输入 + HSI_MOTION_INPUT_CH1,//固高、众为兴、EF1输入 HSI_MOTION_INPUT_CH2, - HSI_MOTION_INPUT_CH3, - //串口控制器输入 - HSI_MOTION_INPUT_CH4, - //众为兴运动控制卡测试输入 - HSI_MOTION_INPUT_ALARM, - //驱动报警 - + HSI_MOTION_INPUT_CH3,//串口控制器输入 + HSI_MOTION_INPUT_CH4,//众为兴运动控制卡测试输入 + HSI_MOTION_INPUT_ALARM,//驱动报警 HSI_MOTION_OUTPUT = 0x0100, HSI_MOTION_OUTPUT_LASER_PEN, - HSI_MOTION_OUTPUT_CH1, - //固高、众为兴、EF1输出 + HSI_MOTION_OUTPUT_CH1,//固高、众为兴、EF1输出 HSI_MOTION_OUTPUT_CH2, - HSI_MOTION_OUTPUT_CH3, - //串口控制器输出 + HSI_MOTION_OUTPUT_CH3,//串口控制器输出 HSI_MOTION_OUTPUT_CH4 //众为兴运动控制卡测试输出 }; diff --git a/HSI_HexagonMI_EF3/HSI_Motion.cpp b/HSI_HexagonMI_EF3/HSI_Motion.cpp index 6ca56fd..5597a95 100644 --- a/HSI_HexagonMI_EF3/HSI_Motion.cpp +++ b/HSI_HexagonMI_EF3/HSI_Motion.cpp @@ -214,7 +214,7 @@ HSI_Motion::HSI_Motion() m_JogDecLine[j][i] = 5; m_JogDecCurve[j][i] = 0; } - m_Home_Machine_Axis[i] = 1; //用于启动时需要回原点的轴号选择 + m_Home_Machine_Axis[i] = 1; //用于启动时需要回原点的轴号选择,赋值1表示该轴需要回家 m_Home_Pos_Axis[i] = 0; //记住关闭电源时的位置,用于判断是否还需要回原点 } m_Home_Machine_Axis[4] = 0; //用于启动时需要回原点的轴号选择 @@ -437,11 +437,13 @@ HSI_STATUS HSI_Motion::Startup(HWND _hWnd, bool _bOfflineOnly) return HSI_STATUS_FAILED; } m_bACSConnected = true; - g_pLogger->SendAndFlushWithTime(L"[ACS Motion] Communication with the controller established success\n"); + g_pLogger->SendAndFlushWithTime( + L"[ACS Motion] Communication with the controller established success\n"); } else { - g_pLogger->SendAndFlushWithTime(L"[ACS Motion] Communication with the controller is already established successfully!\n"); + g_pLogger->SendAndFlushWithTime( + L"[ACS Motion] Communication with the controller is already established successfully!\n"); } } @@ -698,7 +700,12 @@ HSI_STATUS HSI_Motion::Startup(HWND _hWnd, bool _bOfflineOnly) return rStatus; } -//=============================获取EF3固件版本=============================== +//=========================================================================== +/** + * \brief 获取EF3固件版本 + * \param version + * \return + */ HSI_STATUS HSI_Motion::GetFirmwareVersion(byte* version) { m_Thread_StateData = HSI_THREAD_PAUSED; @@ -762,7 +769,7 @@ HSI_STATUS HSI_Motion::GetFirmwareVersion(byte* version) } //================================回家======================================= -HSI_STATUS HSI_Motion::HomeMachine(bool bHomed) +HSI_STATUS HSI_Motion::HomeMachineOld(bool bHomed) { auto rStatus = HSI_STATUS_NORMAL; if (!bHomed) @@ -801,7 +808,8 @@ HSI_STATUS HSI_Motion::HomeMachine(bool bHomed) AfxMessageBox(_T("急停或安全门或安全光幕触发!")); return HSI_STATUS_FAILED; } - + //根据当前位置,保存位置,求解相对运动 + //回家后,设置正负限位 CurrentHomeMachineState = E_EF3_HOME_ING; int GetHomePos[5] = {0}; if (m_SO7_Serial.m_RecvData[0] == 2) @@ -835,6 +843,7 @@ HSI_STATUS HSI_Motion::HomeMachine(bool bHomed) //GetHomePos[3] = (m_SO7_Serial.m_RecvData[9] << 24 | m_SO7_Serial.m_RecvData[10] << 16 | m_SO7_Serial.m_RecvData[11] << 8 | m_SO7_Serial.m_RecvData[12]); //GetHomePos[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | m_SO7_Serial.m_RecvData[15] << 8 | m_SO7_Serial.m_RecvData[16]); } + //取消限位,设置初始速度,加减速等参数 if (HSI_STATUS_NORMAL != HomeFindIndex()) { return HSI_STATUS_FAILED; @@ -879,8 +888,8 @@ HSI_STATUS HSI_Motion::HomeMachine(bool bHomed) m_Thread_State = HSI_THREAD_RUNNING; g_pLogger->SendAndFlushWithTime(L"[HomeMachine] SetPositionXyz\n"); double PrinfMovePos[5] = {0}; - PrinfMovePos[1] = (GetHomePos[1] - SavePos[1]) * m_Resolution[1]; - PrinfMovePos[2] = (GetHomePos[2] - SavePos[2]) * m_Resolution[2]; + PrinfMovePos[1] = (GetHomePos[1] - SavePos[1]) * m_Resolution[1]; //求解相对运动位置 + PrinfMovePos[2] = (GetHomePos[2] - SavePos[2]) * m_Resolution[2]; //相对运动,目标位置和现在位置求移动距离 PrinfMovePos[3] = (GetHomePos[3] - SavePos[3]) * m_Resolution[3]; PrinfMovePos[4] = (GetHomePos[4] - SavePos[4]) * m_Resolution[4]; g_pLogger->SendAndFlushWithTime( @@ -938,6 +947,69 @@ HSI_STATUS HSI_Motion::HomeMachine(bool bHomed) return rStatus; } +HSI_STATUS HSI_Motion::HomeMachine(bool bHomed) +{ + auto rStatus = HSI_STATUS_NORMAL; + if (!bHomed) + { + g_pLogger->SendAndFlushWithTime(L"[HomeMachine] bHomed No Need Reture\n"); + return HSI_STATUS_NORMAL; + } + if (m_IsUseEF3 == 0) + { + return HSI_STATUS_NORMAL; + } + if (g_pHSI_Motion) + { + g_pLogger->SendAndFlushWithTime(L"[HomeMachine] In\n"); + //判断是否需要回家 + bool home(false); + IsHomed(home); + if (home == true) + { + g_pLogger->SendAndFlushWithTime(L"[HomeMachine] IsHomed No Need Go Home\n"); + return HSI_STATUS_NORMAL; + } + sEvenProp.Init(); + sEvenProp.EventType = HSI_EVENT_FUNCTION; + sEvenProp.EventID = HSI_EVENT_MOTION_DCC_HOME; + sEvenProp.EventResponse = HSI_EVENT_RESPONSE_CANCEL; + EventCallback(sEvenProp); + if (sEvenProp.EventCallbackID == HSI_EVENT_RESPONSE_CANCEL) + { + g_pLogger->SendAndFlushWithTime(L"[HomeMachine] Cancel\n"); + return HSI_STATUS_NORMAL; + } + + if (m_bEmergencyState) + { + AfxMessageBox(_T("急停或安全门或安全光幕触发!")); + return HSI_STATUS_FAILED; + } + + CurrentHomeMachineState = E_EF3_HOME_ING; + + //运行 ACS 控制器内buffer0 自动回家动作 + //回家后,启用正负限位 + if (!acsc_RunBuffer(handleACS, 0, NULL, ACSC_SYNCHRONOUS)) + { + g_pLogger->SendAndFlushWithTime(L"[HomeMachine] ACS Run Buffer 0 error\n"); + return HSI_STATUS_FAILED; + } + + //再次读取回家标志位,或者上个动作完成回调 + IsHomed(home); + if (home == true) + { + g_pLogger->SendAndFlushWithTime(L"[HomeMachine] Go Home success\n"); + return HSI_STATUS_NORMAL; + } + + g_pLogger->SendAndFlushWithTime(L"[HomeMachine] Out\n"); + } + return rStatus; +} + //=========================================================================== HSI_STATUS HSI_Motion::HomeJog(short AxisNumber, short Dir, bool Wait) { @@ -952,6 +1024,10 @@ HSI_STATUS HSI_Motion::HomeJog(short AxisNumber, short Dir, bool Wait) } //=========================================================================== +/** + * \brief 取消限位,设置初始速度,加减速等参数 + * \return + */ HSI_STATUS HSI_Motion::HomeFindIndex() { auto rStatus = HSI_STATUS_NORMAL; @@ -1127,6 +1203,7 @@ HSI_STATUS HSI_Motion::HomeFindIndex() bool bHomed = true; if ((m_SO7_Serial.m_RecvData[38] & 0x01) == 0 && m_Home_Machine_Axis[1] == 1) { + /* strcat_s 是系统的安全函数,微软在 2005 后建议用一系统所谓安全的函数,这中间就有 strcat_s 取代了 strcat ,原来 strcat 函数,没有方法来保证有效的缓冲区尺寸,所以它只能假定缓冲足够大来容纳要拷贝的字符串, 容易产生程序崩溃。而strcat_s函数能很好的规避这个问题*/ strcat_s(MessageHome, 30, "1、"); bHomed = false; } @@ -1197,7 +1274,7 @@ HSI_STATUS HSI_Motion::GetAppPath(CString& Path) } //=========================================================================== -HSI_STATUS HSI_Motion::IsHomed(bool& bHomed) +HSI_STATUS HSI_Motion::IsHomedOld(bool& bHomed) { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) @@ -1268,6 +1345,63 @@ HSI_STATUS HSI_Motion::IsHomed(bool& bHomed) return rStatus; } +HSI_STATUS HSI_Motion::IsHomed(bool& bHomed) +{ + auto rStatus = HSI_STATUS_NORMAL; + if (g_pHSI_Motion && handleACS != ACSC_INVALID) + { + g_pLogger->SendAndFlushWithTime(L"[IsHomed] In\n"); + int isHomed[5] = {1, 1, 1, 1, 1}; + + + //所有轴都不需要回家 + if (m_Home_Machine_Axis[1] == 0 && m_Home_Machine_Axis[2] == 0 && m_Home_Machine_Axis[3] == 0 && + m_Home_Machine_Axis[4] == 0) + { + g_pLogger->SendAndFlushWithTime(L"[IsHomed] No Axis Go Home E_GTS_HOME_FINISHED\n"); + CurrentHomeMachineState = E_EF3_HOME_FINISHED; + bHomed = true; + return HSI_STATUS_NORMAL; + } + + // 判断是否需要回家,读取ACS控制器回家标志位,来判断本次上电是否已经回过家 + if (!acsc_ReadInteger(handleACS, ACSC_NONE, "ISHOMED", ACSC_NONE, ACSC_NONE, ACSC_NONE, ACSC_NONE, isHomed, + NULL)) + { + g_pLogger->SendAndFlushWithTime(L"[IsHomed] ACS Read ISHOMED Flag Error\n"); + CurrentHomeMachineState = E_EF3_HOME_NONE; + rStatus = HSI_STATUS_FAILED; + bHomed = false; + } + + g_pLogger->SendAndFlushWithTime(L"[IsHomed] ACS Read ISHOMED X:%d Y1:%d Y2:%d Z:%d\n", isHomed[0], isHomed[1], + isHomed[2], isHomed[3]); + + //如果各个轴标志位 已经回过家 + if (isHomed[1] == 1 && isHomed[2] == 1 && isHomed[3] == 1 && isHomed[4] == 1) + { + g_pLogger->SendAndFlushWithTime(L"[IsHomed] E_GTS_HOME_FINISHED\n"); + CurrentHomeMachineState = E_EF3_HOME_FINISHED; + bHomed = true; + } + else + { + g_pLogger->SendAndFlushWithTime(L"[IsHomed] Is No Go Home E_GTS_HOME_NONE\n"); + CurrentHomeMachineState = E_EF3_HOME_NONE; + bHomed = false; + } + g_pLogger->SendAndFlushWithTime(L"[IsHomed] Out\n"); + } + else + { + g_pLogger->SendAndFlushWithTime(L"[IsHomed] ACS Communication error\n"); + CurrentHomeMachineState = E_EF3_HOME_NONE; + rStatus = HSI_STATUS_FAILED; + bHomed = false; + } + return rStatus; +} + //=========================================================================== HSI_STATUS HSI_Motion::ZeroPos(bool bZeroPos) { @@ -1314,7 +1448,13 @@ HSI_STATUS HSI_Motion::ZeroPos(bool bZeroPos) return rStatus; } -//===============================JOG模式============================================ +//=========================================================================== +/** + * \brief JOG模式 + * \param AxisTypes + * \param Speed + * \return + */ HSI_STATUS HSI_Motion::Jog(UINT AxisTypes, double Speed) { auto rStatus = HSI_STATUS_NORMAL; @@ -2086,8 +2226,90 @@ HSI_STATUS HSI_Motion::GetPositionEncPrfMulti(UINT AxisTypes, double* EncPos, do } //=========================================================================== +/** + * \brief 获取轴当前运动位置 + * \param AxisTypes + * \param PositionX + * \param PositionY + * \param PositionZ + * \param Time + * \return + */ +HSI_STATUS HSI_Motion::GetPositionXyzOld(UINT AxisTypes, double& PositionX, double& PositionY, double& PositionZ, + double& Time) +{ + auto rStatus = HSI_STATUS_NORMAL; + UNREFERENCED_PARAMETER(AxisTypes); + //读取3个轴的位置 + CString tempStr; + if (g_pHSI_Motion) + { + if (m_SO7_Serial.m_RecvData[0] == 2) + { + if (m_DeviceType != 1) + { + if (m_IsHavePattern & 0x01) + PositionX = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial. + m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]) * m_Resolution[1]; + else + PositionX = (m_SO7_Serial.m_RecvData[17] << 24 | m_SO7_Serial.m_RecvData[18] << 16 | m_SO7_Serial. + m_RecvData[19] << 8 | m_SO7_Serial.m_RecvData[20]) * m_Resolution[1]; + if (m_IsHavePattern & 0x02) + PositionY = (m_SO7_Serial.m_RecvData[5] << 24 | m_SO7_Serial.m_RecvData[6] << 16 | m_SO7_Serial. + m_RecvData[7] << 8 | m_SO7_Serial.m_RecvData[8]) * m_Resolution[2]; + else + PositionY = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial. + m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]) * m_Resolution[2]; + if (m_IsHavePattern & 0x04) + PositionZ = (m_SO7_Serial.m_RecvData[9] << 24 | m_SO7_Serial.m_RecvData[10] << 16 | m_SO7_Serial. + m_RecvData[11] << 8 | m_SO7_Serial.m_RecvData[12]) * m_Resolution[3]; + else + PositionZ = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial. + m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]) * m_Resolution[3]; + if (m_IsHavePattern & 0x08) + m_PosForAllAxis[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | + m_SO7_Serial.m_RecvData[15] << 8 | m_SO7_Serial.m_RecvData[16]) * m_Resolution[4]; + else + m_PosForAllAxis[4] = (m_SO7_Serial.m_RecvData[29] << 24 | m_SO7_Serial.m_RecvData[30] << 16 | + m_SO7_Serial.m_RecvData[31] << 8 | m_SO7_Serial.m_RecvData[32]) * m_Resolution[4]; + } + else + { + PositionX = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial. + m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]) * m_Resolution[1]; + PositionY = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial. + m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]) * m_Resolution[2]; + PositionZ = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial. + m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]) * m_Resolution[3]; + } + m_EncPos[1] = PositionX; + m_EncPos[2] = PositionY; + m_EncPos[3] = PositionZ; + m_EncPos[4] = m_PosForAllAxis[4]; + m_PosForAllAxis[1] = PositionX; + m_PosForAllAxis[2] = PositionY; + m_PosForAllAxis[3] = PositionZ; + } + else + { + PositionX = m_EncPos[1]; + PositionY = m_EncPos[2]; + PositionZ = m_EncPos[3]; + m_PosForAllAxis[1] = m_EncPos[1]; + m_PosForAllAxis[2] = m_EncPos[2]; + m_PosForAllAxis[3] = m_EncPos[3]; + m_PosForAllAxis[4] = m_EncPos[4]; + g_pLogger->SendAndFlushWithTime(L"[GetPositionEncPrfMulti] failed\n"); + } + //LARGE_INTEGER tima; + //QueryPerformanceCounter(&tima); + //Time = static_cast(tima.QuadPart); + Time = set_end - set_start; + } + return rStatus; +} 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); @@ -2226,8 +2448,18 @@ HSI_STATUS HSI_Motion::GetPositionXyzaProbe(UINT AxisTypes, double& PositionX, d } //=========================================================================== -HSI_STATUS HSI_Motion::SetPositionXyz(UINT AxisTypes, double PositionX, double PositionY, double PositionZ, - HSI_MOTION_MOVE_TYPE eType, double dFlyRadius) +/** + * \brief 设置多轴运动到指定位置 + * \param AxisTypes + * \param PositionX + * \param PositionY + * \param PositionZ + * \param eType + * \param dFlyRadius + * \return + */ +HSI_STATUS HSI_Motion::SetPositionXyzOld(UINT AxisTypes, double PositionX, double PositionY, double PositionZ, + HSI_MOTION_MOVE_TYPE eType, double dFlyRadius) { WaitForSingleObject(g_WR_ToMove_Mutex, INFINITE); auto rStatus = HSI_STATUS_NORMAL; @@ -2240,6 +2472,7 @@ HSI_STATUS HSI_Motion::SetPositionXyz(UINT AxisTypes, double PositionX, double P axis_start = 0; unsigned char direct_pos = 0; unsigned char xyzAxis = 0; + //如果状态非 运动中,设置为运动中 if (CurrentMotionState != E_SO7_MOTION_MOVETO) { CurrentMotionState = E_SO7_MOTION_MOVETO; @@ -2266,7 +2499,8 @@ HSI_STATUS HSI_Motion::SetPositionXyz(UINT AxisTypes, double PositionX, double P if (m_SO7_Serial.m_RecvData[0] == 2) { - if (m_DeviceType != 1) + //根据设备类型获取现在位置 + 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. @@ -2320,6 +2554,8 @@ HSI_STATUS HSI_Motion::SetPositionXyz(UINT AxisTypes, double PositionX, double P } 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]); @@ -2393,12 +2629,12 @@ HSI_STATUS HSI_Motion::SetPositionXyz(UINT AxisTypes, double PositionX, double P Pos[4] = (int)(m_PositionA / m_Resolution[4]) - NowPos[4]; } }*/ - target_pos[1] = static_cast(PositionX / m_Resolution[1]); + 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[1] = target_pos[1]; //将目标位置设置为 开始位置 begin_position[2] = target_pos[2]; begin_position[3] = target_pos[3]; begin_position[4] = target_pos[4]; @@ -2562,7 +2798,7 @@ HSI_STATUS HSI_Motion::SetPositionXyz(UINT AxisTypes, double PositionX, double P //} int axisCount = 4; - if (fourthAxisFlag) + if (fourthAxisFlag) //第4轴 { fourthAxisFlag = false; if (Pos[4] > 0) direct_pos |= 0x08; @@ -2572,6 +2808,8 @@ HSI_STATUS HSI_Motion::SetPositionXyz(UINT AxisTypes, double PositionX, double P } else xyzAxis = AXIS_XYZ; + + //设置运动参数 for (int i = 1; i < axisCount; i++) { int time_out_send = 0; @@ -2666,6 +2904,7 @@ HSI_STATUS HSI_Motion::SetPositionXyz(UINT AxisTypes, double PositionX, double P Sleep(6); } + //清除缓存位置 while (m_SO7_Serial.m_RecvData[39]) { send_pos_data[0] = CT_ORDER; @@ -2733,7 +2972,7 @@ HSI_STATUS HSI_Motion::SetPositionXyz(UINT AxisTypes, double PositionX, double P send_pos_data[40] = Stepdriverspeed[4] & 0xff; - if (bCircleRun) + if (bCircleRun) //圆弧插补 { bCircleRun = false; send_pos_data[1] = CT_CIRCLERUN_POSITION; @@ -2786,6 +3025,102 @@ HSI_STATUS HSI_Motion::SetPositionXyz(UINT AxisTypes, double PositionX, double P return rStatus; } +HSI_STATUS HSI_Motion::SetPositionXyz(UINT AxisTypes, double PositionX, double PositionY, double PositionZ, + HSI_MOTION_MOVE_TYPE eType, double dFlyRadius) +{ + WaitForSingleObject(g_WR_ToMove_Mutex, INFINITE); + auto rStatus = HSI_STATUS_NORMAL; + if (g_pHSI_Motion) + { + g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] In\n"); + g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Pos[1] = %.4f,Pos[2] = %.4f,Pos[3] = %.4f\n", PositionX, + PositionY, PositionZ); + unsigned char send_pos_data[64] = {0}; + axis_start = 0; + unsigned char direct_pos = 0; + unsigned char xyzAxis = 0; + if (CurrentMotionState != E_SO7_MOTION_MOVETO) + { + CurrentMotionState = E_SO7_MOTION_MOVETO; + //限位功能 + LimitOver(HSI_MOTION_AXIS_X, PositionX); + LimitOver(HSI_MOTION_AXIS_Y, PositionY); + LimitOver(HSI_MOTION_AXIS_Z, PositionZ); + LimitOver(HSI_MOTION_AXIS_R, m_PositionA); + + m_PosThread[1] = PositionX; //SetpositionXyz的目标位置 + m_PosThread[2] = PositionY; + m_PosThread[3] = PositionZ; + m_PosThread[4] = m_PositionA; + + + //打印当前位置,目标位置 + //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); + + //打印轴当前运动参数 + double motionParam[5] = { 0 }; + GetMotorParam(AXIS_X, motionParam); + g_pLogger->SendAndFlushWithTime( + L"[SetPositionXyz] axis= %d,GetVelocity = %d,GetAcceleration= %d,GetDeceleration= %d, GetKillDeceleration= %d, GetJerk= %d\n",AXIS_X,motionParam[0],motionParam[1],motionParam[2],motionParam[3],motionParam[4]); + + + //设置轴运动速度,TO-DO + + + //开始运动到指定位置 + int Axes[] = { ACSC_AXIS_0, ACSC_AXIS_1, ACSC_AXIS_2 }; //需要运动的轴 + double Points[] = { PositionX ,PositionY,PositionZ }; //目标位置点 + if(!acsc_ToPointM(handleACS, 0, Axes, Points, nullptr)) + { + g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] ACS Multi Motion Error"); + } + + //启动插补和定位功能 + + + //圆弧插补 + + + if (eType == HSI_MOTION_MOVE_NOWAIT)//非等待 + { + g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Nowait SetEvent\n"); + g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Nowait move!\n"); + g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Out Success Nowait\n"); + m_IsExMotion = 0; + bRunGlueDispenser = HSI_THREAD_PAUSED; + m_Thread_State = HSI_THREAD_RUNNING; + SetEvent(m_hTriggerEvent); + } + if (eType == HSI_MOTION_MOVE_WAIT)//等待模式 + { + g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Out Success Wait Mode\n"); + m_Thread_State = HSI_THREAD_RUNNING; + m_IsExMotion = 0; + UpdateMotionState(); + m_Thread_State = HSI_THREAD_PAUSED; + } + } + else + { + g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] HSI_STATUS_MOTION_MOVING\n"); + g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Out\n"); + rStatus = HSI_STATUS_MOTION_MOVING; + } + g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Out\n"); + targetpos_l[1] = PositionX; + targetpos_l[2] = PositionY; + targetpos_l[3] = PositionZ; + } + ReleaseMutex(g_WR_ToMove_Mutex); + return rStatus; +} + //=========================================================================== int HSI_Motion::CaculateStepMotorACC(int pos, int maxacc, int minacc) { @@ -2798,6 +3133,44 @@ int HSI_Motion::CaculateStepMotorACC(int pos, int maxacc, int minacc) return acc; } +//=========================================================================== +/** + * \brief 求各轴运动配置参数 + * \param AXIS + * \param motionParam + * \return + */ +HSI_STATUS HSI_Motion::GetMotorParam(int AXIS, double motionParam[5]) +{ + auto rStatus = HSI_STATUS_NORMAL; + if (handleACS != ACSC_INVALID) + { + //依次是 速度、加速度、减速、杀死速度、抖动 + if (!acsc_GetVelocity(handleACS, AXIS, motionParam, nullptr)) + { + rStatus = HSI_ACS_ERROR; + } + if (!acsc_GetAcceleration(handleACS, AXIS, motionParam + 1, nullptr)) + { + rStatus = HSI_ACS_ERROR; + } + if (!acsc_GetDeceleration(handleACS, AXIS, motionParam + 2, nullptr)) + { + rStatus = HSI_ACS_ERROR; + } + if (!acsc_GetKillDeceleration(handleACS, AXIS, motionParam + 3, nullptr)) + { + rStatus = HSI_ACS_ERROR; + } + if (!acsc_GetJerk(handleACS, AXIS, motionParam + 4, nullptr)) + { + rStatus = HSI_ACS_ERROR; + } + } + + return rStatus; +} + //=========================================================================== HSI_STATUS HSI_Motion::SetPositionXyza(UINT AxisTypes, double PositionX, double PositionY, double PositionZ, double PositionA, HSI_MOTION_MOVE_TYPE eType, double dFlyRadius) @@ -2827,6 +3200,13 @@ HSI_STATUS HSI_Motion::SetPositionXyzCache(UINT AxisTypes, HSI_MOTION_MOVE_TYPE } //=========================================================================== +/** + * \brief 圆弧插补 + * \param PositionX + * \param PositionY + * \param PositionZ + * \return + */ HSI_STATUS HSI_Motion::SetCircleInterpolate(double PositionX, double PositionY, double PositionZ) { auto rStatus = HSI_STATUS_NORMAL; @@ -2842,7 +3222,11 @@ HSI_STATUS HSI_Motion::SetCircleInterpolate(double PositionX, double PositionY, return rStatus; } -//===========================探针接口================================================ +//=========================================================================== +/** + * \brief 探针接口 + * \param RetractManDist + */ void HSI_Motion::ProbeRetractManDist(int RetractManDist) { if (g_pHSI_Motion) @@ -2974,7 +3358,12 @@ HSI_STATUS HSI_Motion::JogProbe(UINT AxisTypes, double Speed) return rStatus; } -//=============================读取配置============================================== +//=========================================================================== +/** + * \brief 读取配置 + * \param GoogolIniFile + * \return + */ HSI_STATUS HSI_Motion::Load_EF3_Motion_Inifile(CString GoogolIniFile) { auto rStatus = HSI_STATUS_NORMAL; @@ -3229,7 +3618,14 @@ HSI_STATUS HSI_Motion::Load_EF3_Config_Inifile(CString GoogolIniFile) return rStatus; } -//===============================读取/设置光栅尺精度============================================ +//=========================================================================== +/** + * \brief 读取/设置光栅尺精度 + * \param _ScaleX + * \param _ScaleY + * \param _ScaleZ + * \return + */ HSI_STATUS HSI_Motion::GetScaleResolution(double& _ScaleX, double& _ScaleY, double& _ScaleZ) { auto rStatus = HSI_STATUS_NORMAL; @@ -3255,7 +3651,10 @@ HSI_STATUS HSI_Motion::SetScaleResolution(double _ScaleX, double _ScaleY, double return rStatus; } -//============================定位完成事件=============================================== +//=========================================================================== +/** + * \brief 定位完成事件 + */ void HSI_Motion::SendMsgMotionFinished() { sEvenProp.Init(); @@ -3265,7 +3664,10 @@ void HSI_Motion::SendMsgMotionFinished() EventCallback(sEvenProp); } -//=============================回调探针运行事件============================================== +//=========================================================================== +/** + * \brief 回调探针运行事件 + */ void HSI_Motion::SendMsgProbeFinished() { sEvenProp.Init(); @@ -3405,7 +3807,7 @@ void HSI_Motion::UpdateMotionState() m_PrfPos[3] - m_EncPos[3], 2.0); if (dis > 0) { - PntToPntDistance = sqrt(dis); + PntToPntDistance = sqrt(dis); //欧氏距离 } else { @@ -3544,7 +3946,10 @@ void HSI_Motion::UpdateMotionStateIO() } } -//========================获取运动状态=================================================== +//=========================================================================== +/** + * \brief 获取运动状态 + */ void HSI_Motion::UpdateMotionStateData() { while (m_Thread_StateData == HSI_THREAD_RUNNING) @@ -3624,13 +4029,16 @@ void HSI_Motion::UpdateMotionStateProbe() void HSI_Motion::DoEvents() { MSG msg; - //和函数PeekMessage不一样的是,GetMessage:从系统获取消息,将消息从系统中移除,属于阻塞函数。 - //当系统无消息时,GetMessage会等待下一条消息。而函数PeekMesssge是以查看的方式从系统中获取消息,可以不将消息从系统中移除, - //是非阻塞函数;当系统无消息时,返回FALSE,继续执行后续代码 + //GetMessage:从系统获取消息,将消息从系统中移除,属于阻塞函数。当系统无消息时,GetMessage会等待下一条消息。 + + //函数PeekMesssge是以查看的方式从系统中获取消息,可以不将消息从系统中移除,是非阻塞函数;当系统无消息时,返回FALSE,继续执行后续代码 while (PeekMessage(&msg, nullptr, 0, 0, PM_REMOVE)) { __try { + //DispatchMessage 函数分发一个消息给窗口程序。通常消息从GetMessage函数获得或者TranslateMessage函数传递的。 + // 消息被分发到回调函数(过程函数),作用是消息传递给操作系统, + // 然后操作系统去调用我们的回调函数,也就是说我们在窗体的过程函数中处理消息。 DispatchMessage(&msg); TranslateMessage(&msg); } @@ -3823,7 +4231,11 @@ HSI_STATUS HSI_Motion::GetAxisStatus(int* _Status) return rStatus; } -//=============================暂停和关闭============================================== +//=========================================================================== +/** + * \brief 暂停和关闭 + * \return + */ HSI_STATUS HSI_Motion::AbortMotion() //需要运动实现 { auto rStatus = HSI_STATUS_NORMAL; @@ -3863,7 +4275,11 @@ HSI_STATUS HSI_Motion::AbortMotion() // return rStatus; } -//===============================关闭============================================ +//=========================================================================== +/** + * \brief 关闭 + * \return + */ HSI_STATUS HSI_Motion::Shutdown() { auto rStatus = HSI_STATUS_NORMAL; @@ -3927,7 +4343,16 @@ HSI_STATUS HSI_Motion::Shutdown() return rStatus; } -//==============================触发灯光============================================= +//=========================================================================== +/** + * \brief 触发灯光 + * \param triggleNum + * \param delayLighting + * \param delayLightBefor + * \param triggleMode + * \param Intensities + * \return + */ HSI_STATUS HSI_Motion::SetTriggerLight(int triggleNum, int delayLighting, int delayLightBefor, int triggleMode, double* Intensities) { @@ -3990,7 +4415,12 @@ HSI_STATUS HSI_Motion::SetTriggerLight(int triggleNum, int delayLighting, int de return rStatus; } -//=============================硬件触发拍照============================================== +//=========================================================================== +/** + * \brief 硬件触发拍照 + * \param startPoint + * \return + */ HSI_STATUS HSI_Motion::DCCPPStartPoint(double* startPoint) { auto rStatus = HSI_STATUS_NORMAL; @@ -4545,7 +4975,17 @@ HSI_STATUS HSI_Motion::DCCForLightPlate() return rStatus; } -//===============================转盘============================================ +//=========================================================================== +/** + * \brief 转盘 + * \param CamerasDis + * \param BinsDis + * \param SubArea + * \param filterTime1 + * \param filterTime2 + * \param pluseSumDis + * \return + */ HSI_STATUS HSI_Motion::StartPlcJob(int* CamerasDis, int* BinsDis, int SubArea, int filterTime1, int filterTime2, int pluseSumDis) { @@ -5575,7 +6015,19 @@ HSI_STATUS HSI_Motion::GetPntsDistance(double& pDistance, int& spTimeCount) return rStatus; } -//========================运动控制参数读取及设置=================================================== +//=========================================================================== +/** + * \brief 运动控制参数读取及设置 + * \param AxisNum + * \param Speed + * \param DirveSpeed + * \param StartSpeed + * \param AccLine + * \param DecLine + * \param AccCurve + * \param DecCurve + * \return + */ int HSI_Motion::SpeedPercent(int AxisNum, double& Speed, int& DirveSpeed, int& StartSpeed, int& AccLine, int& DecLine, int& AccCurve, int& DecCurve) { @@ -5638,7 +6090,19 @@ int HSI_Motion::SpeedPercent(int AxisNum, double& Speed, int& DirveSpeed, int& S return static_cast(Speed); } -//==========================JoyStick运动控制参数读取及设置================================================= +//=========================================================================== +/** + * \brief JoyStick运动控制参数读取及设置 + * \param AxisNum + * \param Speed + * \param DirveSpeed + * \param StartSpeed + * \param AccLine + * \param DecLine + * \param AccCurve + * \param DecCurve + * \return + */ bool HSI_Motion::SpeedPercentJoyStick(int AxisNum, long& Speed, int& DirveSpeed, int& StartSpeed, int& AccLine, int& DecLine, int& AccCurve, int& DecCurve) { diff --git a/HSI_HexagonMI_EF3/HSI_Motion.h b/HSI_HexagonMI_EF3/HSI_Motion.h index 16876ff..8ebfb64 100644 --- a/HSI_HexagonMI_EF3/HSI_Motion.h +++ b/HSI_HexagonMI_EF3/HSI_Motion.h @@ -159,12 +159,26 @@ public: HSI_STATUS IsSupported(UINT& Types) override; HSI_STATUS Startup(HWND _hWnd, bool _bOfflineOnly) override; HSI_STATUS GetFirmwareVersion(byte* version); - + /** + * \brief 判断机台是否回家,如果没有,则执行回家动作 + * \param bHomed + * \return + */ + HSI_STATUS HomeMachineOld(bool bHomed); HSI_STATUS HomeMachine(bool bHomed); + HSI_STATUS HomeJog(short AxisNumber, short Dir, bool Wait = false); HSI_STATUS HomeFindIndex(); HSI_STATUS ZeroPos(bool bZeroPos); + + /** + * \brief 判断机台是否回家,仅获取是否回家的状态 + * \param bHomed 是否回家标志位 + * \return + */ + HSI_STATUS IsHomedOld(bool& bHomed); HSI_STATUS IsHomed(bool& bHomed); + HSI_STATUS GetSpeedXyz(int AxisNum, double& Speed); HSI_STATUS SetSpeedXyz(double Speed); HSI_STATUS GetFocusSpeed(double& Speed); @@ -186,6 +200,16 @@ public: HSI_STATUS StopJogEx(UINT AxisTypes); HSI_STATUS GetPositionEncPrfMulti(UINT AxisTypes, double* EncPos, double* PrfPos, int Count); + /** + * \brief 获取轴当前位置 + * \param AxisTypes 轴号 + * \param PositionX X坐标 + * \param PositionY Y坐标 + * \param PositionZ Z坐标 + * \param Time + * \return + */ + HSI_STATUS GetPositionXyzOld(UINT AxisTypes, double& PositionX, double& PositionY, double& PositionZ, double& Time); HSI_STATUS GetPositionXyz(UINT AxisTypes, double& PositionX, double& PositionY, double& PositionZ, double& Time); HSI_STATUS GetPositionXyzaProbe(UINT AxisTypes, double& PositionX, double& PositionY, double& PositionZ, double& PositionA); @@ -193,14 +217,35 @@ public: HSI_STATUS JogProbe(UINT AxisTypes, double Speed); void ProbeRetractManDist(int RetractManDist); int CaculateStepMotorACC(int pos, int maxacc, int minacc); + /** + * \brief 求各轴运动配置参数 + * \param AXIS + * \param motionParam + * \return + */ + HSI_STATUS HSI_Motion::GetMotorParam(int AXIS, double motionParam[5]); + + /** + * \brief 运行到指定位置 + * \param AxisTypes 轴号 + * \param PositionX X坐标 + * \param PositionY Y坐标 + * \param PositionZ Z坐标 + * \param eType 运动模式, + * \param dFlyRadius + * \return + */ + HSI_STATUS SetPositionXyzOld(UINT AxisTypes, double PositionX, double PositionY, double PositionZ, + HSI_MOTION_MOVE_TYPE eType, double dFlyRadius); HSI_STATUS SetPositionXyz(UINT AxisTypes, double PositionX, double PositionY, double PositionZ, 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_STATUS SetPositionXyzCache(UINT AxisTypes, HSI_MOTION_MOVE_TYPE eType, int DataCount, Point* CacheData); HSI_STATUS GetPositionR(UINT AxisTypes, double& PositionR, double& Time); HSI_STATUS SetPositionR(UINT AxisTypes, double PositionR, HSI_MOTION_AXIS_R_MOVE_TYPE DirectionType, bool bWait); - HSI_STATUS SetCircleInterpolate(double PositionX, double PositionY, double PositionZ); + HSI_STATUS SetCircleInterpolate(double PositionX, double PositionY, double PositionZ);//圆弧插补 HSI_STATUS Load_EF3_Motion_Inifile(CString GoogolIniFile); HSI_STATUS Load_EF3_Config_Inifile(CString GoogolIniFile); HSI_STATUS AbortMotion(); @@ -445,7 +490,7 @@ public: double m_PrfPos[5]; double m_PosForAllAxis[5]; //记录4轴位置 bool m_bConnected; - bool m_bACSConnected;// ACS是否连接成功 + bool m_bACSConnected; // ACS是否连接成功 int m_SendDataLength; unsigned char m_cSendData[64]; unsigned char m_direct_pos; diff --git a/HSI_HexagonMI_EF3/version.h b/HSI_HexagonMI_EF3/version.h index 0b15520..34ff873 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 "2022.10.12 / 13:49 " -#define HSI_FILE_CSDESCRIPTION _T("2022.10.12 / 13:49 ") +#define HSI_FILE_DESCRIPTION "2022.10.13 / 15:29 " +#define HSI_FILE_CSDESCRIPTION _T("2022.10.13 / 15:29 ") diff --git a/HSI_SEVENOCEAN_EF1_CsTest/Program.cs b/HSI_SEVENOCEAN_EF1_CsTest/Program.cs index 6aa4c76..65beeb6 100644 --- a/HSI_SEVENOCEAN_EF1_CsTest/Program.cs +++ b/HSI_SEVENOCEAN_EF1_CsTest/Program.cs @@ -39,7 +39,7 @@ namespace HSI_SEVENOCEAN_EF1_CsTest //if (Motion.IsActive(true)) { - rStatus = Motion.Startup(true); //杩愬姩鍒濆鍖 + rStatus = Motion.Startup(true); //杩愬姩鍒濆鍖栵紝鍥炲鍒ゆ柇 Console.WriteLine("Motion.Startup:{0}", rStatus); //鑾峰彇EF3鍥轰欢鐗堟湰鍙,寰呮祴璇