// HSI_Motion.cpp : 定义 DLL 的初始化例程。 #include "stdafx.h" #include "HSI.h" #include "HSI_Sevenocean_EF3.h" #include "HSI_Motion.h" #include #include "logger.h" #include "SevenOcean/CMMIO_SERIAL.h" #include "version.h" #include #include #include #include #include #include #include "windows.h" #include "ACS/ACSC.h" //引入ACS运动控制卡头文件 using namespace std; #ifdef _DEBUG #define new DEBUG_NEW #endif //=========================================================================== HSI_Motion* g_pHSI_Motion = nullptr; CLogger extern* g_pLogger = nullptr; HANDLE hCom; //串口句柄 HANDLE handleACS; // 运动控制句柄 const int WSA_MAJOR_VERSION = 2; const int WSA_MINOR_VERSION = 2; #define WSA_VERSION MAKEWORD(WSA_MAJOR_VERSION, WSA_MINOR_VERSION) #define SOCKADDR_LEN sizeof(SOCKADDR_IN) //=========================================================================== HANDLE HSI_Motion::m_Thread_Id = nullptr; HANDLE HSI_Motion::m_Thread_Mutex = nullptr; HANDLE HSI_Motion::g_RW_Data_Mutex = nullptr; HANDLE HSI_Motion::g_WR_ToMove_Mutex = nullptr; HANDLE HSI_Motion::g_Lock_JogAndTrigger = nullptr; HANDLE HSI_Motion::m_hTriggerEvent; HANDLE HSI_Motion::m_Thread_IdIO = nullptr; HANDLE HSI_Motion::m_Thread_MutexIO = nullptr; HANDLE HSI_Motion::m_hTriggerEventIO; HANDLE HSI_Motion::m_Thread_IdData = nullptr; HANDLE HSI_Motion::m_Thread_MutexData = nullptr; HANDLE HSI_Motion::m_hTriggerEventData; HANDLE HSI_Motion::m_Thread_IdProbe = nullptr; HANDLE HSI_Motion::m_Thread_MutexProbe = nullptr; HANDLE HSI_Motion::m_hTriggerEventProbe; HANDLE HSI_Motion::m_Thread_IdJOGStop = nullptr; HANDLE HSI_Motion::m_Thread_MutexJOGStop = nullptr; HANDLE HSI_Motion::m_hTriggerEventJOGStop; int HSI_Motion::m_Thread_StateJOGStop = THREAD_PAUSED; int HSI_Motion::m_Thread_State = THREAD_PAUSED; int HSI_Motion::m_Thread_StateIO = THREAD_PAUSED; int HSI_Motion::m_Thread_StateData = THREAD_PAUSED; int HSI_Motion::m_Thread_StateProbe = THREAD_PAUSED; int HSI_Motion::bRunGlueDispenser = THREAD_PAUSED; //=========================================================================== HANDLE HSI_Motion::m_ThreadTCP_Id = nullptr; int HSI_Motion::m_ThreadTCP_State = TCPIP_THREAD_PAUSED; SOCKET m_socket[4] = {0}; void HSI_Motion::ErrorsHandler() { if (handleACS != ACSC_INVALID) { char ErrorStr[256]; int ErrorCode = 0; int Received = 0; ErrorCode = GetLastError(); if (acsc_GetErrorString(handleACS, // communication handle ErrorCode, // error code ErrorStr, // buffer for the error explanation 255, // available buffer length &Received // number of actually received bytes )) { ErrorStr[Received] = '\0'; printf("Motion Error: %d [%s]\n", ErrorCode, ErrorStr); g_pLogger->SendAndFlushWithTime(L"[ACS Motion] Motion Error,Code: %d, %S\n", ErrorCode, ErrorStr); //向上层发送错误信息 sEvenProp.Init(); sEvenProp.EventType = HSI_EVENT_FUNCTION; sEvenProp.EventID = HSI_EVENT_DEBUG_LOG; sEvenProp.EventResponse = HSI_EVENT_FUNCTION_FAILED; EventCallback(sEvenProp); g_pLogger->SendAndFlushWithTime(L"[ACS Motion] Motion Error Event Set\n"); } } else { g_pLogger->SendAndFlushWithTime(L"[ACS Motion] ACS Communication handle is invalid\n"); } }; //=========================================================================== //运动类构造函数，涉及 运动控制参数、插补、回家、摇杆、日志等配置信息的加载 HSI_Motion::HSI_Motion() { TRACE0("HSI_Motion Constructor!\n"); sEvenProp.Init(); sEvenProp.EventCallbackID = 0; sEvenProp.EventResponse = HSI_EVENT_FUNCTION_OK; CurrentMotionState = E_SO7_MOTION_NONE; CurrentReadDataType = E_DATA_TYPE_NONE; GluerunCount = 0; m_IsExMotion = 2; //0是xyz用的，1是单轴用的，2都不用 g_IsClose = false; //用于DoEvents()的退出，而不异常 setLightFlag = false; m_bISUseMoreLights = 0; //更多灯光 m_Jog_Auto_Focus = 1; //变焦使用的速度 m_StartInputPort = 1; //外部输入按钮启动程序 iaxisNum = 0; iScanMotionType = 0; iTriggleNum = 0; iMotionDirection = 1; m_InputStatus = 0; m_ForStatus = 0; m_setPositionDelay = 0; //设置定位超时 m_setPositionPrecision = 0; //设置定位精度 m_IsUse_HSICompensation = 0; //是否启用HSI进行定位补偿 0为不启用 1为启用 默认为0 m_Compensation_Pluse = 20; //补偿脉冲数 m_IsHardLimit = 7; //设备轴硬限位设置 为0表示所有轴都为软限位 1为X轴为硬限位 2为Y轴 3为XY轴 4为Z轴 7为XYZ轴 默认为7 m_IsEnableAxis = 7; //设备启用轴设置 为0表示所有轴不启用 1为X轴启用 2为Y轴启用 3为XY轴启用 4为Z轴启用 7为XYZ轴启用 默认为7 m_IsHavePattern = 15; //是否有光栅 m_IsUseExternalTrigger = 1; //是否启用外触发功能 0为不启用 1为启用 默认为1 m_IsUseSixRingEightArea = 0; //是否启用六环八区灯功能 0为不启用 1为启用 2为二环八区灯 默认为0 m_IsUseTwentySixLight = 0; //是否启用26路灯光 0为不启用 1位启用 默认为0 m_IsUseEF3 = 0; //是否启用EF3 m_IsUseACS = 0; //是否启用ACS m_IsUseRocker = 0; //是否启用摇杆 0为不启用 1为启用旧摇杆，2为新摇杆， 默认为0 m_IsCloseRocker = 0; m_DeviceType = 0; //设备类型，0为通用设备，1为三激光, 2为大视野，3为转盘设备 默认为0 m_iJoyStick = 0; //摇杆类型：0：无 1：老式摇杆 m_IsProbe = 0; m_ProbeAllAxis = 3; //探针触发时，锁存的轴号，默认3表示锁存XYZ共3轴，4表示XYZA共4轴 m_ProbeReturnPos = 10.0; //探针触发时，调试时返回的距离mm，点击启动按钮时不起作用,默认10.0mm m_ProbeReturnSpeed = 40; //探针触发后，轴的回退速度 m_IsHomeEncPos = 0; //是否启动实际位置判断是否回家，默认0，1启用，0关闭 m_IsHomePrfPos = 1; //是否启动规划位置判断是否回家，默认1，1启用，0关闭 m_IsIOFuntion = 0; //是否启动IO功能，1为打开，0为关闭 m_IsStartInput = 0; //是否启用脚踏开关功能，1为启用，0为关闭，默认0 m_IsUsePPS = false; m_MSTRunFlag = false; //MST软件运行标志，trueMST软件已经启动，falseMST软件停止 m_SendDataLength = 8; // 串口发送数据长度 m_LightType = 1; m_IsUseFourthSpeed = 0; m_ETIPort = 1; //外部触发拍照输入端口号 m_EF3LightType = 1; m_IbinCount = 0; //记录获取到的分bin数 m_IsUseJerk = 0; //是否启用急停 0为不启用 1为启用 t_start = 0; //获取jog运行的开始时间 t_end = 0; //获取jog运行的结束时间 m_isOKGlint = 0; //是否开启ok/ng 闪烁 m_selectedIndex = 0; m_setPositionNum = 5; m_axisStatus = 0; //运动各轴的状态 m_axisAlarmStatus = 0; //轴报警状态 m_EF3COMPort = 2; //EF3板com口，默认为2 m_AxisHomeDirection = 15; //轴回家时的方向 m_PositionA = 0.0; //第四轴的地位位置 m_ForSoft = 0; //针对使用软件 0为MST 1为Metus m_SaveAxisNum = 0; //保存轴号 m_SaveAxisSpeed = 0; //保存速度 bSaveSpeedFlag = false; m_IsUseManualRunin = 0; //是否开启手动插补（只针对步进电机） fourthAxisFlag = false; bCircleRun = false; //圆弧插补 m_UseAxisNum = 1; //转盘设备使用轴号 jogAxisNum = 0; //jog运动的轴号 jogDirFlag = false; jogMoving = false; jogspeed = 0; set_start = 0; //获取定位运行开始时间 set_end = 0; //获取定位运行结束时间 m_iSpeedType = 0; m_axisDirX = 0; //探针运动时X轴的运动方向 m_axisDirY = 0; //探针运动时Y轴的运动方向 m_axisDirZ = 0; //探针运动时Z轴的运动方向 m_probeSeekSpeed = 0; bUseGlueDispenser = false; //是否开启点胶 m_iGlueStartSpeed = 1; m_iGlueDriveSpeed = 1; m_iGlueAccSpeed = 1; GlueDispenserindexNum = 0; //点胶段数 m_isUseAport = 0; //A串口 m_portAnum = 0; m_isUseBport = 0; //B串口 m_portBnum = 0; m_bEmergencyState = false; SpCompleteTStart = 0; SpCompleteTEnd = 0; SpTimeCount = 0; SetPotionRunEnd = false; PntToPntDistance = 0.0; m_IsLightDebug = 0; //是否不回家也能调试灯光 0为不启用 1为启用 默认为0 for (int i = 0; i < 4; i++) { m_IsOpenTCPIP[i] = ""; //可提供的tcp通信的ip m_tcpCntFlag[i] = false; m_Led8MotionFlag[i] = false; //是否为8路光源板 } for (int i = 0; i < 4; i++) { m_rockerHStartSpeed[i] = 5; //摇杆XYZ轴高初始速度 m_rockerHDriveSpeed[i] = 20; //摇杆XYZ轴高驱动速度 m_rockerLStartSpeed[i] = 2; //摇杆XYZ轴低初始速度 m_rockerLDriveSpeed[i] = 10; //摇杆XYZ轴低驱动速度 m_rockerDSpeed[i] = 100; //XYZ轴加减速2 m_rockerASpeed[i] = 100; //XYZ轴加减速1 begin_position[i] = 0; //外触发到初始点需要发送的脉冲数 } for (size_t i = 0; i < 8; i++) { m_SixEightSubArea[i] = 0; //六环八区分区功能 } //日志处理 //DeleteDirectory() CTime tm = CTime::GetCurrentTime(); //CString csTime = tm.Format("%Y-%m-%d_%H-%M-%S"); //构造时间字符串 CString csTime = tm.Format("%Y-%m-%d"); //构造时间字符串 CString dir = L"\\Log\\" + csTime += L"_EF3.Log"; g_pLogger = new CLogger(dir); g_pLogger2 = new CLogger(L"\\Log\\EF3_SumTime.Log"); //档位参数 for (int i = 0; i < 5; i++) { for (int j = 0; j < 5; j++) { m_JogDriveSpeed[j][i] = 20; //表示5个档位 轴号,从1开始；5：档位 m_JogStartSpeed[j][i] = 20; m_JogAccLine[j][i] = 5; m_JogAccCurve[j][i] = 0; m_JogDecLine[j][i] = 5; m_JogDecCurve[j][i] = 0; } m_Home_Machine_Axis[i] = 1; //用于启动时需要回原点的轴号选择,赋值1表示该轴需要回家 m_Home_Pos_Axis[i] = 0; //记住关闭电源时的位置，用于判断是否还需要回原点 } m_Home_Machine_Axis[4] = 0; //用于启动时需要回原点的轴号选择 for (int i = 0; i < 5; i++) { m_N_Work_Limit[i] = -40; //负限位 m_P_Work_Limit[i] = 160; //正限位 m_Resolution[i] = 0.0004; //从1开始,0不用 m_Home_AddJogGears[i] = 4; m_Home_DecJogGears[i] = 3; m_SetPotion_StartSpeed[i] = 20; m_SetPotion_DriveSpeed[i] = 20; m_SetPotion_Line[i] = 150; m_SetPotion_Buffer[i] = 40; m_SetPotion_AccCurve[i] = 0; m_SetPotion_DecCurve[i] = 0; m_SpeedAdjustPeriod[i] = 1; m_SpeedMax[i] = 150; m_precisionCount[i] = 5; m_precisionTime[i] = 350; m_Home_Time[i] = 1500; m_SetPotion_Count[i] = 200; m_PosThread[i] = 0; //SetpositionXyz的目标位置 m_PosNow[i] = 0; //调用SetpositionXyz时，读取当前位置 m_LogIsOpen[i] = 0; //是否打开记录，0为打开，非0为关闭 m_StopJogMode[i] = 0; //JOG模式采用急停还是平滑停止 m_LockPos[i] = 0.0; m_EncPos[i] = 0; m_PrfPos[i] = 0; m_PosForAllAxis[i] = 0.0; //记录4轴位置 targetpos_n[i] = 0; targetpos_l[i] = 0; m_ProbeCapturePos[i] = 0; //锁存各轴的位置 iCircleRunPnt[i] = 0; //圆弧插补时的圆心位置 m_ijk[i] = 0; } //是否启用日志 GetAppPath(m_AppPath); m_LogIsOpen[0] = GetPrivateProfileInt(L"LOG", L"LOG_IS_OPEN_0", 0, m_AppPath + _T("\\Config\\EF3_Motion.ini")); g_pLogger->IsEnabledLog = m_LogIsOpen[0] == 1 ? true : false; g_pLogger->SendAndFlushWithTime(L"\n"); g_pLogger->SendAndFlushWithTime(L"==========================================================\n"); m_Set_XYZA_Reserve = 0; //XYZA轴方向 m_motorType = 0; //电机类型 1为伺服电机 0为步进电机 m_AxisHex[0] = 0; m_direct_pos = 0; m_AxisHex[1] = AXIS_X; m_AxisHex[2] = AXIS_Y; m_AxisHex[3] = AXIS_Z; m_AxisHex[4] = AXIS_U; for (int i = 0; i < 64; i++) { m_cSendData[i] = 0; } m_cSendData[0] = 2; m_cSendData[1] = 2; m_cSendData[2] = 2; m_cSendData[3] = 48; m_cSendData[4] = 4; m_cSendData[6] = 4; m_bConnected = false; m_bACSConnected = false; //初始化，默认打开失败 first = true; LightSend = 0; OrderSend = 0; IOSend = 0; for (int i = 0; i < 64; i++) { lightdata[i] = 0; IOdata[i] = 0; Orderdata[i] = 0; IOCheck[i] = 0; TempData[i] = 0; } IOCheck[0] = 0x01; IOCheck[1] = 0x04; memset(tReciveData, 0x00, TCPIP_MAX_DAT_SIZE); g_RW_Data_Mutex = CreateMutex(nullptr, FALSE, nullptr); g_WR_ToMove_Mutex = CreateMutex(nullptr, FALSE, nullptr); g_Lock_JogAndTrigger = CreateMutex(nullptr, FALSE, nullptr); } //=========================================================================== HSI_Motion::~HSI_Motion() { TRACE0("HSI_Motion Destructor!\n"); } //=========================================================================== bool HSI_Motion::PortInit(int iSerialComPort, int iBuadRate) { if (hCom == nullptr) { char buf[10]; sprintf_s(buf, "COM%d", iSerialComPort); CString comName(buf); hCom = CreateFile(comName, GENERIC_READ | GENERIC_WRITE, //允许读和写 0, //独占方式,串口必须为0 nullptr, OPEN_EXISTING, //打开而不是创建，串口必须为打开 0, //同步方式,同步执行时，函数直到操作完成后才返回 nullptr); //串口必须为NULL if (hCom != (HANDLE)-1) { PurgeComm(hCom, PURGE_TXCLEAR | PURGE_RXCLEAR); } SetupComm(hCom, 1024, 1024); DCB dcb; FillMemory(&dcb, sizeof(dcb), 0); dcb.DCBlength = sizeof(dcb); BuildCommDCB(L"2400,n,8,1", &dcb); if (!SetCommState(hCom, &dcb)) { return false; //Setting Error!!!! } //设置读写超时时间 COMMTIMEOUTS to; memset(&to, 0, sizeof(to)); to.ReadIntervalTimeout = 100; to.ReadTotalTimeoutMultiplier = 10; to.ReadTotalTimeoutConstant = 10; SetCommTimeouts(hCom, &to); PurgeComm(hCom, PURGE_TXCLEAR | PURGE_RXCLEAR); } return true; } //=========================================================================== HSI_STATUS HSI_Motion::IsSupported(UINT& Types) { auto rStatus = HSI_STATUS_NORMAL; Types = 1; return rStatus; } //=========================================================================== HSI_STATUS HSI_Motion::Startup(HWND _hWnd, bool _bOfflineOnly) { auto rStatus = HSI_STATUS_NORMAL; hWnd = _hWnd; bOfflineOnly = _bOfflineOnly; if (!bOfflineOnly) { CString GoogolMotionConfigFile(_T("")); if (!g_pHSI_Motion) { g_pHSI_Motion = new HSI_Motion(); } if (CurrentHomeMachineState == E_EF3_HOME_ING) //正在回家中 { g_pLogger->SendAndFlushWithTime(L"[Startup] Going Home\n"); return HSI_STATUS_NORMAL; } g_pLogger->SendAndFlushWithTime(L"[Startup] In\n"); g_pLogger->SendAndFlushWithTime(L"[Startup] HMQ HSI.dll version = %s, date = %s\n", HSI_VERSION_CSTRING, HSI_FILE_CSDESCRIPTION); //输出HSI.dll 版本号 GoogolMotionConfigFile = m_AppPath + _T("\\Config\\EF3_Config.ini"); Load_EF3_Config_Inifile(GoogolMotionConfigFile); //加载 EF3_Config.ini 配置项 GoogolMotionConfigFile = m_AppPath + _T("\\Config\\EF3_Motion.ini"); Load_EF3_Motion_Inifile(GoogolMotionConfigFile); // 加载 EF3_Motion.ini 配置项 //如果使用 EF3 if (m_IsUseEF3 == 1) { if (!m_bConnected) { //直线电机平台，采用串口通信，8个数据位，1个停止位，无奇偶校验，波特率256000 m_SO7_Serial.SetPort(m_EF3COMPort, 256000, 0, 8, 1, 0); //打开串口 #ifndef OFFLINE if (!m_SO7_Serial.Open()) //如果串口打开失败 { g_pLogger->SendAndFlushWithTime(L"[Startup] m_SO7_Serial.Open is false,connected failed\n"); sEvenProp.Init(); sEvenProp.EventType = HSI_EVENT_ERROR; sEvenProp.EventID = HSI_EVENT_MOTION; sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK; strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "EF3控制器打开失败,串口"); EventCallback(sEvenProp); return HSI_STATUS_FAILED; } #endif // OFFLINE m_SO7_Serial.SetTimeouts(1000, 1000); m_bConnected = true; g_pLogger->SendAndFlushWithTime(L"[Startup] Serial: [COM%d] is open success\n", m_EF3COMPort); } else { g_pLogger->SendAndFlushWithTime(L"[Startup] Serial is opened\n"); } } //如果启用ACS if (m_IsUseACS == 1) { if (!m_bACSConnected) { //尝试打开ACS控制器 g_pLogger->SendAndFlushWithTime(L"[ACS Motion] In\n"); g_pLogger->SendAndFlushWithTime( L"[ACS Motion] Wait for opening of communication with the controller... \n"); #ifdef OFFLINE //如果定义离线模式 handleACS = acsc_OpenCommSimulator(); #else // 10.0.0.100 - default IP address of the controller handleACS = acsc_OpenCommEthernet("100.0.0.100", ACSC_SOCKET_DGRAM_PORT); // for the connection to the controller via local network or Internet // hComm = acsc_OpenCommEthernet("10.0.0.100", ACSC_SOCKET_STREAM_PORT); if (handleACS == ACSC_INVALID) //打开失败 { g_pLogger->SendAndFlushWithTime(L"[ACS Motion] error while opening communication.\n"); ErrorsHandler(); sEvenProp.Init(); sEvenProp.EventType = HSI_EVENT_ERROR; sEvenProp.EventID = HSI_EVENT_MOTION; sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK; strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "ACS控制器打开失败"); EventCallback(sEvenProp); return HSI_STATUS_FAILED; } #endif //使能电机 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"); ErrorsHandler(); } else { g_pLogger->SendAndFlushWithTime(L"[ACS Motion] ACS Motors Enable Success!\n"); } m_bACSConnected = true; g_pLogger->SendAndFlushWithTime(L"[ACS Motion] ACS Initialization Success!\n"); //获取ACS 控制器版本号, 待测试，2.70 char Firmware[256]; int Received; if (!acsc_GetFirmwareVersion(handleACS, Firmware, 255, &Received, nullptr)) { g_pLogger->SendAndFlushWithTime(L"[ACS Motion] GET ACS Controller Version failed!\n"); ErrorsHandler(); } Firmware[Received - 1] = '\0'; //printf("%s", Firmware); //https://www.cnblogs.com/MichaelOwen/articles/2128771.html //g_pLogger->SendAndFlushWithTime(L"%s%s\n", _T("[ACS Motion] ACS Controller Version: "), L"你好"); g_pLogger->SendAndFlushWithTime(L"[ACS Motion] ACS Controller Version: %S\n", Firmware); //获取SPiiPlus C Library version unsigned int Ver = acsc_GetLibraryVersion(); /* printf("SPiiPlus C Library version is %d.%d.%d.%d\n", HIBYTE(HIWORD(Ver)), LOBYTE(HIWORD(Ver)), HIBYTE(LOWORD(Ver)), LOBYTE(LOWORD(Ver)));*/ g_pLogger->SendAndFlushWithTime(L"[ACS Motion] SPiiPlus C Library version is %d.%d.%d.%d\n", HIBYTE(HIWORD(Ver)), LOBYTE(HIWORD(Ver)), HIBYTE(LOWORD(Ver)), LOBYTE(LOWORD(Ver))); } else { g_pLogger->SendAndFlushWithTime( L"[ACS Motion] Communication with the controller is already established successfully!\n"); } //如果各个轴标志位 未回过家，需要回家 rStatus = g_pHSI_Motion->HomeMachine(TRUE); //执行回家 } // 04 05 0F 4A 04 00 04 00 00 00 00 00 00 00 00 00 // //01 01 0F 04 00 00 00 00 00 00 00 00 00 00 00 00 //01 01 0F 04 00 00 00 00 00 00 00 00 00 00 00 00 // //01 01 01 07 02 01 01 00 00 00 00 00 00 00 00 00 //01 09 01 01 01 07 07 00 00 00 07 04 00 00 00 00 // //01 01 01 05 01 07 07 00 00 00 07 04 00 00 00 00 //01 01 02 05 01 07 07 00 00 00 07 04 00 00 00 00 //01 01 04 05 01 07 07 00 00 00 07 04 00 00 00 00 //01 01 08 05 01 07 07 00 00 00 07 04 00 00 00 00 //01 01 01 06 01 07 07 00 00 00 07 04 00 00 00 00 //01 01 01 04 00 00 00 00 00 00 00 00 00 00 00 00 //01 01 01 02 69 00 00 00 E8 03 00 00 F4 01 00 00 //01 01 01 03 69 00 00 00 E8 03 00 00 F4 01 00 00 //01 01 02 04 00 00 00 00 00 00 00 00 F4 01 00 00 //01 01 02 02 00 00 00 00 F0 00 00 00 F4 01 00 00 //01 01 02 03 00 00 00 00 F0 00 00 00 F4 01 00 00 //01 01 04 04 00 00 00 00 00 00 00 00 F4 01 00 00 //01 01 04 02 00 00 00 00 F4 01 00 00 F4 01 00 00 //01 01 04 03 00 00 00 00 F4 01 00 00 F4 01 00 00 //01 06 07 53 01 0F 00 00 F4 01 00 00 F4 01 00 00 //AbortMotion(); //m_cSendData[0] = CT_ORDER;//停止运动 //m_cSendData[1] = CT_STOP; //m_cSendData[2] = AXIS_XYZU; //m_StopJogMode[1] == 0 ? m_cSendData[3] = 0X4A : m_cSendData[3] = 0X05; //m_StopJogMode[2] == 0 ? m_cSendData[3] = 0X4A : m_cSendData[3] = 0X05; //m_StopJogMode[3] == 0 ? m_cSendData[3] = 0X4A : m_cSendData[3] = 0X05; //m_StopJogMode[4] == 0 ? m_cSendData[3] = 0X4A : m_cSendData[3] = 0X05; //m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength); //Sleep(5); if (m_DeviceType == 3) //转盘设备,当前未使用 { m_cSendData[0] = CT_TURNTABLE; m_cSendData[1] = CT_RTSTOP; m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength); Sleep(5); m_Thread_StateData = HSI_THREAD_RUNNING; SetEvent(m_hTriggerEventData); } if (bUseGlueDispenser) //点胶，当前未使用 { bUseGlueDispenser = false; m_cSendData[0] = CT_MOTOR; m_cSendData[1] = CT_GLUEDISPENSER_STOP; m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength); Sleep(5); } g_pLogger->SendAndFlushWithTime(L"[Startup] Connected success\n"); if (HSI_STATUS_FAILED == DriverAlarmStatus()) { g_pLogger->SendAndFlushWithTime(L"[Startup] DriverAlarmStatus HSI_STATUS_FAILED\n"); //return HSI_STATUS_FAILED; } //无效软限位，当前未使用 //m_cSendData[0] = CT_MOTOR; //m_cSendData[1] = CT_MOTOR_SET; //m_cSendData[2] = AXIS_XYZU; //m_cSendData[3] = SOFT_LIMIT_POS_NEG; //m_cSendData[4] = 0; //m_cSendData[5] = 0; //m_cSendData[6] = 0; //m_cSendData[7] = 0; //m_cSendData[8] = 0; //m_cSendData[9] = 0; //m_cSendData[10] = 0; //m_cSendData[11] = 0; //m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength); //初始化防止第一次无效 //Sleep(5); //m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength); //g_pLogger->SendAndFlushWithTime(L"[Startup] Limit no Enable\n"); //设置方向4个轴的方向，按位 //m_cSendData[0] = CT_MOTOR; //m_cSendData[1] = CT_MOTOR_SET; //m_cSendData[2] = AXIS_X; //m_cSendData[3] = ENC_POS_DIR; //m_cSendData[4] = m_Set_XYZA_Reserve; //m_cSendData[5] = m_setPositionDelay; //m_cSendData[6] = m_setPositionPrecision; //m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength); //Sleep(10); //m_cSendData[0] = CT_MOTOR; //m_cSendData[1] = CT_MDATA_INIT; //m_cSendData[2] = m_motorType & 0xff; //电机类型（步进电机或伺服电机） //m_cSendData[3] = m_IsUseExternalTrigger; //是否启用外触发功能 //m_cSendData[4] = m_IsUseSixRingEightArea; //是否启用六环八区灯功能 //m_cSendData[5] = m_IsHardLimit; //m_cSendData[6] = m_IsEnableAxis; //m_cSendData[7] = m_IsProbe; //是否启用探针 //m_cSendData[8] = m_EF3LightType; //5V高频灯光配置 //m_cSendData[9] = m_IsUseRocker; //是否启用摇杆 //m_cSendData[10] = m_IsHavePattern; //光栅 //m_cSendData[11] = m_AxisHomeDirection; //轴回家方向 //m_cSendData[12] = m_IsCollectPos; //是否从串口打印位置 //m_cSendData[16] = m_IsLightDebug; //是否不回家也能调试灯光 //新摇杆,并启动脚踏功能 //if (m_IsStartInput == 1 && m_IsUseRocker == 2) //{ // m_cSendData[14] = m_StartInputPort >> 8 & 0xff; //外部启动端口号 H // m_cSendData[15] = m_StartInputPort & 0xff; //外部启动端口号 L //} //m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength); //Sleep(10); ////步进电机补偿值设定 //if (m_motorType == 0) //{ // m_cSendData[0] = CT_MOTOR; // m_cSendData[1] = CT_MDATA_COM; // for (int i = 1, j = 2; i < 5; i++) // { // m_cSendData[j++] = (m_SpeedMax[i] & 0xff); // m_cSendData[j++] = ((m_SpeedMax[i] >> 8) & 0xff); // m_cSendData[j++] = ((m_SpeedMax[i] >> 16) & 0xff); // m_cSendData[j++] = ((m_SpeedMax[i] >> 24) & 0xff); // } // m_WriteByte = Send_Command(0,(const char*)m_cSendData, m_SendDataLength); // g_pLogger->SendAndFlushWithTime(L"[Startup] Set Encoder Dir\n"); //} //多光源板 if (m_bISUseMoreLights > 0) { for (int i = 0; i < m_bISUseMoreLights; i++) { if (m_tcpCntFlag[i]) { continue; } USES_CONVERSION; char* ip = T2A(m_IsOpenTCPIP[i]); u_short port = 8088; TCPIP_RETURN_CODE rCode = TCPConnect(i, ip, port); if (rCode != TCPIP_CONNECT_OK) { g_pLogger->SendAndFlushWithTime(L"[Startup] m_SO7_Serial.Open is false,connected failed\n"); sEvenProp.Init(); sEvenProp.EventType = HSI_EVENT_ERROR; sEvenProp.EventID = HSI_EVENT_MOTION; sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK; strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "光源板打开网口失败"); EventCallback(sEvenProp); return HSI_STATUS_FAILED; } m_tcpCntFlag[i] = true; } Create_Thread(); for (int i = 0; i < m_bISUseMoreLights; i++) { if (m_Led8MotionFlag[i]) { m_selectedIndex = i; Orderdata[0] = 0x01; Orderdata[1] = 0x05; Orderdata[2] = m_LightType; OrderSend++; Orderdata[0] = 0x01; Orderdata[1] = 0x06; Orderdata[2] = 0x00; OrderSend++; } } } //摇杆速度设置 if (m_IsUseRocker == 1) { m_cSendData[0] = CT_MOTOR; m_cSendData[1] = CT_ROCKER; int j = 2; for (int i = 0; i < 3; i++) { m_cSendData[j++] = (m_rockerHStartSpeed[i] & 0xff); m_cSendData[j++] = ((m_rockerHStartSpeed[i] >> 8) & 0xff); m_cSendData[j++] = (m_rockerHDriveSpeed[i] & 0xff); m_cSendData[j++] = ((m_rockerHDriveSpeed[i] >> 8) & 0xff); m_cSendData[j++] = (m_rockerASpeed[i] & 0xff); m_cSendData[j++] = ((m_rockerASpeed[i] >> 8) & 0xff); } for (int i = 0; i < 3; i++) { m_cSendData[j++] = (m_rockerLStartSpeed[i] & 0xff); m_cSendData[j++] = ((m_rockerLStartSpeed[i] >> 8) & 0xff); m_cSendData[j++] = (m_rockerLDriveSpeed[i] & 0xff); m_cSendData[j++] = ((m_rockerLDriveSpeed[i] >> 8) & 0xff); m_cSendData[j++] = (m_rockerDSpeed[i] & 0xff); m_cSendData[j++] = ((m_rockerDSpeed[i] >> 8) & 0xff); } m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength); Sleep(5); g_pLogger->SendAndFlushWithTime(L"[Startup] Set Rocker Success\n"); } //摇杆2下载档位 if (m_IsUseRocker == 2) { SetAllGears(); Sleep(5); g_pLogger->SendAndFlushWithTime(L"[Startup] Set Gears Success\n"); } //刷新位置状态线程 CreateThread(); SetEvent(m_hTriggerEvent); m_Thread_State = HSI_THREAD_PAUSED; g_pLogger->SendAndFlushWithTime(L"[Startup] SetpositionXyz Enable\n"); //读取EF3数据状态线程 //CreateThreadData(); //SetEvent(m_hTriggerEventData); //m_Thread_StateData = HSI_THREAD_RUNNING; //g_pLogger->SendAndFlushWithTime(L"[Startup] Read EF3 Status Run\n"); //JOG运行到软限位的运动调节线程 //if (m_DeviceType != 3) //{ // CreateThreadJOGStop(); // SetEvent(m_hTriggerEventJOGStop); // m_Thread_StateJOGStop = HSI_THREAD_PAUSED; //} //是否启用IO功能线程，通过配置文件启用 if (m_IsIOFuntion == 1) { m_Thread_StateIO = HSI_THREAD_RUNNING; CreateThreadIO(); //IO发消息使用 SetEvent(m_hTriggerEventIO); g_pLogger->SendAndFlushWithTime(L"[Startup] m_IsIOFuntion Enable\n"); SetDIO(HSI_MOTION_OUTPUT_CH1, 0xfffff); } //是否启用探针捕获功能，通过配置文件启用 if (m_IsProbe == 1) { CreateThreadProbe(); //探针捕获功能 m_Thread_StateProbe = HSI_THREAD_RUNNING; SetEvent(m_hTriggerEventProbe); g_pLogger->SendAndFlushWithTime(L"[Startup] m_IsProbe Enable\n"); } g_pLogger->SendAndFlushWithTime(L"[Startup] Out\n"); //启用EF3锁存模式设置，定时模式 if (m_IsUseEF3 ) { //设置锁存频率 1秒钟（） m_cSendData[0] = 0x01; m_cSendData[1] = 0x01; m_cSendData[2] = 0x02; //m_cSendData[3] = 0x03; // 10000 （0x27 0x10）对应1秒，3-4字节表示锁存周期 //m_cSendData[4] = 0xE8; //m_cSendData[3] = 0x01; // 10000 （0x27 0x10）对应1秒，3-4字节表示锁存周期 //m_cSendData[4] = 0xF4; // 10000 （0x27 0x10）对应1秒，3-4字节表示锁存周期 m_cSendData[3] = (m_axisLatchFrequency >> 8) & 0xFF; m_cSendData[4] = m_axisLatchFrequency & 0xFF; m_WriteByte = Send_Command(0, (const char*)m_cSendData, 5, 2); Sleep(10); g_pLogger->SendAndFlushWithTime(L"[Startup] Set EF3 Timing latch Success\n"); } } else { g_pLogger->SendAndFlushWithTime(L"[Startup] No enable controller\n"); rStatus = HSI_STATUS_FAILED; } return rStatus; } //=========================================================================== /** * \brief 获取EF3固件版本 * \param version * \return */ HSI_STATUS HSI_Motion::GetFirmwareVersion(byte* version) { m_Thread_StateData = HSI_THREAD_PAUSED; Sleep(3); int waite_count = 0; unsigned char senddata[8] = {0}; //senddata[0] = 0x04; //senddata[1] = 0x03; senddata[0] = 0x01; senddata[1] = 0x08; m_SO7_Serial.m_RecvData[0] = 0; m_WriteByte = Send_Command(0, (const char*)senddata, 2, 20); Sleep(30); //m_SO7_Serial.m_RecvData[0] = 0; //m_WriteByte = Send_Command(0, (const char*)senddata, sizeof(senddata)); //Sleep(30); //m_SO7_Serial.m_RecvData[0] = 0; //m_WriteByte = Send_Command(0, (const char*)senddata, sizeof(senddata)); //Sleep(30); while ((m_SO7_Serial.m_RecvData[0] != 0x32) && (m_SO7_Serial.m_RecvData[1] != 0x30)) { waite_count++; if (waite_count > 100) break; Sleep(1); } if (waite_count > 100) { waite_count = 0; m_WriteByte = Send_Command(0, (const char*)senddata, 2, 20); //再次重发，并等待回应 while ((m_SO7_Serial.m_RecvData[0] != 0x32) && (m_SO7_Serial.m_RecvData[1] != 0x30)) { waite_count++; if (waite_count > 100) break; Sleep(1); } } if (waite_count > 100) { version[0] = 't'; version[1] = 'i'; version[2] = 'm'; version[3] = 'e'; version[4] = 'o'; version[5] = 'u'; version[6] = 't'; } else { for (int i = 0; i < 20; i++) { version[i] = m_SO7_Serial.m_RecvData[i]; } } g_pLogger->SendAndFlushWithTime(L"[GetFirmwareVersion] EF3 Firmware Version %S\n", version); m_Thread_StateData = HSI_THREAD_RUNNING; SetEvent(m_hTriggerEventData); //触发事件，其中hEvent表示句柄，返回值：如果操作成功，则返回非零值，否则为0。 return HSI_STATUS_NORMAL; } //=========================================================================== /** * \brief 回家 * \param bHomed * \return */ HSI_STATUS HSI_Motion::HomeMachineOld(bool bHomed) { auto rStatus = HSI_STATUS_NORMAL; if (!bHomed) { g_pLogger->SendAndFlushWithTime(L"[HomeMachine] bHomed No Need Reture\n"); return HSI_STATUS_NORMAL; } if (m_IsUseEF3 == 0) { return HSI_STATUS_NORMAL; } if (g_pHSI_Motion) { g_pLogger->SendAndFlushWithTime(L"[HomeMachine] In\n"); //判断是否需要回家 bool home(false); IsHomed(home); if (home == true) { g_pLogger->SendAndFlushWithTime(L"[HomeMachine] IsHomed No Need Go Home\n"); return HSI_STATUS_NORMAL; } sEvenProp.Init(); sEvenProp.EventType = HSI_EVENT_FUNCTION; sEvenProp.EventID = HSI_EVENT_MOTION_DCC_HOME; sEvenProp.EventResponse = HSI_EVENT_RESPONSE_CANCEL; EventCallback(sEvenProp); if (sEvenProp.EventCallbackID == HSI_EVENT_RESPONSE_CANCEL) { g_pLogger->SendAndFlushWithTime(L"[HomeMachine] Cancel\n"); return HSI_STATUS_NORMAL; } if (m_bEmergencyState) { AfxMessageBox(_T("急停或安全门或安全光幕触发！")); return HSI_STATUS_FAILED; } //根据当前位置，保存位置，求解相对运动 //回家后，设置正负限位 CurrentHomeMachineState = E_EF3_HOME_ING; int GetHomePos[5] = {0}; if (m_SO7_Serial.m_RecvData[0] == 2) { if (m_IsHavePattern & 0x01 == 0x01) GetHomePos[1] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial. m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]); else GetHomePos[1] = (m_SO7_Serial.m_RecvData[17] << 24 | m_SO7_Serial.m_RecvData[18] << 16 | m_SO7_Serial. m_RecvData[19] << 8 | m_SO7_Serial.m_RecvData[20]); if (m_IsHavePattern & 0x02) GetHomePos[2] = (m_SO7_Serial.m_RecvData[5] << 24 | m_SO7_Serial.m_RecvData[6] << 16 | m_SO7_Serial. m_RecvData[7] << 8 | m_SO7_Serial.m_RecvData[8]); else GetHomePos[2] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial. m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]); if (m_IsHavePattern & 0x04) GetHomePos[3] = (m_SO7_Serial.m_RecvData[9] << 24 | m_SO7_Serial.m_RecvData[10] << 16 | m_SO7_Serial. m_RecvData[11] << 8 | m_SO7_Serial.m_RecvData[12]); else GetHomePos[3] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial. m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]); if (m_IsHavePattern & 0x08) GetHomePos[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | m_SO7_Serial. m_RecvData[15] << 8 | m_SO7_Serial.m_RecvData[16]); else GetHomePos[4] = (m_SO7_Serial.m_RecvData[29] << 24 | m_SO7_Serial.m_RecvData[30] << 16 | m_SO7_Serial. m_RecvData[31] << 8 | m_SO7_Serial.m_RecvData[32]); //GetHomePos[1] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial.m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]); //GetHomePos[2] = (m_SO7_Serial.m_RecvData[5] << 24 | m_SO7_Serial.m_RecvData[6] << 16 | m_SO7_Serial.m_RecvData[7] << 8 | m_SO7_Serial.m_RecvData[8]); //GetHomePos[3] = (m_SO7_Serial.m_RecvData[9] << 24 | m_SO7_Serial.m_RecvData[10] << 16 | m_SO7_Serial.m_RecvData[11] << 8 | m_SO7_Serial.m_RecvData[12]); //GetHomePos[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | m_SO7_Serial.m_RecvData[15] << 8 | m_SO7_Serial.m_RecvData[16]); } //取消限位，设置初始速度，加减速等参数 if (HSI_STATUS_NORMAL != HomeFindIndex()) { return HSI_STATUS_FAILED; } int SavePos[5] = {0}; if (m_SO7_Serial.m_RecvData[0] == 2) { SavePos[1] = (m_SO7_Serial.m_RecvData[41] << 24 | m_SO7_Serial.m_RecvData[42] << 16 | m_SO7_Serial. m_RecvData[43] << 8 | m_SO7_Serial.m_RecvData[44]); SavePos[2] = (m_SO7_Serial.m_RecvData[45] << 24 | m_SO7_Serial.m_RecvData[46] << 16 | m_SO7_Serial. m_RecvData[47] << 8 | m_SO7_Serial.m_RecvData[48]); SavePos[3] = (m_SO7_Serial.m_RecvData[49] << 24 | m_SO7_Serial.m_RecvData[50] << 16 | m_SO7_Serial. m_RecvData[51] << 8 | m_SO7_Serial.m_RecvData[52]); SavePos[4] = (m_SO7_Serial.m_RecvData[53] << 24 | m_SO7_Serial.m_RecvData[54] << 16 | m_SO7_Serial. m_RecvData[55] << 8 | m_SO7_Serial.m_RecvData[56]); } double PrinfPos[5] = {0}; PrinfPos[1] = SavePos[1] * m_Resolution[1]; PrinfPos[2] = SavePos[2] * m_Resolution[2]; PrinfPos[3] = SavePos[3] * m_Resolution[3]; PrinfPos[4] = SavePos[4] * m_Resolution[4]; g_pLogger->SendAndFlushWithTime( L"[HomeMachine] SavePos[1] = %d,SavePos[2] = %d,SavePos[3] = %d,SavePos[4] = %d\n", SavePos[1], SavePos[2], SavePos[3], SavePos[4]); g_pLogger->SendAndFlushWithTime( L"[HomeMachine] PrinfPos[1] = %.4f,PrinfPos[2] = %.4f,PrinfPos[3] = %.4f,PrinfPos[4] = %.4f\n", PrinfPos[1], PrinfPos[2], PrinfPos[3], PrinfPos[4]); int LastPos[5] = {0}; if (m_SO7_Serial.m_RecvData[0] == 2) { LastPos[1] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial.m_RecvData[ 3] << 8 | m_SO7_Serial.m_RecvData[4]); LastPos[2] = (m_SO7_Serial.m_RecvData[5] << 24 | m_SO7_Serial.m_RecvData[6] << 16 | m_SO7_Serial.m_RecvData[ 7] << 8 | m_SO7_Serial.m_RecvData[8]); LastPos[3] = (m_SO7_Serial.m_RecvData[9] << 24 | m_SO7_Serial.m_RecvData[10] << 16 | m_SO7_Serial.m_RecvData [11] << 8 | m_SO7_Serial.m_RecvData[12]); LastPos[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | m_SO7_Serial. m_RecvData[15] << 8 | m_SO7_Serial.m_RecvData[16]); } m_Thread_State = HSI_THREAD_RUNNING; g_pLogger->SendAndFlushWithTime(L"[HomeMachine] SetPositionXyz\n"); double PrinfMovePos[5] = {0}; PrinfMovePos[1] = (GetHomePos[1] - SavePos[1]) * m_Resolution[1]; //求解相对运动位置 PrinfMovePos[2] = (GetHomePos[2] - SavePos[2]) * m_Resolution[2]; //相对运动，目标位置和现在位置求移动距离 PrinfMovePos[3] = (GetHomePos[3] - SavePos[3]) * m_Resolution[3]; PrinfMovePos[4] = (GetHomePos[4] - SavePos[4]) * m_Resolution[4]; g_pLogger->SendAndFlushWithTime( L"[HomeMachine] SetPos[1] = %.4fmm,SetPos[2] = %.4fmm,SetPos[3] = %.4fmm,SetPos[4] = %.4fmm\n", PrinfMovePos[1], PrinfMovePos[2], PrinfMovePos[3], PrinfMovePos[4]); if (m_DeviceType == 0 || m_DeviceType == 1) { for (short i = 1; i < 5; i++) { if (m_Home_Machine_Axis[i] == 0) { PrinfMovePos[i] = GetHomePos[i] * m_Resolution[i]; } } Sleep(20); SetPositionXyza(0, PrinfMovePos[1], PrinfMovePos[2], PrinfMovePos[3], PrinfMovePos[4], HSI_MOTION_MOVE_WAIT, 0); } CurrentHomeMachineState = E_EF3_HOME_FINISHED; for (int AxisTypes = 1; AxisTypes <= 8;) { if (CurrentHomeMachineState == E_EF3_HOME_FINISHED) { byte AxisNumber = static_cast(AxisConvertIndex(AxisTypes)); //HomeMachineOld m_cSendData[0] = CT_MOTOR; m_cSendData[1] = CT_MOTOR_SET; m_cSendData[2] = AxisTypes; m_cSendData[3] = SOFT_LIMIT_POS_NEG; m_cSendData[4] = static_cast(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) & 0xff; m_cSendData[5] = (static_cast(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 8) & 0xff; m_cSendData[6] = (static_cast(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 16) & 0xff; m_cSendData[7] = (static_cast(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 24) & 0xff; m_cSendData[8] = static_cast(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) & 0xff; m_cSendData[9] = (static_cast(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 8) & 0xff; m_cSendData[10] = (static_cast(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 16) & 0xff; m_cSendData[11] = (static_cast(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 24) & 0xff; m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength); Sleep(10); AxisTypes = 2 * AxisTypes; g_pLogger->SendAndFlushWithTime( L"[HomeMachine] Limit Enable, m_P_Work_Limit[AxisNumber] = %f,m_N_Work_Limit[AxisNumber] = %f\n", m_P_Work_Limit[AxisNumber], m_N_Work_Limit[AxisNumber]); } } //SetPositionXyz(0, -10,-10,-10, HSI_MOTION_MOVE_NOWAIT, 0); //Sleep(100); //SetPositionXyz(0, PrinfMovePos[1], PrinfMovePos[2], PrinfMovePos[3], HSI_MOTION_MOVE_NOWAIT, 0); g_pLogger->SendAndFlushWithTime(L"[HomeMachine] Out\n"); } return rStatus; } HSI_STATUS HSI_Motion::HomeMachine(bool bHomed) { auto rStatus = HSI_STATUS_NORMAL; g_pLogger->SendAndFlushWithTime(L"[HomeMachine] In\n"); if (!bHomed) { g_pLogger->SendAndFlushWithTime(L"[HomeMachine] bHomed No Need Reture\n"); return HSI_STATUS_NORMAL; } if (g_pHSI_Motion && (handleACS != ACSC_INVALID)) { //判断是否需要回家 bool home(false); IsHomed(home); if (home == true) { g_pLogger->SendAndFlushWithTime(L"[HomeMachine] IsHomed No Need Go Home\n"); return HSI_STATUS_NORMAL; } sEvenProp.Init(); sEvenProp.EventType = HSI_EVENT_FUNCTION; sEvenProp.EventID = HSI_EVENT_MOTION_DCC_HOME; sEvenProp.EventResponse = HSI_EVENT_RESPONSE_CANCEL; EventCallback(sEvenProp); if (sEvenProp.EventCallbackID == HSI_EVENT_RESPONSE_CANCEL) { g_pLogger->SendAndFlushWithTime(L"[HomeMachine] Cancel\n"); return HSI_STATUS_NORMAL; } if (m_bEmergencyState) { AfxMessageBox(_T("急停或安全门或安全光幕触发！")); return HSI_STATUS_FAILED; } CurrentHomeMachineState = E_EF3_HOME_ING; //正在回家中 //运行 ACS 控制器内 buffer6 自动回家动作 g_pLogger->SendAndFlushWithTime(L"[HomeMachine] RunBuffer 6 \n"); //回家后，启用正负限位 if (!acsc_RunBuffer(handleACS, ACSC_BUFFER_6, nullptr, ACSC_SYNCHRONOUS)) { g_pLogger->SendAndFlushWithTime(L"[HomeMachine] ACS Run Buffer %d error\n", ACSC_BUFFER_6); ErrorsHandler(); return HSI_STATUS_FAILED; } //等待运动结束 ，方式1 /* acsc_WaitMotionEnd(handleACS, ACSC_AXIS_1, INFINITE); g_pLogger->SendAndFlushWithTime(L"[HomeMachine] X homed\n"); acsc_WaitMotionEnd(handleACS, ACSC_AXIS_0, INFINITE); g_pLogger->SendAndFlushWithTime(L"[HomeMachine] Y homed\n"); acsc_WaitMotionEnd(handleACS, ACSC_AXIS_4, INFINITE); g_pLogger->SendAndFlushWithTime(L"[HomeMachine] Z homed\n");*/ //等待运动结束 ，方式2 do { //再次读取回家标志位，或者上个动作完成回调 IsHomed(home); Sleep(200); } while (!home); if (m_IsUseEF3) //启用EF3锁存功能，需要再回家完成后，设置锁存板的位置 { //回家后，将锁存板的位置设置为0 unsigned char m_cSendData[8] = { 0 }; m_cSendData[0] = 0x01; m_cSendData[1] = 0x06; m_WriteByte = Send_Command(0, (const char*)m_cSendData, 2); Sleep(5); //清除锁存板Flash区 m_cSendData[0] = 0x01; m_cSendData[1] = 0x04; m_WriteByte = Send_Command(0, (const char*)m_cSendData, 2); Sleep(5); } //回家表示改为1 m_Home_Machine_Axis[1] = 1; m_Home_Machine_Axis[0] = 1; m_Home_Machine_Axis[8] = 1; g_pLogger->SendAndFlushWithTime(L"[HomeMachine] Go Home success\n"); g_pLogger->SendAndFlushWithTime(L"[HomeMachine] Out\n"); } else { g_pLogger->SendAndFlushWithTime(L"[HomeMachine] faild, g_pHSI_Motion or handleACS not invaild! \n"); } return rStatus; } //=========================================================================== HSI_STATUS HSI_Motion::HomeJog(short AxisNumber, short Dir, bool Wait) { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { g_pLogger->SendAndFlushWithTime(L"[HomeJog] In\n"); g_pLogger->SendAndFlushWithTime(L"[HomeJog] Out\n"); } return rStatus; } //=========================================================================== /** * \brief 取消限位，设置初始速度，加减速等参数 * \return */ HSI_STATUS HSI_Motion::HomeFindIndex() { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { g_pLogger->SendAndFlushWithTime(L"[HomeFindIndex] In\n"); int AxisTypes = 1; int AxisAll = 0; g_IsClose = false; g_pLogger->SendAndFlushWithTime(L"[HomeFindIndex] Limit no Enable\n"); ZeroPos(true); for (short i = 1; i < 5; i++) { if (m_Home_Machine_Axis[i] == 1) { AxisTypes = IndexConvertAxis(i); //无效软限位 m_cSendData[0] = CT_MOTOR; m_cSendData[1] = CT_MOTOR_SET; m_cSendData[2] = AxisTypes; m_cSendData[3] = SOFT_LIMIT_POS_NEG; m_cSendData[4] = 0; m_cSendData[5] = 0; m_cSendData[6] = 0; m_cSendData[7] = 0; m_cSendData[8] = 0; m_cSendData[9] = 0; m_cSendData[10] = 0; m_cSendData[11] = 0; m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength); Sleep(10); int DriveSpeed(1); int StartSpeed(1); int AccLine(1); int DecLine(1); int AccCurve(1); int DecCurve(1); m_cSendData[0] = CT_MOTOR; m_cSendData[1] = CT_MOTOR_SET; m_cSendData[2] = AxisTypes; m_cSendData[3] = HOME_LIMIT_SPEED_ACC_DEC; HomeJogGearsChoice(i, m_Home_AddJogGears[i], DriveSpeed, StartSpeed, AccLine, DecLine, AccCurve, DecCurve); m_cSendData[4] = (StartSpeed & 0xff); m_cSendData[5] = ((StartSpeed >> 8) & 0xff); m_cSendData[6] = ((StartSpeed >> 16) & 0xff); m_cSendData[7] = ((StartSpeed >> 24) & 0xff); m_cSendData[8] = (DriveSpeed & 0xff); m_cSendData[9] = ((DriveSpeed >> 8) & 0xff); m_cSendData[10] = ((DriveSpeed >> 16) & 0xff); m_cSendData[11] = ((DriveSpeed >> 24) & 0xff); m_cSendData[12] = (AccCurve & 0xff); m_cSendData[13] = ((AccCurve >> 8) & 0xff); m_cSendData[14] = ((AccCurve >> 16) & 0xff); m_cSendData[15] = ((AccCurve >> 24) & 0xff); m_cSendData[16] = (AccLine & 0xff); m_cSendData[17] = ((AccLine >> 8) & 0xff); m_cSendData[18] = ((AccLine >> 16) & 0xff); m_cSendData[19] = ((AccLine >> 24) & 0xff); m_cSendData[20] = (DecCurve & 0xff); m_cSendData[21] = ((DecCurve >> 8) & 0xff); m_cSendData[22] = ((DecCurve >> 16) & 0xff); m_cSendData[23] = ((DecCurve >> 24) & 0xff); m_cSendData[24] = (DecLine & 0xff); m_cSendData[25] = ((DecLine >> 8) & 0xff); m_cSendData[26] = ((DecLine >> 16) & 0xff); m_cSendData[27] = ((DecLine >> 24) & 0xff); m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength); g_pLogger->SendAndFlushWithTime(L"[HomeFindIndex] m_Home_Speed_High Go Limit\n"); Sleep(10); m_cSendData[0] = CT_MOTOR; m_cSendData[1] = CT_MOTOR_SET; m_cSendData[2] = AxisTypes; m_cSendData[3] = HOME_ORG_SPEED_ACC_DEC; HomeJogGearsChoice(i, m_Home_DecJogGears[i], DriveSpeed, StartSpeed, AccLine, DecLine, AccCurve, DecCurve); m_cSendData[4] = (StartSpeed & 0xff); m_cSendData[5] = ((StartSpeed >> 8) & 0xff); m_cSendData[6] = ((StartSpeed >> 16) & 0xff); m_cSendData[7] = ((StartSpeed >> 24) & 0xff); m_cSendData[8] = (DriveSpeed & 0xff); m_cSendData[9] = ((DriveSpeed >> 8) & 0xff); m_cSendData[10] = ((DriveSpeed >> 16) & 0xff); m_cSendData[11] = ((DriveSpeed >> 24) & 0xff); m_cSendData[12] = (AccCurve & 0xff); m_cSendData[13] = ((AccCurve >> 8) & 0xff); m_cSendData[14] = ((AccCurve >> 16) & 0xff); m_cSendData[15] = ((AccCurve >> 24) & 0xff); m_cSendData[16] = (AccLine & 0xff); m_cSendData[17] = ((AccLine >> 8) & 0xff); m_cSendData[18] = ((AccLine >> 16) & 0xff); m_cSendData[19] = ((AccLine >> 24) & 0xff); m_cSendData[20] = (DecCurve & 0xff); m_cSendData[21] = ((DecCurve >> 8) & 0xff); m_cSendData[22] = ((DecCurve >> 16) & 0xff); m_cSendData[23] = ((DecCurve >> 24) & 0xff); m_cSendData[24] = (DecLine & 0xff); m_cSendData[25] = ((DecLine >> 8) & 0xff); m_cSendData[26] = ((DecLine >> 16) & 0xff); m_cSendData[27] = ((DecLine >> 24) & 0xff); m_cSendData[28] = (m_Home_Time[i] & 0xff); m_cSendData[29] = ((m_Home_Time[i] >> 8) & 0xff); m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength); g_pLogger->SendAndFlushWithTime(L"[HomeFindIndex] m_Home_Speed_High Go Limit and find index signal\n"); Sleep(10); AxisAll += (1 << (i - 1)); } } m_cSendData[0] = CT_MOTOR; m_cSendData[1] = CT_GOHOME; m_cSendData[2] = AxisAll; m_cSendData[3] = 0x53; m_cSendData[4] = m_motorType & 0xff; m_cSendData[5] = 0x0f; m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength); g_pLogger->SendAndFlushWithTime(L"[HomeFindIndex] Starting, AxisAll = %d\n", AxisAll); Sleep(1000); int Count = 0; do { DoEvents(); if (g_IsClose) { g_IsClose = false; return HSI_STATUS_FAILED; } Sleep(1); if (m_SO7_Serial.m_RecvData[0] == 2) { if (m_SO7_Serial.m_RecvData[38] & 16) { g_pLogger->SendAndFlushWithTime(L"[HomeFindIndex] Go Home Motion Done\n"); break; } } if (m_bEmergencyState) { AfxMessageBox(_T("急停或安全门或安全光幕触发！")); return HSI_STATUS_FAILED; } if (Count > 25000) { g_pLogger->SendAndFlushWithTime(L"[HomeFindIndex] Go Home Timeout HSI_STATUS_FAILED\n"); AbortMotion(); sEvenProp.EventType = HSI_EVENT_ERROR; sEvenProp.EventID = HSI_EVENT_MOTION; sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK; strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "回家超时"); EventCallback(sEvenProp); return HSI_STATUS_FAILED; } Count++; g_pLogger->SendAndFlushWithTime(L"[HomeFindIndex] Going Home Count=%d\n", Count); } while (true); char MessageHome[100] = ""; bool bHomed = true; if ((m_SO7_Serial.m_RecvData[38] & 0x01) == 0 && m_Home_Machine_Axis[1] == 1) { /* strcat_s 是系统的安全函数，微软在 2005 后建议用一系统所谓安全的函数，这中间就有 strcat_s 取代了 strcat ，原来 strcat 函数，没有方法来保证有效的缓冲区尺寸，所以它只能假定缓冲足够大来容纳要拷贝的字符串, 容易产生程序崩溃。而strcat_s函数能很好的规避这个问题*/ strcat_s(MessageHome, 30, "1、"); bHomed = false; } if ((m_SO7_Serial.m_RecvData[38] & 0x02) == 0 && m_Home_Machine_Axis[2] == 1) { strcat_s(MessageHome, 30, "2、"); bHomed = false; } if ((m_SO7_Serial.m_RecvData[38] & 0x04) == 0 && m_Home_Machine_Axis[3] == 1) { strcat_s(MessageHome, 30, "3、"); bHomed = false; } if ((m_SO7_Serial.m_RecvData[38] & 0x08) == 0 && m_Home_Machine_Axis[4] == 1) { strcat_s(MessageHome, 30, "4、"); bHomed = false; } if (!bHomed) { sEvenProp.Init(); sEvenProp.EventType = HSI_EVENT_ERROR; sEvenProp.EventID = HSI_EVENT_MOTION; sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK; strcat_s(MessageHome, 100, "轴回家失败!"); strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, MessageHome); EventCallback(sEvenProp); return HSI_STATUS_FAILED; } g_pLogger->SendAndFlushWithTime(L"[HomeFindIndex] Go Home Success\n"); g_pLogger->SendAndFlushWithTime(L"[HomeFindIndex] Out\n"); } return rStatus; } //=========================================================================== void HSI_Motion::HomeJogGearsChoice(int AxisTypes, int JogGears, int& DriveSpeed, int& StartSpeed, int& AccLine, int& DecLine, int& AccCurve, int& DecCurve) { DriveSpeed = m_JogDriveSpeed[AxisTypes][4 - JogGears]; StartSpeed = m_JogStartSpeed[AxisTypes][4 - JogGears]; AccLine = m_JogAccLine[AxisTypes][4 - JogGears]; DecLine = m_JogDecLine[AxisTypes][4 - JogGears]; AccCurve = m_JogAccCurve[AxisTypes][4 - JogGears]; DecCurve = m_JogDecCurve[AxisTypes][4 - JogGears]; } //=========================================================================== HSI_STATUS HSI_Motion::GetAppPath(CString& Path) { auto rStatus = HSI_STATUS_NORMAL; Path = _T(""); if (Path.IsEmpty()) { CString tmpPath; GetModuleFileName(nullptr, tmpPath.GetBuffer(255), 255); tmpPath.ReleaseBuffer(); tmpPath.TrimRight(); int nLastSlash = tmpPath.ReverseFind('\\'); if (nLastSlash >= 0) tmpPath = tmpPath.Left(nLastSlash); else tmpPath.Empty(); Path = tmpPath; } return rStatus; } //=========================================================================== HSI_STATUS HSI_Motion::IsHomedOld(bool& bHomed) { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { g_pLogger->SendAndFlushWithTime(L"[IsHomed] In\n"); short isHomed[5] = {1, 1, 1, 1, 1}; //int Count = 0; //if (bHomed == true)//定位是增大判断精度 //{ // Count = 1000; //} //所有轴都不需要回家 if (m_Home_Machine_Axis[1] == 0 && m_Home_Machine_Axis[2] == 0 && m_Home_Machine_Axis[3] == 0 && m_Home_Machine_Axis[4] == 0) { g_pLogger->SendAndFlushWithTime(L"[IsHomed] No Axis Go Home E_GTS_HOME_FINISHED\n"); CurrentHomeMachineState = E_EF3_HOME_FINISHED; bHomed = true; return HSI_STATUS_NORMAL; } //判断是否需要回家 int Delay = 0; while (m_SO7_Serial.m_RecvData[0] != 2) { DoEvents(); Sleep(1); if (m_SO7_Serial.m_RecvData[0] == 2) { g_pLogger->SendAndFlushWithTime(L"[IsHomed] normal break Delay = %d\n", Delay); break; } if (Delay > 300) { g_pLogger->SendAndFlushWithTime(L"[IsHomed] timeout break Delay = %d\n", Delay); break; } Delay++; } for (short i = 1; i < 5; i++) { if (m_Home_Machine_Axis[i] == 1) { if (m_SO7_Serial.m_RecvData[0] == 2) { isHomed[i] = (m_SO7_Serial.m_RecvData[38] & m_AxisHex[i] ? 1 : 0); } else { isHomed[i] = 0; } } } if (isHomed[1] == 1 && isHomed[2] == 1 && isHomed[3] == 1 && isHomed[4] == 1) { g_pLogger->SendAndFlushWithTime(L"[IsHomed] E_GTS_HOME_FINISHED\n"); CurrentHomeMachineState = E_EF3_HOME_FINISHED; bHomed = true; } else { g_pLogger->SendAndFlushWithTime(L"[IsHomed] Is No Go Home E_GTS_HOME_NONE\n"); CurrentHomeMachineState = E_EF3_HOME_NONE; bHomed = false; } g_pLogger->SendAndFlushWithTime(L"[IsHomed] Out\n"); } return rStatus; } HSI_STATUS HSI_Motion::IsHomed(bool& bHomed) { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion && handleACS != ACSC_INVALID) { g_pLogger->SendAndFlushWithTime(L"[IsHomed] In\n"); int isHomed[5] = {0, 1, 1, 1, 1}; //暂定只有一个回家标志位，即全部回家完成，没有按单个轴回家来看 //所有轴都不需要回家 if (m_Home_Machine_Axis[1] == 0 && m_Home_Machine_Axis[2] == 0 && m_Home_Machine_Axis[3] == 0 && m_Home_Machine_Axis[4] == 0) { g_pLogger->SendAndFlushWithTime(L"[IsHomed] No Axis Go Home E_GTS_HOME_FINISHED\n"); CurrentHomeMachineState = E_EF3_HOME_FINISHED; bHomed = true; return HSI_STATUS_NORMAL; } // 判断是否需要回家，读取ACS控制器回家标志位，来判断本次上电是否已经回过家 ， 1：已经回过家，0：未回过家 if (!acsc_ReadInteger(handleACS, ACSC_NONE, "YAW_HOME_DONE", ACSC_NONE, ACSC_NONE, ACSC_NONE, ACSC_NONE, isHomed, nullptr)) { g_pLogger->SendAndFlushWithTime(L"[IsHomed] ACS Read ISHOMED Flag Error\n"); ErrorsHandler(); CurrentHomeMachineState = E_EF3_HOME_NONE; rStatus = HSI_STATUS_FAILED; bHomed = false; } g_pLogger->SendAndFlushWithTime(L"[IsHomed] ACS Read YAW_HOME_DONE X:[%d] Y:[%d] Z:[%d]\n", isHomed[0], isHomed[1], isHomed[2]); //如果各个轴标志位 已经回过家 if (isHomed[0] == 1 && isHomed[1] == 1 && isHomed[2] == 1 && isHomed[3] == 1) { g_pLogger->SendAndFlushWithTime(L"[IsHomed] E_GTS_HOME_FINISHED\n"); CurrentHomeMachineState = E_EF3_HOME_FINISHED; bHomed = true; } else { g_pLogger->SendAndFlushWithTime(L"[IsHomed] Is No Go Home E_GTS_HOME_NONE\n"); CurrentHomeMachineState = E_EF3_HOME_NONE; bHomed = false; } g_pLogger->SendAndFlushWithTime(L"[IsHomed] Out\n"); } else { g_pLogger->SendAndFlushWithTime(L"[IsHomed] ACS Communication error\n"); CurrentHomeMachineState = E_EF3_HOME_NONE; rStatus = HSI_STATUS_FAILED; bHomed = false; } return rStatus; } //=========================================================================== HSI_STATUS HSI_Motion::ZeroPos(bool bZeroPos) { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { g_pLogger->SendAndFlushWithTime(L"[ZeroPos] In\n"); if (bZeroPos == true) { m_cSendData[0] = CT_MOTOR; m_cSendData[1] = CT_MOTOR_SET; m_cSendData[2] = AXIS_X; m_cSendData[3] = POS_CLEAR; Send_Command(0, (const char*)m_cSendData, m_SendDataLength); Sleep(8); m_cSendData[0] = CT_MOTOR; m_cSendData[1] = CT_MOTOR_SET; m_cSendData[2] = AXIS_Y; m_cSendData[3] = POS_CLEAR; Send_Command(0, (const char*)m_cSendData, m_SendDataLength); Sleep(8); m_cSendData[0] = CT_MOTOR; m_cSendData[1] = CT_MOTOR_SET; m_cSendData[2] = AXIS_Z; m_cSendData[3] = POS_CLEAR; Send_Command(0, (const char*)m_cSendData, m_SendDataLength); Sleep(8); m_cSendData[0] = CT_MOTOR; m_cSendData[1] = CT_MOTOR_SET; m_cSendData[2] = AXIS_U; m_cSendData[3] = POS_CLEAR; Send_Command(0, (const char*)m_cSendData, m_SendDataLength); Sleep(8); g_pLogger->SendAndFlushWithTime(L"[ZeroPos] All Pos ZeroPos\n"); } m_cSendData[0] = CT_MOTOR; m_cSendData[1] = CT_MOTOR_SET; m_cSendData[2] = AXIS_X; m_cSendData[3] = HOME_CLEAR; Send_Command(0, (const char*)m_cSendData, m_SendDataLength); g_pLogger->SendAndFlushWithTime(L"[ZeroPos] All HomeFlag ZeroPos\n"); g_pLogger->SendAndFlushWithTime(L"[ZeroPos] Out\n"); } return rStatus; } //=========================================================================== /** * \brief JOG模式 * \param AxisTypes 单轴 * \param Speed 速度，Speed > 0 正移动 * \return */ HSI_STATUS HSI_Motion::JogOld(UINT AxisTypes, double Speed) { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { if (m_DeviceType != 3) { m_Thread_StateJOGStop = HSI_THREAD_RUNNING; SetEvent(m_hTriggerEventJOGStop); } int DriveSpeed(1); int StartSpeed(1); int AccLine(1); int DecLine(1); int AccCurve(1); int DecCurve(1); int JogSpeed(1); bool bJOGDir = Speed > 0 ? true : false; byte AxisNumber = static_cast(AxisConvertIndex(AxisTypes)); //JogOld jogAxisNum = AxisNumber; jogDirFlag = bJOGDir; m_Thread_State = HSI_THREAD_PAUSED; if (CurrentHomeMachineState == E_EF3_HOME_FINISHED) { m_cSendData[0] = CT_MOTOR; m_cSendData[1] = CT_MOTOR_SET; m_cSendData[2] = AxisTypes; m_cSendData[3] = SOFT_LIMIT_POS_NEG; m_cSendData[4] = static_cast(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) & 0xff; //正限位 m_cSendData[5] = (static_cast(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 8) & 0xff; m_cSendData[6] = (static_cast(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 16) & 0xff; m_cSendData[7] = (static_cast(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 24) & 0xff; m_cSendData[8] = static_cast(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) & 0xff; //负限位 m_cSendData[9] = (static_cast(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 8) & 0xff; m_cSendData[10] = (static_cast(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 16) & 0xff; m_cSendData[11] = (static_cast(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 24) & 0xff; m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength); g_pLogger->SendAndFlushWithTime( L"[Jog] Limit Enable, m_P_Work_Limit[AxisNumber] = %f,m_N_Work_Limit[AxisNumber] = %f\n", m_P_Work_Limit[AxisNumber], m_N_Work_Limit[AxisNumber]); } else { //无效软限位 m_cSendData[0] = CT_MOTOR; m_cSendData[1] = CT_MOTOR_SET; m_cSendData[2] = AxisTypes; m_cSendData[3] = SOFT_LIMIT_POS_NEG; m_cSendData[4] = 0; m_cSendData[5] = 0; m_cSendData[6] = 0; m_cSendData[7] = 0; m_cSendData[8] = 0; m_cSendData[9] = 0; m_cSendData[10] = 0; m_cSendData[11] = 0; m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength); g_pLogger->SendAndFlushWithTime(L"[Jog] Limit No Enable\n"); } if (m_Home_Machine_Axis[AxisNumber] == 0) { return rStatus; } double now_pos[5] = {0}; double Prf_pos[5] = {0}; double limitpos[4] = {0}; int RemainPul = 0; int limitSDPul = 0; double time; /*GetPositionXyz(1, now_pos[0], now_pos[1], now_pos[2], time);*/ GetPositionEncPrfMulti(1, now_pos, Prf_pos, 1); JogSpeed = abs(SpeedPercent(AxisNumber, Speed, DriveSpeed, StartSpeed, AccLine, DecLine, AccCurve, DecCurve)); if (m_DeviceType != 3) { if (AxisTypes == AXIS_X && m_motorType & 0x01) { if (!bJOGDir) //负方向 { RemainPul = static_cast(now_pos[1] / m_Resolution[1]) - static_cast(m_N_Work_Limit[1] / m_Resolution[1]); limitSDPul = (DriveSpeed - StartSpeed) * 13; if ((RemainPul < limitSDPul * 2) && (RemainPul > 0)) { float SpeedRatio = limitSDPul * 2 / RemainPul; DriveSpeed = DriveSpeed / SpeedRatio; if (DriveSpeed < StartSpeed) { DriveSpeed = StartSpeed; } } } else { RemainPul = static_cast(m_P_Work_Limit[1] / m_Resolution[1]) - static_cast(now_pos[1] / m_Resolution[1]); limitSDPul = (DriveSpeed - StartSpeed) * 13; if ((RemainPul < limitSDPul * 2) && (RemainPul > 0)) { float SpeedRatio = limitSDPul * 2 / RemainPul; DriveSpeed = DriveSpeed / SpeedRatio; if (DriveSpeed < StartSpeed) { DriveSpeed = StartSpeed; } } } } else if (AxisTypes == AXIS_Y && m_motorType & 0x02) { if (!bJOGDir) //负方向 { RemainPul = static_cast(now_pos[2] / m_Resolution[2]) - static_cast(m_N_Work_Limit[2] / m_Resolution[2]); limitSDPul = (DriveSpeed - StartSpeed) * 13; if ((RemainPul < limitSDPul * 2) && (RemainPul > 0)) { float SpeedRatio = 1 + limitSDPul * 2 / RemainPul; DriveSpeed = DriveSpeed / SpeedRatio; if (DriveSpeed < StartSpeed) { DriveSpeed = StartSpeed; } } } else { RemainPul = static_cast(m_P_Work_Limit[2] / m_Resolution[2]) - static_cast(now_pos[2] / m_Resolution[2]); limitSDPul = (DriveSpeed - StartSpeed) * 13; if ((RemainPul < limitSDPul * 2) && (RemainPul > 0)) { float SpeedRatio = limitSDPul * 2 / RemainPul; DriveSpeed = DriveSpeed / SpeedRatio; if (DriveSpeed < StartSpeed) { DriveSpeed = StartSpeed; } } } } if (AxisTypes == AXIS_Z && m_motorType & 0x04) { if (!bJOGDir) //负方向 { RemainPul = static_cast(now_pos[3] / m_Resolution[3]) - static_cast(m_N_Work_Limit[3] / m_Resolution[3]); limitSDPul = (DriveSpeed - StartSpeed) * 13; if ((RemainPul < limitSDPul * 2) && (RemainPul > 0)) { float SpeedRatio = limitSDPul * 2 / RemainPul; DriveSpeed = DriveSpeed / SpeedRatio; if (DriveSpeed < StartSpeed) { DriveSpeed = StartSpeed; } } } else { RemainPul = static_cast(m_P_Work_Limit[3] / m_Resolution[3]) - static_cast(now_pos[3] / m_Resolution[3]); limitSDPul = (DriveSpeed - StartSpeed) * 13; if ((RemainPul < limitSDPul * 2) && (RemainPul > 0)) { float SpeedRatio = limitSDPul * 2 / RemainPul; DriveSpeed = DriveSpeed / SpeedRatio; if (DriveSpeed < StartSpeed) { DriveSpeed = StartSpeed; } } } } if (AxisTypes == AXIS_U && m_motorType & 0x08) { if (!bJOGDir) //负方向 { RemainPul = static_cast(now_pos[4] / m_Resolution[4]) - static_cast(m_N_Work_Limit[4] / m_Resolution[4]); limitSDPul = (DriveSpeed - StartSpeed) * 13; if ((RemainPul < limitSDPul * 2) && (RemainPul > 0)) { float SpeedRatio = limitSDPul * 2 / RemainPul; DriveSpeed = DriveSpeed / SpeedRatio; if (DriveSpeed < StartSpeed) { DriveSpeed = StartSpeed; } } } else { RemainPul = static_cast(m_P_Work_Limit[4] / m_Resolution[4]) - static_cast(now_pos[4] / m_Resolution[4]); limitSDPul = (DriveSpeed - StartSpeed) * 13; if ((RemainPul < limitSDPul * 2) && (RemainPul > 0)) { float SpeedRatio = limitSDPul * 2 / RemainPul; DriveSpeed = DriveSpeed / SpeedRatio; if (DriveSpeed < StartSpeed) { DriveSpeed = StartSpeed; } } } } } m_cSendData[0] = CT_MOTOR; m_cSendData[1] = CT_MOTOR_SET; m_cSendData[2] = AxisTypes; m_cSendData[3] = JOG_SPEED_ACC_DEC; m_cSendData[4] = (StartSpeed & 0xff); m_cSendData[5] = ((StartSpeed >> 8) & 0xff); m_cSendData[6] = ((StartSpeed >> 16) & 0xff); m_cSendData[7] = ((StartSpeed >> 24) & 0xff); m_cSendData[8] = (DriveSpeed & 0xff); m_cSendData[9] = ((DriveSpeed >> 8) & 0xff); m_cSendData[10] = ((DriveSpeed >> 16) & 0xff); m_cSendData[11] = ((DriveSpeed >> 24) & 0xff); m_cSendData[12] = (AccCurve & 0xff); m_cSendData[13] = ((AccCurve >> 8) & 0xff); m_cSendData[14] = ((AccCurve >> 16) & 0xff); m_cSendData[15] = ((AccCurve >> 24) & 0xff); m_cSendData[16] = (AccLine & 0xff); m_cSendData[17] = ((AccLine >> 8) & 0xff); m_cSendData[18] = ((AccLine >> 16) & 0xff); m_cSendData[19] = ((AccLine >> 24) & 0xff); m_cSendData[20] = (DecCurve & 0xff); m_cSendData[21] = ((DecCurve >> 8) & 0xff); m_cSendData[22] = ((DecCurve >> 16) & 0xff); m_cSendData[23] = ((DecCurve >> 24) & 0xff); m_cSendData[24] = (DecLine & 0xff); m_cSendData[25] = ((DecLine >> 8) & 0xff); m_cSendData[26] = ((DecLine >> 16) & 0xff); m_cSendData[27] = ((DecLine >> 24) & 0xff); jogspeed = DriveSpeed; if ((StartSpeed < 250) && (DriveSpeed < 6)) { m_IsUseJerk = 1; } else { m_IsUseJerk = 0; } m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength); Sleep(5); m_cSendData[0] = CT_MOTOR; if (AxisNumber == 1 || AxisNumber == 2) { if (!bJOGDir) //方向 { m_cSendData[1] = CT_START_JOG_NEG; } else { m_cSendData[1] = CT_START_JOG_POS; } } else { if (bJOGDir) //方向 { m_cSendData[1] = CT_START_JOG_POS; } else { m_cSendData[1] = CT_START_JOG_NEG; } } m_cSendData[2] = AxisTypes; m_cSendData[3] = 0x53; t_start = GetTickCount(); m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength); jogMoving = true; g_pLogger->SendAndFlushWithTime(L"[Jog] Out,bJOGDir = %s\n", bJOGDir); } return rStatus; } HSI_STATUS HSI_Motion::Jog(UINT AxisTypes, double Speed) { auto rStatus = HSI_STATUS_NORMAL; g_pLogger->SendAndFlushWithTime(L"[Jog] Aixs: [%d] SpeedGear: [%lf]\n", AxisTypes, Speed); if (g_pHSI_Motion) { if (m_DeviceType != 3) //非转盘设备 { m_Thread_StateJOGStop = HSI_THREAD_RUNNING; SetEvent(m_hTriggerEventJOGStop); } int DriveSpeed(1); int StartSpeed(1); int AccLine(1); int DecLine(1); int AccCurve(1); int DecCurve(1); int JogSpeed(1); bool bJOGDir = Speed > 0 ? true : false; //运动方向 jogDirFlag = bJOGDir; m_Thread_State = HSI_THREAD_PAUSED; if (CurrentHomeMachineState == E_EF3_HOME_FINISHED) { //软限位 g_pLogger->SendAndFlushWithTime( L"[Jog] Limit Enable, Axis = %d, m_P_Work_Limit = %f,m_N_Work_Limit = %f\n", AxisTypes,m_P_Work_Limit[AxisTypes], m_N_Work_Limit[AxisTypes]); } else { //无效软限位 g_pLogger->SendAndFlushWithTime(L"[Jog] Limit No Enable\n"); } //是否回过家判断 if (m_Home_Machine_Axis[AxisTypes] == 0) { g_pLogger->SendAndFlushWithTime(L"[Jog] Current Axis[%d] not homed \n", AxisTypes); return rStatus; } //设置 JOG运动参数 加减速 JOG_SPEED_ACC_DEC double now_pos[5] = {0}; double Prf_pos[5] = {0}; double limitpos[4] = {0}; int RemainPul = 0; int limitSDPul = 0; double time; /*GetPositionXyz(1, now_pos[0], now_pos[1], now_pos[2], time);*/ //GetPositionEncPrfMulti(1, now_pos, Prf_pos, 1); //if (m_DeviceType != 3) //{ // if (AxisTypes == AXIS_X /*&& m_motorType & 0x01*/) // { // if (!bJOGDir) //负方向 // { // RemainPul = static_cast(now_pos[1] / m_Resolution[1]) - static_cast(m_N_Work_Limit[1] / // m_Resolution[1]); // limitSDPul = (DriveSpeed - StartSpeed) * 13; // if ((RemainPul < limitSDPul * 2) && (RemainPul > 0)) // { // float SpeedRatio = limitSDPul * 2 / RemainPul; // DriveSpeed = DriveSpeed / SpeedRatio; // if (DriveSpeed < StartSpeed) // { // DriveSpeed = StartSpeed; // } // } // } // else // { // RemainPul = static_cast(m_P_Work_Limit[1] / m_Resolution[1]) - static_cast(now_pos[1] / // m_Resolution[1]); // limitSDPul = (DriveSpeed - StartSpeed) * 13; // if ((RemainPul < limitSDPul * 2) && (RemainPul > 0)) // { // float SpeedRatio = limitSDPul * 2 / RemainPul; // DriveSpeed = DriveSpeed / SpeedRatio; // if (DriveSpeed < StartSpeed) // { // DriveSpeed = StartSpeed; // } // } // } // } // else if (AxisTypes == AXIS_Y /*&& m_motorType & 0x02*/) // { // if (!bJOGDir) //负方向 // { // RemainPul = static_cast(now_pos[2] / m_Resolution[2]) - static_cast(m_N_Work_Limit[2] / // m_Resolution[2]); // limitSDPul = (DriveSpeed - StartSpeed) * 13; // if ((RemainPul < limitSDPul * 2) && (RemainPul > 0)) // { // float SpeedRatio = 1 + limitSDPul * 2 / RemainPul; // DriveSpeed = DriveSpeed / SpeedRatio; // if (DriveSpeed < StartSpeed) // { // DriveSpeed = StartSpeed; // } // } // } // else // { // RemainPul = static_cast(m_P_Work_Limit[2] / m_Resolution[2]) - static_cast(now_pos[2] / // m_Resolution[2]); // limitSDPul = (DriveSpeed - StartSpeed) * 13; // if ((RemainPul < limitSDPul * 2) && (RemainPul > 0)) // { // float SpeedRatio = limitSDPul * 2 / RemainPul; // DriveSpeed = DriveSpeed / SpeedRatio; // if (DriveSpeed < StartSpeed) // { // DriveSpeed = StartSpeed; // } // } // } // } // if (AxisTypes == AXIS_Z /*&& m_motorType & 0x04*/) // { // if (!bJOGDir) //负方向 // { // RemainPul = static_cast(now_pos[3] / m_Resolution[3]) - static_cast(m_N_Work_Limit[3] / // m_Resolution[3]); // limitSDPul = (DriveSpeed - StartSpeed) * 13; // if ((RemainPul < limitSDPul * 2) && (RemainPul > 0)) // { // float SpeedRatio = limitSDPul * 2 / RemainPul; // DriveSpeed = DriveSpeed / SpeedRatio; // if (DriveSpeed < StartSpeed) // { // DriveSpeed = StartSpeed; // } // } // } // else // { // RemainPul = static_cast(m_P_Work_Limit[3] / m_Resolution[3]) - static_cast(now_pos[3] / // m_Resolution[3]); // limitSDPul = (DriveSpeed - StartSpeed) * 13; // if ((RemainPul < limitSDPul * 2) && (RemainPul > 0)) // { // float SpeedRatio = limitSDPul * 2 / RemainPul; // DriveSpeed = DriveSpeed / SpeedRatio; // if (DriveSpeed < StartSpeed) // { // DriveSpeed = StartSpeed; // } // } // } // } //} //速度限制 JogSpeed = abs(SpeedPercent(AxisTypes, Speed, DriveSpeed, StartSpeed, AccLine, DecLine, AccCurve, DecCurve)); g_pLogger->SendAndFlushWithTime( L"[Jog] Speed: [%d], DriveSpeed: [%d],AccLine: [%d], DecLine: [%d] AccCurve: [%d], DecCurve: [%d],\n", Speed, DriveSpeed, AccLine, DecLine,AccCurve, DecCurve ); //转到真实ACS平台轴号，并开始执行 byte AxisNumber = static_cast(AxisConvertIndex(AxisTypes)); //Jog double motionParam[5] = { DriveSpeed,AccLine*10 , DecLine*10, AccCurve,DecCurve }; //速度，加速度，减速度，Kill, jerk SetSingleAxisMotionParams(AxisNumber, motionParam); // 急停判断 if ((StartSpeed < 250) && (DriveSpeed < 6)) { m_IsUseJerk = 1; g_pLogger->SendAndFlushWithTime(L"[Jog] Use Jerk\n"); } else { m_IsUseJerk = 0; g_pLogger->SendAndFlushWithTime(L"[Jog] No Use Jerk\n"); } //使能电机 int Axes[] = { ACSC_AXIS_1, ACSC_AXIS_0, ACSC_AXIS_8, -1 }; //根据电气层面定义，这里的0对应X轴，1对应Y轴，8对应Z轴 if (!acsc_EnableM(handleACS, Axes, nullptr)) { g_pLogger->SendAndFlushWithTime(L"[ACS Motion] ACS Motors Enable Failed!\n"); ErrorsHandler(); } else { g_pLogger->SendAndFlushWithTime(L"[ACS Motion] ACS Motors Enable Success!\n"); } //开始JOG运动 if (!bJOGDir) { 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"); ErrorsHandler(); } jogMoving = true; g_pLogger->SendAndFlushWithTime(L"[Jog] Out, AxisNumber = %d, DriveSpeed = %d AccCurve:[%d] DecCurve:[%d]\n", AxisNumber, DriveSpeed, AccLine, AccLine); } return rStatus; } //JOG模式 //=========================================================================== HSI_STATUS HSI_Motion::JoyStick(UINT AxisTypes, long Speed) { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { if (m_DeviceType != 3) { m_Thread_StateJOGStop = HSI_THREAD_RUNNING; SetEvent(m_hTriggerEventJOGStop); } int DriveSpeed(1); int StartSpeed(1); int AccLine(1); int DecLine(1); int AccCurve(1); int DecCurve(1); int JogSpeed(1); bool bJOGDir = Speed > 0 ? true : false; byte AxisNumber = static_cast(AxisConvertIndex(AxisTypes)); //JoyStick jogAxisNum = AxisNumber; jogDirFlag = bJOGDir; m_Thread_State = HSI_THREAD_PAUSED; if (CurrentHomeMachineState == E_EF3_HOME_FINISHED) { m_cSendData[0] = CT_MOTOR; m_cSendData[1] = CT_MOTOR_SET; m_cSendData[2] = AxisTypes; m_cSendData[3] = SOFT_LIMIT_POS_NEG; m_cSendData[4] = static_cast(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) & 0xff; m_cSendData[5] = (static_cast(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 8) & 0xff; m_cSendData[6] = (static_cast(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 16) & 0xff; m_cSendData[7] = (static_cast(m_P_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 24) & 0xff; m_cSendData[8] = static_cast(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) & 0xff; m_cSendData[9] = (static_cast(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 8) & 0xff; m_cSendData[10] = (static_cast(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 16) & 0xff; m_cSendData[11] = (static_cast(m_N_Work_Limit[AxisNumber] / m_Resolution[AxisNumber]) >> 24) & 0xff; m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength); g_pLogger->SendAndFlushWithTime( L"[Jog] Limit Enable, m_P_Work_Limit[AxisNumber] = %f,m_N_Work_Limit[AxisNumber] = %f\n", m_P_Work_Limit[AxisNumber], m_N_Work_Limit[AxisNumber]); } else { //无效软限位 m_cSendData[0] = CT_MOTOR; m_cSendData[1] = CT_MOTOR_SET; m_cSendData[2] = AxisTypes; m_cSendData[3] = SOFT_LIMIT_POS_NEG; m_cSendData[4] = 0; m_cSendData[5] = 0; m_cSendData[6] = 0; m_cSendData[7] = 0; m_cSendData[8] = 0; m_cSendData[9] = 0; m_cSendData[10] = 0; m_cSendData[11] = 0; m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength); g_pLogger->SendAndFlushWithTime(L"[Jog] Limit No Enable\n"); } if (m_Home_Machine_Axis[AxisNumber] == 0) { return rStatus; } double now_pos[5] = {0}; double Prf_pos[5] = {0}; double limitpos[4] = {0}; int RemainPul = 0; int limitSDPul = 0; double time; /*GetPositionXyz(1, now_pos[0], now_pos[1], now_pos[2], time);*/ GetPositionEncPrfMulti(1, now_pos, Prf_pos, 1); long lSpeed = abs(Speed); if (!abs(SpeedPercentJoyStick(AxisNumber, lSpeed, DriveSpeed, StartSpeed, AccLine, DecLine, AccCurve, DecCurve))) { return rStatus; } if (m_DeviceType != 3) { if (AxisTypes == AXIS_X && m_motorType & 0x01) { if (!bJOGDir) //负方向 { RemainPul = static_cast(now_pos[1] / m_Resolution[1]) - static_cast(m_N_Work_Limit[1] / m_Resolution[1]); limitSDPul = (DriveSpeed - StartSpeed) * 13; if ((RemainPul < limitSDPul * 2) && (RemainPul > 0)) { float SpeedRatio = limitSDPul * 2 / RemainPul; DriveSpeed = DriveSpeed / SpeedRatio; if (DriveSpeed < StartSpeed) { DriveSpeed = StartSpeed; } } } else { RemainPul = static_cast(m_P_Work_Limit[1] / m_Resolution[1]) - static_cast(now_pos[1] / m_Resolution[1]); limitSDPul = (DriveSpeed - StartSpeed) * 13; if ((RemainPul < limitSDPul * 2) && (RemainPul > 0)) { float SpeedRatio = limitSDPul * 2 / RemainPul; DriveSpeed = DriveSpeed / SpeedRatio; if (DriveSpeed < StartSpeed) { DriveSpeed = StartSpeed; } } } } else if (AxisTypes == AXIS_Y && m_motorType & 0x02) { if (!bJOGDir) //负方向 { RemainPul = static_cast(now_pos[2] / m_Resolution[2]) - static_cast(m_N_Work_Limit[2] / m_Resolution[2]); limitSDPul = (DriveSpeed - StartSpeed) * 13; if ((RemainPul < limitSDPul * 2) && (RemainPul > 0)) { float SpeedRatio = 1 + limitSDPul * 2 / RemainPul; DriveSpeed = DriveSpeed / SpeedRatio; if (DriveSpeed < StartSpeed) { DriveSpeed = StartSpeed; } } } else { RemainPul = static_cast(m_P_Work_Limit[2] / m_Resolution[2]) - static_cast(now_pos[2] / m_Resolution[2]); limitSDPul = (DriveSpeed - StartSpeed) * 13; if ((RemainPul < limitSDPul * 2) && (RemainPul > 0)) { float SpeedRatio = limitSDPul * 2 / RemainPul; DriveSpeed = DriveSpeed / SpeedRatio; if (DriveSpeed < StartSpeed) { DriveSpeed = StartSpeed; } } } } if (AxisTypes == AXIS_Z && m_motorType & 0x04) { if (!bJOGDir) //负方向 { RemainPul = static_cast(now_pos[3] / m_Resolution[3]) - static_cast(m_N_Work_Limit[3] / m_Resolution[3]); limitSDPul = (DriveSpeed - StartSpeed) * 13; if ((RemainPul < limitSDPul * 2) && (RemainPul > 0)) { float SpeedRatio = limitSDPul * 2 / RemainPul; DriveSpeed = DriveSpeed / SpeedRatio; if (DriveSpeed < StartSpeed) { DriveSpeed = StartSpeed; } } } else { RemainPul = static_cast(m_P_Work_Limit[3] / m_Resolution[3]) - static_cast(now_pos[3] / m_Resolution[3]); limitSDPul = (DriveSpeed - StartSpeed) * 13; if ((RemainPul < limitSDPul * 2) && (RemainPul > 0)) { float SpeedRatio = limitSDPul * 2 / RemainPul; DriveSpeed = DriveSpeed / SpeedRatio; if (DriveSpeed < StartSpeed) { DriveSpeed = StartSpeed; } } } } if (AxisTypes == AXIS_U && m_motorType & 0x08) { if (!bJOGDir) //负方向 { RemainPul = static_cast(now_pos[4] / m_Resolution[4]) - static_cast(m_N_Work_Limit[4] / m_Resolution[4]); limitSDPul = (DriveSpeed - StartSpeed) * 13; if ((RemainPul < limitSDPul * 2) && (RemainPul > 0)) { float SpeedRatio = limitSDPul * 2 / RemainPul; DriveSpeed = DriveSpeed / SpeedRatio; if (DriveSpeed < StartSpeed) { DriveSpeed = StartSpeed; } } } else { RemainPul = static_cast(m_P_Work_Limit[4] / m_Resolution[4]) - static_cast(now_pos[4] / m_Resolution[4]); limitSDPul = (DriveSpeed - StartSpeed) * 13; if ((RemainPul < limitSDPul * 2) && (RemainPul > 0)) { float SpeedRatio = limitSDPul * 2 / RemainPul; DriveSpeed = DriveSpeed / SpeedRatio; if (DriveSpeed < StartSpeed) { DriveSpeed = StartSpeed; } } } } } m_cSendData[0] = CT_MOTOR; m_cSendData[1] = CT_MOTOR_SET; m_cSendData[2] = AxisTypes; m_cSendData[3] = JOG_SPEED_ACC_DEC; m_cSendData[4] = (StartSpeed & 0xff); m_cSendData[5] = ((StartSpeed >> 8) & 0xff); m_cSendData[6] = ((StartSpeed >> 16) & 0xff); m_cSendData[7] = ((StartSpeed >> 24) & 0xff); m_cSendData[8] = (DriveSpeed & 0xff); m_cSendData[9] = ((DriveSpeed >> 8) & 0xff); m_cSendData[10] = ((DriveSpeed >> 16) & 0xff); m_cSendData[11] = ((DriveSpeed >> 24) & 0xff); m_cSendData[12] = (AccCurve & 0xff); m_cSendData[13] = ((AccCurve >> 8) & 0xff); m_cSendData[14] = ((AccCurve >> 16) & 0xff); m_cSendData[15] = ((AccCurve >> 24) & 0xff); m_cSendData[16] = (AccLine & 0xff); m_cSendData[17] = ((AccLine >> 8) & 0xff); m_cSendData[18] = ((AccLine >> 16) & 0xff); m_cSendData[19] = ((AccLine >> 24) & 0xff); m_cSendData[20] = (DecCurve & 0xff); m_cSendData[21] = ((DecCurve >> 8) & 0xff); m_cSendData[22] = ((DecCurve >> 16) & 0xff); m_cSendData[23] = ((DecCurve >> 24) & 0xff); m_cSendData[24] = (DecLine & 0xff); m_cSendData[25] = ((DecLine >> 8) & 0xff); m_cSendData[26] = ((DecLine >> 16) & 0xff); m_cSendData[27] = ((DecLine >> 24) & 0xff); jogspeed = DriveSpeed; if ((StartSpeed < 250) && (DriveSpeed < 6)) { m_IsUseJerk = 1; } else { m_IsUseJerk = 0; } m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength); Sleep(5); m_cSendData[0] = CT_MOTOR; if (AxisNumber == 1 || AxisNumber == 2) { if (!bJOGDir) //方向 { m_cSendData[1] = CT_START_JOG_NEG; } else { m_cSendData[1] = CT_START_JOG_POS; } } else { if (bJOGDir) //方向 { m_cSendData[1] = CT_START_JOG_POS; } else { m_cSendData[1] = CT_START_JOG_NEG; } } m_cSendData[2] = AxisTypes; m_cSendData[3] = 0x53; t_start = GetTickCount(); m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength); jogMoving = true; g_pLogger->SendAndFlushWithTime(L"[Jog] Out,bJOGDir = %d\n", bJOGDir); } return rStatus; } //=========================================================================== /** * \brief 停止Jog运动 * \return */ HSI_STATUS HSI_Motion::StopJogOld() { WaitForSingleObject(g_Lock_JogAndTrigger, INFINITE); auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { g_pLogger->SendAndFlushWithTime(L"[StopJog] In\n"); t_end = GetTickCount(); g_pLogger->SendAndFlushWithTime(L"[StopJog] t_start = %d, t_end = %d\n", t_start, t_end); jogMoving = false; if (t_end - t_start < 100) { DWORD t_use = 100 - (t_end - t_start); g_pLogger->SendAndFlushWithTime(L"[StopJog] PushButtonTime = %d\n", t_use); Sleep(t_use); } unsigned char m_SendJogData[64] = {0}; if (m_IsUseJerk == 0) { m_SendJogData[0] = CT_MOTOR; m_SendJogData[1] = CT_STOP; m_SendJogData[2] = AXIS_XYZU; m_StopJogMode[1] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49; m_StopJogMode[2] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49; m_StopJogMode[3] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49; m_StopJogMode[4] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49; m_WriteByte = Send_Command(0, (const char*)m_SendJogData, m_SendDataLength); } else { m_SendJogData[0] = CT_MOTOR; m_SendJogData[1] = CT_STOP; m_SendJogData[2] = AXIS_XYZU; m_StopJogMode[1] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49; m_StopJogMode[2] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49; m_StopJogMode[3] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49; m_StopJogMode[4] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49; m_WriteByte = Send_Command(0, (const char*)m_SendJogData, m_SendDataLength); } m_Thread_StateJOGStop = HSI_THREAD_PAUSED; g_pLogger->SendAndFlushWithTime(L"[StopJog] Out\n"); ReleaseMutex(g_Lock_JogAndTrigger); } return rStatus; } HSI_STATUS HSI_Motion::StopJog() { WaitForSingleObject(g_Lock_JogAndTrigger, INFINITE); auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { g_pLogger->SendAndFlushWithTime(L"[StopJog] In\n"); t_end = GetTickCount(); //g_pLogger->SendAndFlushWithTime(L"[StopJog] t_start = %d, t_end = %d\n", t_start, t_end); jogMoving = false; if (t_end - t_start < 100) { DWORD t_use = 100 - (t_end - t_start); g_pLogger->SendAndFlushWithTime(L"[StopJog] PushButtonTime = %d\n", t_use); Sleep(t_use); } unsigned char m_SendJogData[64] = {0}; //是否启用急停 /*if (m_IsUseJerk == 0) { m_SendJogData[0] = CT_MOTOR; m_SendJogData[1] = CT_STOP; m_SendJogData[2] = AXIS_XYZU; m_StopJogMode[1] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49; m_StopJogMode[2] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49; m_StopJogMode[3] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49; m_StopJogMode[4] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49; m_WriteByte = Send_Command(0, (const char*)m_SendJogData, m_SendDataLength); } else { m_SendJogData[0] = CT_MOTOR; m_SendJogData[1] = CT_STOP; m_SendJogData[2] = AXIS_XYZU; m_StopJogMode[1] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49; m_StopJogMode[2] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49; m_StopJogMode[3] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49; m_StopJogMode[4] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49; m_WriteByte = Send_Command(0, (const char*)m_SendJogData, m_SendDataLength); }*/ int Axes[] = {ACSC_AXIS_1, ACSC_AXIS_0, ACSC_AXIS_8, -1}; if (handleACS != ACSC_INVALID) { if (!acsc_HaltM(handleACS, Axes, nullptr)) //停止JOG运动 { g_pLogger->SendAndFlushWithTime(L"[StopJog] ACS acsc_HaltM error!\n"); ErrorsHandler(); } } m_Thread_StateJOGStop = HSI_THREAD_PAUSED; g_pLogger->SendAndFlushWithTime(L"[StopJog] Out\n"); ReleaseMutex(g_Lock_JogAndTrigger); } return rStatus; } //=========================================================================== HSI_STATUS HSI_Motion::StopJogEx(UINT AxisTypes) { WaitForSingleObject(g_Lock_JogAndTrigger, INFINITE); auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { byte AxisNumber = static_cast(IndexConvertAxis(AxisTypes)); g_pLogger->SendAndFlushWithTime(L"[StopJog] In\n"); t_end = GetTickCount(); g_pLogger->SendAndFlushWithTime(L"[StopJog] t_start = %d, t_end = %d\n", t_start, t_end); jogMoving = false; if (t_end - t_start < 100) { DWORD t_use = 100 - (t_end - t_start); g_pLogger->SendAndFlushWithTime(L"[StopJog] PushButtonTime = %d\n", t_use); Sleep(t_use); } unsigned char m_SendJogData[64] = {0}; if (m_IsUseJerk == 0) { m_SendJogData[0] = CT_MOTOR; m_SendJogData[1] = CT_STOP; m_SendJogData[2] = AxisNumber; m_StopJogMode[1] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49; m_StopJogMode[2] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49; m_StopJogMode[3] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49; m_StopJogMode[4] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49; for (int i = 0; i < 5; i++) { m_WriteByte = Send_Command(0, (const char*)m_SendJogData, m_SendDataLength); Sleep(5); } } else { m_SendJogData[0] = CT_MOTOR; m_SendJogData[1] = CT_STOP; m_SendJogData[2] = AxisNumber; m_StopJogMode[1] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49; m_StopJogMode[2] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49; m_StopJogMode[3] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49; m_StopJogMode[4] == 0 ? m_SendJogData[3] = 0X4A : m_SendJogData[3] = 0X49; for (int i = 0; i < 5; i++) { m_WriteByte = Send_Command(0, (const char*)m_SendJogData, m_SendDataLength); Sleep(5); } } m_Thread_StateJOGStop = HSI_THREAD_PAUSED; g_pLogger->SendAndFlushWithTime(L"[StopJog] Out\n"); ReleaseMutex(g_Lock_JogAndTrigger); } return rStatus; } //=========================================================================== int HSI_Motion::P2P(short AxisNumber, long Pos, double Speed, double Acc) { if (g_pHSI_Motion) { } return 0; } //=========================================================================== //运动控制部分 //=========================================================================== /** * \brief 对比现在编码器位置和期望位置, position给上层用的, HSI内部用的是EncPrf * \param AxisTypes * \param EncPos * \param PrfPos * \param Count * \return */ HSI_STATUS HSI_Motion::GetPositionEncPrfMultiOld(UINT AxisTypes, double* EncPos, double* PrfPos, int Count) { auto rStatus = HSI_STATUS_NORMAL; UNREFERENCED_PARAMETER(AxisTypes); CString tempStr; if (g_pHSI_Motion) { g_pLogger->SendAndFlushWithTime(L"[GetPositionEncPrfMulti] In\n"); if (m_SO7_Serial.m_RecvData[0] == 2) { EncPos[1] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial.m_RecvData[ 3] << 8 | m_SO7_Serial.m_RecvData[4]) * m_Resolution[1]; EncPos[2] = (m_SO7_Serial.m_RecvData[5] << 24 | m_SO7_Serial.m_RecvData[6] << 16 | m_SO7_Serial.m_RecvData[ 7] << 8 | m_SO7_Serial.m_RecvData[8]) * m_Resolution[2]; EncPos[3] = (m_SO7_Serial.m_RecvData[9] << 24 | m_SO7_Serial.m_RecvData[10] << 16 | m_SO7_Serial.m_RecvData[ 11] << 8 | m_SO7_Serial.m_RecvData[12]) * m_Resolution[3]; EncPos[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | m_SO7_Serial.m_RecvData [15] << 8 | m_SO7_Serial.m_RecvData[16]) * m_Resolution[4]; PrfPos[1] = (m_SO7_Serial.m_RecvData[17] << 24 | m_SO7_Serial.m_RecvData[18] << 16 | m_SO7_Serial.m_RecvData [19] << 8 | m_SO7_Serial.m_RecvData[20]) * m_Resolution[1]; PrfPos[2] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial.m_RecvData [23] << 8 | m_SO7_Serial.m_RecvData[24]) * m_Resolution[2]; PrfPos[3] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial.m_RecvData [27] << 8 | m_SO7_Serial.m_RecvData[28]) * m_Resolution[3]; PrfPos[4] = (m_SO7_Serial.m_RecvData[29] << 24 | m_SO7_Serial.m_RecvData[30] << 16 | m_SO7_Serial.m_RecvData [31] << 8 | m_SO7_Serial.m_RecvData[32]) * m_Resolution[4]; if (m_IsHavePattern & 0x01) m_EncPos[1] = EncPos[1]; else m_EncPos[1] = PrfPos[1]; if (m_IsHavePattern & 0x02) m_EncPos[2] = EncPos[2]; else m_EncPos[2] = PrfPos[2]; if (m_IsHavePattern & 0x04) m_EncPos[3] = EncPos[3]; else m_EncPos[3] = PrfPos[3]; if (m_IsHavePattern & 0x08) m_EncPos[4] = EncPos[4]; else m_EncPos[4] = PrfPos[4]; m_PrfPos[1] = PrfPos[1]; m_PrfPos[2] = PrfPos[2]; m_PrfPos[3] = PrfPos[3]; m_PrfPos[4] = PrfPos[4]; //begin_position[1] = EncPos[1] / m_Resolution[1]; //begin_position[2] = EncPos[2] / m_Resolution[1]; //begin_position[3] = EncPos[3] / m_Resolution[1]; //begin_position[4] = EncPos[4] / m_Resolution[1]; } else { EncPos[1] = m_EncPos[1]; EncPos[2] = m_EncPos[2]; EncPos[2] = m_EncPos[3]; EncPos[4] = m_EncPos[4]; PrfPos[1] = m_PrfPos[1]; PrfPos[2] = m_PrfPos[2]; PrfPos[2] = m_PrfPos[3]; PrfPos[4] = m_PrfPos[4]; g_pLogger->SendAndFlushWithTime(L"[GetPositionEncPrfMulti] failed\n"); } g_pLogger->SendAndFlushWithTime(L"[GetPositionEncPrfMulti] Out\n"); } return rStatus; } HSI_STATUS HSI_Motion::GetPositionEncPrfMulti(UINT AxisTypes, double* EncPos, double* PrfPos, int Count) { auto rStatus = HSI_STATUS_NORMAL; UNREFERENCED_PARAMETER(AxisTypes); CString tempStr; if (g_pHSI_Motion) { g_pLogger->SendAndFlushWithTime(L"[GetPositionEncPrfMulti] In\n"); if (m_SO7_Serial.m_RecvData[0] == 2) { EncPos[1] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial.m_RecvData[ 3] << 8 | m_SO7_Serial.m_RecvData[4]) * m_Resolution[1]; EncPos[2] = (m_SO7_Serial.m_RecvData[5] << 24 | m_SO7_Serial.m_RecvData[6] << 16 | m_SO7_Serial.m_RecvData[ 7] << 8 | m_SO7_Serial.m_RecvData[8]) * m_Resolution[2]; EncPos[3] = (m_SO7_Serial.m_RecvData[9] << 24 | m_SO7_Serial.m_RecvData[10] << 16 | m_SO7_Serial.m_RecvData[ 11] << 8 | m_SO7_Serial.m_RecvData[12]) * m_Resolution[3]; EncPos[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | m_SO7_Serial.m_RecvData [15] << 8 | m_SO7_Serial.m_RecvData[16]) * m_Resolution[4]; PrfPos[1] = (m_SO7_Serial.m_RecvData[17] << 24 | m_SO7_Serial.m_RecvData[18] << 16 | m_SO7_Serial.m_RecvData [19] << 8 | m_SO7_Serial.m_RecvData[20]) * m_Resolution[1]; PrfPos[2] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial.m_RecvData [23] << 8 | m_SO7_Serial.m_RecvData[24]) * m_Resolution[2]; PrfPos[3] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial.m_RecvData [27] << 8 | m_SO7_Serial.m_RecvData[28]) * m_Resolution[3]; PrfPos[4] = (m_SO7_Serial.m_RecvData[29] << 24 | m_SO7_Serial.m_RecvData[30] << 16 | m_SO7_Serial.m_RecvData [31] << 8 | m_SO7_Serial.m_RecvData[32]) * m_Resolution[4]; if (m_IsHavePattern & 0x01) m_EncPos[1] = EncPos[1]; else m_EncPos[1] = PrfPos[1]; if (m_IsHavePattern & 0x02) m_EncPos[2] = EncPos[2]; else m_EncPos[2] = PrfPos[2]; if (m_IsHavePattern & 0x04) m_EncPos[3] = EncPos[3]; else m_EncPos[3] = PrfPos[3]; if (m_IsHavePattern & 0x08) m_EncPos[4] = EncPos[4]; else m_EncPos[4] = PrfPos[4]; m_PrfPos[1] = PrfPos[1]; m_PrfPos[2] = PrfPos[2]; m_PrfPos[3] = PrfPos[3]; m_PrfPos[4] = PrfPos[4]; //begin_position[1] = EncPos[1] / m_Resolution[1]; //begin_position[2] = EncPos[2] / m_Resolution[1]; //begin_position[3] = EncPos[3] / m_Resolution[1]; //begin_position[4] = EncPos[4] / m_Resolution[1]; } else { EncPos[1] = m_EncPos[1]; EncPos[2] = m_EncPos[2]; EncPos[2] = m_EncPos[3]; EncPos[4] = m_EncPos[4]; PrfPos[1] = m_PrfPos[1]; PrfPos[2] = m_PrfPos[2]; PrfPos[2] = m_PrfPos[3]; PrfPos[4] = m_PrfPos[4]; g_pLogger->SendAndFlushWithTime(L"[GetPositionEncPrfMulti] failed\n"); } g_pLogger->SendAndFlushWithTime(L"[GetPositionEncPrfMulti] Out\n"); } return rStatus; } //=========================================================================== /** * \brief 获取轴当前运动位置 * \param AxisTypes * \param PositionX * \param PositionY * \param PositionZ * \param Time 耗时 * \return */ HSI_STATUS HSI_Motion::GetPositionXyzOld(UINT AxisTypes, double& PositionX, double& PositionY, double& PositionZ, double& Time) { auto rStatus = HSI_STATUS_NORMAL; UNREFERENCED_PARAMETER(AxisTypes); //读取3个轴的位置 CString tempStr; if (g_pHSI_Motion) { if (m_SO7_Serial.m_RecvData[0] == 2) { if (m_DeviceType != 1) { if (m_IsHavePattern & 0x01) PositionX = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial. m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]) * m_Resolution[1]; else PositionX = (m_SO7_Serial.m_RecvData[17] << 24 | m_SO7_Serial.m_RecvData[18] << 16 | m_SO7_Serial. m_RecvData[19] << 8 | m_SO7_Serial.m_RecvData[20]) * m_Resolution[1]; if (m_IsHavePattern & 0x02) PositionY = (m_SO7_Serial.m_RecvData[5] << 24 | m_SO7_Serial.m_RecvData[6] << 16 | m_SO7_Serial. m_RecvData[7] << 8 | m_SO7_Serial.m_RecvData[8]) * m_Resolution[2]; else PositionY = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial. m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]) * m_Resolution[2]; if (m_IsHavePattern & 0x04) PositionZ = (m_SO7_Serial.m_RecvData[9] << 24 | m_SO7_Serial.m_RecvData[10] << 16 | m_SO7_Serial. m_RecvData[11] << 8 | m_SO7_Serial.m_RecvData[12]) * m_Resolution[3]; else PositionZ = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial. m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]) * m_Resolution[3]; if (m_IsHavePattern & 0x08) m_PosForAllAxis[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | m_SO7_Serial.m_RecvData[15] << 8 | m_SO7_Serial.m_RecvData[16]) * m_Resolution[4]; else m_PosForAllAxis[4] = (m_SO7_Serial.m_RecvData[29] << 24 | m_SO7_Serial.m_RecvData[30] << 16 | m_SO7_Serial.m_RecvData[31] << 8 | m_SO7_Serial.m_RecvData[32]) * m_Resolution[4]; } else { PositionX = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial. m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]) * m_Resolution[1]; PositionY = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial. m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]) * m_Resolution[2]; PositionZ = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial. m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]) * m_Resolution[3]; } m_EncPos[1] = PositionX; m_EncPos[2] = PositionY; m_EncPos[3] = PositionZ; m_EncPos[4] = m_PosForAllAxis[4]; m_PosForAllAxis[1] = PositionX; m_PosForAllAxis[2] = PositionY; m_PosForAllAxis[3] = PositionZ; } else { PositionX = m_EncPos[1]; PositionY = m_EncPos[2]; PositionZ = m_EncPos[3]; m_PosForAllAxis[1] = m_EncPos[1]; m_PosForAllAxis[2] = m_EncPos[2]; m_PosForAllAxis[3] = m_EncPos[3]; m_PosForAllAxis[4] = m_EncPos[4]; g_pLogger->SendAndFlushWithTime(L"[GetPositionEncPrfMulti] failed\n"); } //LARGE_INTEGER tima; //QueryPerformanceCounter(&tima); //Time = static_cast(tima.QuadPart); Time = set_end - set_start; } return rStatus; } HSI_STATUS HSI_Motion::GetPositionXyz(UINT AxisTypes, double& PositionX, double& PositionY, double& PositionZ, double& Time) { auto rStatus = HSI_STATUS_NORMAL; //UNREFERENCED_PARAMETER(AxisTypes)的意思就是告诉编译器， // AxisTypes参数我使用过了，别报警告了，仅此而已。 UNREFERENCED_PARAMETER(AxisTypes); //读取3个轴的位置 CString tempStr; bool bGetPosition = true; if (g_pHSI_Motion && (handleACS != ACSC_INVALID)) //句柄有效 { if (!acsc_GetFPosition(handleACS, ACSC_AXIS_1, &PositionX, nullptr)) { g_pLogger->SendAndFlushWithTime(L"[GetPositionEncPrfMulti] get PositionX failed\n"); ErrorsHandler(); bGetPosition = false; return HSI_ACS_ERROR; } if (!acsc_GetFPosition(handleACS, ACSC_AXIS_0, &PositionY, nullptr)) { g_pLogger->SendAndFlushWithTime(L"[GetPositionEncPrfMulti] get PositionY failed\n"); ErrorsHandler(); bGetPosition = false; return HSI_ACS_ERROR; } if (!acsc_GetFPosition(handleACS, ACSC_AXIS_8, &PositionZ, nullptr)) { g_pLogger->SendAndFlushWithTime(L"[GetPositionEncPrfMulti] get PositionZ failed\n"); ErrorsHandler(); bGetPosition = false; return HSI_ACS_ERROR; } if (bGetPosition) //读取成功 { m_EncPos[1] = PositionX; m_EncPos[2] = PositionY; m_EncPos[3] = PositionZ; m_EncPos[4] = m_PosForAllAxis[4]; // m_PosForAllAxis 记录4轴位置 m_PosForAllAxis[1] = PositionX; m_PosForAllAxis[2] = PositionY; m_PosForAllAxis[3] = PositionZ; //g_pLogger->SendAndFlushWithTime( // L"[GetPositionEncPrfMulti] GetPosition Success, Pos[1] = %.4f, Pos[2] = %.4f, Pos[3] = %.4f\n", // PositionX, PositionY, PositionZ); } else //读取将之前的值进行返回 { PositionX = m_EncPos[1]; PositionY = m_EncPos[2]; PositionZ = m_EncPos[3]; m_PosForAllAxis[1] = m_EncPos[1]; // m_PosForAllAxis 记录4轴位置 m_PosForAllAxis[2] = m_EncPos[2]; m_PosForAllAxis[3] = m_EncPos[3]; m_PosForAllAxis[4] = m_EncPos[4]; g_pLogger->SendAndFlushWithTime( L"[GetPositionEncPrfMulti] GetPosition failed, return position record before, Pos[1] = %.4f,Pos[2] = %.4f,Pos[3] = %.4f,Pos[4] = %.4f\n", m_EncPos[1], m_EncPos[2], m_EncPos[3], m_PosForAllAxis[4]); } Time = set_end - set_start; } return rStatus; } //=========================================================================== HSI_STATUS HSI_Motion::GetEncoderXyz(long* lEncoderVal)//原读取编码器值使用串口获取EF3的光栅尺读数，待测试 { auto rStatus = HSI_STATUS_NORMAL; //读取3个轴的编码器值 if (g_pHSI_Motion) { if (m_SO7_Serial.m_RecvData[0] == 2) { if (m_DeviceType != 1) { if (m_IsHavePattern & 0x01) { lEncoderVal[0] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial .m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]); g_pLogger->SendAndFlushWithTime(L"[GetEncoderXyz] lEncoderVal[0] %ld \n", lEncoderVal[0]); } else { lEncoderVal[0] = (m_SO7_Serial.m_RecvData[17] << 24 | m_SO7_Serial.m_RecvData[18] << 16 | m_SO7_Serial.m_RecvData[19] << 8 | m_SO7_Serial.m_RecvData[20]); g_pLogger->SendAndFlushWithTime(L"[GetEncoderXyz] lEncoderVal[00] %ld \n", lEncoderVal[0]); } if (m_IsHavePattern & 0x02) { lEncoderVal[1] = (m_SO7_Serial.m_RecvData[5] << 24 | m_SO7_Serial.m_RecvData[6] << 16 | m_SO7_Serial .m_RecvData[7] << 8 | m_SO7_Serial.m_RecvData[8]); g_pLogger->SendAndFlushWithTime(L"[GetEncoderXyz] lEncoderVal[1] %ld \n", lEncoderVal[1]); } else { lEncoderVal[1] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial.m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]); g_pLogger->SendAndFlushWithTime(L"[GetEncoderXyz] lEncoderVal[01] %ld \n", lEncoderVal[1]); } if (m_IsHavePattern & 0x04) { lEncoderVal[2] = (m_SO7_Serial.m_RecvData[9] << 24 | m_SO7_Serial.m_RecvData[10] << 16 | m_SO7_Serial.m_RecvData[11] << 8 | m_SO7_Serial.m_RecvData[12]); g_pLogger->SendAndFlushWithTime(L"[GetEncoderXyz] lEncoderVal[2] %ld \n", lEncoderVal[2]); } else { lEncoderVal[2] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial.m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]); g_pLogger->SendAndFlushWithTime(L"[GetEncoderXyz] lEncoderVal[02] %ld \n", lEncoderVal[2]); } } else { lEncoderVal[0] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial. m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]); lEncoderVal[1] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial. m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]); lEncoderVal[2] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial. m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]); g_pLogger->SendAndFlushWithTime(L"[GetEncoderXyz] lEncoderVal[0]= %ld,EncoderVal[1]= %ld,,EncoderVal[2]= %ld, \n", lEncoderVal[0], lEncoderVal[1], lEncoderVal[2]); } } else { auto rStatus = HSI_STATUS_FAILED; g_pLogger->SendAndFlushWithTime(L"[GetEncoderXyz] failed\n"); } } return rStatus; } //=========================================================================== HSI_STATUS HSI_Motion::GetPositionXyzaProbe(UINT AxisTypes, double& PositionX, double& PositionY, double& PositionZ, double& PositionA) { auto rStatus = HSI_STATUS_NORMAL; UNREFERENCED_PARAMETER(AxisTypes); if (g_pHSI_Motion) { PositionX = m_LockPos[1]; PositionY = m_LockPos[2]; PositionZ = m_LockPos[3]; PositionA = m_LockPos[4]; } return rStatus; } //=========================================================================== /** * \brief 设置多轴运动到指定位置 * \param AxisTypes * \param PositionX * \param PositionY * \param PositionZ * \param eType * \param dFlyRadius * \return */ HSI_STATUS HSI_Motion::SetPositionXyzOld(UINT AxisTypes, double PositionX, double PositionY, double PositionZ, HSI_MOTION_MOVE_TYPE eType, double dFlyRadius) { WaitForSingleObject(g_WR_ToMove_Mutex, INFINITE); auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] In\n"); g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Pos[1] = %.4f,Pos[2] = %.4f,Pos[3] = %.4f\n", PositionX, PositionY, PositionZ); unsigned char send_pos_data[64] = {0}; axis_start = 0; unsigned char direct_pos = 0; unsigned char xyzAxis = 0; //如果状态非 运动中，设置为运动中 if (CurrentMotionState != E_SO7_MOTION_MOVETO) { CurrentMotionState = E_SO7_MOTION_MOVETO; LimitOver(HSI_MOTION_AXIS_X, PositionX); LimitOver(HSI_MOTION_AXIS_Y, PositionY); LimitOver(HSI_MOTION_AXIS_Z, PositionZ); LimitOver(HSI_MOTION_AXIS_R, m_PositionA); m_PosThread[1] = PositionX; //SetpositionXyz的目标位置 m_PosThread[2] = PositionY; m_PosThread[3] = PositionZ; m_PosThread[4] = m_PositionA; targetpos_n[1] = PositionX; targetpos_n[2] = PositionY; targetpos_n[3] = PositionZ; targetpos_n[4] = m_PositionA; int Pos_t[5] = {0}; int Pos_n[5] = {0}; int Pos[5] = {0}; int NowPos[5] = {0}; int target_pos[5] = {0}; if (m_SO7_Serial.m_RecvData[0] == 2) { //根据设备类型获取现在位置 if (m_DeviceType != 1) //设备类型非三激光 { if (m_IsHavePattern & 0x01 == 0x01) NowPos[1] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial. m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]); else NowPos[1] = (m_SO7_Serial.m_RecvData[17] << 24 | m_SO7_Serial.m_RecvData[18] << 16 | m_SO7_Serial.m_RecvData[19] << 8 | m_SO7_Serial.m_RecvData[20]); if (m_IsHavePattern & 0x02) NowPos[2] = (m_SO7_Serial.m_RecvData[5] << 24 | m_SO7_Serial.m_RecvData[6] << 16 | m_SO7_Serial. m_RecvData[7] << 8 | m_SO7_Serial.m_RecvData[8]); else NowPos[2] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial.m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]); if (m_IsHavePattern & 0x04) NowPos[3] = (m_SO7_Serial.m_RecvData[9] << 24 | m_SO7_Serial.m_RecvData[10] << 16 | m_SO7_Serial .m_RecvData[11] << 8 | m_SO7_Serial.m_RecvData[12]); else NowPos[3] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial.m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]); if (m_IsHavePattern & 0x08) NowPos[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | m_SO7_Serial.m_RecvData[15] << 8 | m_SO7_Serial.m_RecvData[16]); else NowPos[4] = (m_SO7_Serial.m_RecvData[29] << 24 | m_SO7_Serial.m_RecvData[30] << 16 | m_SO7_Serial.m_RecvData[31] << 8 | m_SO7_Serial.m_RecvData[32]); } else { NowPos[1] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial. m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]); NowPos[2] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial. m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]); NowPos[3] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial. m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]); NowPos[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | m_SO7_Serial. m_RecvData[15] << 8 | m_SO7_Serial.m_RecvData[16]); } //NowPos[1] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial.m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]); //NowPos[2] = (m_SO7_Serial.m_RecvData[5] << 24 | m_SO7_Serial.m_RecvData[6] << 16 | m_SO7_Serial.m_RecvData[7] << 8 | m_SO7_Serial.m_RecvData[8]); //NowPos[3] = (m_SO7_Serial.m_RecvData[9] << 24 | m_SO7_Serial.m_RecvData[10] << 16 | m_SO7_Serial.m_RecvData[11] << 8 | m_SO7_Serial.m_RecvData[12]); //NowPos[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | m_SO7_Serial.m_RecvData[15] << 8 | m_SO7_Serial.m_RecvData[16]); Pos_t[1] = (m_SO7_Serial.m_RecvData[17] << 24 | m_SO7_Serial.m_RecvData[18] << 16 | m_SO7_Serial. m_RecvData[19] << 8 | m_SO7_Serial.m_RecvData[20]); Pos_t[2] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial. m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]); Pos_t[3] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial. m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]); Pos_t[4] = (m_SO7_Serial.m_RecvData[29] << 24 | m_SO7_Serial.m_RecvData[30] << 16 | m_SO7_Serial. m_RecvData[31] << 8 | m_SO7_Serial.m_RecvData[32]); } else { /* 编译器隐式执行的任何类型转换都可以由static_cast显式完成。 static_cast可以用来将枚举类型转换成整型，或者整型转换成浮点型*/ Pos_t[1] = NowPos[1] = static_cast(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) { WaitForSingleObject(g_WR_ToMove_Mutex, INFINITE); auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] In\n"); g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] AxisTypes = %d, PositionX = %.4f,PositionY = %.4f,PositionZ = %.4f\n", AxisTypes,PositionX, PositionY, PositionZ); axis_start = 0; unsigned char direct_pos = 0; unsigned char xyzAxis = 0; if (CurrentMotionState != E_SO7_MOTION_MOVETO) //当前运动状态 { CurrentMotionState = E_SO7_MOTION_MOVETO; //限位功能 LimitOver(HSI_MOTION_AXIS_X, PositionX); LimitOver(HSI_MOTION_AXIS_Y, PositionY); LimitOver(HSI_MOTION_AXIS_Z, PositionZ); LimitOver(HSI_MOTION_AXIS_R, m_PositionA); g_pLogger->SendAndFlushWithTime( L"[SetPositionXyz] LimitOver, PositionX = %.4f, PositionY = %.4f, PositionZ = %.4f, m_PositionA = %.4f\n", PositionX, PositionY, PositionZ, m_PositionA); //SetpositionXyz的目标位置 m_PosThread[1] = PositionX; m_PosThread[2] = PositionY; m_PosThread[3] = PositionZ; m_PosThread[4] = m_PositionA; //目标位置 g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] PositionX = %.4f, PositionY = %.4f, PositionZ = %.4f\n", PositionX,PositionY, PositionZ); //设置速度，对应配置文件中定位合成速度 SET_POTION_DRIVESPEED_1 g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] DriveSpeed[0] = %.4f, DriveSpeed[1] = %.4f, DriveSpeed[2] = %.4f, DriveSpeed[3] = %.4f, DriveSpeed[4] = %.4f\n", m_SetPotion_DriveSpeed[0], m_SetPotion_DriveSpeed[1], m_SetPotion_DriveSpeed[2], m_SetPotion_DriveSpeed[3], m_SetPotion_DriveSpeed[4]); double X_SetmotionParam[5] = { m_SetPotion_DriveSpeed[1], m_SetPotion_DriveSpeed[1] * 10, m_SetPotion_DriveSpeed[1] * 10, 0, 0 }; SetSingleAxisMotionParams(ACSC_AXIS_1, X_SetmotionParam);//设置X轴定位速度 double Y_SetmotionParam[5] = { m_SetPotion_DriveSpeed[2], m_SetPotion_DriveSpeed[2] * 10 , m_SetPotion_DriveSpeed[2] * 10, 0, 0 }; SetSingleAxisMotionParams(ACSC_AXIS_0, Y_SetmotionParam);//设置Y轴定位速度 double Z_SetmotionParam[5] = { m_SetPotion_DriveSpeed[3], m_SetPotion_DriveSpeed[3] * 10 , m_SetPotion_DriveSpeed[3] * 10, 0, 0 }; SetSingleAxisMotionParams(ACSC_AXIS_8, Z_SetmotionParam);//设置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"); ErrorsHandler(); } else { g_pLogger->SendAndFlushWithTime(L"[ACS Motion] ACS Motors Enable Success!\n"); } //开始运动到指定位置，多轴运动 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) //非等待 { g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Nowait SetEvent\n"); g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Nowait move!\n"); g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Out Success Nowait\n"); m_IsExMotion = 0; bRunGlueDispenser = HSI_THREAD_PAUSED; m_Thread_State = HSI_THREAD_RUNNING; SetEvent(m_hTriggerEvent); } if (eType == HSI_MOTION_MOVE_WAIT) //等待模式 { g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Out Success Wait Mode\n"); m_Thread_State = HSI_THREAD_RUNNING; m_IsExMotion = 0; UpdateMotionState(); m_Thread_State = HSI_THREAD_PAUSED; } } else { g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] HSI_STATUS_MOTION_MOVING\n"); g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Out\n"); rStatus = HSI_STATUS_MOTION_MOVING; } g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Out\n"); targetpos_l[1] = PositionX; targetpos_l[2] = PositionY; targetpos_l[3] = PositionZ; } ReleaseMutex(g_WR_ToMove_Mutex); return rStatus; } //=========================================================================== int HSI_Motion::CaculateStepMotorACC(int pos, int maxacc, int minacc) { double mpos = static_cast(abs(pos)) / 1000; int acc = (mpos * mpos) / 20 + mpos / 2 + 8; if (acc < minacc) acc = minacc; if (acc > maxacc) acc = maxacc; return acc; } //=========================================================================== //&符号用来处理一个变量，但不是通常的 - 访问这个变量的内容，而是取出这个变量的地址 // //int* b；此时b是一个指向int空间的指针，也就是说它是一个未分配的地址 //int* 只用来定义，定义的变量是一个地址（索引），可以通过这个变量来对这段空间操作 // //而 & 是对一个已存在的变量取地址，取完地址之后同样可以通过地址操作 // //* x是找到x地址的变量，取它的值，所以 * 和 & 是反操作， & x得到x的地址 * ， * x得到x的值x //———————————————— //版权声明：本文为CSDN博主「nick__huang」的原创文章，遵循CC 4.0 BY - SA版权协议，转载请附上原文出处链接及本声明。 //原文链接：https ://blog.csdn.net/u012610237/article/details/58599083 //void func3(void); //利用全局变量返回数组 //void func2(uchar* s); //利用指针返回数组 //uchar* func1(); //利用指针函数返回数组 //void func0(uchar*& r); //利用引用返回数组 /** * \brief 获取单轴运动参数 * \param AXIS * \param motionParam * \return */ HSI_STATUS HSI_Motion::GetSingleAxisParam(int AXIS, double motionParam[5]) { auto rStatus = HSI_STATUS_NORMAL; if (handleACS != ACSC_INVALID) { g_pLogger->SendAndFlushWithTime(L"[GetSingleAxisParam] In\n"); //依次是 速度 if (!acsc_GetVelocity(handleACS, AXIS, motionParam + 0, nullptr)) { g_pLogger->SendAndFlushWithTime(L"[GetSingleAxisParam] acsc_GetVelocity error\n"); rStatus = HSI_ACS_ERROR; ErrorsHandler(); } // 加速度 if (!acsc_GetAcceleration(handleACS, AXIS, motionParam + 1, nullptr)) { g_pLogger->SendAndFlushWithTime(L"[GetSingleAxisParam] acsc_GetAcceleration error\n"); rStatus = HSI_ACS_ERROR; ErrorsHandler(); } //减速、 if (!acsc_GetDeceleration(handleACS, AXIS, motionParam + 2, nullptr)) { g_pLogger->SendAndFlushWithTime(L"[GetSingleAxisParam] acsc_GetDeceleration error\n"); rStatus = HSI_ACS_ERROR; ErrorsHandler(); } // 杀死速度 if (!acsc_GetKillDeceleration(handleACS, AXIS, motionParam + 3, nullptr)) { g_pLogger->SendAndFlushWithTime(L"[GetSingleAxisParam] acsc_GetKillDeceleration error\n"); rStatus = HSI_ACS_ERROR; ErrorsHandler(); } //抖动 if (!acsc_GetJerk(handleACS, AXIS, motionParam + 4, nullptr)) { g_pLogger->SendAndFlushWithTime(L"[GetSingleAxisParam] acsc_GetJerk error\n"); rStatus = HSI_ACS_ERROR; ErrorsHandler(); } g_pLogger->SendAndFlushWithTime(L"[GetSingleAxisParam] Out\n"); } return rStatus; } //=========================================================================== HSI_STATUS HSI_Motion::SetSingleAxisParam(int AXIS, double motionParam[5]) { auto rStatus = HSI_STATUS_NORMAL; if (handleACS != ACSC_INVALID) { g_pLogger->SendAndFlushWithTime(L"[SetSingleAxisParam] In\n"); //依次是 速度 if (!acsc_SetVelocity(handleACS, AXIS, motionParam[0], nullptr)) { g_pLogger->SendAndFlushWithTime(L"[SetSingleAxisParam] acsc_SetVelocity error\n"); rStatus = HSI_ACS_ERROR; ErrorsHandler(); } // 加速度 if (!acsc_SetAcceleration(handleACS, AXIS, motionParam[1], nullptr)) { g_pLogger->SendAndFlushWithTime(L"[SetSingleAxisParam] acsc_SetAcceleration error\n"); rStatus = HSI_ACS_ERROR; ErrorsHandler(); } //减速度 if (!acsc_SetDeceleration(handleACS, AXIS, motionParam[2], nullptr)) { g_pLogger->SendAndFlushWithTime(L"[SetSingleAxisParam] acsc_SetDeceleration error\n"); rStatus = HSI_ACS_ERROR; ErrorsHandler(); } //// 杀死速度 //if (!acsc_SetKillDeceleration(handleACS, AXIS, motionParam[3], nullptr)) //{ // g_pLogger->SendAndFlushWithTime(L"[SetSingleAxisParam] acsc_SetKillDeceleration error\n"); // rStatus = HSI_ACS_ERROR; // ErrorsHandler(); //} ////抖动 //if (!acsc_SetJerk(handleACS, AXIS, motionParam[4], nullptr)) //{ // g_pLogger->SendAndFlushWithTime(L"[SetSingleAxisParam] acsc_SetJerk error\n"); // rStatus = HSI_ACS_ERROR; // ErrorsHandler(); //} g_pLogger->SendAndFlushWithTime(L"[SetSingleAxisParam] Out\n"); } return rStatus; } //=========================================================================== HSI_STATUS HSI_Motion::SetPositionXyza(UINT AxisTypes, double PositionX, double PositionY, double PositionZ, double PositionA, HSI_MOTION_MOVE_TYPE eType, double dFlyRadius) { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { g_pLogger->SendAndFlushWithTime(L"[SetPositionXyza] SetPositionXyza\n"); fourthAxisFlag = true; m_PositionA = PositionA; rStatus = SetPositionXyz(AxisTypes, PositionX, PositionY, PositionZ, eType, dFlyRadius); } return rStatus; } //=========================================================================== HSI_STATUS HSI_Motion::SetPositionXyzCache(UINT AxisTypes, HSI_MOTION_MOVE_TYPE eType, int DataCount, Point* CacheData) { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { g_pLogger->SendAndFlushWithTime(L"[SetPositionXyzCache] start\n"); g_pLogger->SendAndFlushWithTime(L"[SetPositionXyzCache] end\n"); } return rStatus; } //=========================================================================== /** * \brief 获取缓存点 * \param CacheData * \param DataCount * \return */ HSI_STATUS HSI_Motion::GetPositionXyzCache(unsigned char* CacheData, int& DataCount) { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { g_pLogger->SendAndFlushWithTime(L"[GetPositionXyzCache] start\n"); if (m_SO7_Serial.IsOpen()) { //发送获取点数量命令 0x01 0x05 unsigned char m_cSendData[8] = {0}; m_cSendData[0] = 0x01; m_cSendData[1] = 0x05; m_WriteByte = Send_Command(0, (const char*)m_cSendData, 2, 8); //期望回复8个字节，其中 4-7字节为点数量 //解析缓存点数量 if (m_SO7_Serial.m_iRecvState) { //特殊帧头, 表示该内容为回复点数 if ((m_SO7_Serial.m_RecvData[0] == 0x01) && (m_SO7_Serial.m_RecvData[1] == 0x1B) && (m_SO7_Serial. m_iRecvBytes == 8)) { DataCount = (m_SO7_Serial.m_RecvData[4] << 24 | m_SO7_Serial.m_RecvData[5] << 16 | m_SO7_Serial. m_RecvData[6] << 8 | m_SO7_Serial.m_RecvData[7]); //详细 https://blog.csdn.net/hebbely/article/details/79577880 g_pLogger->SendAndFlushWithTime(L"[GetPositionXyzCache] DataCount：%d, %s\n", DataCount, m_SO7_Serial.HexToStr((const char*)m_SO7_Serial.m_RecvData, 8)); } else //查询失败 { g_pLogger->SendAndFlushWithTime(L"[GetPositionXyzCache] DataCount：%d, %s\n", DataCount, m_SO7_Serial.HexToStr((const char*)m_SO7_Serial.m_RecvData, 8)); } } //读取锁存点，给出期望结果 m_cSendData[0] = 0x01; m_cSendData[1] = 0x09; m_WriteByte = Send_Command(0, (const char*)m_cSendData, 2, DataCount * 12); Sleep(100); //读取点的字节数，每个点占用12个字节 if (m_SO7_Serial.m_iRecvBytes != (DataCount * 12)) { m_SO7_Serial.m_RecvData[0] = 0xff; memcpy(CacheData, m_SO7_Serial.m_RecvData, m_SO7_Serial.m_iRecvBytes); //返回内容 g_pLogger->SendAndFlushWithTime(L"[GetPositionXyzCache] m_iRecvBytes: %d != Points: %d\n", m_SO7_Serial.m_iRecvBytes, DataCount * 12); rStatus = HSI_STATUS_FAILED; } else { m_SO7_Serial.m_RecvData[0] = 0xff; memcpy(CacheData, m_SO7_Serial.m_RecvData, m_SO7_Serial.m_iRecvBytes); //返回内容 DataCount = m_SO7_Serial.m_iRecvBytes ; //返回点数 g_pLogger->SendAndFlushWithTime(L"[GetPositionXyzCache] m_iRecvBytes ok, Points: %d\n", m_SO7_Serial.m_iRecvBytes); m_SO7_Serial.m_iRecvState = FALSE; //将接收标志重置 rStatus = HSI_STATUS_NORMAL; } } else { g_pLogger->SendAndFlushWithTime(L"[GetPositionXyzCache] Serial Port is unavailable! \n"); rStatus = HSI_STATUS_FAILED; } g_pLogger->SendAndFlushWithTime(L"[GetPositionXyzCache] end\n"); } return rStatus; } //=========================================================================== /** * \brief 圆弧插补 * \param PositionX * \param PositionY * \param PositionZ * \return */ HSI_STATUS HSI_Motion::SetCircleInterpolate(double PositionX, double PositionY, double PositionZ) { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { g_pLogger->SendAndFlushWithTime(L"SetCircleInterpolate : start\n"); bCircleRun = true; iCircleRunPnt[1] = static_cast(PositionX / m_Resolution[1]); iCircleRunPnt[2] = static_cast(PositionY / m_Resolution[2]); iCircleRunPnt[3] = static_cast(PositionZ / m_Resolution[3]); g_pLogger->SendAndFlushWithTime(L"SetCircleInterpolate : end\n"); } return rStatus; } //=========================================================================== /** * \brief 探针接口 * \param RetractManDist */ void HSI_Motion::ProbeRetractManDist(int RetractManDist) { if (g_pHSI_Motion) { m_cSendData[0] = 0x01; m_cSendData[1] = 23; m_cSendData[2] = (RetractManDist >> 24 & 0xff); m_cSendData[3] = ((RetractManDist >> 16) & 0xff); m_cSendData[4] = ((RetractManDist >> 8) & 0xff); m_cSendData[5] = ((RetractManDist) & 0xff); m_cSendData[6] = (m_ProbeReturnSpeed >> 8) & 0xff; m_cSendData[7] = m_ProbeReturnSpeed & 0xff; m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength); Sleep(10); } } HSI_STATUS HSI_Motion::JogProbe(UINT AxisTypes, double Speed) { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { while (m_SO7_Serial.m_RecvData[57]) { m_cSendData[0] = CT_ORDER; m_cSendData[1] = CT_POSFLAG_CLEAR; m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength); Sleep(10); } unsigned char send_probe_data[64] = {0}; unsigned char motionAxis = 0; unsigned char motionDir = 0; for (size_t i = 0; i < 3; i++) { if ((i == 0) && (m_axisDirX != 0) || (i == 1) && (m_axisDirY != 0) || (i == 2) && (m_axisDirZ != 0)) { if ((m_axisDirX == 1) && (i == 0)) { motionAxis |= 0x01; motionDir |= 0x01; } else if ((i == 0) && (m_axisDirX == 2)) { motionAxis |= 0x01; } if ((m_axisDirY == 1) && (i == 1)) { motionAxis |= 0x02; motionDir |= 0x02; } else if ((i == 1) && (m_axisDirY == 2)) { motionAxis |= 0x02; } if ((m_axisDirZ == 1) && (i == 2)) { motionAxis |= 0x04; motionDir |= 0x04; } else if ((i == 2) && (m_axisDirZ == 2)) { motionAxis |= 0x04; } int DriveSpeed(1); int StartSpeed(1); int AccLine(1); int DecLine(1); int AccCurve(1); int DecCurve(1); int JogSpeed(1); byte AxisNumber = static_cast(IndexConvertAxis(i + 1)); m_Thread_State = HSI_THREAD_PAUSED; JogSpeed = abs(SpeedPercent(AxisNumber, Speed, DriveSpeed, StartSpeed, AccLine, DecLine, AccCurve, DecCurve)); send_probe_data[0] = CT_MOTOR; send_probe_data[1] = CT_MOTOR_SET; send_probe_data[2] = AxisNumber; send_probe_data[3] = JOG_SPEED_ACC_DEC; send_probe_data[4] = (StartSpeed & 0xff); send_probe_data[5] = ((StartSpeed >> 8) & 0xff); send_probe_data[6] = ((StartSpeed >> 16) & 0xff); send_probe_data[7] = ((StartSpeed >> 24) & 0xff); send_probe_data[8] = (m_probeSeekSpeed & 0xff); send_probe_data[9] = ((m_probeSeekSpeed >> 8) & 0xff); send_probe_data[10] = ((m_probeSeekSpeed >> 16) & 0xff); send_probe_data[11] = ((m_probeSeekSpeed >> 24) & 0xff); send_probe_data[12] = (AccCurve & 0xff); send_probe_data[13] = ((AccCurve >> 8) & 0xff); send_probe_data[14] = ((AccCurve >> 16) & 0xff); send_probe_data[15] = ((AccCurve >> 24) & 0xff); send_probe_data[16] = (AccLine & 0xff); send_probe_data[17] = ((AccLine >> 8) & 0xff); send_probe_data[18] = ((AccLine >> 16) & 0xff); send_probe_data[19] = ((AccLine >> 24) & 0xff); send_probe_data[20] = (DecCurve & 0xff); send_probe_data[21] = ((DecCurve >> 8) & 0xff); send_probe_data[22] = ((DecCurve >> 16) & 0xff); send_probe_data[23] = ((DecCurve >> 24) & 0xff); send_probe_data[24] = (DecLine & 0xff); send_probe_data[25] = ((DecLine >> 8) & 0xff); send_probe_data[26] = ((DecLine >> 16) & 0xff); send_probe_data[27] = ((DecLine >> 24) & 0xff); m_WriteByte = Send_Command(0, (const char*)send_probe_data, m_SendDataLength); Sleep(5); } } send_probe_data[0] = CT_MOTOR; send_probe_data[1] = CT_START_LATCH; send_probe_data[2] = motionAxis; send_probe_data[3] = motionDir; int returnPos = m_ProbeReturnPos / m_Resolution[AxisTypes]; send_probe_data[4] = (returnPos >> 8) & 0xff; send_probe_data[5] = returnPos & 0xff; send_probe_data[6] = (m_ProbeReturnSpeed >> 8) & 0xff; send_probe_data[7] = m_ProbeReturnSpeed & 0xff; m_WriteByte = Send_Command(0, (const char*)send_probe_data, m_SendDataLength); Sleep(3); // } return rStatus; } //=========================================================================== /** * \brief 读取配置 * \param GoogolIniFile * \return */ HSI_STATUS HSI_Motion::Load_EF3_Motion_Inifile(CString GoogolIniFile) { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { CString csAppPath = GoogolIniFile; USES_CONVERSION; CString temp = L""; CString strGear[5] = {L"GEAR0_", L"GEAR1_", L"GEAR2_", L"GEAR3_", L"GEAR4_"}; CString axisNum[5] = {L"0", L"1", L"2", L"3", L"4"}; //判断Log目录是否存在，不存在就创建 if (CreateDirectory(m_AppPath + L"\\Log", nullptr)) { g_pLogger->SendAndFlushWithTime(L"[Load_EF3_Motion_Inifile] Create Log Directory\n"); CreateDirectory(m_AppPath + L"\\Log", nullptr); } if (GetFileAttributes(GoogolIniFile) == -1) { g_pLogger->SendAndFlushWithTime(L"[Load_EF3_Motion_Inifile] EF3_Motion.ini file no find\n"); sEvenProp.Init(); sEvenProp.EventType = HSI_EVENT_ERROR; sEvenProp.EventID = HSI_EVENT_MOTION; sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK; strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "EF3_Motion.ini文件不存在！"); EventCallback(sEvenProp); return HSI_STATUS_FAILED; } m_motorType = GetPrivateProfileInt(L"MOTORTYPE", L"SERVOMOTOR", 1, csAppPath); for (int i = 1; i < 5; i++) { m_precisionCount[i] = GetPrivateProfileInt(L"PRECISION", L"PRECISION_COUNT_" + axisNum[i], 14000, csAppPath); //回家误差脉冲个数 m_precisionTime[i] = static_cast(GetPrivateProfileInt(L"PRECISION", L"PRECISION_TIME_" + axisNum[i], 14000, csAppPath)); //超时时间(0.1ms) GetPrivateProfileString(L"RESOLUTION", L"SCALE_RESOLUTION_" + axisNum[i], L"0.0004", temp.GetBufferSetLength(50), 50, csAppPath); m_Resolution[i] = (atof(T2A(temp)));//分辨率 GetPrivateProfileString(L"LIMIT", L"NEG_WORKING_LIMIT_" + axisNum[i], L"-40", temp.GetBufferSetLength(50), 50, csAppPath); m_N_Work_Limit[i] = (atof(T2A(temp))); //各轴负限位 GetPrivateProfileString(L"LIMIT", L"POS_WORKING_LIMIT_" + axisNum[i], L"160", temp.GetBufferSetLength(50), 50, csAppPath); m_P_Work_Limit[i] = (atof(T2A(temp)));//各轴正限位 //打印正负限位 g_pLogger->SendAndFlushWithTime(L"[Load_EF3_Motion_Inifile] m_N_Work_Limit[%d]: %.2f, m_P_Work_Limit[%d]: %.2f\n", i, m_N_Work_Limit[i], i, m_P_Work_Limit[i]); m_Home_Time[i] = static_cast(GetPrivateProfileInt(L"HOME", L"HOME_TIME_" + axisNum[i], 1500, csAppPath)); //回家超时时间(0.1ms) m_Home_AddJogGears[i] = GetPrivateProfileInt(L"HOME", L"HOME_ADD_JOGCHOICE_" + axisNum[i], 100, csAppPath); m_Home_DecJogGears[i] = GetPrivateProfileInt(L"HOME", L"HOME_DEC_JOGCHOICE_" + axisNum[i], 100, csAppPath); m_SetPotion_Count[i] = GetPrivateProfileInt(L"SET_POSITION_SPEED", L"SET_POSITION_COUNT_" + axisNum[i], 10, csAppPath); m_SetPotion_DriveSpeed[i] = GetPrivateProfileInt(L"SET_POSITION_SPEED", L"SET_POTION_DRIVESPEED_" + axisNum[i], 100, csAppPath); //m_SetPotion_DriveSpeed[i] = m_SetPotion_DriveSpeed[i] / (m_Resolution[i] * 50); //打印定位合成速度 g_pLogger->SendAndFlushWithTime(L"[Load_EF3_Motion_Inifile] m_SetPotion_DriveSpeed[%d]: %d\n", i, m_SetPotion_DriveSpeed[i]); m_SetPotion_Line[i] = GetPrivateProfileInt(L"SET_POSITION_SPEED", L"SET_POTION_LINE_" + axisNum[i], 100, csAppPath); m_SetPotion_StartSpeed[i] = GetPrivateProfileInt(L"SET_POSITION_SPEED", L"SET_POTION_STARTSPEED_" + axisNum[i], 100, csAppPath); m_SetPotion_StartSpeed[i] = m_SetPotion_StartSpeed[i] / (m_Resolution[i] * 500); m_SetPotion_Buffer[i] = GetPrivateProfileInt(L"SET_POSITION_SPEED", L"SET_POTION_BUFFER_" + axisNum[i], 105, csAppPath); m_LogIsOpen[i] = GetPrivateProfileInt(L"LOG", L"LOG_IS_OPEN_" + axisNum[i], 0, csAppPath); m_Home_Machine_Axis[i] = GetPrivateProfileInt(L"HOME", L"HOME_MACHINE_AXIS_" + axisNum[i], 0, csAppPath); m_Home_Pos_Axis[i] = GetPrivateProfileInt(L"HOME", L"HOME_POS_AXIS_" + axisNum[i], 0, csAppPath); m_StopJogMode[i] = GetPrivateProfileInt(L"JOG_SPEED", L"JOG_STOP_MODE_" + axisNum[i], 0, csAppPath); m_SpeedAdjustPeriod[i] = GetPrivateProfileInt(L"SET_POSITION_SPEED", L"SPEED_ADJUSTMENT_" + axisNum[i], 1, csAppPath); m_SpeedMax[i] = GetPrivateProfileInt(L"SET_POSITION_SPEED", L"SPEED_RUNMAX_" + axisNum[i], 150, csAppPath); } for (int i = 0; i < 5; i++) // i 轴 { for (int j = 1; j < 5; j++) //j 档位 { GetPrivateProfileString(L"JOG_SPEED", L"JOG_DRIVESPEED_" + strGear[i] + axisNum[j], L"10", temp.GetBufferSetLength(50), 10, csAppPath); float speed = (atof(T2A(temp))); //printf("spedd:%.2F %S\n",speed, L"JOG_DRIVESPEED_" + strGear[i] + axisNum[j]); //调试打印 /*m_JogDriveSpeed[j][i] = GetPrivateProfileInt(L"JOG_SPEED", L"JOG_DRIVESPEED_" , 10, csAppPath);*/ //m_JogDriveSpeed[j][i] = speed / (m_Resolution[j] * 50);//速度转换为脉冲 m_JogDriveSpeed[j][i] = speed; //直接读取速度 g_pLogger->SendAndFlushWithTime(L"[Load_EF3_Motion_Inifile] m_JogDriveSpeed[%d][%d]: %ld %ld\n", i, j, speed, m_JogDriveSpeed[j][i]); //打印配置文件 档位速度 GetPrivateProfileString(L"JOG_SPEED", L"JOG_STARTSPEED_" + strGear[i] + axisNum[j], L"10", temp.GetBufferSetLength(50), 10, csAppPath); speed = (atof(T2A(temp))); m_JogStartSpeed[j][i] = GetPrivateProfileInt(L"JOG_SPEED", L"JOG_STARTSPEED_" + strGear[i] + axisNum[j], 10, csAppPath); //m_JogStartSpeed[j][i] = speed / (m_Resolution[j] * 50); m_JogAccLine[j][i] = GetPrivateProfileInt(L"JOG_SPEED", L"JOG_LINE_" + strGear[i] + axisNum[j], 10, csAppPath); m_JogDecLine[j][i] = GetPrivateProfileInt(L"JOG_SPEED", L"JOG_LINE_" + strGear[i] + axisNum[j], 10, csAppPath); } } m_Jog_Auto_Focus = m_JogDriveSpeed[3][4]; m_Home_Machine_Axis[1] = GetPrivateProfileInt(L"HOME", L"HOME_MACHINE_AXIS_1", 1, csAppPath); m_Home_Machine_Axis[2] = GetPrivateProfileInt(L"HOME", L"HOME_MACHINE_AXIS_2", 1, csAppPath); m_Home_Machine_Axis[3] = GetPrivateProfileInt(L"HOME", L"HOME_MACHINE_AXIS_3", 1, csAppPath); m_Home_Machine_Axis[4] = GetPrivateProfileInt(L"HOME", L"HOME_MACHINE_AXIS_4", 1, csAppPath); m_IsHomeEncPos = GetPrivateProfileInt(L"HOME", L"IS_HOME_ENC_POS", 0, csAppPath); m_IsHomePrfPos = GetPrivateProfileInt(L"HOME", L"IS_HOME_PRF_POS", 1, csAppPath); m_Set_XYZA_Reserve = GetPrivateProfileInt(L"SET_XYZ", L"SET_XYZ_RESERVE", 2, csAppPath); m_setPositionDelay = GetPrivateProfileInt(L"SETPOSITION", L"SETPOSITION_DELAY", 1, csAppPath); m_setPositionPrecision = GetPrivateProfileInt(L"SETPOSITION", L"SETPPSITION_PRECISION", 1, csAppPath); m_setPositionNum = GetPrivateProfileInt(L"SETPOSITION", L"SETPOSITION_NUMBER", 1, csAppPath); //两块四路光源板 m_isUseAport = GetPrivateProfileInt(L"COMPORT", L"IS_COM_PORT_A", 0, csAppPath); m_portAnum = GetPrivateProfileInt(L"COMPORT", L"COM_PORT_A", 0, csAppPath); m_isUseBport = GetPrivateProfileInt(L"COMPORT", L"IS_COM_PORT_B", 0, csAppPath); m_portBnum = GetPrivateProfileInt(L"COMPORT", L"COM_PORT_B", 0, csAppPath); m_iGlueStartSpeed = GetPrivateProfileInt(L"GLUEDISPENSER", L"DISPENSER_STARTSPEED", 0, csAppPath); m_iGlueStartSpeed = m_iGlueStartSpeed / (m_Resolution[1] * 500); m_iGlueDriveSpeed = GetPrivateProfileInt(L"GLUEDISPENSER", L"DISPENSER_DRIVESPEED", 0, csAppPath); m_iGlueDriveSpeed = m_iGlueDriveSpeed / (m_Resolution[1] * 50); m_iGlueAccSpeed = GetPrivateProfileInt(L"GLUEDISPENSER", L"DISPENSER_ACCSPEED", 0, csAppPath); m_LogIsOpen[0] = GetPrivateProfileInt(L"LOG", L"LOG_IS_OPEN_0", 0, csAppPath); g_pLogger->IsEnabledLog = m_LogIsOpen[0] == 1 ? true : false; m_iSpeedType = GetPrivateProfileInt(L"SET_SPEED", L"SPEEDTYPE", 0, csAppPath); CString comNum[5] = {L"0", L"1", L"2", L"3"}; for (int i = 0; i < 4; i++) { GetPrivateProfileString(L"ROCKER_SPEED", L"ROCKER_HSTARTSPEED_" + comNum[i], L"5", temp.GetBufferSetLength(50), 10, csAppPath); float speed = (atof(T2A(temp))); m_rockerHStartSpeed[i] = speed / (m_Resolution[i] * 50); GetPrivateProfileString(L"ROCKER_SPEED", L"ROCKER_HDRIVESPEED_" + comNum[i], L"20", temp.GetBufferSetLength(50), 10, csAppPath); speed = (atof(T2A(temp))); m_rockerHDriveSpeed[i] = speed / (m_Resolution[i] * 50); GetPrivateProfileString(L"ROCKER_SPEED", L"ROCKER_LSTARTSPEED_" + comNum[i], L"2", temp.GetBufferSetLength(50), 10, csAppPath); speed = (atof(T2A(temp))); m_rockerLStartSpeed[i] = speed / (m_Resolution[i] * 50); GetPrivateProfileString(L"ROCKER_SPEED", L"ROCKER_LDRIVESPEED_" + comNum[i], L"10", temp.GetBufferSetLength(50), 10, csAppPath); speed = (atof(T2A(temp))); m_rockerLDriveSpeed[i] = speed / (m_Resolution[i] * 50); GetPrivateProfileString(L"ROCKER_SPEED", L"ROCKER_ASPEED_" + comNum[i], L"100", temp.GetBufferSetLength(50), 10, csAppPath); m_rockerASpeed[i] = (atoi(T2A(temp))); GetPrivateProfileString(L"ROCKER_SPEED", L"ROCKER_DSPEED_" + comNum[i], L"100", temp.GetBufferSetLength(50), 10, csAppPath); m_rockerDSpeed[i] = (atoi(T2A(temp))); } CString strAxis[10] = {L"0", L"1", L"2", L"3", L"4", L"5", L"6", L"7", L"8", L"9"}; for (int i = 0; i < 10; i++) { m_stepPosition_L_speed[i] = GetPrivateProfileInt(L"STEPMOTORPOSITIONINIT", L"StepPositionLSpeed_" + strAxis[i], 1, csAppPath); m_stepPosition_L_speed[i] = m_stepPosition_L_speed[i] / (m_Resolution[1] * 50); m_stepPosition_H_speed[i] = GetPrivateProfileInt(L"STEPMOTORPOSITIONINIT", L"StepPositionHSpeed_" + strAxis[i], 1, csAppPath); m_stepPosition_H_speed[i] = m_stepPosition_H_speed[i] / (m_Resolution[1] * 50); m_stepPosition_Load[i] = GetPrivateProfileInt(L"STEPMOTORPOSITIONINIT", L"StepPositionLoad_" + strAxis[i], 1, csAppPath); m_stepPosition_Load[i] = m_stepPosition_Load[i] / (m_Resolution[1]); m_stepPosition_acc[i] = GetPrivateProfileInt(L"STEPMOTORPOSITIONINIT", L"StepPositionAcc_" + strAxis[i], 1, csAppPath); } CString SubArea[8] = {L"0", L"1", L"2", L"3", L"4", L"5", L"6", L"7"}; for (int i = 0; i < 8; i++) { m_SixEightSubArea[i] = GetPrivateProfileInt(L"SIXEIGHTSUBAREA", L"SIXEIGHT_SUBARE_" + SubArea[i], 0, csAppPath); } } return rStatus; } //=========================================================================== HSI_STATUS HSI_Motion::Load_EF3_Config_Inifile(CString GoogolIniFile) { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { CString csAppPath = GoogolIniFile; USES_CONVERSION; CString temp; if (GetFileAttributes(GoogolIniFile) == -1) { g_pLogger->SendAndFlushWithTime(L"[Load_EF3_Config_Inifile] EF3_Config.ini file no find\n"); sEvenProp.Init(); sEvenProp.EventType = HSI_EVENT_ERROR; sEvenProp.EventID = HSI_EVENT_MOTION; sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK; strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "EF3_Config.ini文件不存在！"); EventCallback(sEvenProp); return HSI_STATUS_FAILED; } m_IsUseEF3 = GetPrivateProfileInt(L"EF3", L"IS_USEEF3", 0, csAppPath); m_IsUseACS = GetPrivateProfileInt(L"EF3", L"IS_USEACS", 0, csAppPath); m_ForSoft = GetPrivateProfileInt(L"SOFTWARE", L"USE_SOFTWARE", 0, csAppPath); m_IsUse_HSICompensation = GetPrivateProfileInt(L"ASIX", L"IS_USE_HSICOMPENSATION", 0, csAppPath); m_Compensation_Pluse = GetPrivateProfileInt(L"ASIX", L"COMPENSATE_PLUSE", 20, csAppPath); m_IsHardLimit = GetPrivateProfileInt(L"ASIX", L"IS_HARD_LIMIT", 7, csAppPath); m_IsEnableAxis = GetPrivateProfileInt(L"ASIX", L"ISENABLE_AXIS", 7, csAppPath); m_IsHavePattern = GetPrivateProfileInt(L"ASIX", L"ISHAVE_AXISPATTERN", 15, csAppPath); m_AxisHomeDirection = GetPrivateProfileInt(L"ASIX", L"AXIS_HOMEDIRECTION", 15, csAppPath); m_IsUseManualRunin = GetPrivateProfileInt(L"ASIX", L"IS_USEMANUALRUNIN", 0, csAppPath); m_axisLatchFrequency = GetPrivateProfileInt(L"ASIX", L"AXIS_LATCHFREQUENCY", 1000, csAppPath); //switch (m_axisLatchFrequency) //{ //case 1: // m_axisLatchFrequency = 1000; // break; //case 2: // m_axisLatchFrequency = 2000; // break; //case 3: // m_axisLatchFrequency = 4000; // break; //case 4: // m_axisLatchFrequency = 8000; // break; //case 5: // m_axisLatchFrequency = 16000; // break; //} //锁存频率处理 m_IsUseExternalTrigger = GetPrivateProfileInt(L"FUNCTION", L"IS_USEEXTERNALTRIGGER", 1, csAppPath); m_IsUseSixRingEightArea = GetPrivateProfileInt(L"FUNCTION", L"IS_USESIXRINGEIGHTAREA", 0, csAppPath); m_IsUseTwentySixLight = GetPrivateProfileInt(L"FUNCTION", L"IS_USETWENTYSIXLIGHT", 0, csAppPath); m_EF3LightType = GetPrivateProfileInt(L"FUNCTION", L"IS_LIGHTTYPE", 1, csAppPath); m_IsCollectPos = GetPrivateProfileInt(L"FUNCTION", L"IS_COLLECT_POS", 0, csAppPath); m_IsLightDebug = GetPrivateProfileInt(L"FUNCTION", L"IS_LIGHT_DEBUG", 0, csAppPath); m_IsIOFuntion = GetPrivateProfileInt(L"IO", L"IS_IO_FUNTION", 0, csAppPath); //是否启用IO功能，1为启用，0为关闭，默认0 m_IsStartInput = GetPrivateProfileInt(L"IO", L"IS_START_INPUT", 0, csAppPath); m_StartInputPort = GetPrivateProfileInt(L"IO", L"START_INPUT_PORT", 1, csAppPath); m_IsProbe = GetPrivateProfileInt(L"PROBE", L"IS_PROBE", 0, csAppPath); // 是否启用探针捕获功能，1启用，默认0关闭 m_ProbeAllAxis = GetPrivateProfileInt(L"PROBE", L"PROBE_All_AXIS", 3, csAppPath); m_ProbeReturnSpeed = GetPrivateProfileInt(L"PROBE", L"PROBE_RETURN_SPEED", 40, csAppPath); m_ProbeReturnSpeed = m_ProbeReturnSpeed * 50; GetPrivateProfileString(L"PROBE", L"PROBE_RETURN_POS", L"10.0", temp.GetBufferSetLength(50), 50, csAppPath); m_ProbeReturnPos = atof(T2A(temp)); m_IsUseRocker = GetPrivateProfileInt(L"ROCKER", L"IS_USEROCKER", 0, csAppPath); //是否启用摇杆功能 0不启用 1为启用 默认为0 m_IsCloseRocker = GetPrivateProfileInt(L"ROCKER", L"IS_CLOSEROCKER", 0, csAppPath); m_bISUseMoreLights = GetPrivateProfileInt(L"IPADDRESS", L"IS_USENUM", 0, csAppPath); m_LightType = GetPrivateProfileInt(L"LIGHTCONTROL", L"IS_LIGHTTYPE", 1, csAppPath); m_IsUseFourthSpeed = GetPrivateProfileInt(L"FOURTHSPEED", L"IS_USEFOURTHSPEED", 1, csAppPath); m_isOKGlint = GetPrivateProfileInt(L"LIGHTCONTROL", L"IS_OKGLINT", 0, csAppPath); m_ETIPort = GetPrivateProfileInt(L"LIGHTCONTROL", L"IS_ETIPORT", 0, csAppPath); m_DeviceType = GetPrivateProfileInt(L"DEVICE", L"DEVICE_TYPE", 1, csAppPath); m_UseAxisNum = GetPrivateProfileInt(L"DEVICE", L"USE_AXISNUM", 0, csAppPath); m_iJoyStick = GetPrivateProfileInt(L"JOYSTICK", L"JOY_STICK_TYPE", 0, csAppPath); CString comNum[5] = {L"0", L"1", L"2", L"3"}; for (int i = 0; i < 4; i++) { GetPrivateProfileString(L"IPADDRESS", L"IP_ADDRESS_" + comNum[i], L"192.168.0.1", temp.GetBufferSetLength(50), 50, csAppPath); m_IsOpenTCPIP[i] = temp; temp = temp.Trim(); int iNext = temp.Find(':'); CString ss = temp.Mid(0, iNext); if (ss == "8") { m_Led8MotionFlag[i] = true; } m_IsOpenTCPIP[i] = temp.Mid(iNext + 1, temp.GetLength() - iNext); } m_EF3COMPort = GetPrivateProfileInt(L"EF3COM", L"COMPORT", 0, csAppPath); } return rStatus; } //=========================================================================== /** * \brief 读取/设置光栅尺精度 * \param _ScaleX * \param _ScaleY * \param _ScaleZ * \return */ HSI_STATUS HSI_Motion::GetScaleResolution(double& _ScaleX, double& _ScaleY, double& _ScaleZ) { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { _ScaleX = m_Resolution[1]; _ScaleY = m_Resolution[2]; _ScaleZ = m_Resolution[3]; } return rStatus; } //=========================================================================== HSI_STATUS HSI_Motion::SetScaleResolution(double _ScaleX, double _ScaleY, double _ScaleZ) { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { m_Resolution[1] = static_cast(_ScaleX * 10000); m_Resolution[2] = static_cast(_ScaleY * 10000); m_Resolution[3] = static_cast(_ScaleZ * 10000); } return rStatus; } //=========================================================================== /** * \brief 定位完成事件 */ void HSI_Motion::SendMsgMotionFinished() { sEvenProp.Init(); sEvenProp.EventType = HSI_EVENT_FUNCTION; sEvenProp.EventID = HSI_EVENT_MOVE_POINT; //定位完成事件 sEvenProp.EventResponse = HSI_EVENT_FUNCTION_OK; EventCallback(sEvenProp); } //=========================================================================== /** * \brief 回调探针运行事件 */ void HSI_Motion::SendMsgProbeFinished() { sEvenProp.Init(); sEvenProp.EventType = HSI_EVENT_FUNCTION; sEvenProp.EventID = HSI_EVENT_MOTION_PROBE; sEvenProp.EventResponse = HSI_EVENT_FUNCTION_OK; EventCallback(sEvenProp); } //=========================================================================== /** * \brief 刷新运动状态，运动命令发出后，通过EF3板判断是否运动停止，读脉冲数进行到位轴判断（是否超时），欧式距离判断（是否超时），发送运动完成。 */ void HSI_Motion::UpdateMotionState() { if (g_pHSI_Motion && m_IsExMotion == 0) { g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] In\n"); while (m_Thread_State == HSI_THREAD_RUNNING) { g_IsClose = false; bool interpolationflag = false; bool timeoutflag = false; int timeStart = GetTickCount(); //do //{ // if (g_IsClose) // { // g_IsClose = false; // break; // } // // // if (m_SO7_Serial.m_RecvData[0] == 2) // { // if ((m_SO7_Serial.m_RecvData[39] & 0x10)) // { // m_SO7_Serial.m_RecvData[39] = 0; // SpCompleteTStart = GetTickCount(); // interpolationflag = true; // break; // } // //else if ((m_SO7_Serial.m_RecvData[39] == 0x07))//axis_start // if ((m_SO7_Serial.m_RecvData[39] == axis_start)/* &&(m_motorType == 0)*/) // { // m_SO7_Serial.m_RecvData[39] = 0; // SpCompleteTStart = GetTickCount(); // interpolationflag = true; // break; // } // if (m_SO7_Serial.m_RecvData[39] & 0x20) // { // m_SO7_Serial.m_RecvData[39] = 0; // timeoutflag = true; // break; // } // } // Sleep(1); // int timeEnd = GetTickCount(); // if (timeStart - timeEnd > 10 * 1000) // { // timeoutflag = true; // break; // } //} //while (true); g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Now wait Run End\n"); printf("\nWaiting for motion end\n"); acsc_WaitMotionEnd(handleACS, ACSC_AXIS_1, INFINITE);//依次等待 X,Y,Z轴运动到位 acsc_WaitMotionEnd(handleACS, ACSC_AXIS_0, INFINITE); 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] }; //用于启动时需要回原点的轴号选择,读取回家误差脉冲数 tempPrecision[1] = m_Home_Machine_Axis[1] == 0 ? 10000000 : m_precisionCount[1]; tempPrecision[2] = m_Home_Machine_Axis[2] == 0 ? 10000000 : m_precisionCount[2]; tempPrecision[3] = m_Home_Machine_Axis[3] == 0 ? 10000000 : m_precisionCount[3]; tempPrecision[4] = m_Home_Machine_Axis[4] == 0 ? 10000000 : m_precisionCount[4]; int i = 0, j = 0; unsigned long Count = 0; double prfpos[5] = {0}; interpolationflag = true; if (interpolationflag && m_motorType) { while (Count < m_SetPotion_Count[1]) //到位次数判断, 读配置文件240 { Sleep(2); //延时2毫秒，1秒 =1000 毫秒 //获取当前位置 GetPositionXyz(HSI_MOTION_AXIS_ALL, prfpos[1], prfpos[2], prfpos[3], prfpos[0]); //-----------TEST Begin------------------ g_pLogger->SendAndFlushWithTime( L"[UpdateMotionState] Moving to target, TargetPos[1] = %.4f, NowPos[2] = %.4f, fabs[3] = %.4f\n", m_PosThread[1], prfpos[1], fabs(m_PosThread[1] - prfpos[1])); //-----------TEST End-------------------- //目标位置 和当前位置 小于回家误差脉冲数 if ((fabs(m_PosThread[1] - prfpos[1]) <= tempPrecision[1] * m_Resolution[1]) && ( fabs(m_PosThread[2] - prfpos[2]) <= tempPrecision[2] * m_Resolution[2]) && ( fabs(m_PosThread[3] - prfpos[3]) <= tempPrecision[3] * m_Resolution[3]) && (fabs( m_PosThread[4] - m_PosForAllAxis[4]) <= tempPrecision[4] * m_Resolution[4])) { i++; if (m_SetPotion_Count[1] > m_setPositionNum) { j = m_setPositionNum; } else { j = m_SetPotion_Count[1]; } if (i == j) { set_end = GetTickCount(); break; } } else { } Count++; //g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] m_SetPotion_Count = %d\n", Count); } //if (Count == m_SetPotion_Count[1]) //超时退出 //{ // if (g_IsClose == false) // { // g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Nowait Timeout\n"); // sEvenProp.Init(); // sEvenProp.EventType = HSI_EVENT_ERROR; // sEvenProp.EventID = HSI_EVENT_MOTION; // sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK; // strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "Nowait_HSI定位超时!"); // EventCallback(sEvenProp); // } // else // { // g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Nowait Timeout Is AbortMotion\n"); // } //} } //计算距离 if (interpolationflag) { SpCompleteTEnd = GetTickCount(); SpTimeCount = SpCompleteTEnd - SpCompleteTStart; double dis = pow(m_PrfPos[1] - m_EncPos[1], 2.0) + pow(m_PrfPos[2] - m_EncPos[2], 2.0) + pow( m_PrfPos[3] - m_EncPos[3], 2.0); if (dis > 0) { PntToPntDistance = sqrt(dis); //欧氏距离 } else { PntToPntDistance = 0.0; } SetPotionRunEnd = true; } g_pLogger->SendAndFlushWithTime( L"[UpdateMotionState] m_PosThread[1] = %.4f,m_PosThread[2] = %.4f,m_PosThread[3] = %.4f\n", m_PosThread[1], m_PosThread[2], m_PosThread[3]); g_pLogger->SendAndFlushWithTime( L"[UpdateMotionState] Pos[1] = %.4f,Pos[2] = %.4f,Pos[3] = %.4f PntToPntDistance = %.4f\n", prfpos[1], prfpos[2], prfpos[3], PntToPntDistance); if (timeoutflag) //定位超时 { g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Nowait Timeout\n"); sEvenProp.Init(); sEvenProp.EventType = HSI_EVENT_ERROR; sEvenProp.EventID = HSI_EVENT_MOTION; sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK; strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "Nowait_EF3定位超时!"); EventCallback(sEvenProp); } switch (CurrentMotionState) { case E_SO7_MOTION_MOVETO: { g_pLogger->SendAndFlushWithTime( L"[UpdateMotionState] Nowait CurrentMotionState E_SO7_MOTION_MOVETO\n"); m_Thread_State = HSI_THREAD_PAUSED; CurrentMotionState = E_SO7_MOTION_NONE; m_IsExMotion = 2; SendMsgMotionFinished(); //定位完成 break; } default: { g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Nowait CurrentMotionState default\n"); break; } } } g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Out\n"); } } void HSI_Motion::UpdateMotionStateOld() { if (g_pHSI_Motion && m_IsExMotion == 0) { g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] In\n"); while (m_Thread_State == HSI_THREAD_RUNNING) { g_IsClose = false; bool interpolationflag = false; bool timeoutflag = false; int timeStart = GetTickCount(); // bool singleaxisflag_x = false; // bool singleaxisflag_y = false; // bool singleaxisflag_z = false; // bool singleaxisflag_a = false; do { if (g_IsClose) { g_IsClose = false; break; } if (m_SO7_Serial.m_RecvData[0] == 2) { if ((m_SO7_Serial.m_RecvData[39] & 0x10)) { m_SO7_Serial.m_RecvData[39] = 0; SpCompleteTStart = GetTickCount(); interpolationflag = true; break; } //else if ((m_SO7_Serial.m_RecvData[39] == 0x07))//axis_start if ((m_SO7_Serial.m_RecvData[39] == axis_start)/* &&(m_motorType == 0)*/) { m_SO7_Serial.m_RecvData[39] = 0; SpCompleteTStart = GetTickCount(); interpolationflag = true; break; } if (m_SO7_Serial.m_RecvData[39] & 0x20) { m_SO7_Serial.m_RecvData[39] = 0; timeoutflag = true; break; } } Sleep(1); int timeEnd = GetTickCount(); if (timeStart - timeEnd > 10 * 1000) { timeoutflag = true; break; } } while (true); g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Nowait Run End\n"); unsigned int tempPrecision[5] = { 0, m_precisionCount[1], m_precisionCount[2], m_precisionCount[3], m_precisionCount[4] }; tempPrecision[1] = m_Home_Machine_Axis[1] == 0 ? 10000000 : m_precisionCount[1]; //用于启动时需要回原点的轴号选择,读取回家误差脉冲数 tempPrecision[2] = m_Home_Machine_Axis[2] == 0 ? 10000000 : m_precisionCount[2]; tempPrecision[3] = m_Home_Machine_Axis[3] == 0 ? 10000000 : m_precisionCount[3]; tempPrecision[4] = m_Home_Machine_Axis[4] == 0 ? 10000000 : m_precisionCount[4]; int i = 0, j = 0; unsigned long Count = 0; double prfpos[5] = {0}; //GetPositionEncPrfMulti(1, m_EncPos, m_PrfPos, 4); //double EncPos[5] = { 0.0 }; // double ProPulse[5] = { 0.0 }; if (interpolationflag && m_motorType) { while (Count < m_SetPotion_Count[1]) //到位次数判断 { Sleep(2); GetPositionXyz(HSI_MOTION_AXIS_ALL, prfpos[1], prfpos[2], prfpos[3], prfpos[0]); //目标位置 和当前位置 小于回家误差脉冲数 if ((fabs(m_PosThread[1] - prfpos[1]) <= tempPrecision[1] * m_Resolution[1]) && ( fabs(m_PosThread[2] - prfpos[2]) <= tempPrecision[2] * m_Resolution[2]) && ( fabs(m_PosThread[3] - prfpos[3]) <= tempPrecision[3] * m_Resolution[3]) && (fabs( m_PosThread[4] - m_PosForAllAxis[4]) <= tempPrecision[4] * m_Resolution[4])) { i++; if (m_SetPotion_Count[1] > m_setPositionNum) { j = m_setPositionNum; } else { j = m_SetPotion_Count[1]; } if (i == j) { set_end = GetTickCount(); break; } } else { } Count++; g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] m_SetPotion_Count = %d\n", Count); //打印到位轴数量 } //if (Count == m_SetPotion_Count[1]) //超时退出 //{ // if (g_IsClose == false) // { // g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Nowait Timeout\n"); // sEvenProp.Init(); // sEvenProp.EventType = HSI_EVENT_ERROR; // sEvenProp.EventID = HSI_EVENT_MOTION; // sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK; // strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "Nowait_HSI定位超时!"); // EventCallback(sEvenProp); // } // else // { // g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Nowait Timeout Is AbortMotion\n"); // } //} } if (interpolationflag) { SpCompleteTEnd = GetTickCount(); SpTimeCount = SpCompleteTEnd - SpCompleteTStart; double dis = pow(m_PrfPos[1] - m_EncPos[1], 2.0) + pow(m_PrfPos[2] - m_EncPos[2], 2.0) + pow( m_PrfPos[3] - m_EncPos[3], 2.0); if (dis > 0) { PntToPntDistance = sqrt(dis); //欧氏距离 } else { PntToPntDistance = 0.0; } SetPotionRunEnd = true; } g_pLogger->SendAndFlushWithTime( L"[UpdateMotionState] m_PosThread[1] = %.4f,m_PosThread[2] = %.4f,m_PosThread[3] = %.4f\n", m_PosThread[1], m_PosThread[2], m_PosThread[3]); g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Pos[1] = %.4f,Pos[2] = %.4f,Pos[3] = %.4f\n", prfpos[1], prfpos[2], prfpos[3]); if (timeoutflag) //定位超时 { g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Nowait Timeout\n"); sEvenProp.Init(); sEvenProp.EventType = HSI_EVENT_ERROR; sEvenProp.EventID = HSI_EVENT_MOTION; sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK; strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "Nowait_EF3定位超时!"); EventCallback(sEvenProp); } switch (CurrentMotionState) { case E_SO7_MOTION_MOVETO: { g_pLogger->SendAndFlushWithTime( L"[UpdateMotionState] Nowait CurrentMotionState E_SO7_MOTION_MOVETO\n"); m_Thread_State = HSI_THREAD_PAUSED; CurrentMotionState = E_SO7_MOTION_NONE; m_IsExMotion = 2; SendMsgMotionFinished(); break; } default: { g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Nowait CurrentMotionState default\n"); break; } } } g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Out\n"); } } //=========================================================================== void HSI_Motion::UpdateMotionStateEx() { if (g_pHSI_Motion && m_IsExMotion == 1) { g_pLogger->SendAndFlushWithTime(L"[UpdateMotionStateEx] In\n"); g_pLogger->SendAndFlushWithTime(L"[UpdateMotionStateEx] Out\n"); } } //=========================================================================== void HSI_Motion::UpdateMotionStateIO() { if (g_pHSI_Motion) { int StartFlag = 0; UINT recvData = 0; while (m_Thread_StateIO == HSI_THREAD_RUNNING) { //1个脚踏开关 Sleep(3); GetDIO(HSI_MOTION_INPUT_CH1, m_InputStatus); if (m_IsStartInput == 1) { if ((m_InputStatus & m_StartInputPort) == 0) { StartFlag = 1; } else if (StartFlag == 1 && (m_InputStatus & m_StartInputPort) == m_StartInputPort) { if (m_isOKGlint == 1) { memset(m_cSendData, 0x00, 63); m_cSendData[0] = 0x02; m_cSendData[1] = 0x02; m_cSendData[2] = 0x01; m_cSendData[6] = 0x04; m_cSendData[7] = 0xff; m_cSendData[8] = 0xff; m_cSendData[9] = 0xff; m_cSendData[10] = 0xff; m_cSendData[14] = 0xff; m_WriteByte = Send_Command(1, (const char*)m_cSendData, m_SendDataLength); Sleep(5); } g_pLogger->SendAndFlushWithTime(L"[UpdateMotionStateIO] One Foot Switch Trigger\n"); StartFlag = 0; sEvenProp.Init(); sEvenProp.EventType = HSI_EVENT_NOTIFY; sEvenProp.EventID = HSI_NOTIFY_PROGRAM_EXECUTION_START; sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK; EventCallback(sEvenProp); } } if ((m_axisStatus & 0xAA) == 0xAA) { if (!m_bEmergencyState) { Sleep(100); GetDIO(HSI_MOTION_INPUT_CH1, m_InputStatus); if ((m_axisStatus & 0xAA) == 0xAA) { sEvenProp.Init(); sEvenProp.EventType = HSI_EVENT_NOTIFY; sEvenProp.EventID = HSI_NOTIFY_EMERGENCY_STATE; sEvenProp.EventData[0] = 1; m_bEmergencyState = true; EventCallback(sEvenProp); } } } else { if (m_bEmergencyState) { Sleep(100); GetDIO(HSI_MOTION_INPUT_CH1, m_InputStatus); if ((m_axisStatus & 0xAA) != 0xAA) { sEvenProp.Init(); sEvenProp.EventType = HSI_EVENT_NOTIFY; sEvenProp.EventID = HSI_NOTIFY_EMERGENCY_STATE; sEvenProp.EventData[0] = 0; m_bEmergencyState = false; EventCallback(sEvenProp); } } } } } } //=========================================================================== /** * \brief 获取运动状态 */ void HSI_Motion::UpdateMotionStateData() { while (m_Thread_StateData == HSI_THREAD_RUNNING) { if (m_bConnected) { Sleep(3); m_cSendData[0] = CT_ORDER; m_cSendData[1] = MOTORPOSITION; Send_Command(0, (const char*)m_cSendData, m_SendDataLength); } else { } } } //=========================================================================== void HSI_Motion::UpdateMotionStateProbe() { if (g_pHSI_Motion) { bool hitFlag = false; while (m_Thread_StateProbe == HSI_THREAD_RUNNING) { Sleep(3); if (g_IsClose) { g_IsClose = false; break; } if (m_SO7_Serial.m_RecvData[57] == 0) { hitFlag = true; } else if (m_SO7_Serial.m_RecvData[57] == 1) { //AbortMotion(); //Beep(300,1000); //printf("\a"); // m_SO7_Serial.m_RecvData[57] = 0; m_LockPos[1] = (m_SO7_Serial.m_RecvData[41] << 24 | m_SO7_Serial.m_RecvData[42] << 16 | m_SO7_Serial. m_RecvData[43] << 8 | m_SO7_Serial.m_RecvData[44]) * m_Resolution[1]; m_LockPos[2] = (m_SO7_Serial.m_RecvData[45] << 24 | m_SO7_Serial.m_RecvData[46] << 16 | m_SO7_Serial. m_RecvData[47] << 8 | m_SO7_Serial.m_RecvData[48]) * m_Resolution[2]; m_LockPos[3] = (m_SO7_Serial.m_RecvData[49] << 24 | m_SO7_Serial.m_RecvData[50] << 16 | m_SO7_Serial. m_RecvData[51] << 8 | m_SO7_Serial.m_RecvData[52]) * m_Resolution[3]; m_LockPos[4] = (m_SO7_Serial.m_RecvData[53] << 24 | m_SO7_Serial.m_RecvData[54] << 16 | m_SO7_Serial. m_RecvData[55] << 8 | m_SO7_Serial.m_RecvData[56]) * m_Resolution[4]; } else if (m_SO7_Serial.m_RecvData[57] == 2) { Sleep(400); m_LockPos[1] = 0.0; m_LockPos[2] = 0.0; m_LockPos[3] = 0.0; m_LockPos[4] = 0.0; } else if (m_SO7_Serial.m_RecvData[57] == 3 && hitFlag) { Sleep(200); hitFlag = false; double ProbeHitCompletePos[4] = {0}; GetPositionXyz(0, ProbeHitCompletePos[0], ProbeHitCompletePos[1], ProbeHitCompletePos[2], ProbeHitCompletePos[3]); m_ijk[0] = ProbeHitCompletePos[0]; m_ijk[1] = ProbeHitCompletePos[1]; m_ijk[2] = ProbeHitCompletePos[2]; g_pLogger->SendAndFlushWithTime(L"[UpdateMotionStateProbe] Probe hit\n"); SendMsgProbeFinished(); } } } } //=========================================================================== void HSI_Motion::DoEvents() { MSG msg; //GetMessage:从系统获取消息，将消息从系统中移除，属于阻塞函数。当系统无消息时，GetMessage会等待下一条消息。 //PeekMesssge是以查看的方式从系统中获取消息，可以不将消息从系统中移除，是非阻塞函数； //当系统无消息时，返回FALSE，继续执行后续代码 while (PeekMessage(&msg, nullptr, 0, 0, PM_REMOVE)) { __try { //DispatchMessage 函数分发一个消息给窗口程序。通常消息从GetMessage函数获得或者TranslateMessage函数传递的。 // 消息被分发到回调函数（过程函数)，作用是消息传递给操作系统， // 然后操作系统去调用我们的回调函数，也就是说我们在窗体的过程函数中处理消息。 DispatchMessage(&msg); TranslateMessage(&msg); } __except (EXCEPTION_EXECUTE_HANDLER) { g_pLogger->SendAndFlushWithTime(msg.message + L"----------DoEvents---------- : Exception"); sEvenProp.Init(); sEvenProp.EventType = HSI_EVENT_ERROR; sEvenProp.EventID = HSI_EVENT_MOTION; sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK; strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "DoEvents_异常"); EventCallback(sEvenProp); } if (g_IsClose) { break; } } } //=========================================================================== HSI_STATUS HSI_Motion::DriverAlarmStatus() { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { g_pLogger->SendAndFlushWithTime(L"[DriverAlarmStatus] In\n"); g_pLogger->SendAndFlushWithTime(L"[DriverAlarmStatus] Out\n"); } return rStatus; } //=========================================================================== HSI_STATUS HSI_Motion::FirstHome() { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { g_pLogger->SendAndFlushWithTime(L"[FirstHome] In\n"); g_pLogger->SendAndFlushWithTime(L"[FirstHome] Out\n"); } return rStatus; } //=========================================================================== HSI_STATUS HSI_Motion::SpecialMotorHome(short AxisNum) { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { g_pLogger->SendAndFlushWithTime(L"[SpecialMotorHome] In\n"); g_pLogger->SendAndFlushWithTime(L"[SpecialMotorHome] Out\n"); } return rStatus; } //=========================================================================== HSI_STATUS HSI_Motion::SpecialMotorMove(short AxisNumber, double Position) { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { g_pLogger->SendAndFlushWithTime(L"[SpecialMotorMove] In\n"); g_pLogger->SendAndFlushWithTime(L"[SpecialMotorMove] Out\n"); } return rStatus; } //========================IO=================================================== /** * \brief * \param IOChannel * \param _Status * \return */ HSI_STATUS HSI_Motion::GetDIOOld(UINT IOChannel, UINT& _Status) { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { if (m_SO7_Serial.m_RecvData[0] == 2) { if (IOChannel == HSI_MOTION_INPUT_LIMIT_SWITCH) { _Status = m_SO7_Serial.m_RecvData[33]; } if (IOChannel == HSI_MOTION_INPUT_ALARM) { _Status = m_SO7_Serial.m_RecvData[40]; } if (IOChannel == HSI_MOTION_INPUT_CH1) //获取通用输入 { _Status = m_SO7_Serial.m_RecvData[34]; _Status = (m_SO7_Serial.m_RecvData[35] | (_Status << 8)) & 0xffff; } if (IOChannel == HSI_MOTION_OUTPUT_CH1) //获取通用输出 { _Status = m_SO7_Serial.m_RecvData[36]; _Status = (m_SO7_Serial.m_RecvData[37] | (_Status << 8)) & 0xffff; } m_axisStatus = m_SO7_Serial.m_RecvData[58]; m_axisAlarmStatus = m_SO7_Serial.m_RecvData[59]; } else if (m_bISUseMoreLights > 0) { if (m_Led8MotionFlag[m_selectedIndex]) { if (tReciveData[0] == 0x05) { _Status = tReciveData[2]; } } else { if (tReciveData[0] == 0x05) { _Status = tReciveData[1] << 24 | tReciveData[1] << 16 | tReciveData[3] << 8 | tReciveData[4]; } } } else { g_pLogger->SendAndFlushWithTime(L"[GetDIO] failed\n"); } } return rStatus; } HSI_STATUS HSI_Motion::GetDIO(UINT IOChannel, UINT& _Status) { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { g_pLogger->SendAndFlushWithTime(L"[GetDIO] In\n"); if (m_SO7_Serial.m_RecvData[0] == 2) { if (IOChannel == HSI_MOTION_INPUT_LIMIT_SWITCH) { _Status = m_SO7_Serial.m_RecvData[33]; } if (IOChannel == HSI_MOTION_INPUT_ALARM) { _Status = m_SO7_Serial.m_RecvData[40]; } if (IOChannel == HSI_MOTION_INPUT_CH1) //获取通用输入 { _Status = m_SO7_Serial.m_RecvData[34]; _Status = (m_SO7_Serial.m_RecvData[35] | (_Status << 8)) & 0xffff; } if (IOChannel == HSI_MOTION_OUTPUT_CH1) //获取通用输出 { _Status = m_SO7_Serial.m_RecvData[36]; _Status = (m_SO7_Serial.m_RecvData[37] | (_Status << 8)) & 0xffff; } m_axisStatus = m_SO7_Serial.m_RecvData[58]; m_axisAlarmStatus = m_SO7_Serial.m_RecvData[59]; } else if (m_bISUseMoreLights > 0) { if (m_Led8MotionFlag[m_selectedIndex]) { if (tReciveData[0] == 0x05) { _Status = tReciveData[2]; } } else { if (tReciveData[0] == 0x05) { _Status = tReciveData[1] << 24 | tReciveData[1] << 16 | tReciveData[3] << 8 | tReciveData[4]; } } } else { g_pLogger->SendAndFlushWithTime(L"[GetDIO] failed\n"); } g_pLogger->SendAndFlushWithTime(L"[GetDIO] Out\n"); } //-----------TEST Begin------------------ _Status = 0; //-----------TEST End------------------ return rStatus; } //=========================================================================== /** * \brief * \param IOChannel * \param _Status * \return */ HSI_STATUS HSI_Motion::SetDIOOld(UINT IOChannel, UINT _Status) { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { if (m_bISUseMoreLights > 0 && (m_ForStatus != _Status)) { if (m_Led8MotionFlag[m_selectedIndex]) { m_ForStatus = _Status; IOdata[0] = 0x02; IOdata[1] = 0x02; IOdata[2] = 0x01; IOdata[6] = 0x04; IOdata[7] = 0xff; IOdata[8] = 0xff; IOdata[9] = 0xff; IOdata[10] = 0xff; IOdata[14] = _Status & 0xff; IOSend++; } else { m_ForStatus = _Status; IOdata[0] = 0x02; IOdata[1] = 0x02; IOdata[2] = 0x01; IOdata[3] = 0x36; IOdata[4] = 0x04; IOdata[7] = 0xff; IOdata[8] = 0xff; IOdata[9] = 0xff; IOdata[10] = 0xff; IOdata[11] = (_Status >> 24) & 0xff; IOdata[12] = (_Status >> 16) & 0xff; IOdata[13] = (_Status >> 8) & 0xff; IOdata[14] = _Status & 0xff; IOSend++; } } if ((IOChannel == HSI_MOTION_OUTPUT_CH1)/* && (m_ForStatus != _Status)*/) { m_ForStatus = _Status; m_cSendData[0] = CT_PORT; m_cSendData[1] = 0x02; m_cSendData[2] = (_Status >> 8) & 0xff; m_cSendData[3] = _Status & 0xff; m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength); Sleep(5); } } return rStatus; } HSI_STATUS HSI_Motion::SetDIO(UINT IOChannel, UINT _Status) { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { g_pLogger->SendAndFlushWithTime(L"[SetDIO] In\n"); if (m_bISUseMoreLights > 0 && (m_ForStatus != _Status)) { if (m_Led8MotionFlag[m_selectedIndex]) { m_ForStatus = _Status; IOdata[0] = 0x02; IOdata[1] = 0x02; IOdata[2] = 0x01; IOdata[6] = 0x04; IOdata[7] = 0xff; IOdata[8] = 0xff; IOdata[9] = 0xff; IOdata[10] = 0xff; IOdata[14] = _Status & 0xff; IOSend++; } else { m_ForStatus = _Status; IOdata[0] = 0x02; IOdata[1] = 0x02; IOdata[2] = 0x01; IOdata[3] = 0x36; IOdata[4] = 0x04; IOdata[7] = 0xff; IOdata[8] = 0xff; IOdata[9] = 0xff; IOdata[10] = 0xff; IOdata[11] = (_Status >> 24) & 0xff; IOdata[12] = (_Status >> 16) & 0xff; IOdata[13] = (_Status >> 8) & 0xff; IOdata[14] = _Status & 0xff; IOSend++; } } if ((IOChannel == HSI_MOTION_OUTPUT_CH1)/* && (m_ForStatus != _Status)*/) { m_ForStatus = _Status; m_cSendData[0] = CT_PORT; m_cSendData[1] = 0x02; m_cSendData[2] = (_Status >> 8) & 0xff; m_cSendData[3] = _Status & 0xff; //m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength); Sleep(5); } g_pLogger->SendAndFlushWithTime(L"[SetDIO] Out\n"); } //-----------TEST Begin------------------ _Status = 0; //-----------TEST End------------------ return rStatus; } //=========================================================================== HSI_STATUS HSI_Motion::GetAxisStatus(int* _Status) { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { _Status[0] = m_axisStatus; _Status[1] = m_axisAlarmStatus; } return rStatus; } //=========================================================================== /** * \brief 暂停和关闭 * \return */ HSI_STATUS HSI_Motion::AbortMotionOld() //需要运动实现 { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { g_IsClose = true; g_pLogger->SendAndFlushWithTime(L"[AbortMotion] In\n"); m_cSendData[0] = CT_ORDER; m_cSendData[1] = CT_STOP; m_cSendData[2] = AXIS_XYZU; m_StopJogMode[1] == 0 ? m_cSendData[3] = 0X4A : m_cSendData[3] = 0X05; m_StopJogMode[2] == 0 ? m_cSendData[3] = 0X4A : m_cSendData[3] = 0X05; m_StopJogMode[3] == 0 ? m_cSendData[3] = 0X4A : m_cSendData[3] = 0X05; m_StopJogMode[4] == 0 ? m_cSendData[3] = 0X4A : m_cSendData[3] = 0X05; m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength); Sleep(5); if (m_DeviceType == 3) { m_cSendData[0] = CT_TURNTABLE; m_cSendData[1] = CT_RTSTOP; m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength); Sleep(5); m_Thread_StateData = HSI_THREAD_RUNNING; SetEvent(m_hTriggerEventData); } if (bUseGlueDispenser) { bUseGlueDispenser = false; m_cSendData[0] = CT_MOTOR; m_cSendData[1] = CT_GLUEDISPENSER_STOP; m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength); Sleep(5); } g_pLogger->SendAndFlushWithTime(L"[AbortMotion] Out\n"); } return rStatus; } HSI_STATUS HSI_Motion::AbortMotion() //需要运动实现 { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { g_IsClose = true; g_pLogger->SendAndFlushWithTime(L"[AbortMotion] In\n"); int Axes[] = {ACSC_AXIS_1, ACSC_AXIS_0, ACSC_AXIS_8, -1}; if (handleACS != ACSC_INVALID) { if (!acsc_HaltM(handleACS, Axes, nullptr)) //停止JOG运动 { g_pLogger->SendAndFlushWithTime(L"[AbortMotion] ACS acsc_HaltM error!\n"); ErrorsHandler(); } } g_pLogger->SendAndFlushWithTime(L"[AbortMotion] Out\n"); } return rStatus; } //=========================================================================== /** * \brief 关闭 * \return */ HSI_STATUS HSI_Motion::Shutdown() { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { g_pLogger->SendAndFlushWithTime(L"[Shutdown] In\n"); g_IsClose = true; //SetDIO(HSI_MOTION_OUTPUT_CH1, 0xffff); if (m_bConnected) { CloseHandle(hCom); hCom = nullptr; //m_SO7_Serial.Close(); } if (m_IsCloseRocker == 0) { memset(m_cSendData, 0x00, 64); m_cSendData[0] = CT_SOFTSTOP; //m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength); Sleep(5); } if (m_bISUseMoreLights > 0) { for (size_t i = 0; i < m_bISUseMoreLights; i++) { if (m_Led8MotionFlag[i]) { m_selectedIndex = i; Orderdata[0] = 0x01; Orderdata[1] = 0x07; Orderdata[2] = 0x00; OrderSend++; } Sleep(20); } DisConnect(); } AbortMotion(); m_bConnected = false; CurrentMotionState = E_SO7_MOTION_NONE; CurrentHomeMachineState = E_EF3_HOME_NONE; m_Thread_StateData = HSI_THREAD_EXIT; m_Thread_StateData = HSI_THREAD_EXIT; m_Thread_StateIO = HSI_THREAD_EXIT; m_Thread_State = HSI_THREAD_EXIT; m_Thread_StateJOGStop = HSI_THREAD_EXIT; SendMsgMotionFinished(); CloseThread(); CloseThreadIO(); CloseThreadData(); CloseThreadProbe(); CloseThreadJOGStop(); ReleaseMutex(g_RW_Data_Mutex); CloseHandle(g_RW_Data_Mutex); ReleaseMutex(g_WR_ToMove_Mutex); CloseHandle(g_WR_ToMove_Mutex); g_pLogger->SendAndFlushWithTime(L"[Shutdown] Out\n"); } return rStatus; } //=========================================================================== /** * \brief 触发灯光 * \param triggleNum * \param delayLighting * \param delayLightBefor * \param triggleMode * \param Intensities * \return */ HSI_STATUS HSI_Motion::SetTriggerLight(int triggleNum, int delayLighting, int delayLightBefor, int triggleMode, double* Intensities) { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { setLightFlag = true; unsigned char m_SetTriggerLightData[64] = {0}; m_SetTriggerLightData[0] = CT_LIGHT; m_SetTriggerLightData[1] = 0x03; m_SetTriggerLightData[2] = triggleNum; int num = triggleNum / 3; int i = 0; for (i = 0; i < num; i++) { m_SetTriggerLightData[3] = i; int z = 4; for (int j = 24 * i; j < 24 * (i + 1); ++j) { m_SetTriggerLightData[z++] = (static_cast(Intensities[j] * 11.2) >> 8) & 0xff; m_SetTriggerLightData[z++] = static_cast(Intensities[j] * 11.2) & 0xff; } m_SetTriggerLightData[z++] = delayLighting & 0xff; m_SetTriggerLightData[z++] = triggleMode & 0xff; m_SetTriggerLightData[z] = delayLightBefor & 0xff; m_WriteByte = Send_Command(0, (const char*)m_SetTriggerLightData, m_SendDataLength); Sleep(1); } if ((num > 0) && (triggleNum % 3 != 0)) { m_SetTriggerLightData[3] = i; int z = 4; for (int j = 24 * i; j < 8 * triggleNum; ++j) { m_SetTriggerLightData[z++] = (static_cast(Intensities[j] * 11.2) >> 8) & 0xff; m_SetTriggerLightData[z++] = static_cast(Intensities[j] * 11.2) & 0xff; } m_SetTriggerLightData[52] = delayLighting & 0xff; m_SetTriggerLightData[53] = triggleMode & 0xff; m_SetTriggerLightData[54] = delayLightBefor & 0xff; m_WriteByte = Send_Command(0, (const char*)m_SetTriggerLightData, m_SendDataLength); Sleep(1); } else if (num == 0) { m_SetTriggerLightData[3] = 0; int z = 4; for (int j = 0; j < 8 * triggleNum; ++j) { m_SetTriggerLightData[z++] = (static_cast(Intensities[j] * 11.2) >> 8) & 0xff; m_SetTriggerLightData[z++] = static_cast(Intensities[j] * 11.2) & 0xff; } m_SetTriggerLightData[52] = delayLighting & 0xff; m_SetTriggerLightData[53] = triggleMode & 0xff; m_SetTriggerLightData[54] = delayLightBefor & 0xff; m_WriteByte = Send_Command(0, (const char*)m_SetTriggerLightData, m_SendDataLength); Sleep(1); } } return rStatus; } //=========================================================================== /** * \brief 硬件触发拍照 * \param startPoint * \return */ HSI_STATUS HSI_Motion::DCCPPStartPoint(double* startPoint) { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { m_IsUsePPS = true; begin_position[1] = static_cast(startPoint[1] / m_Resolution[1]); begin_position[2] = static_cast(startPoint[2] / m_Resolution[2]); begin_position[3] = static_cast(startPoint[3] / m_Resolution[3]); } return rStatus; } //=========================================================================== HSI_STATUS HSI_Motion::DCCScanSetDataOld(UINT AxisTypes, HSI_SCAN_MOTION_TYPE eType, UINT lTrigNumber, double* dTrigDis) { WaitForSingleObject(g_WR_ToMove_Mutex, INFINITE); auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { //触发的位置为相对位置,用法一般是移动到起点位置，再开始设置触发位置(相对位置)，最终设置终点位置 g_pLogger->SendAndFlushWithTime(L"[DCCScanSetData] In\n"); int axisNum; unsigned char m_SendDCCData[64] = {0}; m_SendDCCData[0] = CT_MOTOR; m_SendDCCData[1] = CT_TRIGGER_DATA; m_SendDCCData[2] = 0x01; iTriggleNum = lTrigNumber; double limit = 0.0; //memset(m_cSendData,0,64); if (AxisTypes & HSI_MOTION_AXIS_X) { iaxisNum = AXIS_X; axisNum = 1; } else if (AxisTypes & HSI_MOTION_AXIS_Y) { iaxisNum = AXIS_Y; axisNum = 2; } else if (AxisTypes & HSI_MOTION_AXIS_Z) { iaxisNum = AXIS_Z; axisNum = 3; } else { iaxisNum = AXIS_X; axisNum = 1; } switch (eType) { case HSI_SCAN_MOTION_SPEC_LOCA: { if (dTrigDis[0] > 0.0001) { iMotionDirection = 0; } else { iMotionDirection = 1; } switch (iaxisNum) { case AXIS_X: { if (!m_IsUsePPS) { begin_position[1] = begin_position[1] + dTrigDis[0] / m_Resolution[axisNum]; } else { if (iMotionDirection == 0) { begin_position[1] = begin_position[1] - 20; } else { begin_position[1] = begin_position[1] + 20; } } if (iMotionDirection == 0) { limit = 1000.0; } else { limit = -1000.0; } break; } case AXIS_Y: { if (!m_IsUsePPS) { begin_position[2] = begin_position[2] + dTrigDis[0] / m_Resolution[axisNum]; } else { if (iMotionDirection == 0) { begin_position[2] = begin_position[2] - 20; } else { begin_position[2] = begin_position[2] + 20; } } if (iMotionDirection == 0) { limit = 1000.0; } else { limit = -1000.0; } break; } case AXIS_Z: { if (!m_IsUsePPS) { begin_position[3] = begin_position[3] + dTrigDis[0] / m_Resolution[axisNum]; } else { if (iMotionDirection == 0) { begin_position[3] = begin_position[3] - 20; } else { begin_position[3] = begin_position[3] + 20; } } if (iMotionDirection == 0) { limit = 1000.0; } else { limit = -1000.0; } break; } default: break; } iScanMotionType = HSI_SCAN_MOTION_SPEC_LOCA; if (lTrigNumber > 100) { return HSI_STATUS_FAILED; } int num = lTrigNumber / 14; int i = 0; for (i = 0; i < num; i++) { m_SendDCCData[3] = i; for (size_t j = 14 * i, z = 4; j < 14 * (i + 1); ++j) { m_SendDCCData[z++] = static_cast(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) { iMotionDirection = 0; } else { iMotionDirection = 1; } switch (iaxisNum) { case AXIS_X: { if (!m_IsUsePPS) { begin_position[1] = begin_position[1] + dTrigDis[0] / m_Resolution[axisNum]; } else { if (iMotionDirection == 0) { begin_position[1] = begin_position[1] - 20; } else { begin_position[1] = begin_position[1] + 20; } } if (iMotionDirection == 0) { limit = 1000; } else { limit = -1000; } break; } case AXIS_Y: { if (!m_IsUsePPS) { begin_position[2] = begin_position[2] + dTrigDis[0] / m_Resolution[axisNum]; } else { if (iMotionDirection == 0) { begin_position[2] = begin_position[2] - 20; } else { begin_position[2] = begin_position[2] + 20; } } if (iMotionDirection == 0) { limit = 1000; } else { limit = -1000; } break; } case AXIS_Z: { if (!m_IsUsePPS) { begin_position[3] = begin_position[3] + dTrigDis[0] / m_Resolution[axisNum]; } else { if (iMotionDirection == 0) { begin_position[3] = begin_position[3] - 20; } else { begin_position[3] = begin_position[3] + 20; } } if (iMotionDirection == 0) { limit = 1000; } else { limit = -1000; } break; } default: break; } iScanMotionType = HSI_SCAN_MOTION_EQ_DIS; m_SendDCCData[3] = 0x00; m_SendDCCData[4] = static_cast(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) { iMotionDirection = 0; } else { iMotionDirection = 1; } switch (iaxisNum) { case AXIS_X: { if (!m_IsUsePPS) { begin_position[1] = begin_position[1] + dTrigDis[0] / m_Resolution[axisNum]; } else { if (iMotionDirection == 0) { begin_position[1] = begin_position[1] - 20; } else { begin_position[1] = begin_position[1] + 20; } } if (iMotionDirection == 0) { limit = 1000; } else { limit = -1000; } break; } case AXIS_Y: { if (!m_IsUsePPS) { begin_position[2] = begin_position[2] + dTrigDis[0] / m_Resolution[axisNum]; } else { if (iMotionDirection == 0) { begin_position[2] = begin_position[2] - 20; } else { begin_position[2] = begin_position[2] + 20; } } if (iMotionDirection == 0) { limit = 1000; } else { limit = -1000; } break; } case AXIS_Z: { if (!m_IsUsePPS) { begin_position[3] = begin_position[3] + dTrigDis[0] / m_Resolution[axisNum]; } else { if (iMotionDirection == 0) { begin_position[3] = begin_position[3] - 20; } else { begin_position[3] = begin_position[3] + 20; } } if (iMotionDirection == 0) { limit = 1000; } else { limit = -1000; } break; } default: break; } iScanMotionType = HSI_SCAN_MOTION_EQ_DIS_II; m_SendDCCData[3] = 0x00; int i = 0, j = 0; for (i = 0, j = 4; i < 2; ++i) { m_SendDCCData[j++] = static_cast(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; } g_pLogger->SendAndFlushWithTime(L"[DCCScanSetData] Out\n"); } ReleaseMutex(g_WR_ToMove_Mutex); return rStatus; } HSI_STATUS HSI_Motion::DCCScanSetData(UINT AxisTypes, HSI_SCAN_MOTION_TYPE eType, UINT lTrigNumber, double* dTrigDis) { WaitForSingleObject(g_WR_ToMove_Mutex, INFINITE); auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { //触发的位置为相对位置,用法一般是移动到起点位置，再开始设置触发位置(相对位置)，最终设置终点位置 g_pLogger->SendAndFlushWithTime(L"[DCCScanSetData] In AxisTypes:%d eType:%d lTrigNumber:%d dTrigDis:ld\n", AxisTypes, eType, lTrigNumber, dTrigDis); int axisNum; unsigned char m_SendDCCData[64] = {0}; m_SendDCCData[0] = CT_MOTOR; m_SendDCCData[1] = CT_TRIGGER_DATA; m_SendDCCData[2] = 0x01; iTriggleNum = lTrigNumber; double limit = 0.0; //memset(m_cSendData,0,64); if (AxisTypes & HSI_MOTION_AXIS_X) { iaxisNum = AXIS_X; axisNum = 1; } else if (AxisTypes & HSI_MOTION_AXIS_Y) { iaxisNum = AXIS_Y; axisNum = 2; } else if (AxisTypes & HSI_MOTION_AXIS_Z) { iaxisNum = AXIS_Z; axisNum = 3; } else { iaxisNum = AXIS_X; axisNum = 1; } switch (eType) { case HSI_SCAN_MOTION_SPEC_LOCA: { if (dTrigDis[0] > 0.0001) { iMotionDirection = 0; } else { iMotionDirection = 1; } switch (iaxisNum) { case AXIS_X: { if (!m_IsUsePPS) { begin_position[1] = begin_position[1] + dTrigDis[0] / m_Resolution[axisNum]; } else { if (iMotionDirection == 0) { begin_position[1] = begin_position[1] - 20; } else { begin_position[1] = begin_position[1] + 20; } } if (iMotionDirection == 0) { limit = 1000.0; } else { limit = -1000.0; } break; } case AXIS_Y: { if (!m_IsUsePPS) { begin_position[2] = begin_position[2] + dTrigDis[0] / m_Resolution[axisNum]; } else { if (iMotionDirection == 0) { begin_position[2] = begin_position[2] - 20; } else { begin_position[2] = begin_position[2] + 20; } } if (iMotionDirection == 0) { limit = 1000.0; } else { limit = -1000.0; } break; } case AXIS_Z: { if (!m_IsUsePPS) { begin_position[3] = begin_position[3] + dTrigDis[0] / m_Resolution[axisNum]; } else { if (iMotionDirection == 0) { begin_position[3] = begin_position[3] - 20; } else { begin_position[3] = begin_position[3] + 20; } } if (iMotionDirection == 0) { limit = 1000.0; } else { limit = -1000.0; } break; } default: break; } iScanMotionType = HSI_SCAN_MOTION_SPEC_LOCA; if (lTrigNumber > 100) { return HSI_STATUS_FAILED; } int num = lTrigNumber / 14; int i = 0; for (i = 0; i < num; i++) { m_SendDCCData[3] = i; for (size_t j = 14 * i, z = 4; j < 14 * (i + 1); ++j) { m_SendDCCData[z++] = static_cast(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) { iMotionDirection = 0; } else { iMotionDirection = 1; } switch (iaxisNum) { case AXIS_X: { if (!m_IsUsePPS) { begin_position[1] = begin_position[1] + dTrigDis[0] / m_Resolution[axisNum]; } else { if (iMotionDirection == 0) { begin_position[1] = begin_position[1] - 20; } else { begin_position[1] = begin_position[1] + 20; } } if (iMotionDirection == 0) { limit = 1000; } else { limit = -1000; } break; } case AXIS_Y: { if (!m_IsUsePPS) { begin_position[2] = begin_position[2] + dTrigDis[0] / m_Resolution[axisNum]; } else { if (iMotionDirection == 0) { begin_position[2] = begin_position[2] - 20; } else { begin_position[2] = begin_position[2] + 20; } } if (iMotionDirection == 0) { limit = 1000; } else { limit = -1000; } break; } case AXIS_Z: { if (!m_IsUsePPS) { begin_position[3] = begin_position[3] + dTrigDis[0] / m_Resolution[axisNum]; } else { if (iMotionDirection == 0) { begin_position[3] = begin_position[3] - 20; } else { begin_position[3] = begin_position[3] + 20; } } if (iMotionDirection == 0) { limit = 1000; } else { limit = -1000; } break; } default: break; } iScanMotionType = HSI_SCAN_MOTION_EQ_DIS; m_SendDCCData[3] = 0x00; m_SendDCCData[4] = static_cast(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) { iMotionDirection = 0; } else { iMotionDirection = 1; } switch (iaxisNum) { case AXIS_X: { if (!m_IsUsePPS) { begin_position[1] = begin_position[1] + dTrigDis[0] / m_Resolution[axisNum]; } else { if (iMotionDirection == 0) { begin_position[1] = begin_position[1] - 20; } else { begin_position[1] = begin_position[1] + 20; } } if (iMotionDirection == 0) { limit = 1000; } else { limit = -1000; } break; } case AXIS_Y: { if (!m_IsUsePPS) { begin_position[2] = begin_position[2] + dTrigDis[0] / m_Resolution[axisNum]; } else { if (iMotionDirection == 0) { begin_position[2] = begin_position[2] - 20; } else { begin_position[2] = begin_position[2] + 20; } } if (iMotionDirection == 0) { limit = 1000; } else { limit = -1000; } break; } case AXIS_Z: { if (!m_IsUsePPS) { begin_position[3] = begin_position[3] + dTrigDis[0] / m_Resolution[axisNum]; } else { if (iMotionDirection == 0) { begin_position[3] = begin_position[3] - 20; } else { begin_position[3] = begin_position[3] + 20; } } if (iMotionDirection == 0) { limit = 1000; } else { limit = -1000; } break; } default: break; } iScanMotionType = HSI_SCAN_MOTION_EQ_DIS_II; m_SendDCCData[3] = 0x00; int i = 0, j = 0; for (i = 0, j = 4; i < 2; ++i) { m_SendDCCData[j++] = static_cast(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}; //清空EF3缓存Flash m_cSendData[0] = 0x01; m_cSendData[1] = 0x04; m_WriteByte = Send_Command(0, (const char*)m_cSendData, 2); Sleep(2000);//延时2秒，等待清空完成，EF3清空Flash比较慢 g_pLogger->SendAndFlushWithTime(L"[DCCScanSetData] Out\n"); } ReleaseMutex(g_WR_ToMove_Mutex); return rStatus; } //=========================================================================== HSI_STATUS HSI_Motion::DCCScanStartOld() { WaitForSingleObject(g_WR_ToMove_Mutex, INFINITE); auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { if (!setLightFlag) { unsigned char m_SetTriggerLightData[64] = {0}; m_SetTriggerLightData[0] = CT_LIGHT; m_SetTriggerLightData[1] = 0x03; m_SetTriggerLightData[53] = 0; m_WriteByte = Send_Command(0, (const char*)m_SetTriggerLightData, m_SendDataLength); Sleep(1); } setLightFlag = false; g_pLogger->SendAndFlushWithTime(L"[DCCScanStart] In\n"); unsigned char m_SendDCCData[64] = {0}; m_SendDCCData[0] = CT_MOTOR; m_SendDCCData[1] = CT_TRIGGER_DATA; m_SendDCCData[2] = 0x00; m_SendDCCData[3] = iaxisNum; m_SendDCCData[4] = iScanMotionType; m_SendDCCData[5] = ((iTriggleNum) & 0xff); m_SendDCCData[6] = (iTriggleNum >> 8) & 0xff; m_SendDCCData[7] = iMotionDirection; switch (iaxisNum) { case AXIS_X: m_SendDCCData[8] = begin_position[1] & 0xff; m_SendDCCData[9] = (begin_position[1] >> 8) & 0xff; m_SendDCCData[10] = (begin_position[1] >> 16) & 0xff; m_SendDCCData[11] = (begin_position[1] >> 24) & 0xff; m_SendDCCData[12] = m_SetPotion_DriveSpeed[1] & 0xff; m_SendDCCData[13] = (m_SetPotion_DriveSpeed[1] >> 8) & 0xff; break; case AXIS_Y: m_SendDCCData[8] = begin_position[2] & 0xff; m_SendDCCData[9] = (begin_position[2] >> 8) & 0xff; m_SendDCCData[10] = (begin_position[2] >> 16) & 0xff; m_SendDCCData[11] = (begin_position[2] >> 24) & 0xff; m_SendDCCData[12] = m_SetPotion_DriveSpeed[2] & 0xff; m_SendDCCData[13] = (m_SetPotion_DriveSpeed[2] >> 8) & 0xff; break; case AXIS_Z: m_SendDCCData[8] = begin_position[3] & 0xff; m_SendDCCData[9] = (begin_position[3] >> 8) & 0xff; m_SendDCCData[10] = (begin_position[3] >> 16) & 0xff; m_SendDCCData[11] = (begin_position[3] >> 24) & 0xff; m_SendDCCData[12] = m_SetPotion_DriveSpeed[3] & 0xff; m_SendDCCData[13] = (m_SetPotion_DriveSpeed[3] >> 8) & 0xff; break; default: break; } m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength); Sleep(2); g_pLogger->SendAndFlushWithTime(L"[DCCScanStart] Out\n"); m_IsUsePPS = false; } ReleaseMutex(g_WR_ToMove_Mutex); return rStatus; } HSI_STATUS HSI_Motion::DCCScanStart() { WaitForSingleObject(g_WR_ToMove_Mutex, INFINITE); auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { if (!setLightFlag) { unsigned char m_SetTriggerLightData[64] = {0}; m_SetTriggerLightData[0] = CT_LIGHT; m_SetTriggerLightData[1] = 0x03; m_SetTriggerLightData[53] = 0; //m_WriteByte = Send_Command(0, (const char*)m_SetTriggerLightData, m_SendDataLength); Sleep(1); } setLightFlag = false; g_pLogger->SendAndFlushWithTime(L"[DCCScanStart] In\n"); unsigned char m_SendDCCData[64] = {0}; m_SendDCCData[0] = CT_MOTOR; m_SendDCCData[1] = CT_TRIGGER_DATA; m_SendDCCData[2] = 0x00; m_SendDCCData[3] = iaxisNum; m_SendDCCData[4] = iScanMotionType; m_SendDCCData[5] = ((iTriggleNum) & 0xff); m_SendDCCData[6] = (iTriggleNum >> 8) & 0xff; m_SendDCCData[7] = iMotionDirection; switch (iaxisNum) { case AXIS_X: m_SendDCCData[8] = begin_position[1] & 0xff; m_SendDCCData[9] = (begin_position[1] >> 8) & 0xff; m_SendDCCData[10] = (begin_position[1] >> 16) & 0xff; m_SendDCCData[11] = (begin_position[1] >> 24) & 0xff; m_SendDCCData[12] = m_SetPotion_DriveSpeed[1] & 0xff; m_SendDCCData[13] = (m_SetPotion_DriveSpeed[1] >> 8) & 0xff; break; case AXIS_Y: m_SendDCCData[8] = begin_position[2] & 0xff; m_SendDCCData[9] = (begin_position[2] >> 8) & 0xff; m_SendDCCData[10] = (begin_position[2] >> 16) & 0xff; m_SendDCCData[11] = (begin_position[2] >> 24) & 0xff; m_SendDCCData[12] = m_SetPotion_DriveSpeed[2] & 0xff; m_SendDCCData[13] = (m_SetPotion_DriveSpeed[2] >> 8) & 0xff; break; case AXIS_Z: m_SendDCCData[8] = begin_position[3] & 0xff; m_SendDCCData[9] = (begin_position[3] >> 8) & 0xff; m_SendDCCData[10] = (begin_position[3] >> 16) & 0xff; m_SendDCCData[11] = (begin_position[3] >> 24) & 0xff; m_SendDCCData[12] = m_SetPotion_DriveSpeed[3] & 0xff; m_SendDCCData[13] = (m_SetPotion_DriveSpeed[3] >> 8) & 0xff; break; default: break; } g_pLogger->SendAndFlushWithTime( L"[DCCScanStart] iaxisNum:%d, iTriggleNum:%d, iMotionDirection:%d, begin_position:%d \n", iaxisNum, iTriggleNum, iMotionDirection, begin_position[1]); //m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength); //启动定时扫描，此处清空旧缓存值 unsigned char m_cSendData[8] = {0}; m_cSendData[0] = 0x01; m_cSendData[1] = 0x04; m_WriteByte = Send_Command(0, (const char*)m_cSendData, 2, 2); Sleep(100); //启动定时锁存的同时启动扫描外部IO m_cSendData[0] = 0x01; m_cSendData[1] = 0x02; m_WriteByte = Send_Command(0, (const char*)m_cSendData, 2, 2); g_pLogger->SendAndFlushWithTime(L"[DCCScanStart] Out\n"); m_IsUsePPS = false; } ReleaseMutex(g_WR_ToMove_Mutex); return rStatus; } //=========================================================================== HSI_STATUS HSI_Motion::DCCScanStopOld() { WaitForSingleObject(g_WR_ToMove_Mutex, INFINITE); auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { g_pLogger->SendAndFlushWithTime(L"[DCCScanStop] In\n"); unsigned char m_SendDCCData[64] = {0}; m_SendDCCData[0] = CT_MOTOR; m_SendDCCData[1] = CT_TRIGGER_DATA; m_SendDCCData[2] = 0x02; m_SendDCCData[3] = iaxisNum; m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength); g_pLogger->SendAndFlushWithTime(L"[DCCScanStop] Out\n"); } ReleaseMutex(g_WR_ToMove_Mutex); return rStatus; } HSI_STATUS HSI_Motion::DCCScanStop() { WaitForSingleObject(g_WR_ToMove_Mutex, INFINITE); auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { g_pLogger->SendAndFlushWithTime(L"[DCCScanStop] In\n"); unsigned char m_SendDCCData[64] = {0}; m_SendDCCData[0] = 0x01; m_SendDCCData[1] = 0x03; m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, 2, 2); g_pLogger->SendAndFlushWithTime(L"[DCCScanStop] Out\n"); } ReleaseMutex(g_WR_ToMove_Mutex); return rStatus; } //=========================================================================== HSI_STATUS HSI_Motion::DCCForLightPlate() { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { g_pLogger->SendAndFlushWithTime(L"[DCCForLightPlate] In\n"); memset(m_cSendData, 0x00, 63); m_cSendData[0] = 0x02; m_cSendData[1] = 0x02; m_cSendData[2] = 0x01; m_cSendData[6] = 0x04; m_cSendData[7] = 0xff; m_cSendData[8] = 0xff; m_cSendData[9] = 0xff; m_cSendData[10] = 0xff; m_cSendData[14] = 0x00; m_WriteByte = Send_Command(1, (const char*)m_cSendData, m_SendDataLength); Sleep(5); memset(m_cSendData, 0x00, 63); m_cSendData[0] = 0x02; m_cSendData[1] = 0x02; m_cSendData[2] = 0x01; m_cSendData[6] = 0x04; m_cSendData[7] = 0xff; m_cSendData[8] = 0xff; m_cSendData[9] = 0xff; m_cSendData[10] = 0xff; m_cSendData[14] = m_ETIPort & 0xff; m_WriteByte = Send_Command(1, (const char*)m_cSendData, m_SendDataLength); Sleep(5); g_pLogger->SendAndFlushWithTime(L"[DCCForLightPlate] Out\n"); } return rStatus; } //=========================================================================== /** * \brief 转盘 * \param CamerasDis * \param BinsDis * \param SubArea * \param filterTime1 * \param filterTime2 * \param pluseSumDis * \return */ HSI_STATUS HSI_Motion::StartPlcJob(int* CamerasDis, int* BinsDis, int SubArea, int filterTime1, int filterTime2, int pluseSumDis) { g_pLogger->SendAndFlushWithTime(L"[StartPlcJob] In\n"); auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { m_Thread_StateData = HSI_THREAD_PAUSED; if (m_EncPos[2] < 0) { sEvenProp.Init(); sEvenProp.EventType = HSI_EVENT_ERROR; sEvenProp.EventID = HSI_EVENT_MOTION; sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK; strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "开始前请确认当前位置大于零！"); EventCallback(sEvenProp); return HSI_STATUS_FAILED; } m_Thread_StateData = HSI_THREAD_PAUSED; m_IbinCount = 0; m_cSendData[0] = CT_TURNTABLE; m_cSendData[1] = CT_RTSET; int index = 2; for (size_t i = 0; i < 4; i++) { m_cSendData[index++] = ((CamerasDis[i] >> 24) & 0xff); m_cSendData[index++] = ((CamerasDis[i] >> 16) & 0xff); m_cSendData[index++] = ((CamerasDis[i] >> 8) & 0xff); m_cSendData[index++] = (CamerasDis[i] & 0xff); } for (size_t i = 0; i < 5; i++) { m_cSendData[index++] = ((BinsDis[i] >> 24) & 0xff); m_cSendData[index++] = ((BinsDis[i] >> 16) & 0xff); m_cSendData[index++] = ((BinsDis[i] >> 8) & 0xff); m_cSendData[index++] = (BinsDis[i] & 0xff); } m_cSendData[index++] = SubArea; m_cSendData[index++] = ((filterTime1 >> 8) & 0xff); m_cSendData[index++] = (filterTime1 & 0xff); m_cSendData[index++] = ((filterTime2 >> 8) & 0xff); m_cSendData[index++] = (filterTime2 & 0xff); m_cSendData[index++] = ((pluseSumDis >> 24) & 0xff); m_cSendData[index++] = ((pluseSumDis >> 16) & 0xff); m_cSendData[index++] = ((pluseSumDis >> 8) & 0xff); m_cSendData[index++] = (pluseSumDis & 0xff); m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength); Sleep(5); Jog(m_UseAxisNum, 0.1); } g_pLogger->SendAndFlushWithTime(L"[StartPlcJob] Out\n"); return rStatus; } //=========================================================================== HSI_STATUS HSI_Motion::SendBinResult(int* BinResult) { g_pLogger->SendAndFlushWithTime(L"[SendBinResult] In\n"); auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { m_IbinCount++; m_cSendData[0] = CT_TURNTABLE; m_cSendData[1] = CT_BINSDATA; int temp = 0; switch (BinResult[0]) { case 1: temp = 0x01; break; case 2: temp = 0x02; break; case 3: temp = 0x04; break; case 4: temp = 0x08; break; default: temp = 0x00; break; } m_cSendData[2] = temp; m_cSendData[3] = ((m_IbinCount >> 24) & 0xff); m_cSendData[4] = ((m_IbinCount >> 16) & 0xff); m_cSendData[5] = ((m_IbinCount >> 8) & 0xff); m_cSendData[6] = (m_IbinCount & 0xff); m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength); g_pLogger->SendAndFlushWithTime(L"[GetSpeedXyz] m_IbinCount = %d\n", m_IbinCount); Sleep(5); } g_pLogger->SendAndFlushWithTime(L"[SendBinResult] out\n"); return rStatus; } //=========================================================================== HSI_STATUS HSI_Motion::GetTriggleCount(int* nCount, int& nArea) { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { if (m_SO7_Serial.m_RecvData[0] == 3) { nArea = m_SO7_Serial.m_RecvData[1]; nCount[0] = m_SO7_Serial.m_RecvData[2] << 24 | m_SO7_Serial.m_RecvData[3] << 16 | m_SO7_Serial.m_RecvData[4] << 8 | m_SO7_Serial.m_RecvData[5]; nCount[1] = m_SO7_Serial.m_RecvData[6] << 24 | m_SO7_Serial.m_RecvData[7] << 16 | m_SO7_Serial.m_RecvData[8] << 8 | m_SO7_Serial.m_RecvData[9]; nCount[2] = m_SO7_Serial.m_RecvData[10] << 24 | m_SO7_Serial.m_RecvData[11] << 16 | m_SO7_Serial.m_RecvData[ 12] << 8 | m_SO7_Serial.m_RecvData[13]; nCount[3] = m_SO7_Serial.m_RecvData[14] << 24 | m_SO7_Serial.m_RecvData[15] << 16 | m_SO7_Serial.m_RecvData[ 16] << 8 | m_SO7_Serial.m_RecvData[17]; nCount[4] = m_SO7_Serial.m_RecvData[18] << 24 | m_SO7_Serial.m_RecvData[19] << 16 | m_SO7_Serial.m_RecvData[ 20] << 8 | m_SO7_Serial.m_RecvData[21]; nCount[5] = m_SO7_Serial.m_RecvData[22] << 24 | m_SO7_Serial.m_RecvData[23] << 16 | m_SO7_Serial.m_RecvData[ 24] << 8 | m_SO7_Serial.m_RecvData[25]; nCount[6] = m_SO7_Serial.m_RecvData[26] << 24 | m_SO7_Serial.m_RecvData[27] << 16 | m_SO7_Serial.m_RecvData[ 28] << 8 | m_SO7_Serial.m_RecvData[29]; nCount[7] = m_SO7_Serial.m_RecvData[30] << 24 | m_SO7_Serial.m_RecvData[31] << 16 | m_SO7_Serial.m_RecvData[ 32] << 8 | m_SO7_Serial.m_RecvData[33]; nCount[8] = m_SO7_Serial.m_RecvData[34] << 24 | m_SO7_Serial.m_RecvData[35] << 16 | m_SO7_Serial.m_RecvData[ 36] << 8 | m_SO7_Serial.m_RecvData[37]; nCount[9] = m_SO7_Serial.m_RecvData[38] << 24 | m_SO7_Serial.m_RecvData[39] << 16 | m_SO7_Serial.m_RecvData[ 40] << 8 | m_SO7_Serial.m_RecvData[41]; g_pLogger->SendAndFlushWithTime(L"[GetTriggleCount] nArea = %d\n", nArea); } } return rStatus; } //=========================================================================== //点胶 //=========================================================================== HSI_STATUS HSI_Motion::GluePPSpnts(int* ppsDir, double* gluePPSPos, int* delayLightBefore, int* lightTime, double* lightData, int num) { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { unsigned char send_gluePPS_data[64] = {0}; send_gluePPS_data[0] = CT_GLUEDISPENSER; send_gluePPS_data[1] = 0x01; //擦除指令 m_WriteByte = Send_Command(0, (const char*)send_gluePPS_data, m_SendDataLength); Sleep(200); int posIndex = 0; int loadFeet[4]; int j = 0; int c = 0; //触发计数 int dirType = 0; int saveDir = 0; int duanNum = 0; double triggerPnt[4]; double savePPSPnt[4]; GlueDispenserindexNum = 0; #pragma region 拍照点 for (size_t i = 0; i < num; i++) { if (i == 0) { dirType = ppsDir[0]; saveDir = dirType; j = 7; } else { dirType = ppsDir[i]; } if (dirType != saveDir) { //-> send_gluePPS_data[0] = CT_GLUEDISPENSER; send_gluePPS_data[1] = 0x02; send_gluePPS_data[2] = GlueDispenserindexNum; send_gluePPS_data[3] = duanNum; send_gluePPS_data[4] = c; int axisNum = 0; int dirnum = 0; switch (saveDir) { case 0: { axisNum = 1; dirnum = 2; break; } case 1: { axisNum = 1; dirnum = 1; break; } case 2: { axisNum = 2; dirnum = 2; break; } case 3: { axisNum = 2; dirnum = 1; break; } default: break; } send_gluePPS_data[5] = axisNum; send_gluePPS_data[6] = dirnum; m_WriteByte = Send_Command(0, (const char*)send_gluePPS_data, m_SendDataLength); Sleep(20); GlueDispenserindexNum++; duanNum = 0; saveDir = dirType; c = 0; j = 7; } switch (dirType) { case 0: { c++; loadFeet[0] = 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; } 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; } 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; } default: break; } if (c == 12) { //-> send_gluePPS_data[0] = CT_GLUEDISPENSER; send_gluePPS_data[1] = 0x02; send_gluePPS_data[2] = GlueDispenserindexNum; send_gluePPS_data[3] = duanNum; send_gluePPS_data[4] = c; int axisNum = 0; int dirnum = 0; switch (saveDir) { case 0: { axisNum = 1; dirnum = 2; break; } case 1: { axisNum = 1; dirnum = 1; break; } case 2: { axisNum = 2; dirnum = 2; break; } case 3: { axisNum = 2; dirnum = 1; break; } default: break; } send_gluePPS_data[5] = axisNum; send_gluePPS_data[6] = dirnum; m_WriteByte = Send_Command(0, (const char*)send_gluePPS_data, m_SendDataLength); Sleep(20); duanNum++; saveDir = dirType; c = 0; j = 7; } } if (c != 0) { send_gluePPS_data[0] = CT_GLUEDISPENSER; send_gluePPS_data[1] = 0x02; send_gluePPS_data[2] = GlueDispenserindexNum; send_gluePPS_data[3] = duanNum; send_gluePPS_data[4] = c; int axisNum = 0; int dirnum = 0; switch (saveDir) { case 0: { axisNum = 1; dirnum = 2; break; } case 1: { axisNum = 1; dirnum = 1; break; } case 2: { axisNum = 2; dirnum = 2; break; } case 3: { axisNum = 2; dirnum = 1; break; } default: break; } send_gluePPS_data[5] = axisNum; send_gluePPS_data[6] = dirnum; m_WriteByte = Send_Command(0, (const char*)send_gluePPS_data, m_SendDataLength); Sleep(20); } #pragma endregion unsigned char send_data_light2[64] = {0}; unsigned char send_data_delay[64] = {0}; c = 0; GlueDispenserindexNum = 0; int ldIndex = 0; int light1 = 0; int light2 = 0; int indexCount = 0; dirType = 0; saveDir = 0; j = 3; int k = 4; int d = 4; for (size_t i = 0; i < num; i++) { if (i == 0) { dirType = ppsDir[0]; saveDir = dirType; j = 4; } else { dirType = ppsDir[i]; } if (dirType != saveDir) { send_gluePPS_data[0] = CT_GLUEDISPENSER; send_gluePPS_data[1] = 0x03; send_gluePPS_data[2] = GlueDispenserindexNum; send_gluePPS_data[3] = 0; m_WriteByte = Send_Command(0, (const char*)send_gluePPS_data, m_SendDataLength); Sleep(20); send_data_light2[0] = CT_GLUEDISPENSER; send_data_light2[1] = 0x03; send_data_light2[2] = GlueDispenserindexNum; send_data_light2[3] = 1; m_WriteByte = Send_Command(0, (const char*)send_data_light2, m_SendDataLength); Sleep(20); send_data_delay[0] = CT_GLUEDISPENSER; send_data_delay[1] = 0x03; send_data_delay[2] = GlueDispenserindexNum; send_data_delay[3] = 2; m_WriteByte = Send_Command(0, (const char*)send_data_delay, m_SendDataLength); Sleep(20); c = 0; j = 4; k = 4; d = 4; indexCount = 0; GlueDispenserindexNum++; } light1 = static_cast(lightData[ldIndex++] * 1120); light2 = static_cast(lightData[ldIndex++] * 1120); //send_gluePPS_data[j++] = (delayLightBefore[i] & 0xff); //send_gluePPS_data[j++] = ((delayLightBefore[i] >> 8) & 0xff); send_gluePPS_data[j++] = (light1 & 0xff); send_gluePPS_data[j++] = ((light1 >> 8) & 0xff); send_data_light2[k++] = (light2 & 0xff); send_data_light2[k++] = ((light2 >> 8) & 0xff); send_data_delay[d++] = (lightTime[i] & 0xff); c++; if (c == 30) { send_gluePPS_data[0] = CT_GLUEDISPENSER; send_gluePPS_data[1] = 0x03; send_gluePPS_data[2] = GlueDispenserindexNum; send_gluePPS_data[3] = 0; m_WriteByte = Send_Command(0, (const char*)send_gluePPS_data, m_SendDataLength); Sleep(20); send_data_light2[0] = CT_GLUEDISPENSER; send_data_light2[1] = 0x03; send_data_light2[2] = GlueDispenserindexNum; send_data_light2[3] = 1; m_WriteByte = Send_Command(0, (const char*)send_data_light2, m_SendDataLength); Sleep(20); send_data_delay[0] = CT_GLUEDISPENSER; send_data_delay[1] = 0x03; send_data_delay[2] = GlueDispenserindexNum; send_data_delay[3] = 2; m_WriteByte = Send_Command(0, (const char*)send_data_delay, m_SendDataLength); Sleep(20); c = 0; j = 4; k = 4; d = 4; indexCount++; GlueDispenserindexNum++; } } if (c != 0) { send_gluePPS_data[0] = CT_GLUEDISPENSER; send_gluePPS_data[1] = 0x03; send_gluePPS_data[2] = GlueDispenserindexNum; send_gluePPS_data[3] = 0; m_WriteByte = Send_Command(0, (const char*)send_gluePPS_data, m_SendDataLength); Sleep(20); send_data_light2[0] = CT_GLUEDISPENSER; send_data_light2[1] = 0x03; send_data_light2[2] = GlueDispenserindexNum; send_data_light2[3] = 1; m_WriteByte = Send_Command(0, (const char*)send_data_light2, m_SendDataLength); Sleep(20); send_data_delay[0] = CT_GLUEDISPENSER; send_data_delay[1] = 0x03; send_data_delay[2] = GlueDispenserindexNum; send_data_delay[3] = 2; m_WriteByte = Send_Command(0, (const char*)send_data_delay, m_SendDataLength); Sleep(20); GlueDispenserindexNum++; } } return rStatus; } HSI_STATUS HSI_Motion::GlueDispenser(int* index, int* cirdirection, double* gluePos, int num) { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { GluerunCount = 0; unsigned char send_glue_data[64] = {0}; send_glue_data[0] = CT_ORDER; send_glue_data[1] = CT_GLUEDISPENSER_CLEAR; send_glue_data[2] = 0x02; m_WriteByte = Send_Command(0, (const char*)send_glue_data, m_SendDataLength); Sleep(200); int posIndex = 0; int loadFeet[4]; double startGluepos[4]; double endGluepos[4]; double saveGluepos[4]; int circlepnt[4]; unsigned char glueaxis = 0; for (size_t i = 0; i < num; i++) { switch (index[i]) { case 0: { glueaxis = 0; if (i == 0) { GluerunCount++; endGluepos[0] = gluePos[posIndex++]; endGluepos[1] = gluePos[posIndex++]; endGluepos[2] = gluePos[posIndex++]; endGluepos[3] = gluePos[posIndex++]; saveGluepos[0] = endGluepos[0]; saveGluepos[1] = endGluepos[1]; saveGluepos[2] = endGluepos[2]; saveGluepos[3] = endGluepos[3]; loadFeet[0] = static_cast(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); } 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) { 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); } 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) { 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); } 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; } } /*send_glue_data[0] = CT_MOTOR; send_glue_data[1] = CT_GLUEDISPENSER_START; send_glue_data[2] = runCount;*/ } return rStatus; } HSI_STATUS HSI_Motion::GlueDispenserStart(double xOffset, double yOffset, double qOffset) { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { int xSkew = xOffset / m_Resolution[1]; int ySkew = yOffset / m_Resolution[2]; int qSkew = qOffset / m_Resolution[1]; unsigned char send_glue_data[64] = {0}; send_glue_data[0] = CT_MOTOR; send_glue_data[1] = CT_GLUEDISPENSER_START; send_glue_data[2] = GlueDispenserindexNum + 1; send_glue_data[3] = GluerunCount; send_glue_data[4] = (m_SetPotion_StartSpeed[1] & 0xff); send_glue_data[5] = ((m_SetPotion_StartSpeed[1] >> 8) & 0xff); send_glue_data[6] = (m_SetPotion_DriveSpeed[1] & 0xff); send_glue_data[7] = ((m_SetPotion_DriveSpeed[1] >> 8) & 0xff); send_glue_data[8] = (m_SetPotion_Line[1] & 0xff); send_glue_data[9] = ((m_SetPotion_Line[1] >> 8) & 0xff); send_glue_data[10] = (xSkew & 0xff); send_glue_data[11] = ((xSkew >> 8) & 0xff); send_glue_data[12] = (ySkew & 0xff); send_glue_data[13] = ((ySkew >> 8) & 0xff); send_glue_data[14] = (qSkew & 0xff); send_glue_data[15] = ((qSkew >> 8) & 0xff); g_pLogger->SendAndFlushWithTime( L"[GlueDispenser] m_SetPotion_StartSpeed = %d, m_SetPotion_DriveSpeed = %d,m_SetPotion_Line = %d \n", m_SetPotion_StartSpeed[1], m_SetPotion_DriveSpeed[1], m_SetPotion_Line[1]); m_WriteByte = Send_Command(0, (const char*)send_glue_data, m_SendDataLength); Sleep(5); g_IsClose = false; bUseGlueDispenser = true; m_Thread_State = HSI_THREAD_RUNNING; bRunGlueDispenser = HSI_THREAD_RUNNING; SetEvent(m_hTriggerEvent); } return rStatus; } void HSI_Motion::GluedispenserDone() //涂胶机完成 { if (bRunGlueDispenser == HSI_THREAD_RUNNING) { Sleep(100); int timeStart = GetTickCount(); do { if (g_IsClose) { g_IsClose = false; break; } if (m_SO7_Serial.m_RecvData[0] == 2) { if ((m_SO7_Serial.m_RecvData[60] & 0x0f)) { g_pLogger->SendAndFlushWithTime(L"[GluedispenserDone] Run End Normal\n"); //涂胶机完成 m_SO7_Serial.m_RecvData[60] = 0; break; } } Sleep(1); int timeEnd = GetTickCount(); if (timeStart - timeEnd > 10 * 1000) { g_pLogger->SendAndFlushWithTime(L"[GluedispenserDone] Run End Abnormal\n"); break; } } while (true); bRunGlueDispenser = HSI_THREAD_PAUSED; sEvenProp.Init(); sEvenProp.EventType = HSI_EVENT_FUNCTION; sEvenProp.EventID = HSI_EVENT_MOTION_DISPENSER; sEvenProp.EventResponse = HSI_EVENT_FUNCTION_OK; EventCallback(sEvenProp); } } //=========================================================================== HSI_STATUS HSI_Motion::GetPntsDistance(double& pDistance, int& spTimeCount) { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { if (SetPotionRunEnd) { g_pLogger->SendAndFlushWithTime(L"[GetPntsDistance] PTPDistance = %.4f,spTimeCount = %d\n", PntToPntDistance, SpTimeCount); SetPotionRunEnd = false; pDistance = PntToPntDistance; spTimeCount = SpTimeCount; } else { pDistance = -100; spTimeCount = -1; } } return rStatus; } //=========================================================================== /** * \brief 界面挡位获取速度 * \param AxisNum * \param Speed * \param DirveSpeed * \param StartSpeed * \param AccLine * \param DecLine * \param AccCurve * \param DecCurve * \return */ int HSI_Motion::SpeedPercent(int AxisNum, double& Speed, int& DirveSpeed, int& StartSpeed, int& AccLine, int& DecLine, int& AccCurve, int& DecCurve) { g_pLogger->SendAndFlushWithTime(L"[SpeedPercent] AxisNum: [%d]\n", AxisNum); int MovetoSpeedGear = 0; int flag = 1; if (fabs(Speed) > 1.01) { MovetoSpeedGear = 0; } else if (fabs(Speed) > 0.81) { DirveSpeed = m_JogDriveSpeed[AxisNum][0]; StartSpeed = m_JogStartSpeed[AxisNum][0]; AccLine = m_JogAccLine[AxisNum][0]; DecLine = m_JogDecLine[AxisNum][0]; AccCurve = m_JogAccCurve[AxisNum][0]; DecCurve = m_JogDecCurve[AxisNum][0]; 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); } else if (fabs(Speed) > 0.61) { DirveSpeed = m_JogDriveSpeed[AxisNum][1]; StartSpeed = m_JogStartSpeed[AxisNum][1]; AccLine = m_JogAccLine[AxisNum][1]; DecLine = m_JogDecLine[AxisNum][1]; AccCurve = m_JogAccCurve[AxisNum][1]; DecCurve = m_JogDecCurve[AxisNum][1]; 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); } else if (fabs(Speed) > 0.41) { DirveSpeed = m_JogDriveSpeed[AxisNum][2]; StartSpeed = m_JogStartSpeed[AxisNum][2]; AccLine = m_JogAccLine[AxisNum][2]; DecLine = m_JogDecLine[AxisNum][2]; AccCurve = m_JogAccCurve[AxisNum][2]; DecCurve = m_JogDecCurve[AxisNum][2]; 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); } else if (fabs(Speed) > 0.21) { DirveSpeed = m_JogDriveSpeed[AxisNum][3]; StartSpeed = m_JogStartSpeed[AxisNum][3]; AccLine = m_JogAccLine[AxisNum][3]; DecLine = m_JogDecLine[AxisNum][3]; AccCurve = m_JogAccCurve[AxisNum][3]; DecCurve = m_JogDecCurve[AxisNum][3]; 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); } else if (fabs(Speed) > 0.01) { DirveSpeed = m_JogDriveSpeed[AxisNum][4]; StartSpeed = m_JogStartSpeed[AxisNum][4]; AccLine = m_JogAccLine[AxisNum][4]; DecLine = m_JogDecLine[AxisNum][4]; AccCurve = m_JogAccCurve[AxisNum][4]; DecCurve = m_JogDecCurve[AxisNum][4]; 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); } else { MovetoSpeedGear = 0; } Speed = MovetoSpeedGear * flag; return static_cast(Speed); } //=========================================================================== /** * \brief JoyStick运动控制参数读取及设置 * \param AxisNum * \param Speed * \param DirveSpeed * \param StartSpeed * \param AccLine * \param DecLine * \param AccCurve * \param DecCurve * \return */ bool HSI_Motion::SpeedPercentJoyStick(int AxisNum, long& Speed, int& DirveSpeed, int& StartSpeed, int& AccLine, int& DecLine, int& AccCurve, int& DecCurve) { Speed = Speed > 1000 ? 1000 : Speed; double dSpeedRate = static_cast(Speed) / 1000; DirveSpeed = m_JogDriveSpeed[AxisNum][0] * (dSpeedRate * dSpeedRate); if (DirveSpeed > m_JogDriveSpeed[AxisNum][1]) { DirveSpeed = m_JogDriveSpeed[AxisNum][0]; StartSpeed = m_JogStartSpeed[AxisNum][0]; AccLine = m_JogAccLine[AxisNum][0]; DecLine = m_JogDecLine[AxisNum][0]; AccCurve = m_JogAccCurve[AxisNum][0]; DecCurve = m_JogDecCurve[AxisNum][0]; } else if (DirveSpeed > m_JogDriveSpeed[AxisNum][2]) { StartSpeed = m_JogStartSpeed[AxisNum][1]; AccLine = m_JogAccLine[AxisNum][1]; DecLine = m_JogDecLine[AxisNum][1]; AccCurve = m_JogAccCurve[AxisNum][1]; DecCurve = m_JogDecCurve[AxisNum][1]; } else if (DirveSpeed > m_JogDriveSpeed[AxisNum][3]) { StartSpeed = m_JogStartSpeed[AxisNum][2]; AccLine = m_JogAccLine[AxisNum][2]; DecLine = m_JogDecLine[AxisNum][2]; AccCurve = m_JogAccCurve[AxisNum][2]; DecCurve = m_JogDecCurve[AxisNum][2]; } else if (DirveSpeed > m_JogDriveSpeed[AxisNum][4]) { StartSpeed = m_JogStartSpeed[AxisNum][3]; AccLine = m_JogAccLine[AxisNum][3]; DecLine = m_JogDecLine[AxisNum][3]; AccCurve = m_JogAccCurve[AxisNum][3]; DecCurve = m_JogDecCurve[AxisNum][3]; } else if (DirveSpeed == 0) { return false; } else { StartSpeed = m_JogStartSpeed[AxisNum][4]; AccLine = m_JogAccLine[AxisNum][4]; DecLine = m_JogDecLine[AxisNum][4]; AccCurve = m_JogAccCurve[AxisNum][4]; DecCurve = m_JogDecCurve[AxisNum][4]; } return true; } //=========================================================================== /** * \brief 获取单轴设置的速度 * \param AxisNum * \param Speed * \return */ HSI_STATUS HSI_Motion::GetSpeedXyzOld(int AxisNum, double& Speed) { g_pLogger->SendAndFlushWithTime(L"[GetSpeedXyz] In\n"); auto rStatus = HSI_STATUS_NORMAL; short AxisNumber = AxisConvertIndex(AxisNum); if (1 == m_iSpeedType) { Speed = m_SetPotion_DriveSpeed[AxisNumber] * (m_Resolution[AxisNumber] * 50); } else { Speed = m_SetPotion_DriveSpeed[AxisNumber]; } g_pLogger->SendAndFlushWithTime(L"[GetSpeedXyz] Out\n"); return rStatus; } HSI_STATUS HSI_Motion::GetSpeedXyz(int AxisNum, double& Speed) { g_pLogger->SendAndFlushWithTime(L"[GetSpeedXyz] In\n"); auto rStatus = HSI_STATUS_NORMAL; short AxisNumber = AxisConvertIndex(AxisNum); if (1 == m_iSpeedType) { Speed = m_SetPotion_DriveSpeed[AxisNumber] * (m_Resolution[AxisNumber] * 50); } else { Speed = m_SetPotion_DriveSpeed[AxisNumber]; } g_pLogger->SendAndFlushWithTime(L"[GetSpeedXyz] Out\n"); return rStatus; } //=========================================================================== HSI_STATUS HSI_Motion::SetSpeedXyz(double Speed) { auto rStatus = HSI_STATUS_NORMAL; g_pLogger->SendAndFlushWithTime(L"[SetSpeedXyz] In\n"); if (Speed >= -1 && Speed <= 0) { g_pLogger->SendAndFlushWithTime(L"[SetSpeedXyz] Speed = %f, m_Jog_Auto_Focus = %f\n", Speed, m_Jog_Auto_Focus); m_JogDriveSpeed[3][4] = static_cast(fabs(Speed * m_Jog_Auto_Focus)); g_pLogger->SendAndFlushWithTime(L"[SetSpeedXyz] m_Jog_SpeedZ[3][4] = %f\n", m_JogDriveSpeed[3][4]); } else { g_pLogger->SendAndFlushWithTime(L"[SetSpeedXyz] Speed = %f\n", Speed); if (1 == m_iSpeedType) { m_SetPotion_DriveSpeed[1] = static_cast(Speed / (m_Resolution[1] * 50)); if (m_SetPotion_DriveSpeed[1] > 1000 / (m_Resolution[1] * 50)) { m_SetPotion_DriveSpeed[1] = 1000 / (m_Resolution[1] * 50); } if (m_SetPotion_DriveSpeed[1] < 0) { m_SetPotion_DriveSpeed[1] = 0; } } else { m_SetPotion_DriveSpeed[1] = static_cast(Speed); if (m_SetPotion_DriveSpeed[1] > 1000 / (m_Resolution[1] * 50)) { m_SetPotion_DriveSpeed[1] = 1000 / (m_Resolution[1] * 50); } if (m_SetPotion_DriveSpeed[1] < 0) { m_SetPotion_DriveSpeed[1] = 0; } } } g_pLogger->SendAndFlushWithTime(L"[SetSpeedXyz] Out\n"); return rStatus; } //=========================================================================== HSI_STATUS HSI_Motion::GetFocusSpeed(double& Speed) { g_pLogger->SendAndFlushWithTime(L"[GetFocusSpeed] In\n"); auto rStatus = HSI_STATUS_NORMAL; if (1 == m_iSpeedType) { Speed = m_JogDriveSpeed[3][4] * (m_Resolution[3] * 50); } else { Speed = m_JogDriveSpeed[3][4]; } g_pLogger->SendAndFlushWithTime(L"[GetFocusSpeed] Out\n"); return rStatus; } //=========================================================================== HSI_STATUS HSI_Motion::SetFocusSpeed(double Speed) { auto rStatus = HSI_STATUS_NORMAL; g_pLogger->SendAndFlushWithTime(L"[SetFocusSpeed] In\n"); g_pLogger->SendAndFlushWithTime(L"[SetFocusSpeed] Speed = %f, m_Jog_Auto_Focus = %f\n", Speed, m_Jog_Auto_Focus); if (1 == m_iSpeedType) { m_JogDriveSpeed[3][4] = Speed / (m_Resolution[3] * 50); } else { m_JogDriveSpeed[3][4] = Speed; } g_pLogger->SendAndFlushWithTime(L"[SetFocusSpeed] m_Jog_SpeedZ[3][4] = %f\n", m_JogDriveSpeed[4][3]); return rStatus; } //=========================================================================== HSI_STATUS HSI_Motion::GetAccelerationXyz(double& AccelX, double& AccelY, double& AccelZ) { auto rStatus = HSI_STATUS_NORMAL; return rStatus; } //=========================================================================== HSI_STATUS HSI_Motion::SetAccelerationXyz(double AccelX, double AccelY, double AccelZ) { auto rStatus = HSI_STATUS_NORMAL; return rStatus; } //=========================================================================== double HSI_Motion::LimitOver(UINT AxisTypes, double& LimitPos) //单轴软限位 { short AxisNumber = AxisConvertIndex(AxisTypes); if (g_pHSI_Motion) { switch (AxisNumber) { case HSI_MOTION_AXIS_X: // X 轴 { if (LimitPos >= m_P_Work_Limit[1]) { LimitPos = m_P_Work_Limit[1] - 5 * m_Resolution[1]; } if (LimitPos <= m_N_Work_Limit[1]) { LimitPos = m_N_Work_Limit[1] + 5 * m_Resolution[1]; } break; } case HSI_MOTION_AXIS_Y: //Y 轴 { if (LimitPos >= m_P_Work_Limit[2]) { LimitPos = m_P_Work_Limit[2] - 5 * m_Resolution[2]; } if (LimitPos <= m_N_Work_Limit[2]) { LimitPos = m_N_Work_Limit[2] + 5 * m_Resolution[2]; } break; } case HSI_MOTION_AXIS_Z: { if (LimitPos >= m_P_Work_Limit[3]) { LimitPos = m_P_Work_Limit[3] - 5 * m_Resolution[3]; } if (LimitPos <= m_N_Work_Limit[3]) { LimitPos = m_N_Work_Limit[3] + 5 * m_Resolution[3]; } break; } case HSI_MOTION_AXIS_R: { if (LimitPos >= m_P_Work_Limit[4]) { LimitPos = m_P_Work_Limit[4] - 5 * m_Resolution[4]; } if (LimitPos <= m_N_Work_Limit[4]) { LimitPos = m_N_Work_Limit[4] + 5 * m_Resolution[4]; } break; } default: break; } } return LimitPos; } //=========================================================================== short HSI_Motion::AxisConvertIndex(UINT AxisTypes) { short AxisNumber(0); switch (AxisTypes) { case HSI_MOTION_AXIS_X: { AxisNumber = 0x01; break; } case HSI_MOTION_AXIS_Y: { AxisNumber = 0x00; break; } case HSI_MOTION_AXIS_Z: { AxisNumber = 0x08; break; } case HSI_MOTION_AXIS_R: { AxisNumber = 0x04; break; } default: { g_pLogger->SendAndFlushWithTime(L"[AxisConvertIndex] failed, AxisTypes = %d,AxisNumber = %d\n", AxisTypes, AxisNumber); break; } } return AxisNumber; } //=========================================================================== void HSI_Motion::SetSingleAxisMotionParams(UINT AxisTypes, double SetmotionParam[5]) //设置单轴运动参数 { g_pLogger->SendAndFlushWithTime(L"[SetSingleAxisMotionParams] In\n"); double getMotionParam[5] = { 0 }; //打印轴当前运动参数 GetSingleAxisParam(AxisTypes, getMotionParam); //获取单轴运动参数 g_pLogger->SendAndFlushWithTime( L"[before] Axis= %.2f, Velocity = %.2f, Acceleration= %.2f, Deceleration= %.2f, KillDeceleration= %.2f, Jerk= %.2f\n", jogAxisNum, getMotionParam[0], getMotionParam[1], getMotionParam[2], getMotionParam[3], getMotionParam[4]); //设置单轴参数 SetSingleAxisParam(AxisTypes, SetmotionParam); //打印轴当前运动参数 GetSingleAxisParam(AxisTypes, getMotionParam); //获取单轴运动参数 g_pLogger->SendAndFlushWithTime( L"[after] Axis= %.2f, Velocity = %.2f, Acceleration= %.2f, Deceleration= %.2f, KillDeceleration= %.2f, Jerk= %.2f\n", jogAxisNum, getMotionParam[0], getMotionParam[1], getMotionParam[2], getMotionParam[3], getMotionParam[4]); g_pLogger->SendAndFlushWithTime(L"[SetSingleAxisMotionParams] Out\n"); } //=========================================================================== short HSI_Motion::IndexConvertAxis(int Index) { short AxisNumber(0); switch (Index) { case 1: { AxisNumber = AXIS_X; break; } case 2: { AxisNumber = AXIS_Y; break; } case 3: { AxisNumber = AXIS_Z; break; } case 4: { AxisNumber = AXIS_U; break; } default: { g_pLogger->SendAndFlushWithTime(L"IndexConvertAxis failed,Index = %d,AxisTypes = %d\n", Index, AxisNumber); break; } } return AxisNumber; } //=========================================================================== HSI_STATUS HSI_Motion::IsSupportedEx(UINT AxisTypes, UINT& Types) { auto rStatus = HSI_STATUS_NORMAL; Types = 1; short AxisNumber = AxisConvertIndex(AxisTypes); if (AxisNumber < 1 && AxisNumber > 4) { rStatus = HSI_STATUS_NOT_SUPPORTED; } return rStatus; } //=========================================================================== HSI_STATUS HSI_Motion::StartupEx(UINT AxisTypes, bool bHome) { auto rStatus = HSI_STATUS_NORMAL; g_pLogger->SendAndFlushWithTime(L"[StartupEx] In\n"); g_pLogger->SendAndFlushWithTime(L"[StartupEx] Out\n"); return rStatus; } //=========================================================================== HSI_STATUS HSI_Motion::GetScaleResolutionEx(UINT AxisTypes, double& Scale) { auto rStatus = HSI_STATUS_NORMAL; short AxisNumber = AxisConvertIndex(AxisTypes); if (g_pHSI_Motion) { Scale = m_Resolution[AxisNumber]; } return rStatus; } //=========================================================================== HSI_STATUS HSI_Motion::SetScaleResolutionEx(UINT AxisTypes, double Scale) { auto rStatus = HSI_STATUS_NORMAL; short AxisNumber = AxisConvertIndex(AxisTypes); if (g_pHSI_Motion) { m_Resolution[AxisNumber] = static_cast(Scale * 10000); } return rStatus; } //=========================================================================== HSI_STATUS HSI_Motion::GetPositionEx(UINT AxisTypes, double& Position, double& Time) { auto rStatus = HSI_STATUS_NORMAL; UNREFERENCED_PARAMETER(AxisTypes); if (g_pHSI_Motion) { double position[4] = {0.0}; short AxisNumber = AxisConvertIndex(AxisTypes); Position = m_PosForAllAxis[AxisNumber]; g_pLogger->SendAndFlushWithTime(L"[GetPositionEx] Position: %d\n", Position); } return rStatus; } //=========================================================================== HSI_STATUS HSI_Motion::SetPositionEx(UINT AxisTypes, double Position, HSI_MOTION_MOVE_TYPE eType, double dSpeedGear) { auto rStatus = HSI_STATUS_NORMAL; UNREFERENCED_PARAMETER(AxisTypes); if (g_pHSI_Motion) { g_pLogger->SendAndFlushWithTime(L"[SetPositionEx] In\n"); short AxisNumber = AxisConvertIndex(AxisTypes); double Time = 0.0; double position[5] = {0.0}; for (size_t i = 0; i < 5; i++) { position[i] = m_PosForAllAxis[i]; } position[AxisNumber] = Position; SetPositionXyza(AxisTypes, position[1], position[2], position[3], position[4], eType, dSpeedGear); g_pLogger->SendAndFlushWithTime(L"[SetPositionEx] Out\n"); } return rStatus; } //=========================================================================== HSI_STATUS HSI_Motion::SetPositionStep(UINT AxisTypes, double Position, HSI_MOTION_MOVE_TYPE eType, double dSpeedGear) { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { } return rStatus; } //=========================================================================== HSI_STATUS HSI_Motion::GetSpeedEx(UINT AxisTypes, double& Speed) { auto rStatus = HSI_STATUS_NORMAL; g_pLogger->SendAndFlushWithTime(L"[GetSpeedEx] In\n"); short AxisNumber = AxisConvertIndex(AxisTypes); if (AxisNumber >= 1 && AxisNumber <= 4) { if (m_motorType == 1) { if (1 == m_iSpeedType) { Speed = m_SetPotion_DriveSpeed[1] * (m_Resolution[1] * 50); } else { Speed = m_SetPotion_DriveSpeed[1]; } } else { if (1 == m_iSpeedType) { Speed = m_SetPotion_DriveSpeed[AxisNumber] * (m_Resolution[AxisNumber] * 50); } else { Speed = m_SetPotion_DriveSpeed[AxisNumber]; } } } g_pLogger->SendAndFlushWithTime(L"[GetSpeedEx] Out\n"); return rStatus; } //=========================================================================== /** * \brief 设置单轴运动速度 * \param AxisTypes * \param Speed * \return */ HSI_STATUS HSI_Motion::SetSpeedExOld(UINT AxisTypes, double Speed) { auto rStatus = HSI_STATUS_NORMAL; g_pLogger->SendAndFlushWithTime(L"[SetSpeedEx] In\n"); short AxisNumber = AxisConvertIndex(AxisTypes); if (Speed <= 0) { m_SetPotion_DriveSpeed[AxisNumber] = 0; } if (1 == m_iSpeedType) { if (Speed >= 65000 * (m_Resolution[AxisNumber] * 50)) { m_SetPotion_DriveSpeed[AxisNumber] = 65000; } } else { if (Speed >= 65000) { m_SetPotion_DriveSpeed[AxisNumber] = 65000; } } if (AxisNumber >= 1 && AxisNumber <= 4) { if (m_ForSoft == 1) { if (m_motorType == 1) { if (1 == m_iSpeedType) { m_SetPotion_DriveSpeed[1] = static_cast(Speed) / (m_Resolution[1] * 50); } else { m_SetPotion_DriveSpeed[1] = static_cast(Speed); } } else { g_pLogger->SendAndFlushWithTime(L"[SetSpeedEx] AxisNumber = %d,Speed = %f \n", AxisNumber, Speed); //if (!bSaveSpeedFlag) //{ // bSaveSpeedFlag = true; // m_SaveAxisNum = AxisNumber; // m_SaveAxisSpeed = m_SetPotion_DriveSpeed[AxisNumber]; // g_pLogger->SendAndFlushWithTime(L"[SetSpeedEx] m_SaveAxisNum = %d,m_SaveAxisSpeed = %d \n", m_SaveAxisNum, m_SaveAxisSpeed); //} //if (bSaveSpeedFlag && ((int)Speed == m_SetPotion_DriveSpeed[1] * (m_Resolution[1] * 50))) //{ // bSaveSpeedFlag = false; // m_SetPotion_DriveSpeed[m_SaveAxisNum] = m_SaveAxisSpeed; // return rStatus; //} if (1 == m_iSpeedType) { m_SetPotion_DriveSpeed[AxisNumber] = static_cast(Speed / (m_Resolution[AxisNumber] * 50)); } else { m_SetPotion_DriveSpeed[AxisNumber] = static_cast(Speed); } g_pLogger->SendAndFlushWithTime(L"[SetSpeedEx] AxisNumber = %d\n", AxisNumber); } } else { if (m_motorType == 1) { m_SetPotion_DriveSpeed[AxisNumber] = static_cast(Speed / (m_Resolution[AxisNumber] * 50)); } else { m_SetPotion_DriveSpeed[AxisNumber] = static_cast(Speed / (m_Resolution[AxisNumber] * 50)); } } } g_pLogger->SendAndFlushWithTime(L"[SetSpeedEx] Out\n"); return rStatus; } HSI_STATUS HSI_Motion::SetSpeedEx(UINT AxisTypes, double Speed) { auto rStatus = HSI_STATUS_NORMAL; g_pLogger->SendAndFlushWithTime(L"[SetSpeedEx] In\n"); short AxisNumber = AxisConvertIndex(AxisTypes); if (Speed <= 0) { m_SetPotion_DriveSpeed[AxisNumber] = 0; } if (1 == m_iSpeedType) { if (Speed >= 65000 * (m_Resolution[AxisNumber] * 50)) { m_SetPotion_DriveSpeed[AxisNumber] = 65000; } } else { if (Speed >= 65000) { m_SetPotion_DriveSpeed[AxisNumber] = 65000; } } if (AxisNumber >= 1 && AxisNumber <= 4) { if (m_ForSoft == 1) { if (m_motorType == 1) { if (1 == m_iSpeedType) { m_SetPotion_DriveSpeed[1] = static_cast(Speed) / (m_Resolution[1] * 50); } else { m_SetPotion_DriveSpeed[1] = static_cast(Speed); } } else { g_pLogger->SendAndFlushWithTime(L"[SetSpeedEx] AxisNumber = %d,Speed = %f \n", AxisNumber, Speed); //if (!bSaveSpeedFlag) //{ // bSaveSpeedFlag = true; // m_SaveAxisNum = AxisNumber; // m_SaveAxisSpeed = m_SetPotion_DriveSpeed[AxisNumber]; // g_pLogger->SendAndFlushWithTime(L"[SetSpeedEx] m_SaveAxisNum = %d,m_SaveAxisSpeed = %d \n", m_SaveAxisNum, m_SaveAxisSpeed); //} //if (bSaveSpeedFlag && ((int)Speed == m_SetPotion_DriveSpeed[1] * (m_Resolution[1] * 50))) //{ // bSaveSpeedFlag = false; // m_SetPotion_DriveSpeed[m_SaveAxisNum] = m_SaveAxisSpeed; // return rStatus; //} if (1 == m_iSpeedType) { m_SetPotion_DriveSpeed[AxisNumber] = static_cast(Speed / (m_Resolution[AxisNumber] * 50)); } else { m_SetPotion_DriveSpeed[AxisNumber] = static_cast(Speed); } g_pLogger->SendAndFlushWithTime(L"[SetSpeedEx] AxisNumber = %d\n", AxisNumber); } } else { if (m_motorType == 1) { m_SetPotion_DriveSpeed[AxisNumber] = static_cast(Speed / (m_Resolution[AxisNumber] * 50)); } else { m_SetPotion_DriveSpeed[AxisNumber] = static_cast(Speed / (m_Resolution[AxisNumber] * 50)); } } } g_pLogger->SendAndFlushWithTime(L"[SetSpeedEx] Out\n"); return rStatus; } //=========================================================================== HSI_STATUS HSI_Motion::GetAccelerationEx(UINT AxisTypes, double& Accel) { auto rStatus = HSI_STATUS_NORMAL; short AxisNumber = AxisConvertIndex(AxisTypes); if (AxisNumber >= 1 && AxisNumber <= 4) { if (m_motorType == 1) { Accel = m_SetPotion_StartSpeed[1] * (m_Resolution[1] * 500); } else { Accel = m_SetPotion_StartSpeed[AxisNumber] * (m_Resolution[AxisNumber] * 500); } } g_pLogger->SendAndFlushWithTime(L"[GetAccelerationEx] Accel = %d\n", Accel); return rStatus; } //=========================================================================== HSI_STATUS HSI_Motion::SetAccelerationEx(UINT AxisTypes, double Accel) { auto rStatus = HSI_STATUS_NORMAL; short AxisNumber = AxisConvertIndex(AxisTypes); if (Accel <= 0) { m_SetPotion_StartSpeed[AxisNumber] = 1; } if (AxisNumber >= 1 && AxisNumber <= 4) { if (m_motorType == 1) { m_SetPotion_StartSpeed[AxisNumber] = static_cast(Accel / (m_Resolution[AxisNumber] * 500)); } else { m_SetPotion_StartSpeed[AxisNumber] = static_cast(Accel / (m_Resolution[AxisNumber] * 500)); } } g_pLogger->SendAndFlushWithTime(L"[SetAccelerationEx] Accel = %d\n", m_SetPotion_StartSpeed[AxisNumber]); return rStatus; } //=========================================================================== VOID HSI_Motion::EventCallback(sHSIEventProperties& sEventProp) { WaitForSingleObject(m_Thread_Mutex, INFINITE); if (g_pHSI_Sevenocean_EF3) { g_pHSI_Sevenocean_EF3->EventCallback(sEventProp); } ReleaseMutex(m_Thread_Mutex); } //=========================================================================== void HSI_Motion::CreateThread() { if (m_Thread_Id == nullptr) { m_Thread_State = HSI_THREAD_RUNNING; m_hTriggerEvent = CreateEvent(nullptr, FALSE, NULL, nullptr); m_Thread_Id = ::CreateThread( nullptr, 0, (LPTHREAD_START_ROUTINE)m_Thread, this, 0, nullptr); m_Thread_Mutex = CreateMutex(nullptr, FALSE, nullptr); TRACE("CreateThread!\n"); } } //=========================================================================== void HSI_Motion::CloseThread() { m_Thread_State = HSI_THREAD_EXIT; Sleep(20); SetEvent(m_hTriggerEvent); if (m_Thread_Id) { DWORD dwCode = STILL_ACTIVE; while (dwCode == STILL_ACTIVE) { GetExitCodeThread(m_Thread_Id, &dwCode); Sleep(1); } } SetEvent(m_hTriggerEvent); CloseHandle(m_hTriggerEvent); m_Thread_State = HSI_THREAD_EXIT; ReleaseMutex(m_Thread_Mutex); CloseHandle(m_Thread_Mutex); m_Thread_Id = nullptr; m_hTriggerEvent = nullptr; m_Thread_Mutex = nullptr; TRACE("CloseThread!\n"); } //=========================================================================== unsigned __stdcall HSI_Motion::m_Thread(LPVOID pThis) { auto _This = static_cast(pThis); for (;;) { TRACE("m_Thread!\n"); if (m_Thread_State == THREAD_EXIT) ExitThread(0); WaitForSingleObject(m_hTriggerEvent, INFINITE); switch (m_Thread_State) { case HSI_THREAD_RUNNING: { TRACE("HSI_THREAD_RUNNING.\r\n"); if (bRunGlueDispenser == HSI_THREAD_RUNNING) _This->GluedispenserDone(); else { _This->UpdateMotionState(); _This->UpdateMotionStateEx(); } break; } case HSI_THREAD_PAUSED: { TRACE("HSI_THREAD_PAUSED.\r\n"); break; } case HSI_THREAD_EXIT: { TRACE("HSI_THREAD_EXIT.\r\n"); ExitThread(0); break; } default: break; } } } //=========================================================================== void HSI_Motion::CreateThreadProbe() { if (m_Thread_IdProbe == nullptr) { m_Thread_StateProbe = HSI_THREAD_RUNNING; m_hTriggerEventProbe = CreateEvent(nullptr, FALSE, NULL, nullptr); m_Thread_IdProbe = ::CreateThread( nullptr, 0, (LPTHREAD_START_ROUTINE)m_ThreadProbe, this, 0, nullptr); m_Thread_MutexProbe = CreateMutex(nullptr, FALSE, nullptr); TRACE("CreateThread!\n"); } } //=========================================================================== void HSI_Motion::CloseThreadProbe() { m_Thread_StateProbe = HSI_THREAD_EXIT; Sleep(20); SetEvent(m_hTriggerEventProbe); if (m_Thread_IdProbe) { DWORD dwCode = STILL_ACTIVE; while (dwCode == STILL_ACTIVE) { GetExitCodeThread(m_Thread_IdProbe, &dwCode); Sleep(1); } } SetEvent(m_hTriggerEventProbe); CloseHandle(m_hTriggerEventProbe); m_Thread_StateProbe = HSI_THREAD_EXIT; ReleaseMutex(m_Thread_MutexProbe); CloseHandle(m_Thread_MutexProbe); m_Thread_IdProbe = nullptr; m_hTriggerEventProbe = nullptr; m_Thread_MutexProbe = nullptr; TRACE("CloseThread!\n"); } //=========================================================================== unsigned __stdcall HSI_Motion::m_ThreadProbe(LPVOID pThis) { auto _This = static_cast(pThis); for (;;) { TRACE("m_Thread!\n"); if (m_Thread_StateProbe == THREAD_EXIT) ExitThread(0); WaitForSingleObject(m_hTriggerEventProbe, INFINITE); switch (m_Thread_StateProbe) { case HSI_THREAD_RUNNING: { TRACE("HSI_THREAD_RUNNING.\r\n"); _This->UpdateMotionStateProbe(); break; } case HSI_THREAD_PAUSED: { TRACE("HSI_THREAD_PAUSED.\r\n"); break; } case HSI_THREAD_EXIT: { TRACE("HSI_THREAD_EXIT.\r\n"); ExitThread(0); break; } default: break; } } } //=========================================================================== void HSI_Motion::CreateThreadIO() { if (m_Thread_IdIO == nullptr) { m_Thread_StateIO = HSI_THREAD_RUNNING; m_hTriggerEventIO = CreateEvent(nullptr, FALSE, NULL, nullptr); m_Thread_IdIO = ::CreateThread( nullptr, 0, (LPTHREAD_START_ROUTINE)m_ThreadIO, this, 0, nullptr); m_Thread_MutexIO = CreateMutex(nullptr, FALSE, nullptr); TRACE("CreateThreadIO!\n"); } } //=========================================================================== void HSI_Motion::CloseThreadIO() { m_Thread_StateIO = HSI_THREAD_EXIT; Sleep(20); SetEvent(m_hTriggerEventIO); if (m_Thread_IdIO) { DWORD dwCode = STILL_ACTIVE; while (dwCode == STILL_ACTIVE) { GetExitCodeThread(m_Thread_IdIO, &dwCode); Sleep(1); } } SetEvent(m_hTriggerEventIO); CloseHandle(m_hTriggerEventIO); m_Thread_StateIO = HSI_THREAD_EXIT; ReleaseMutex(m_Thread_MutexIO); CloseHandle(m_Thread_MutexIO); m_Thread_IdIO = nullptr; m_hTriggerEventIO = nullptr; m_Thread_MutexIO = nullptr; TRACE("CloseThreadIO!\n"); } //=========================================================================== unsigned __stdcall HSI_Motion::m_ThreadIO(LPVOID pThis) { auto _This = static_cast(pThis); for (;;) { TRACE("m_ThreadIO!\n"); if (m_Thread_StateIO == THREAD_EXIT) ExitThread(0); WaitForSingleObject(m_hTriggerEventIO, INFINITE); switch (m_Thread_StateIO) { case HSI_THREAD_RUNNING: { TRACE("HSI_THREAD_RUNNING.\r\n"); _This->UpdateMotionStateIO(); break; } case HSI_THREAD_PAUSED: { TRACE("HSI_THREAD_PAUSED.\r\n"); break; } case HSI_THREAD_EXIT: { TRACE("HSI_THREAD_EXIT.\r\n"); ExitThread(0); break; } default: break; } } } //=========================================================================== void HSI_Motion::CreateThreadData() { if (m_Thread_IdData == nullptr) { m_Thread_StateData = HSI_THREAD_RUNNING; m_hTriggerEventData = CreateEvent(nullptr, FALSE, NULL, nullptr); m_Thread_IdData = ::CreateThread( nullptr, 0, (LPTHREAD_START_ROUTINE)m_ThreadData, this, 0, nullptr); m_Thread_MutexData = CreateMutex(nullptr, FALSE, nullptr); TRACE("CreateThreadData!\n"); } } //=========================================================================== void HSI_Motion::CloseThreadData() { m_Thread_StateData = HSI_THREAD_EXIT; Sleep(20); SetEvent(m_hTriggerEventData); if (m_Thread_IdData) { DWORD dwCode = STILL_ACTIVE; while (dwCode == STILL_ACTIVE) { GetExitCodeThread(m_Thread_IdData, &dwCode); Sleep(1); } } SetEvent(m_hTriggerEventData); CloseHandle(m_hTriggerEventData); m_Thread_StateData = HSI_THREAD_EXIT; ReleaseMutex(m_Thread_MutexData); CloseHandle(m_Thread_MutexData); m_Thread_IdData = nullptr; m_hTriggerEventData = nullptr; m_Thread_MutexData = nullptr; TRACE("CloseThreadData!\n"); } //=========================================================================== unsigned __stdcall HSI_Motion::m_ThreadData(LPVOID pThis) { auto _This = static_cast(pThis); for (;;) { TRACE("m_ThreadData!\n"); if (m_Thread_StateData == THREAD_EXIT) ExitThread(0); //WaitForSingleObject函数用来检测hHandle事件的信号状态， //在某一线程中调用该函数时，线程暂时挂起， //如果在挂起的dwMilliseconds毫秒内，线程所等待的对象变为有信号状态，则该函数立即返回； //如果时间已经到达dwMilliseconds毫秒，但hHandle所指向的对象还没有变成有信号状态，函数照样返回。 WaitForSingleObject(m_hTriggerEventData, INFINITE); switch (m_Thread_StateData) { case HSI_THREAD_RUNNING: { TRACE("HSI_THREAD_RUNNING.\r\n"); _This->UpdateMotionStateData(); //获取运动状态命令 break; } case HSI_THREAD_PAUSED: { TRACE("HSI_THREAD_PAUSED.\r\n"); break; } case HSI_THREAD_EXIT: { TRACE("HSI_THREAD_EXIT.\r\n"); ExitThread(0); break; } default: break; } } } //=========================================================================== /** * \brief JOG运行到软限位的运动调节 */ void HSI_Motion::UpdateMotionStateJOGStop() { unsigned char send_JogLimt_data[64] = {0}; UINT axis = 0; double posx = 0.0; double posy = 0.0; double posz = 0.0; double post = 0.0; int slowdowndistance = 0; while (m_Thread_StateJOGStop == HSI_THREAD_RUNNING) { while (jogMoving) { slowdowndistance = jogspeed * 13; Sleep(3); GetPositionXyz(axis, posx, posy, posz, post); switch (jogAxisNum) { case 1: { if (jogDirFlag) { double limitx = m_P_Work_Limit[1]; if ((limitx - posx) / m_Resolution[1] < slowdowndistance) { send_JogLimt_data[0] = 0x01; send_JogLimt_data[1] = 0x14; send_JogLimt_data[2] = 0x01; m_WriteByte = Send_Command(0, (const char*)send_JogLimt_data, m_SendDataLength); Sleep(5); jogMoving = false; } } else { double limitx = m_N_Work_Limit[1]; if ((posx - limitx) / m_Resolution[1] < slowdowndistance) { send_JogLimt_data[0] = 0x01; send_JogLimt_data[1] = 0x14; send_JogLimt_data[2] = 0x01; m_WriteByte = Send_Command(0, (const char*)send_JogLimt_data, m_SendDataLength); Sleep(5); jogMoving = false; } } } break; case 2: { if (jogDirFlag) { double limity = m_P_Work_Limit[2]; if ((limity - posy) / m_Resolution[2] < slowdowndistance) { send_JogLimt_data[0] = 0x01; send_JogLimt_data[1] = 0x14; send_JogLimt_data[2] = 0x02; m_WriteByte = Send_Command(0, (const char*)send_JogLimt_data, m_SendDataLength); Sleep(5); jogMoving = false; } } else { double limity = m_N_Work_Limit[2]; if ((posy - limity) / m_Resolution[2] < slowdowndistance) { send_JogLimt_data[0] = 0x01; send_JogLimt_data[1] = 0x14; send_JogLimt_data[2] = 0x02; m_WriteByte = Send_Command(0, (const char*)send_JogLimt_data, m_SendDataLength); Sleep(5); jogMoving = false; } } } break; case 3: { if (jogDirFlag) { double limitz = m_P_Work_Limit[3]; if ((limitz - posz) / m_Resolution[3] < slowdowndistance) { send_JogLimt_data[0] = 0x01; send_JogLimt_data[1] = 0x14; send_JogLimt_data[2] = 0x04; m_WriteByte = Send_Command(0, (const char*)send_JogLimt_data, m_SendDataLength); Sleep(5); jogMoving = false; } } else { double limitz = m_N_Work_Limit[3]; if ((posz - limitz) / m_Resolution[3] < slowdowndistance) { send_JogLimt_data[0] = 0x01; send_JogLimt_data[1] = 0x14; send_JogLimt_data[2] = 0x04; m_WriteByte = Send_Command(0, (const char*)send_JogLimt_data, m_SendDataLength); Sleep(5); jogMoving = false; } } } break; case 4: { //第四轴未添加 } break; default: break; } } Sleep(2); } } //=========================================================================== void HSI_Motion::CreateThreadJOGStop() //JOG运行到软限位的运动调节 { if (m_Thread_IdJOGStop == nullptr) { m_Thread_StateJOGStop = HSI_THREAD_RUNNING; m_hTriggerEventJOGStop = CreateEvent(nullptr, FALSE, NULL, nullptr); m_Thread_IdJOGStop = ::CreateThread( nullptr, 0, (LPTHREAD_START_ROUTINE)m_ThreadJOGStop, this, 0, nullptr); m_Thread_MutexJOGStop = CreateMutex(nullptr, FALSE, nullptr); TRACE("CreateThreadJOGStop!\n"); } } //=========================================================================== void HSI_Motion::CloseThreadJOGStop() { m_Thread_StateJOGStop = HSI_THREAD_EXIT; Sleep(20); SetEvent(m_hTriggerEventJOGStop); if (m_Thread_IdData) { DWORD dwCode = STILL_ACTIVE; while (dwCode == STILL_ACTIVE) { GetExitCodeThread(m_Thread_IdJOGStop, &dwCode); Sleep(1); } } SetEvent(m_hTriggerEventJOGStop); CloseHandle(m_hTriggerEventJOGStop); m_Thread_StateJOGStop = HSI_THREAD_EXIT; ReleaseMutex(m_Thread_MutexJOGStop); CloseHandle(m_Thread_MutexJOGStop); m_Thread_IdJOGStop = nullptr; m_hTriggerEventJOGStop = nullptr; m_Thread_MutexJOGStop = nullptr; TRACE("CloseThreadJOGStop!\n"); } //=========================================================================== unsigned __stdcall HSI_Motion::m_ThreadJOGStop(LPVOID pThis) //JOG运行到软限位的运动调节 { auto _This = static_cast(pThis); for (;;) { TRACE("m_ThreadJOGStop!\n"); if (m_Thread_StateData == THREAD_EXIT) ExitThread(0); WaitForSingleObject(m_hTriggerEventJOGStop, INFINITE); switch (m_Thread_StateJOGStop) { case HSI_THREAD_RUNNING: { TRACE("HSI_THREAD_RUNNING.\r\n"); _This->UpdateMotionStateJOGStop(); break; } case HSI_THREAD_PAUSED: { TRACE("HSI_THREAD_PAUSED.\r\n"); break; } case HSI_THREAD_EXIT: { TRACE("HSI_THREAD_EXIT.\r\n"); ExitThread(0); break; } default: break; } } } //=========================================================================== //无用 HSI_STATUS HSI_Motion::IOStep(bool RunSts) { auto rStatus = HSI_STATUS_NORMAL; m_MSTRunFlag = RunSts; if (g_pHSI_Motion) { } return rStatus; } //=========================================================================== HSI_STATUS HSI_Motion::IOprogram(byte* SendData, int length) { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { Send_Command(0, (const char*)SendData, m_SendDataLength); } return rStatus; } HSI_STATUS HSI_Motion::FindOriginTest(bool type) { auto rStatus = HSI_STATUS_NORMAL; byte findorigdata[64] = {0}; findorigdata[0] = 0x04; if (type == true) { findorigdata[1] = 0x18; findorigdata[2] = static_cast(m_Resolution[1] * 10000); Send_Command(0, (const char*)findorigdata, 64); } else { findorigdata[1] = 0x19; findorigdata[2] = static_cast(m_Resolution[1] * 10000); Send_Command(0, (const char*)findorigdata, 64); } return rStatus; } //=========================================================================== /** * \brief 串口发送命令 * \param com * \param _SendData 发送内容指针 * \param SendDataLength 发送字节长度 * \param expectType 期待返回长度，2：返回ok，8：返回8个字节，N:返回N字节，如果本次返回内容不是期待字节，等待或重发该命令。 * \return */ BOOL HSI_Motion::Send_Command(int com, const char* _SendData, DWORD SendDataLength) { BOOL rStatus = TRUE; WaitForSingleObject(g_RW_Data_Mutex, INFINITE); if (m_bConnected && (m_IsUseEF3 == 1) && (com == 0)) { g_pLogger->SendAndFlushWithTime(L"[Send_Command] lenth:%d, %s\n", SendDataLength, m_SO7_Serial.HexToStr(_SendData, SendDataLength)); int iWriteByte = m_SO7_Serial.Send(_SendData, SendDataLength); if (iWriteByte == 0) { int iError = GetLastError(); g_pLogger->SendAndFlushWithTime(L"[Send_Command] GetLastError = %d\n", iError); if (iError == 1167) { g_pLogger->SendAndFlushWithTime(L"[Send_Command] GetLastError is 1167,EF3 device is no connect\n"); m_Thread_StateData = THREAD_PAUSED; if (m_bConnected == true && g_IsClose == false) { g_pLogger->SendAndFlushWithTime(L"[Send_Command] GetLastError is 1167,EF3 device is no connect\n"); sEvenProp.Init(); sEvenProp.EventType = HSI_EVENT_ERROR; sEvenProp.EventID = HSI_EVENT_MOTION; sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK; strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "EF3控制器已断开连接"); EventCallback(sEvenProp); } } g_pLogger->SendAndFlushWithTime(L"[Send_Command] m_SO7_Serial[%d].Send failed\n", com); rStatus = FALSE; } //Sleep(5); //m_SO7_Serial.OnReceive(); } ReleaseMutex(g_RW_Data_Mutex); return rStatus; }; //分3个层次处理该问题：ZZX //1、用户层：业务逻辑层面处理，并在发送的时候，给出期望回应值 //2、发送层：保证发送成功，并对返回数据进行一定的判断，如果通讯超时，进行重发逻辑 //3、接收数据线程，获得单包数据后，根据期望值进行拼包判断处理，返回拼包后的数据 BOOL HSI_Motion::Send_Command(int com, const char* _SendData, DWORD SendDataLength, int expectType) { BOOL rStatus = TRUE; WaitForSingleObject(g_RW_Data_Mutex, INFINITE); //指令回复分两种情况: //1、正常2字节，回复 OK，（0x6F 0x6B ） //2、返回多字节 if (m_bConnected && (m_IsUseEF3 == 1) /*&& (com == 0)*/) { //------------------------------调试区-----------------------------------// //原文链接：https://blog.csdn.net/qq_31073871/article/details/85696092 //printf("Send_Command: "); //for (int i = 0; i < SendDataLength; i++) //{ // printf("%02X ", ((uint8_t*)_SendData)[i]); //} //printf("\r\n"); //------------------------------调试区-----------------------------------// g_pLogger->SendAndFlushWithTime(L"[Send_Command] lenth:%d, expectType:%d, %s\n", SendDataLength, expectType, m_SO7_Serial.HexToStr(_SendData, SendDataLength)); int iWriteByte = m_SO7_Serial.Send(_SendData, SendDataLength); if (iWriteByte == 0) { int iError = GetLastError(); g_pLogger->SendAndFlushWithTime(L"[Send_Command] GetLastError = %d\n", iError); if (iError == 1167) { g_pLogger->SendAndFlushWithTime(L"[Send_Command] GetLastError is 1167,EF3 device is no connect\n"); m_Thread_StateData = THREAD_PAUSED; if (m_bConnected == true && g_IsClose == false) { g_pLogger->SendAndFlushWithTime(L"[Send_Command] GetLastError is 1167,EF3 device is no connect\n"); sEvenProp.Init(); sEvenProp.EventType = HSI_EVENT_ERROR; sEvenProp.EventID = HSI_EVENT_MOTION; sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK; strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "EF3控制器已断开连接"); EventCallback(sEvenProp); } } g_pLogger->SendAndFlushWithTime(L"[Send_Command] m_SO7_Serial[%d].Send failed\n", com); rStatus = FALSE; } if (iWriteByte != SendDataLength) // 写入字节数不等于发送字节数 { rStatus = FALSE; g_pLogger->SendAndFlushWithTime(L"[Send_Command] WriteByte:%d,!= SendDataLength:%d\n", iWriteByte, SendDataLength); } m_SO7_Serial.m_iRecvState = false; //发送完毕，接收状态置为false，并开始等待EF3回复 m_SO7_Serial.m_iExpectBytes = expectType; //给出期望结果，传递到下层，由下层进行拼包 //判断回复值，是否为ok 6F 6B int iRetrys = 0; while (!m_SO7_Serial.m_iRecvState && iRetrys < 100) //再次接收等待 { iRetrys++; Sleep(3); } g_pLogger->SendAndFlushWithTime(L"[Send_Command] Timeout Retrys:%d\n", iRetrys); //两种情况，1、成功获得期望值；2、如果超时 if (m_SO7_Serial.m_iRecvState && (m_SO7_Serial.m_iRecvBytes == expectType)) { rStatus = TRUE; g_pLogger->SendAndFlushWithTime(L"[Send_Command] EF3 Reply OK, RecvBytes:%d, %S\n", m_SO7_Serial.m_iRecvBytes, m_SO7_Serial.HexToStr((const char*)m_SO7_Serial.m_RecvData, m_SO7_Serial.m_iRecvBytes)); /* g_pLogger->SendAndFlushWithTime(L"[Send_Command] EF3 Reply OK, RecvBytes:%d\n", m_SO7_Serial.m_iRecvBytes);*/ } else if (iRetrys > 100) //获取超时，重发 { iRetrys = 0; g_pLogger->SendAndFlushWithTime(L"[Send_Command] EF3 Reply Timeout ,ReSend\n"); int iWriteByte = m_SO7_Serial.Send(_SendData, SendDataLength); //再次重发，并等待回应 m_SO7_Serial.m_iRecvState = false; //发送完毕，接收状态置为false，并开始等待EF3回复 m_SO7_Serial.m_iExpectBytes = expectType; //给出期望结果，传递到下层，由下层进行拼包 while (!m_SO7_Serial.m_iRecvState && iRetrys < 100) { iRetrys++; Sleep(2); } if (iRetrys > 100) { rStatus = HSI_STATUS_FAILED; g_pLogger->SendAndFlushWithTime(L"[Send_Command] EF3 Reply Error!"); } else //获得期望结果 { rStatus = HSI_STATUS_NORMAL; g_pLogger->SendAndFlushWithTime(L"[Send_Command] EF3 Reply OK, RecvBytes:%d, %s\n", m_SO7_Serial.m_iRecvBytes, m_SO7_Serial.HexToStr((const char*)m_SO7_Serial.m_RecvData, m_SO7_Serial.m_iRecvBytes)); } } //发送完当前指令后，第一次结束完成标志 //if (m_SO7_Serial.m_iRecvBytes != expectType) //获取数量不等于期望数量，打印错误，并重发 //{ // g_pLogger->SendAndFlushWithTime(L"[Send_Command] EF3 Reply Error, RecvBytes:%d, %s\n", // m_SO7_Serial.m_iRecvBytes, // m_SO7_Serial.HexToStr((const char*)m_SO7_Serial.m_RecvData, // m_SO7_Serial.m_iRecvBytes)); // m_SO7_Serial.m_iRecvState = false; // int iWriteByte = m_SO7_Serial.Send(_SendData, SendDataLength); //再次重发，并等待回应 // while (!m_SO7_Serial.m_iRecvState && iRetrys < 100) //等待300毫秒 // { // iRetrys++; // Sleep(3); // } // if (m_SO7_Serial.m_iRecvState) // { // if (iRetrys > 100) // { // iRetrys = 0; // while (m_SO7_Serial.m_iRecvBytes != expectType) // { // iRetrys++; // if (iRetrys > 100) // break; // Sleep(2); // } // } // if (iRetrys > 100) // { // rStatus = HSI_STATUS_FAILED; // g_pLogger->SendAndFlushWithTime(L"[Send_Command] EF3 Reply Error!"); // } // else //获得期望结果 // { // rStatus = HSI_STATUS_NORMAL; // g_pLogger->SendAndFlushWithTime(L"[Send_Command] EF3 Reply OK, RecvBytes:%d, %s\n", // m_SO7_Serial.m_iRecvBytes, // m_SO7_Serial.HexToStr((const char*)m_SO7_Serial.m_RecvData, // m_SO7_Serial.m_iRecvBytes)); // } // } //} //else //{ // rStatus = FALSE; // g_pLogger->SendAndFlushWithTime(L"[Send_Command] EF3 Reply OK, RecvBytes:%d, %s\n", // m_SO7_Serial.m_iRecvBytes, // m_SO7_Serial.HexToStr((const char*)m_SO7_Serial.m_RecvData, // m_SO7_Serial.m_iRecvBytes)); //} } ReleaseMutex(g_RW_Data_Mutex); return rStatus; }; ///////////////////////////////////////////////////////////////////////// #pragma region //网口通信8路 26路光源板通信 #pragma warning(disable:4996) TCPIP_RETURN_CODE HSI_Motion::TCPConnect(int index, char* Address, u_short port) { TCPIP_RETURN_CODE rCode = TCPIP_CONNECT_OK; if (!m_tcpCntFlag[index]) { int retVal(0), errorCode(0); first = true; retVal = Init_Winsock(); if (!retVal) { return TCPIP_INIT_WINSOCK_ERROR; } m_socket[index] = socket(AF_INET, SOCK_STREAM, 0); if (m_socket[index] == INVALID_SOCKET) { rCode = TCPIP_INVAILD_SOCKET; } else { int iMode = 1; int i = 0; int retVal = ioctlsocket(m_socket[index], FIONBIO, (u_long FAR*)&iMode); //非阻塞连接 if (retVal == SOCKET_ERROR) { closesocket(m_socket[index]); WSACleanup(); return TCPIP_INIT_WINSOCK_ERROR; } SOCKADDR_IN addSrv; addSrv.sin_addr.S_un.S_addr = inet_addr(Address); addSrv.sin_family = AF_INET; addSrv.sin_port = htons(port); while (true) { retVal = connect(m_socket[index], (SOCKADDR*)&addSrv, sizeof(SOCKADDR)); if (retVal) { errorCode = WSAGetLastError(); if (errorCode == WSAEWOULDBLOCK) { i++; if (i > 400) break; Sleep(5); continue; } if (errorCode == WSAEISCONN) { break; } first = true; closesocket(m_socket[index]); WSACleanup(); rCode = static_cast(errorCode); return rCode; } } iMode = 0; retVal = ioctlsocket(m_socket[index], FIONBIO, (u_long FAR*)&iMode); //设置阻塞 if (retVal == SOCKET_ERROR) { closesocket(m_socket[index]); WSACleanup(); return TCPIP_INIT_WINSOCK_ERROR; } } } return rCode; } void HSI_Motion::DisConnect() { if (m_socket) { if (LightSend != 0 || IOSend != 0 || OrderSend != 0) { Sleep(20); } first = true; Exit_Thread(); WSACleanup(); for (size_t i = 0; i < m_bISUseMoreLights; i++) { closesocket(m_socket[i]); m_socket[i] = NULL; } } } unsigned __stdcall HSI_Motion::m_ThreadSendTCP(LPVOID pThis) { auto _This = static_cast(pThis); for (;;) { TRACE("m_Thread!\n"); Sleep(2); if (m_ThreadTCP_State == TCPIP_THREAD_EXIT) ExitThread(0); switch (m_ThreadTCP_State) { case TCPIP_THREAD_RUNNING: { TRACE("TCPIP_THREAD_RUNNING.\r\n"); _This->TCPSend(); break; } case TCPIP_THREAD_PAUSED: { break; } case TCPIP_THREAD_EXIT: { ExitThread(0); break; } default: break; } } } TCPIP_RETURN_CODE HSI_Motion::TCPSend() { if (m_tcpCntFlag[m_selectedIndex]) { bool recvFlag = false; int bytesSend = 0; if (LightSend > 0) { g_pLogger->SendAndFlushWithTime(L"[TCPSend] LightSend = %d,m_selectedIndex = %d\n", LightSend, m_selectedIndex); LightSend--; bytesSend = send(m_socket[m_selectedIndex], (const char*)lightdata, 64, 0); for (size_t i = 0; i < 64; i++) { TempData[i] = lightdata[i]; } } else if (IOSend > 0) { IOSend--; bytesSend = send(m_socket[m_selectedIndex], (const char*)IOdata, 64, 0); for (size_t i = 0; i < 64; i++) { TempData[i] = IOdata[i]; } } else if (OrderSend > 0) { OrderSend--; bytesSend = send(m_socket[m_selectedIndex], (const char*)Orderdata, 64, 0); for (size_t i = 0; i < 64; i++) { TempData[i] = Orderdata[i]; } } else { if (m_Led8MotionFlag[m_selectedIndex]) { //g_pLogger->SendAndFlushWithTime(L"[TCPSend] IOCheck8\n"); IOCheck[0] = 0x01; IOCheck[1] = 0x04; } else { //g_pLogger->SendAndFlushWithTime(L"[TCPSend] IOCheck26\n"); IOCheck[0] = 0x01; IOCheck[1] = 0x01; } bytesSend = send(m_socket[m_selectedIndex], (const char*)IOCheck, 64, 0); for (size_t i = 0; i < 64; i++) { TempData[i] = IOCheck[i]; } } if (bytesSend == SOCKET_ERROR) { bytesSend = WSAGetLastError(); if (bytesSend == WSAEWOULDBLOCK || bytesSend == WSAENOTSOCK) { Sleep(2); bytesSend = send(m_socket[m_selectedIndex], (const char*)TempData, 64, 0); //have to wait for the next receive event } else { g_pLogger->SendAndFlushWithTime(L"[TCPSend] ERROR1 bytesSend = %d\n", bytesSend); sEvenProp.Init(); sEvenProp.EventType = HSI_EVENT_ERROR; sEvenProp.EventID = HSI_EVENT_MOTION; sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK; strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "光源板已断开连接"); EventCallback(sEvenProp); m_ThreadTCP_State = TCPIP_THREAD_EXIT; closesocket(m_socket[m_selectedIndex]); WSACleanup(); return TCPIP_CONNECT_STATUS_UNKNOWN; } } //ZeroMemory(tReciveData, TCPIP_MAX_DAT_SIZE); int bytesReceived = 0; int num = 0; bytesReceived = recv(m_socket[m_selectedIndex], (char*)tReciveData, TCPIP_MAX_DAT_SIZE, 0); if (bytesReceived == SOCKET_ERROR) { bytesReceived = WSAGetLastError(); if (bytesReceived == WSAEWOULDBLOCK || bytesReceived == WSAENOTSOCK) { //ZeroMemory(tReciveData, TCPIP_MAX_DAT_SIZE); bytesReceived = recv(m_socket[m_selectedIndex], (char*)tReciveData, TCPIP_MAX_DAT_SIZE, 0); if (bytesReceived == SOCKET_ERROR) { recvFlag = false; } else { recvFlag = true; } //if (num > 5) //{ // break; //} //Sleep(1); //num++; ////have to wait for the next receive event //continue; } else { g_pLogger->SendAndFlushWithTime(L"[TCPReceived] ERROR1 bytesReceived = %d\n", bytesReceived); sEvenProp.Init(); sEvenProp.EventType = HSI_EVENT_ERROR; sEvenProp.EventID = HSI_EVENT_MOTION; sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK; strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "光源板已断开连接"); EventCallback(sEvenProp); m_ThreadTCP_State = TCPIP_THREAD_EXIT; closesocket(m_socket[m_selectedIndex]); WSACleanup(); return TCPIP_CONNECT_STATUS_UNKNOWN; } } else { recvFlag = true; } if (!recvFlag) { bytesSend = send(m_socket[m_selectedIndex], (const char*)TempData, 64, 0); if (bytesSend == SOCKET_ERROR) { bytesSend = WSAGetLastError(); if (bytesSend == WSAEWOULDBLOCK) { bytesSend = send(m_socket[m_selectedIndex], (const char*)TempData, 64, 0); if (bytesSend == SOCKET_ERROR) { g_pLogger->SendAndFlushWithTime(L"[TCPSend] ERROR2\n"); } //have to wait for the next receive event } } bytesReceived = recv(m_socket[m_selectedIndex], (char*)tReciveData, TCPIP_MAX_DAT_SIZE, 0); if (bytesReceived == SOCKET_ERROR) { bytesReceived = WSAGetLastError(); if (bytesReceived == WSAEWOULDBLOCK) { //ZeroMemory(tReciveData, TCPIP_MAX_DAT_SIZE); bytesReceived = recv(m_socket[m_selectedIndex], (char*)tReciveData, TCPIP_MAX_DAT_SIZE, 0); if (bytesReceived == SOCKET_ERROR) { g_pLogger->SendAndFlushWithTime(L"[TCPReceived] ERROR2\n"); } } } } Sleep(5); } return TCPIP_CONNECT_OK; } void HSI_Motion::Create_Thread() { if (!m_ThreadTCP_Id) { m_ThreadTCP_State = TCPIP_THREAD_RUNNING; m_ThreadTCP_Id = ::CreateThread(nullptr, 0, (LPTHREAD_START_ROUTINE)m_ThreadSendTCP, this, 0, nullptr); } } void HSI_Motion::Exit_Thread() { m_ThreadTCP_State = TCPIP_THREAD_EXIT; if (m_ThreadTCP_Id) { DWORD dwCode = STILL_ACTIVE; while (dwCode == STILL_ACTIVE) { GetExitCodeThread(m_ThreadTCP_Id, &dwCode); Sleep(1); } } m_ThreadTCP_State = TCPIP_THREAD_EXIT; m_ThreadTCP_Id = nullptr; } int HSI_Motion::Init_Winsock() { int nRet; if (first) { WSADATA stWSAData; /* WinSock DLL Info */ nRet = WSAStartup(WSA_VERSION, &stWSAData); if (nRet) { return FALSE; } first = false; } return TRUE; } #pragma endregion ////////////////////////////////////////////////////////////////////////// //=========================================================================== HSI_STATUS HSI_Motion::CollectPos(bool isEnable, MOTOR_AXISCHOOES_CMD axis, short cycle) { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { unsigned char send_glue_data[64] = {0}; send_glue_data[0] = 0x01; send_glue_data[1] = 0x1E; send_glue_data[2] = isEnable ? 1 : 2; send_glue_data[3] = ((cycle >> 8) & 0xff); send_glue_data[4] = (cycle & 0xff); send_glue_data[5] = axis; Send_Command(0, (const char*)send_glue_data, 64); } return rStatus; } //=========================================================================== HSI_STATUS HSI_Motion::SetAllGears() { auto rStatus = HSI_STATUS_NORMAL; if (g_pHSI_Motion) { for (size_t i = 1; i < 5; i++) { unsigned char send_glue_data[64] = {0}; send_glue_data[0] = 0x01; send_glue_data[1] = 0x01; send_glue_data[2] = 0x01 << (i - 1); //轴号 send_glue_data[3] = 0x08; for (size_t j = 0; j < 5; j++) { send_glue_data[4 + 6 * j] = ((m_JogStartSpeed[i][4 - j] >> 8) & 0xff); send_glue_data[5 + 6 * j] = (m_JogStartSpeed[i][4 - j] & 0xff); send_glue_data[6 + 6 * j] = ((m_JogDriveSpeed[i][4 - j] >> 8) & 0xff); send_glue_data[7 + 6 * j] = (m_JogDriveSpeed[i][4 - j] & 0xff); send_glue_data[8 + 6 * j] = ((m_JogAccLine[i][4 - j] >> 8) & 0xff); send_glue_data[9 + 6 * j] = (m_JogAccLine[i][4 - j] & 0xff); } Send_Command(0, (const char*)send_glue_data, 64); Sleep(10); } } return rStatus; } //===========================================================================