xhouse/LM-Middleware
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

回家判断，运动到指定点，获取当前位置，如何获取运动结束判断

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