diff --git a/HSI_HexagonMI_EF3/HSI.h b/HSI_HexagonMI_EF3/HSI.h index a8c6c29..b828134 100644 --- a/HSI_HexagonMI_EF3/HSI.h +++ b/HSI_HexagonMI_EF3/HSI.h @@ -3,7 +3,7 @@ #include #include - +#include "ACS/ACSC.h" #pragma once /////////////////////////////////////////////////////////////////////////////// // @@ -372,8 +372,8 @@ enum HSI_MOTION_IO_TYPE HSI_MOTION_OUTPUT_CH4 //众为兴运动控制卡测试输出 }; -const UINT HSI_MOTION_AXIS_ALL = HSI_MOTION_AXIS_X | HSI_MOTION_AXIS_Y | HSI_MOTION_AXIS_Z; - +//const UINT HSI_MOTION_AXIS_ALL = HSI_MOTION_AXIS_X | HSI_MOTION_AXIS_Y | HSI_MOTION_AXIS_Z; +const UINT HSI_MOTION_AXIS_ALL = ACSC_AXIS_X | ACSC_AXIS_Y | ACSC_AXIS_Z; enum HSI_MOTION_MOVE_TYPE { HSI_MOTION_MOVE_WAIT = 1, diff --git a/HSI_HexagonMI_EF3/HSI_Motion.cpp b/HSI_HexagonMI_EF3/HSI_Motion.cpp index 032e9e5..e3fb752 100644 --- a/HSI_HexagonMI_EF3/HSI_Motion.cpp +++ b/HSI_HexagonMI_EF3/HSI_Motion.cpp @@ -11,6 +11,7 @@ #include #include #include +#include #include "windows.h" #include "ACS/ACSC.h" //引入ACS运动控制卡头文件 using namespace std; @@ -65,6 +66,7 @@ SOCKET m_socket[4] = {0}; //=========================================================================== //运动类构造函数，涉及 运动控制参数、插补、回家、摇杆、日志等配置信息的加载 + void ErrorsHandler(const char* ErrorMessage, BOOL fCloseComm) { printf(ErrorMessage); @@ -73,6 +75,32 @@ void ErrorsHandler(const char* ErrorMessage, BOOL fCloseComm) _getch(); }; +void ErrorsHandler() +{ + if (handleACS != ACSC_INVALID) + { + char ErrorStr[256]; + int ErrorCode = 0; + int Received = 0; + ErrorCode = GetLastError(); + if (acsc_GetErrorString(handleACS, // communication handle + ErrorCode, // error code + ErrorStr, // buffer for the error explanation + 255, // available buffer length + &Received // number of actually received bytes + )) + { + ErrorStr[Received] = '\0'; + printf("function returned error: %d (%s)\n", ErrorCode, ErrorStr); + g_pLogger->SendAndFlushWithTime(L"[ACS Motion] motion failed {%s}\n", ErrorStr); + } + } + else + { + g_pLogger->SendAndFlushWithTime(L"[ACS Motion] ACS Communication handle is invalid\n"); + } +}; + HSI_Motion::HSI_Motion() { @@ -258,7 +286,7 @@ HSI_Motion::HSI_Motion() } GetAppPath(m_AppPath); m_LogIsOpen[0] = GetPrivateProfileInt(L"LOG", L"LOG_IS_OPEN_0", 0, m_AppPath + _T("\\Config\\EF3_Motion.ini")); - g_pLogger->IsEnabledLog = m_LogIsOpen[0] == 1 ? true : false;//是否启用日志 + g_pLogger->IsEnabledLog = m_LogIsOpen[0] == 1 ? true : false; //是否启用日志 m_Set_XYZA_Reserve = 0; //XYZA轴方向 m_motorType = 0; //电机类型 1为伺服电机 0为步进电机 @@ -380,8 +408,7 @@ HSI_STATUS HSI_Motion::Startup(HWND _hWnd, bool _bOfflineOnly) return HSI_STATUS_NORMAL; } g_pLogger->SendAndFlushWithTime(L"[Startup] In\n"); - g_pLogger->SendAndFlushWithTime(L"[Startup] HMQ HSI.dll version = %s, date = %s\n", HSI_VERSION_CSTRING, - HSI_FILE_CSDESCRIPTION); //输出HSI.dll 版本号 + g_pLogger->SendAndFlushWithTime(L"[Startup] HMQ HSI.dll version = %s, date = %s\n", HSI_VERSION_CSTRING, HSI_FILE_CSDESCRIPTION); //输出HSI.dll 版本号 GoogolMotionConfigFile = m_AppPath + _T("\\Config\\EF3_Config.ini"); Load_EF3_Config_Inifile(GoogolMotionConfigFile); //加载 EF3_Config.ini 配置项 @@ -431,8 +458,9 @@ HSI_STATUS HSI_Motion::Startup(HWND _hWnd, bool _bOfflineOnly) if (handleACS == ACSC_INVALID) //打开失败 { - ErrorsHandler("error while opening communication.\n", FALSE); g_pLogger->SendAndFlushWithTime(L"[ACS Motion] error while opening communication.\n"); + ErrorsHandler(); + sEvenProp.Init(); sEvenProp.EventType = HSI_EVENT_ERROR; sEvenProp.EventID = HSI_EVENT_MOTION; @@ -442,17 +470,23 @@ HSI_STATUS HSI_Motion::Startup(HWND _hWnd, bool _bOfflineOnly) return HSI_STATUS_FAILED; } m_bACSConnected = true; - g_pLogger->SendAndFlushWithTime(L"[ACS Motion] ACS Established Success\n"); + g_pLogger->SendAndFlushWithTime(L"[ACS Motion] ACS Established Success!\n"); - //获取ACS 控制器版本号 + //获取ACS 控制器版本号, 待测试，2.70 char Firmware[256]; int Received; if (!acsc_GetFirmwareVersion(handleACS, Firmware, 255, &Received, nullptr)) { - g_pLogger->SendAndFlushWithTime(L"[ACS Motion] GET ACS Controller Version failed\n"); + g_pLogger->SendAndFlushWithTime(L"[ACS Motion] GET ACS Controller Version failed!\n"); + ErrorsHandler(); } - - g_pLogger->SendAndFlushWithTime(L"[ACS Motion] ACS Controller Version: %s\n", Firmware); + g_pLogger->SendAndFlushWithTime(L"[ACS Motion] ACS Controller Version: %s\n", + convertToString(Firmware, Received)); + //获取SPiiPlus C Library version + unsigned int Ver = acsc_GetLibraryVersion(); + printf("SPiiPlus C Library version is %d.%d.%d.%d\n", HIBYTE(HIWORD(Ver)), LOBYTE(HIWORD(Ver)), HIBYTE(LOWORD(Ver)), + LOBYTE(LOWORD(Ver))); + g_pLogger->SendAndFlushWithTime(L"[ACS Motion] SPiiPlus C Library version is %d.%d.%d.%d\n", HIBYTE(HIWORD(Ver)), LOBYTE(HIWORD(Ver)), HIBYTE(LOWORD(Ver)),LOBYTE(LOWORD(Ver))); } else { @@ -671,10 +705,10 @@ HSI_STATUS HSI_Motion::Startup(HWND _hWnd, bool _bOfflineOnly) g_pLogger->SendAndFlushWithTime(L"[Startup] Set Gears Success\n"); } - //CreateThread(); //刷新位置状态线程 - //SetEvent(m_hTriggerEvent); - //m_Thread_State = HSI_THREAD_PAUSED; - //g_pLogger->SendAndFlushWithTime(L"[Startup] SetpositionXyz Enable\n"); + CreateThread(); //刷新位置状态线程 + SetEvent(m_hTriggerEvent); + m_Thread_State = HSI_THREAD_PAUSED; + g_pLogger->SendAndFlushWithTime(L"[Startup] SetpositionXyz Enable\n"); //CreateThreadData(); //读取EF3数据状态线程 //SetEvent(m_hTriggerEventData); @@ -862,11 +896,13 @@ HSI_STATUS HSI_Motion::HomeMachineOld(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; } + int SavePos[5] = {0}; if (m_SO7_Serial.m_RecvData[0] == 2) { @@ -1012,7 +1048,8 @@ HSI_STATUS HSI_Motion::HomeMachine(bool bHomed) //回家后，启用正负限位 if (!acsc_RunBuffer(handleACS, ACSC_BUFFER_6, nullptr, ACSC_SYNCHRONOUS)) { - g_pLogger->SendAndFlushWithTime(L"[HomeMachine] ACS Run Buffer 0 error\n"); + g_pLogger->SendAndFlushWithTime(L"[HomeMachine] ACS Run Buffer %d error\n", ACSC_BUFFER_6); + ErrorsHandler(); return HSI_STATUS_FAILED; } @@ -1370,7 +1407,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] = {1, 1, 1, 1, 1}; + int isHomed[5] = {0, 1, 1, 1, 1}; //暂定只有一个回家标志位，即全部回家完成，没有按单个轴回家来看 //所有轴都不需要回家 @@ -1389,17 +1426,18 @@ HSI_STATUS HSI_Motion::IsHomed(bool& bHomed) nullptr)) { g_pLogger->SendAndFlushWithTime(L"[IsHomed] ACS Read ISHOMED Flag Error\n"); + ErrorsHandler(); + CurrentHomeMachineState = E_EF3_HOME_NONE; rStatus = HSI_STATUS_FAILED; bHomed = false; } - g_pLogger->SendAndFlushWithTime(L"[IsHomed] ACS Read YAW_HOME_DONE X:%d Y1:%d Y2:%d Z:%d\n", isHomed[0], - isHomed[1], - isHomed[2], isHomed[3]); + g_pLogger->SendAndFlushWithTime(L"[IsHomed] ACS Read YAW_HOME_DONE 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) + if (isHomed[0] == 1 && isHomed[1] == 1 && isHomed[2] == 1 && isHomed[3] == 1) { g_pLogger->SendAndFlushWithTime(L"[IsHomed] E_GTS_HOME_FINISHED\n"); CurrentHomeMachineState = E_EF3_HOME_FINISHED; @@ -2446,23 +2484,26 @@ HSI_STATUS HSI_Motion::GetPositionXyz(UINT AxisTypes, double& PositionX, double& CString tempStr; bool bGetPosition = true; - if (g_pHSI_Motion) + if (g_pHSI_Motion &&(handleACS!=ACSC_INVALID)) //句柄有效 { if (!acsc_GetFPosition(handleACS, ACSC_AXIS_0, &PositionX, nullptr)) { g_pLogger->SendAndFlushWithTime(L"[GetPositionEncPrfMulti] get PositionX failed\n"); + ErrorsHandler(); bGetPosition = false; return HSI_ACS_ERROR; } if (!acsc_GetFPosition(handleACS, ACSC_AXIS_1, &PositionY, nullptr)) { g_pLogger->SendAndFlushWithTime(L"[GetPositionEncPrfMulti] get PositionY failed\n"); + ErrorsHandler(); bGetPosition = false; return HSI_ACS_ERROR; } if (!acsc_GetFPosition(handleACS, ACSC_AXIS_2, &PositionZ, nullptr)) { g_pLogger->SendAndFlushWithTime(L"[GetPositionEncPrfMulti] get PositionZ failed\n"); + ErrorsHandler(); bGetPosition = false; return HSI_ACS_ERROR; } @@ -2476,6 +2517,8 @@ HSI_STATUS HSI_Motion::GetPositionXyz(UINT AxisTypes, double& PositionX, double& m_PosForAllAxis[1] = PositionX; m_PosForAllAxis[2] = PositionY; m_PosForAllAxis[3] = PositionZ; + + g_pLogger->SendAndFlushWithTime(L"[GetPositionEncPrfMulti] GetPosition Success, Pos[1] = %.4f,Pos[2] = %.4f,Pos[3] = %.4f,Pos[4] = %.4f\n", PositionX, PositionY, PositionZ, m_PosForAllAxis[4]); } else //读取将之前的值进行返回 { @@ -2486,7 +2529,8 @@ HSI_STATUS HSI_Motion::GetPositionXyz(UINT AxisTypes, double& PositionX, double& 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"); + + g_pLogger->SendAndFlushWithTime(L"[GetPositionEncPrfMulti] GetPosition failed, return position record before, Pos[1] = %.4f,Pos[2] = %.4f,Pos[3] = %.4f,Pos[4] = %.4f\n", m_EncPos[1], m_EncPos[2], m_EncPos[3], m_PosForAllAxis[4]); } Time = set_end - set_start; @@ -3145,8 +3189,7 @@ HSI_STATUS HSI_Motion::SetPositionXyz(UINT AxisTypes, double PositionX, double P 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); + 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; @@ -3160,7 +3203,8 @@ HSI_STATUS HSI_Motion::SetPositionXyz(UINT AxisTypes, double PositionX, double P LimitOver(HSI_MOTION_AXIS_Z, PositionZ); LimitOver(HSI_MOTION_AXIS_R, m_PositionA); - m_PosThread[1] = PositionX; //SetpositionXyz的目标位置 + //SetpositionXyz的目标位置 + m_PosThread[1] = PositionX; m_PosThread[2] = PositionY; m_PosThread[3] = PositionZ; m_PosThread[4] = m_PositionA; @@ -3185,13 +3229,16 @@ HSI_STATUS HSI_Motion::SetPositionXyz(UINT AxisTypes, double PositionX, double P //设置轴运动速度，TO-DO + + //开始运动到指定位置 - int Axes[] = {ACSC_AXIS_0, ACSC_AXIS_1, ACSC_AXIS_2}; //需要运动的轴 - double Points[] = {PositionX, PositionY, PositionZ}; //目标位置点 + int Axes[] = {ACSC_AXIS_0, ACSC_AXIS_1,-1}; //需要运动的轴 + double Points[] = {PositionX, PositionY,-1}; //目标位置点 if (!acsc_ToPointM(handleACS, 0, Axes, Points, nullptr)) { - g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] ACS Multi Motion Error"); + g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] ACS Multi Motion Error\n"); + ErrorsHandler(); } //启动插补和定位功能 @@ -3791,6 +3838,9 @@ void HSI_Motion::SendMsgProbeFinished() } //=========================================================================== +/** + * \brief 刷新运动状态，运动命令发出后，通过EF3板判断是否运动停止，读脉冲数进行到位轴判断（是否超时），欧式距离判断（是否超时），发送运动完成。 + */ void HSI_Motion::UpdateMotionState() { if (g_pHSI_Motion && m_IsExMotion == 0) @@ -3802,50 +3852,53 @@ void HSI_Motion::UpdateMotionState() bool interpolationflag = false; bool timeoutflag = false; int timeStart = GetTickCount(); - // bool singleaxisflag_x = false; - // bool singleaxisflag_y = false; - // bool singleaxisflag_z = false; - // bool singleaxisflag_a = false; - do - { - if (g_IsClose) - { - g_IsClose = false; - break; - } - if (m_SO7_Serial.m_RecvData[0] == 2) - { - if ((m_SO7_Serial.m_RecvData[39] & 0x10)) - { - m_SO7_Serial.m_RecvData[39] = 0; - SpCompleteTStart = GetTickCount(); - interpolationflag = true; - break; - } - //else if ((m_SO7_Serial.m_RecvData[39] == 0x07))//axis_start - if ((m_SO7_Serial.m_RecvData[39] == axis_start)/* &&(m_motorType == 0)*/) - { - m_SO7_Serial.m_RecvData[39] = 0; - SpCompleteTStart = GetTickCount(); - interpolationflag = true; - break; - } - if (m_SO7_Serial.m_RecvData[39] & 0x20) - { - m_SO7_Serial.m_RecvData[39] = 0; - timeoutflag = true; - break; - } - } - Sleep(1); - int timeEnd = GetTickCount(); - if (timeStart - timeEnd > 10 * 1000) - { - timeoutflag = true; - break; - } - } - while (true); + + //do + //{ + // if (g_IsClose) + // { + // g_IsClose = false; + // break; + // } + // // + + // printf("wait for motion end\n"); + // acsc_WaitMotionEnd(handleACS, ACSC_AXIS_0, INFINITE); + // printf("motion end\n"); + + // if (m_SO7_Serial.m_RecvData[0] == 2) + // { + // if ((m_SO7_Serial.m_RecvData[39] & 0x10)) + // { + // m_SO7_Serial.m_RecvData[39] = 0; + // SpCompleteTStart = GetTickCount(); + // interpolationflag = true; + // break; + // } + // //else if ((m_SO7_Serial.m_RecvData[39] == 0x07))//axis_start + // if ((m_SO7_Serial.m_RecvData[39] == axis_start)/* &&(m_motorType == 0)*/) + // { + // m_SO7_Serial.m_RecvData[39] = 0; + // SpCompleteTStart = GetTickCount(); + // interpolationflag = true; + // break; + // } + // if (m_SO7_Serial.m_RecvData[39] & 0x20) + // { + // m_SO7_Serial.m_RecvData[39] = 0; + // timeoutflag = true; + // break; + // } + // } + // Sleep(1); + // int timeEnd = GetTickCount(); + // if (timeStart - timeEnd > 10 * 1000) + // { + // timeoutflag = true; + // break; + // } + //} + //while (true); g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Nowait Run End\n"); unsigned int tempPrecision[5] = { 0, m_precisionCount[1], m_precisionCount[2], m_precisionCount[3], m_precisionCount[4] @@ -3952,7 +4005,7 @@ void HSI_Motion::UpdateMotionState() m_Thread_State = HSI_THREAD_PAUSED; CurrentMotionState = E_SO7_MOTION_NONE; m_IsExMotion = 2; - SendMsgMotionFinished(); + SendMsgMotionFinished();//定位完成 break; } default: @@ -3966,6 +4019,180 @@ void HSI_Motion::UpdateMotionState() } } +void HSI_Motion::UpdateMotionStateOld() +{ + if (g_pHSI_Motion && m_IsExMotion == 0) + { + g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] In\n"); + while (m_Thread_State == HSI_THREAD_RUNNING) + { + g_IsClose = false; + bool interpolationflag = false; + bool timeoutflag = false; + int timeStart = GetTickCount(); + // bool singleaxisflag_x = false; + // bool singleaxisflag_y = false; + // bool singleaxisflag_z = false; + // bool singleaxisflag_a = false; + do + { + if (g_IsClose) + { + g_IsClose = false; + break; + } + if (m_SO7_Serial.m_RecvData[0] == 2) + { + if ((m_SO7_Serial.m_RecvData[39] & 0x10)) + { + m_SO7_Serial.m_RecvData[39] = 0; + SpCompleteTStart = GetTickCount(); + interpolationflag = true; + break; + } + //else if ((m_SO7_Serial.m_RecvData[39] == 0x07))//axis_start + if ((m_SO7_Serial.m_RecvData[39] == axis_start)/* &&(m_motorType == 0)*/) + { + m_SO7_Serial.m_RecvData[39] = 0; + SpCompleteTStart = GetTickCount(); + interpolationflag = true; + break; + } + if (m_SO7_Serial.m_RecvData[39] & 0x20) + { + m_SO7_Serial.m_RecvData[39] = 0; + timeoutflag = true; + break; + } + } + Sleep(1); + int timeEnd = GetTickCount(); + if (timeStart - timeEnd > 10 * 1000) + { + timeoutflag = true; + break; + } + } while (true); + g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Nowait Run End\n"); + unsigned int tempPrecision[5] = { + 0, m_precisionCount[1], m_precisionCount[2], m_precisionCount[3], m_precisionCount[4] + }; + tempPrecision[1] = m_Home_Machine_Axis[1] == 0 ? 10000000 : m_precisionCount[1]; //用于启动时需要回原点的轴号选择,读取回家误差脉冲数 + tempPrecision[2] = m_Home_Machine_Axis[2] == 0 ? 10000000 : m_precisionCount[2]; + tempPrecision[3] = m_Home_Machine_Axis[3] == 0 ? 10000000 : m_precisionCount[3]; + tempPrecision[4] = m_Home_Machine_Axis[4] == 0 ? 10000000 : m_precisionCount[4]; + int i = 0, j = 0; + unsigned long Count = 0; + double prfpos[5] = { 0 }; + + //GetPositionEncPrfMulti(1, m_EncPos, m_PrfPos, 4); + //double EncPos[5] = { 0.0 }; + // double ProPulse[5] = { 0.0 }; + if (interpolationflag && m_motorType) + { + while (Count < m_SetPotion_Count[1]) //到位次数判断 + { + Sleep(2); + GetPositionXyz(HSI_MOTION_AXIS_ALL, prfpos[1], prfpos[2], prfpos[3], prfpos[0]); + //目标位置 和当前位置 小于回家误差脉冲数 + if ((fabs(m_PosThread[1] - prfpos[1]) <= tempPrecision[1] * m_Resolution[1]) && ( + fabs(m_PosThread[2] - prfpos[2]) <= tempPrecision[2] * m_Resolution[2]) && ( + fabs(m_PosThread[3] - prfpos[3]) <= tempPrecision[3] * m_Resolution[3]) && (fabs( + m_PosThread[4] - m_PosForAllAxis[4]) <= tempPrecision[4] * m_Resolution[4])) + { + i++; + if (m_SetPotion_Count[1] > m_setPositionNum) + { + j = m_setPositionNum; + } + else + { + j = m_SetPotion_Count[1]; + } + if (i == j) + { + set_end = GetTickCount(); + break; + } + } + else + { + } + Count++; + g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] m_SetPotion_Count = %d\n", Count); //打印到位轴数量 + } + //if (Count == m_SetPotion_Count[1]) //超时退出 + //{ + // if (g_IsClose == false) + // { + // g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Nowait Timeout\n"); + // sEvenProp.Init(); + // sEvenProp.EventType = HSI_EVENT_ERROR; + // sEvenProp.EventID = HSI_EVENT_MOTION; + // sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK; + // strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "Nowait_HSI定位超时!"); + // EventCallback(sEvenProp); + // } + // else + // { + // g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Nowait Timeout Is AbortMotion\n"); + // } + //} + } + if (interpolationflag) + { + SpCompleteTEnd = GetTickCount(); + SpTimeCount = SpCompleteTEnd - SpCompleteTStart; + double dis = pow(m_PrfPos[1] - m_EncPos[1], 2.0) + pow(m_PrfPos[2] - m_EncPos[2], 2.0) + pow( + m_PrfPos[3] - m_EncPos[3], 2.0); + if (dis > 0) + { + PntToPntDistance = sqrt(dis); //欧氏距离 + } + else + { + PntToPntDistance = 0.0; + } + SetPotionRunEnd = true; + } + g_pLogger->SendAndFlushWithTime( + L"[UpdateMotionState] m_PosThread[1] = %.4f,m_PosThread[2] = %.4f,m_PosThread[3] = %.4f\n", + m_PosThread[1], m_PosThread[2], m_PosThread[3]); + g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Pos[1] = %.4f,Pos[2] = %.4f,Pos[3] = %.4f\n", + prfpos[1], prfpos[2], prfpos[3]); + if (timeoutflag) //定位超时 + { + g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Nowait Timeout\n"); + sEvenProp.Init(); + sEvenProp.EventType = HSI_EVENT_ERROR; + sEvenProp.EventID = HSI_EVENT_MOTION; + sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK; + strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "Nowait_EF3定位超时!"); + EventCallback(sEvenProp); + } + switch (CurrentMotionState) + { + case E_SO7_MOTION_MOVETO: + { + g_pLogger->SendAndFlushWithTime( + L"[UpdateMotionState] Nowait CurrentMotionState E_SO7_MOTION_MOVETO\n"); + m_Thread_State = HSI_THREAD_PAUSED; + CurrentMotionState = E_SO7_MOTION_NONE; + m_IsExMotion = 2; + SendMsgMotionFinished(); + break; + } + default: + { + g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Nowait CurrentMotionState default\n"); + break; + } + } + } + g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Out\n"); + } +} + //=========================================================================== void HSI_Motion::UpdateMotionStateEx() { diff --git a/HSI_HexagonMI_EF3/HSI_Motion.h b/HSI_HexagonMI_EF3/HSI_Motion.h index 3210f9d..daefed5 100644 --- a/HSI_HexagonMI_EF3/HSI_Motion.h +++ b/HSI_HexagonMI_EF3/HSI_Motion.h @@ -538,7 +538,12 @@ public: static unsigned __stdcall m_Thread(LPVOID pThis); void CreateThread(); void CloseThread(); + /** + * \brief + * + */ void UpdateMotionState(); + void UpdateMotionStateOld(); void UpdateMotionStateEx(); void GluedispenserDone(); diff --git a/HSI_HexagonMI_EF3/logger.cpp b/HSI_HexagonMI_EF3/logger.cpp index dd0e1a7..eb5831f 100644 --- a/HSI_HexagonMI_EF3/logger.cpp +++ b/HSI_HexagonMI_EF3/logger.cpp @@ -131,7 +131,17 @@ void CLogger::SendAndFlushPerMode(LPCTSTR format, ...) //} //删除当前目录下txt文件 -void CLogger::DeleteOldFile() +void DeleteOldFile() { +} + +string convertToString(char* a, int size) +{ + int i; + string s = ""; + for (i = 0; i < size; i++) { + s = s + a[i]; + } + return s; } \ No newline at end of file diff --git a/HSI_HexagonMI_EF3/logger.h b/HSI_HexagonMI_EF3/logger.h index e71a116..14f50db 100644 --- a/HSI_HexagonMI_EF3/logger.h +++ b/HSI_HexagonMI_EF3/logger.h @@ -3,12 +3,14 @@ #include #include +#include #include #include #include #include #include +using namespace std; const long LOGINIT = 0x0001; const long LOGACTIONS = 0x0002; const long LOGCOMM = 0x0004; @@ -51,7 +53,8 @@ public: void SendAndFlush(LPCTSTR, ...); void SendAndFlushPerMode(LPCTSTR, ...); void SendAndFlushWithTime(LPCTSTR, ...); - void CLogger::DeleteOldFile(); + void DeleteOldFile(); + bool IsEnabledLog/* = false*/; //鏄惁鍚敤鏃ュ織 CString m_FileName; @@ -62,4 +65,5 @@ public: CRITICAL_SECTION m_lockLogger; }; +extern string convertToString(char* a, int size); #endif // !defined(LOGGER_H__5142BB38_5565_4124_88A4_56EA08298154__INCLUDED_) diff --git a/HSI_HexagonMI_EF3/version.h b/HSI_HexagonMI_EF3/version.h index 261fb7b..4146657 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.14 / 11:53 " -#define HSI_FILE_CSDESCRIPTION _T("2022.10.14 / 11:53 ") +#define HSI_FILE_DESCRIPTION "2022.10.17 / 20:19 " +#define HSI_FILE_CSDESCRIPTION _T("2022.10.17 / 20:19 ") diff --git a/HSI_SEVENOCEAN_EF1_CsTest/HSI/HSI.cs b/HSI_SEVENOCEAN_EF1_CsTest/HSI/HSI.cs index a15c576..6467468 100644 --- a/HSI_SEVENOCEAN_EF1_CsTest/HSI/HSI.cs +++ b/HSI_SEVENOCEAN_EF1_CsTest/HSI/HSI.cs @@ -68,7 +68,7 @@ namespace HSI_SEVENOCEAN_EF1_CsTest.HSI var eventFunctionId = (Def.HSI_EVENT_FUNCTION_ID)eventId; switch (eventFunctionId) { - case Def.HSI_EVENT_FUNCTION_ID.HSI_EVENT_MOTION_DCC_HOME: + case Def.HSI_EVENT_FUNCTION_ID.HSI_EVENT_MOTION_DCC_HOME://准备回家 { MessageBox.Show(Resources.Interface_Tips_Home_Machine, Resources.Interface_Tips, msgBtn, MessageBoxIcon.Information); @@ -80,7 +80,7 @@ namespace HSI_SEVENOCEAN_EF1_CsTest.HSI msgBtn, MessageBoxIcon.Information); break; } - case Def.HSI_EVENT_FUNCTION_ID.HSI_EVENT_MOVE_POINT: + case Def.HSI_EVENT_FUNCTION_ID.HSI_EVENT_MOVE_POINT: //定位完成 { MessageBox.Show(Resources.Interface_Tips_Motion_Finished, Resources.Interface_Tips, msgBtn, MessageBoxIcon.Information); diff --git a/HSI_SEVENOCEAN_EF1_CsTest/HSI_SEVENOCEAN_EF1_CsTest.csproj b/HSI_SEVENOCEAN_EF1_CsTest/HSI_SEVENOCEAN_EF1_CsTest.csproj index 5640e49..a4819e1 100644 --- a/HSI_SEVENOCEAN_EF1_CsTest/HSI_SEVENOCEAN_EF1_CsTest.csproj +++ b/HSI_SEVENOCEAN_EF1_CsTest/HSI_SEVENOCEAN_EF1_CsTest.csproj @@ -121,6 +121,7 @@ ResXFileCodeGenerator Resources.Designer.cs + Designer diff --git a/HSI_SEVENOCEAN_EF1_CsTest/Program.cs b/HSI_SEVENOCEAN_EF1_CsTest/Program.cs index b1fbccc..ef78ff7 100644 --- a/HSI_SEVENOCEAN_EF1_CsTest/Program.cs +++ b/HSI_SEVENOCEAN_EF1_CsTest/Program.cs @@ -66,7 +66,8 @@ namespace HSI_SEVENOCEAN_EF1_CsTest dPos[1] += 10.0; rStatus = Motion.SetPositionXyz(Def.HSI_MOTION_AXIS_ALL, dPos[0], dPos[1], dPos[2], Def.HSI_MOTION_MOVE_TYPE.HSI_MOTION_MOVE_NOWAIT, 0.0); - + Console.WriteLine("Motion.SetPositionXyz:{0},{1},{2} {3}", dPos[0], dPos[1], dPos[2],rStatus); + break; case ConsoleKey.Enter: rStatus = Motion.GetPositionXyz(Def.HSI_MOTION_AXIS_ALL, ref dPos[0], ref dPos[1],