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

1、运动到指定点，JOG运动，运动结束的判断

Browse Source 
2、优化测试用例
This commit is contained in:
zhengxuan.zhang
2022-10-19 20:20:52 +08:00
parent 3473b1d637
commit df5c6c7c3f
12 changed files with 958 additions and 745 deletions
Download Patch File Download Diff File Expand all files Collapse all files
HSI_HexagonMI_EF3/HSI.h
+16 -31
View File
@@ -322,36 +322,21 @@ enum HSI_MOTION_TYPE
enum HSI_MOTION_AXIS_TYPE
{
HSI_MOTION_AXIS_X = 0x0001,
// This is the default "Sensor level" X Axis - use on single X axis machines
HSI_MOTION_AXIS_Y = 0x0002,
// This is the default "Sensor level" Y Axis - use on single Y axis machines
HSI_MOTION_AXIS_Z = 0x0004,
// This is the default "Sensor level" Z Axis - use on single Z axis machines
HSI_MOTION_AXIS_R = 0x0008,
// This is the default "Sensor level" R Axis - use on single R axis machines
HSI_MOTION_AXIS_X1 = 0x0010,
// This is the 1st X Axis - use on multiple axis machines when specific axis needed
HSI_MOTION_AXIS_Y1 = 0x0020,
// This is the 1st Y Axis - use on multiple axis machines when specific axis needed
HSI_MOTION_AXIS_Z1 = 0x0040,
// This is the 1st Z Axis - use on multiple axis machines when specific axis needed
HSI_MOTION_AXIS_R1 = 0x0080,
// This is the 1st R Axis - use on multiple axis machines when specific axis needed
HSI_MOTION_AXIS_X2 = 0x0100,
// This is the 2nd X Axis - use on multiple axis machines when specific axis needed
HSI_MOTION_AXIS_Y2 = 0x0200,
// This is the 2nd Y Axis - use on multiple axis machines when specific axis needed
HSI_MOTION_AXIS_Z2 = 0x0400,
// This is the 2nd Z Axis - use on multiple axis machines when specific axis needed
HSI_MOTION_AXIS_R2 = 0x0800,
// This is the 2nd R Axis - use on multiple axis machines when specific axis needed
HSI_MOTION_AXIS_X3 = 0x1000,
// This is the 3rd X Axis - use on multiple axis machines when specific axis needed
HSI_MOTION_AXIS_Y3 = 0x2000,
// This is the 3rd Y Axis - use on multiple axis machines when specific axis needed
HSI_MOTION_AXIS_Z3 = 0x4000,
// This is the 3rd Z Axis - use on multiple axis machines when specific axis needed
HSI_MOTION_AXIS_X = 0x0001,// This is the default "Sensor level" X Axis - use on single X axis machines
HSI_MOTION_AXIS_Y = 0x0000,// This is the default "Sensor level" Y Axis - use on single Y axis machines
HSI_MOTION_AXIS_Z = 0x0004,// This is the default "Sensor level" Z Axis - use on single Z axis machines
HSI_MOTION_AXIS_R = 0x0008,// This is the default "Sensor level" R Axis - use on single R axis machines
HSI_MOTION_AXIS_X1 = 0x0010,// This is the 1st X Axis - use on multiple axis machines when specific axis needed
HSI_MOTION_AXIS_Y1 = 0x0020,// This is the 1st Y Axis - use on multiple axis machines when specific axis needed
HSI_MOTION_AXIS_Z1 = 0x0040,// This is the 1st Z Axis - use on multiple axis machines when specific axis needed
HSI_MOTION_AXIS_R1 = 0x0080,// This is the 1st R Axis - use on multiple axis machines when specific axis needed
HSI_MOTION_AXIS_X2 = 0x0100,// This is the 2nd X Axis - use on multiple axis machines when specific axis needed
HSI_MOTION_AXIS_Y2 = 0x0200,// This is the 2nd Y Axis - use on multiple axis machines when specific axis needed
HSI_MOTION_AXIS_Z2 = 0x0400,// This is the 2nd Z Axis - use on multiple axis machines when specific axis needed
HSI_MOTION_AXIS_R2 = 0x0800,// This is the 2nd R Axis - use on multiple axis machines when specific axis needed
HSI_MOTION_AXIS_X3 = 0x1000,// This is the 3rd X Axis - use on multiple axis machines when specific axis needed
HSI_MOTION_AXIS_Y3 = 0x2000,// This is the 3rd Y Axis - use on multiple axis machines when specific axis needed
HSI_MOTION_AXIS_Z3 = 0x4000,// This is the 3rd Z Axis - use on multiple axis machines when specific axis needed
HSI_MOTION_AXIS_R3 = 0x8000 // This is the 3rd R Axis - use on multiple axis machines when specific axis needed
};
@@ -373,7 +358,7 @@ enum HSI_MOTION_IO_TYPE
};
//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;
const UINT HSI_MOTION_AXIS_ALL = ACSC_AXIS_0 | ACSC_AXIS_1 | ACSC_AXIS_4;
enum HSI_MOTION_MOVE_TYPE
{
HSI_MOTION_MOVE_WAIT = 1,
HSI_HexagonMI_EF3/HSI_Motion.cpp
+371 -256
View File
@@ -91,8 +91,8 @@ void ErrorsHandler()
))
{
ErrorStr[Received] = '\0';
printf("function returned error: %d (%s)\n", ErrorCode, ErrorStr);
g_pLogger->SendAndFlushWithTime(L"[ACS Motion] motion failed {%s}\n", ErrorStr);
printf("Motion Error: %d [%s]\n", ErrorCode, ErrorStr);
g_pLogger->SendAndFlushWithTime(L"[ACS Motion] Motion Error, {%s}\n", ErrorStr);
}
}
else
@@ -105,6 +105,7 @@ void ErrorsHandler()
HSI_Motion::HSI_Motion()
{
TRACE0("HSI_Motion Constructor!\n");
sEvenProp.Init();
sEvenProp.EventCallbackID = 0;
sEvenProp.EventResponse = HSI_EVENT_FUNCTION_OK;
@@ -230,11 +231,12 @@ HSI_Motion::HSI_Motion()
//DeleteDirectory()
CTime tm = CTime::GetCurrentTime();
CString csTime = tm.Format("%Y-%m-%d_%H-%M-%S"); //构造时间字符串
//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++)
{
@@ -284,9 +286,13 @@ HSI_Motion::HSI_Motion()
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->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为步进电机
@@ -408,14 +414,15 @@ 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 配置项
GoogolMotionConfigFile = m_AppPath + _T("\\Config\\EF3_Motion.ini");
Load_EF3_Motion_Inifile(GoogolMotionConfigFile); // 加载 EF3_Motion.ini 配置项
if (m_IsUseEF3 == 1) //如果使用 EF3
//如果使用 EF3
if (m_IsUseEF3 == 1)
{
if (!m_bConnected)
{
@@ -434,7 +441,7 @@ HSI_STATUS HSI_Motion::Startup(HWND _hWnd, bool _bOfflineOnly)
}
m_SO7_Serial.SetTimeouts(1000, 1000);
m_bConnected = true;
g_pLogger->SendAndFlushWithTime(L"[Startup] Serial: [%d] is open success\n", m_EF3COMPort);
g_pLogger->SendAndFlushWithTime(L"[Startup] Serial: [COM%d] is open success\n", m_EF3COMPort);
}
else
{
@@ -442,7 +449,8 @@ HSI_STATUS HSI_Motion::Startup(HWND _hWnd, bool _bOfflineOnly)
}
}
if (m_IsUseACS == 1) //如果启用ACS
//如果启用ACS
if (m_IsUseACS == 1)
{
if (!m_bACSConnected)
{
@@ -469,24 +477,41 @@ HSI_STATUS HSI_Motion::Startup(HWND _hWnd, bool _bOfflineOnly)
EventCallback(sEvenProp);
return HSI_STATUS_FAILED;
}
//使能电机
int Axes[] = { ACSC_AXIS_0, ACSC_AXIS_1, ACSC_AXIS_4, -1 };
if(!acsc_EnableM(handleACS, Axes,NULL))
{
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 Established Success!\n");
g_pLogger->SendAndFlushWithTime(L"[ACS Motion] ACS Initialization Success!\n");
//获取ACS 控制器版本号, 待测试，2.70
char Firmware[256];
/*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();
}
g_pLogger->SendAndFlushWithTime(L"[ACS Motion] ACS Controller Version: %s\n",
convertToString(Firmware, Received));
Firmware[Received-1] = '\0';
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)));
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
{
@@ -549,7 +574,7 @@ HSI_STATUS HSI_Motion::Startup(HWND _hWnd, bool _bOfflineOnly)
Sleep(5);
}
g_pLogger->SendAndFlushWithTime(L"[Startup] Connected scuuess\n");
g_pLogger->SendAndFlushWithTime(L"[Startup] Connected success\n");
if (HSI_STATUS_FAILED == DriverAlarmStatus())
{
@@ -600,7 +625,8 @@ HSI_STATUS HSI_Motion::Startup(HWND _hWnd, bool _bOfflineOnly)
//m_cSendData[12] = m_IsCollectPos; //是否从串口打印位置
//m_cSendData[16] = m_IsLightDebug; //是否不回家也能调试灯光
//if (m_IsStartInput == 1 && m_IsUseRocker == 2) //新摇杆,并启动脚踏功能
//新摇杆,并启动脚踏功能
//if (m_IsStartInput == 1 && m_IsUseRocker == 2)
//{
// m_cSendData[14] = m_StartInputPort >> 8 & 0xff; //外部启动端口号 H
// m_cSendData[15] = m_StartInputPort & 0xff; //外部启动端口号 L
@@ -705,7 +731,8 @@ HSI_STATUS HSI_Motion::Startup(HWND _hWnd, bool _bOfflineOnly)
g_pLogger->SendAndFlushWithTime(L"[Startup] Set Gears Success\n");
}
CreateThread(); //刷新位置状态线程
//刷新位置状态线程
CreateThread();
SetEvent(m_hTriggerEvent);
m_Thread_State = HSI_THREAD_PAUSED;
g_pLogger->SendAndFlushWithTime(L"[Startup] SetpositionXyz Enable\n");
@@ -722,7 +749,8 @@ HSI_STATUS HSI_Motion::Startup(HWND _hWnd, bool _bOfflineOnly)
// m_Thread_StateJOGStop = HSI_THREAD_PAUSED;
//}
if (m_IsIOFuntion == 1) //是否启用IO功能线程，通过配置文件启用
//是否启用IO功能线程，通过配置文件启用
if (m_IsIOFuntion == 1)
{
m_Thread_StateIO = HSI_THREAD_RUNNING;
CreateThreadIO(); //IO发消息使用
@@ -731,7 +759,8 @@ HSI_STATUS HSI_Motion::Startup(HWND _hWnd, bool _bOfflineOnly)
SetDIO(HSI_MOTION_OUTPUT_CH1, 0xfffff);
}
if (m_IsProbe == 1) //是否启用探针捕获功能，通过配置文件启用
//是否启用探针捕获功能，通过配置文件启用
if (m_IsProbe == 1)
{
CreateThreadProbe(); //探针捕获功能
m_Thread_StateProbe = HSI_THREAD_RUNNING;
@@ -811,11 +840,14 @@ HSI_STATUS HSI_Motion::GetFirmwareVersion(byte* version)
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 回家
@@ -1042,9 +1074,9 @@ HSI_STATUS HSI_Motion::HomeMachine(bool bHomed)
return HSI_STATUS_FAILED;
}
CurrentHomeMachineState = E_EF3_HOME_ING;
CurrentHomeMachineState = E_EF3_HOME_ING; //正在回家中
//运行 ACS 控制器内buffer0 自动回家动作
//运行 ACS 控制器内 buffer6 自动回家动作
//回家后，启用正负限位
if (!acsc_RunBuffer(handleACS, ACSC_BUFFER_6, nullptr, ACSC_SYNCHRONOUS))
{
@@ -1058,7 +1090,6 @@ HSI_STATUS HSI_Motion::HomeMachine(bool bHomed)
if (home == true)
{
g_pLogger->SendAndFlushWithTime(L"[HomeMachine] Go Home success\n");
return HSI_STATUS_NORMAL;
}
g_pLogger->SendAndFlushWithTime(L"[HomeMachine] Out\n");
@@ -1797,7 +1828,7 @@ HSI_STATUS HSI_Motion::JogOld(UINT AxisTypes, double Speed)
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);
g_pLogger->SendAndFlushWithTime(L"[Jog] Out,bJOGDir = %s\n", bJOGDir);
}
return rStatus;
}
@@ -1807,7 +1838,7 @@ HSI_STATUS HSI_Motion::Jog(UINT AxisTypes, double Speed)
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
if (m_DeviceType != 3)
if (m_DeviceType != 3) //非转盘设备
{
m_Thread_StateJOGStop = HSI_THREAD_RUNNING;
SetEvent(m_hTriggerEventJOGStop);
@@ -1818,6 +1849,7 @@ HSI_STATUS HSI_Motion::Jog(UINT AxisTypes, double Speed)
jogAxisNum = AxisNumber;
jogDirFlag = bJOGDir;
m_Thread_State = HSI_THREAD_PAUSED;
if (CurrentHomeMachineState == E_EF3_HOME_FINISHED)
{
//软限位
@@ -1826,14 +1858,13 @@ HSI_STATUS HSI_Motion::Jog(UINT AxisTypes, double Speed)
{
//无效软限位
}
//是否回过家
if (m_Home_Machine_Axis[AxisNumber] == 0)
{
return rStatus;
}
//设置 JOG运动参数 加减速 JOG_SPEED_ACC_DEC
// 是否启用急停
//if ((StartSpeed < 250) && (DriveSpeed < 6))
//{
@@ -1844,15 +1875,19 @@ HSI_STATUS HSI_Motion::Jog(UINT AxisTypes, double Speed)
// m_IsUseJerk = 0;
//}
//设置 JOG运动参数 加减速 JOG_SPEED_ACC_DEC
//开始JOG运动
int acsDirection = bJOGDir ? ACSC_POSITIVE_DIRECTION : ACSC_NEGATIVE_DIRECTION;
int acsDirection = bJOGDir ? ACSC_POSITIVE_DIRECTION : ACSC_NEGATIVE_DIRECTION; //正方向，或 负方向
if (!acsc_Jog(handleACS, 0, AxisTypes, acsDirection, nullptr))
{
g_pLogger->SendAndFlushWithTime(L"[Jog] [%d] 轴 [%s] 方向移动失败", AxisTypes, bJOGDir ? "正" : "负");
ErrorsHandler();
}
jogMoving = true;
g_pLogger->SendAndFlushWithTime(L"[Jog] Out,bJOGDir = %d\n", bJOGDir);
g_pLogger->SendAndFlushWithTime(L"[Jog] Out, bJOGDir = %s\n", bJOGDir ? "positive":"negative");
}
return rStatus;
}
@@ -2154,10 +2189,10 @@ HSI_STATUS HSI_Motion::JoyStick(UINT AxisTypes, long Speed)
//===========================================================================
/**
* \brief
* \brief 停止Jog运动
* \return
*/
HSI_STATUS HSI_Motion::StopJog()
HSI_STATUS HSI_Motion::StopJogOld()
{
WaitForSingleObject(g_Lock_JogAndTrigger, INFINITE);
auto rStatus = HSI_STATUS_NORMAL;
@@ -2173,7 +2208,7 @@ HSI_STATUS HSI_Motion::StopJog()
g_pLogger->SendAndFlushWithTime(L"[StopJog] PushButtonTime = %d\n", t_use);
Sleep(t_use);
}
unsigned char m_SendJogData[64] = {0};
unsigned char m_SendJogData[64] = { 0 };
if (m_IsUseJerk == 0)
{
m_SendJogData[0] = CT_MOTOR;
@@ -2203,7 +2238,7 @@ HSI_STATUS HSI_Motion::StopJog()
return rStatus;
}
HSI_STATUS HSI_Motion::StopJogOld()
HSI_STATUS HSI_Motion::StopJog()
{
WaitForSingleObject(g_Lock_JogAndTrigger, INFINITE);
auto rStatus = HSI_STATUS_NORMAL;
@@ -2220,7 +2255,9 @@ HSI_STATUS HSI_Motion::StopJogOld()
Sleep(t_use);
}
unsigned char m_SendJogData[64] = {0};
if (m_IsUseJerk == 0)
//是否启用急停
/*if (m_IsUseJerk == 0)
{
m_SendJogData[0] = CT_MOTOR;
m_SendJogData[1] = CT_STOP;
@@ -2241,7 +2278,18 @@ HSI_STATUS HSI_Motion::StopJogOld()
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_0, ACSC_AXIS_1, ACSC_AXIS_4, -1 };
if(handleACS !=ACSC_INVALID)
{
if(!acsc_HaltM(handleACS, Axes, NULL))//停止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);
@@ -2484,23 +2532,23 @@ HSI_STATUS HSI_Motion::GetPositionXyz(UINT AxisTypes, double& PositionX, double&
CString tempStr;
bool bGetPosition = true;
if (g_pHSI_Motion &&(handleACS!=ACSC_INVALID)) //句柄有效
if (g_pHSI_Motion && (handleACS != ACSC_INVALID)) //句柄有效
{
if (!acsc_GetFPosition(handleACS, ACSC_AXIS_0, &PositionX, nullptr))
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_1, &PositionY, nullptr))
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_2, &PositionZ, nullptr))
if (!acsc_GetFPosition(handleACS, ACSC_AXIS_4, &PositionZ, nullptr))
{
g_pLogger->SendAndFlushWithTime(L"[GetPositionEncPrfMulti] get PositionZ failed\n");
ErrorsHandler();
@@ -2518,7 +2566,9 @@ HSI_STATUS HSI_Motion::GetPositionXyz(UINT AxisTypes, double& PositionX, double&
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]);
//g_pLogger->SendAndFlushWithTime(
// L"[GetPositionEncPrfMulti] GetPosition Success, Pos[1] = %.4f, Pos[2] = %.4f, Pos[3] = %.4f\n",
// PositionX, PositionY, PositionZ);
}
else //读取将之前的值进行返回
{
@@ -2529,8 +2579,10 @@ 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] 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]);
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;
@@ -3189,12 +3241,13 @@ 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);
unsigned char send_pos_data[64] = {0};
g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Pos[1] = %.4f,Pos[2] = %.4f,Pos[3] = %.4f\n", PositionX,
PositionY, PositionZ);
axis_start = 0;
unsigned char direct_pos = 0;
unsigned char xyzAxis = 0;
if (CurrentMotionState != E_SO7_MOTION_MOVETO)
if (CurrentMotionState != E_SO7_MOTION_MOVETO) //当前运动状态
{
CurrentMotionState = E_SO7_MOTION_MOVETO;
//限位功能
@@ -3202,6 +3255,7 @@ HSI_STATUS HSI_Motion::SetPositionXyz(UINT AxisTypes, double PositionX, double P
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, Pos[1] = %.4f, Pos[2] = %.4f, Pos[3] = %.4f, m_PositionA = %.4f\n",PositionX, PositionY, PositionZ, m_PositionA);
//SetpositionXyz的目标位置
m_PosThread[1] = PositionX;
@@ -3209,33 +3263,29 @@ HSI_STATUS HSI_Motion::SetPositionXyz(UINT AxisTypes, double PositionX, double P
m_PosThread[3] = PositionZ;
m_PosThread[4] = m_PositionA;
//打印当前位置，目标位置
//g_pLogger->SendAndFlushWithTime(
// L"[SetPositionXyzNowPos] Pos[1] = %.4f,Pos[2] = %.4f,Pos[3] = %.4f,Pos[4] = %.4f\n",
// NowPos[1] * m_Resolution[1], NowPos[2] * m_Resolution[2], NowPos[3] * m_Resolution[3],
// NowPos[4] * m_Resolution[4]);
/* g_pLogger->SendAndFlushWithTime(
L"[SetPositionXyzNowPos] Pos[1] = %.4f,Pos[2] = %.4f,Pos[3] = %.4f,Pos[4] = %.4f, Resolution[1] = %.4f\n",
m_PosThread[1] * m_Resolution[1], m_PosThread[2] * m_Resolution[2], m_PosThread[3] * m_Resolution[3],
m_PosThread[4] * m_Resolution[4], m_Resolution[1]);
g_pLogger->SendAndFlushWithTime(
L"[SetPositionXyzTagPos] Pos[1] = %.4f,Pos[2] = %.4f,Pos[3] = %.4f,Pos[4] = %.4f\n", PositionX,
PositionY, PositionZ, m_PositionA);
L"[SetPositionXyzTagPos] Pos[1] = %.4f, Pos[2] = %.4f, Pos[3] = %.4f\n", PositionX,
PositionY, PositionZ);*/
//打印轴当前运动参数
double motionParam[5] = {0};
GetMotorParam(AXIS_X, motionParam);
double* motionParam[5] = {0};
GetMotorParam(ACSC_AXIS_0, motionParam);
g_pLogger->SendAndFlushWithTime(
L"[SetPositionXyz] axis= %d,GetVelocity = %d,GetAcceleration= %d,GetDeceleration= %d, GetKillDeceleration= %d, GetJerk= %d\n",
L"[SetPositionXyz] axis= %d, GetVelocity = %d,GetAcceleration= %d,GetDeceleration= %d, GetKillDeceleration= %d, GetJerk= %d\n",
AXIS_X, motionParam[0], motionParam[1], motionParam[2], motionParam[3], motionParam[4]);
//设置轴运动速度，TO-DO
//开始运动到指定位置
int Axes[] = {ACSC_AXIS_0, ACSC_AXIS_1,-1}; //需要运动的轴
double Points[] = {PositionX, PositionY,-1}; //目标位置点
if (!acsc_ToPointM(handleACS, 0, Axes, Points, nullptr))
//开始运动到指定位置，多轴运动
int Axes[] = {ACSC_AXIS_0, ACSC_AXIS_1,ACSC_AXIS_4, -1}; //需要运动的轴
double Points[] = {PositionY, PositionX, PositionZ}; //目标位置点
if (!acsc_ToPointM(handleACS, 0, Axes, Points, nullptr))//移动到绝对位置
{
g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] ACS Multi Motion Error\n");
ErrorsHandler();
@@ -3243,10 +3293,9 @@ HSI_STATUS HSI_Motion::SetPositionXyz(UINT AxisTypes, double PositionX, double P
//启动插补和定位功能
//圆弧插补
//状态更新
if (eType == HSI_MOTION_MOVE_NOWAIT) //非等待
{
g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Nowait SetEvent\n");
@@ -3300,31 +3349,59 @@ int HSI_Motion::CaculateStepMotorACC(int pos, int maxacc, int minacc)
* \param motionParam
* \return
*/
HSI_STATUS HSI_Motion::GetMotorParam(int AXIS, double motionParam[5])
//&符号用来处理一个变量，但不是通常的 - 访问这个变量的内容，而是取出这个变量的地址
//
//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); //利用引用返回数组
HSI_STATUS HSI_Motion::GetMotorParam(int AXIS, double* motionParam[5])
{
auto rStatus = HSI_STATUS_NORMAL;
if (handleACS != ACSC_INVALID)
{
//依次是 速度、加速度、减速、杀死速度、抖动
if (!acsc_GetVelocity(handleACS, AXIS, motionParam, nullptr))
if (!acsc_GetVelocity(handleACS, AXIS, motionParam[0], nullptr))
{
g_pLogger->SendAndFlushWithTime(L"[GetMotorParam] acsc_GetVelocity error\n");
rStatus = HSI_ACS_ERROR;
ErrorsHandler();
}
if (!acsc_GetAcceleration(handleACS, AXIS, motionParam + 1, nullptr))
if (!acsc_GetAcceleration(handleACS, AXIS, motionParam[1], nullptr))
{
g_pLogger->SendAndFlushWithTime(L"[GetMotorParam] acsc_GetAcceleration error\n");
rStatus = HSI_ACS_ERROR;
ErrorsHandler();
}
if (!acsc_GetDeceleration(handleACS, AXIS, motionParam + 2, nullptr))
if (!acsc_GetDeceleration(handleACS, AXIS, motionParam[2], nullptr))
{
g_pLogger->SendAndFlushWithTime(L"[GetMotorParam] acsc_GetDeceleration error\n");
rStatus = HSI_ACS_ERROR;
ErrorsHandler();
}
if (!acsc_GetKillDeceleration(handleACS, AXIS, motionParam + 3, nullptr))
if (!acsc_GetKillDeceleration(handleACS, AXIS, motionParam[3], nullptr))
{
g_pLogger->SendAndFlushWithTime(L"[GetMotorParam] acsc_GetKillDeceleration error\n");
rStatus = HSI_ACS_ERROR;
ErrorsHandler();
}
if (!acsc_GetJerk(handleACS, AXIS, motionParam + 4, nullptr))
if (!acsc_GetJerk(handleACS, AXIS, motionParam[4], nullptr))
{
g_pLogger->SendAndFlushWithTime(L"[GetMotorParam] acsc_GetJerk error\n");
rStatus = HSI_ACS_ERROR;
ErrorsHandler();
}
}
@@ -3852,7 +3929,7 @@ void HSI_Motion::UpdateMotionState()
bool interpolationflag = false;
bool timeoutflag = false;
int timeStart = GetTickCount();
//do
//{
// if (g_IsClose)
@@ -3861,11 +3938,6 @@ void HSI_Motion::UpdateMotionState()
// 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))
@@ -3899,6 +3971,201 @@ void HSI_Motion::UpdateMotionState()
// }
//}
//while (true);
g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Now wait Run End\n");
printf("\nWaiting for motion end\n");
acsc_WaitMotionEnd(handleACS, ACSC_AXIS_0, INFINITE);
acsc_WaitMotionEnd(handleACS, ACSC_AXIS_1, INFINITE);
acsc_WaitMotionEnd(handleACS, ACSC_AXIS_4, 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]
@@ -4005,7 +4272,7 @@ void HSI_Motion::UpdateMotionState()
m_Thread_State = HSI_THREAD_PAUSED;
CurrentMotionState = E_SO7_MOTION_NONE;
m_IsExMotion = 2;
SendMsgMotionFinished();//定位完成
SendMsgMotionFinished();
break;
}
default:
@@ -4019,180 +4286,6 @@ 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()
{
@@ -4371,7 +4464,8 @@ void HSI_Motion::DoEvents()
MSG msg;
//GetMessage:从系统获取消息，将消息从系统中移除，属于阻塞函数。当系统无消息时，GetMessage会等待下一条消息。
//函数PeekMesssge是以查看的方式从系统中获取消息，可以不将消息从系统中移除，是非阻塞函数；当系统无消息时，返回FALSE，继续执行后续代码
//PeekMesssge是以查看的方式从系统中获取消息，可以不将消息从系统中移除，是非阻塞函数；
//当系统无消息时，返回FALSE，继续执行后续代码
while (PeekMessage(&msg, nullptr, 0, 0, PM_REMOVE))
{
__try
@@ -4576,7 +4670,7 @@ HSI_STATUS HSI_Motion::GetAxisStatus(int* _Status)
* \brief 暂停和关闭
* \return
*/
HSI_STATUS HSI_Motion::AbortMotion() //需要运动实现
HSI_STATUS HSI_Motion::AbortMotionOld() //需要运动实现
{
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
@@ -4615,6 +4709,27 @@ HSI_STATUS HSI_Motion::AbortMotion() //
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_0, ACSC_AXIS_1, ACSC_AXIS_4, -1 };
if (handleACS != ACSC_INVALID)
{
if (!acsc_HaltM(handleACS, Axes, NULL))//停止JOG运动
{
g_pLogger->SendAndFlushWithTime(L"[AbortMotion] ACS acsc_HaltM error!\n");
ErrorsHandler();
}
}
g_pLogger->SendAndFlushWithTime(L"[AbortMotion] Out\n");
}
return rStatus;
}
//===========================================================================
/**
* \brief 关闭
@@ -7315,7 +7430,7 @@ unsigned __stdcall HSI_Motion::m_ThreadData(LPVOID pThis)
case HSI_THREAD_RUNNING:
{
TRACE("HSI_THREAD_RUNNING.\r\n");
//_This->UpdateMotionStateData();//获取运动状态命令
_This->UpdateMotionStateData();//获取运动状态命令
break;
}
case HSI_THREAD_PAUSED:
HSI_HexagonMI_EF3/HSI_Motion.h
+17 -23
View File
@@ -95,7 +95,7 @@ enum MOTOR_START_POSOTION //
enum MOTOR_AXISCHOOES_CMD //第三级指令
{
AXIS_X = 0x01,
AXIS_Y = 0x02,
AXIS_Y = 0x00,
AXIS_Z = 0x04,
AXIS_U = 0x08,
AXIS_XY = 0x03,
@@ -171,7 +171,6 @@ public:
*/
HSI_STATUS HomeMachine(bool bHomed);
HSI_STATUS HomeMachineOld(bool bHomed);
HSI_STATUS HomeJog(short AxisNumber, short Dir, bool Wait = false);
HSI_STATUS HomeFindIndex();
HSI_STATUS ZeroPos(bool bZeroPos);
@@ -183,7 +182,6 @@ public:
*/
HSI_STATUS IsHomed(bool& bHomed);
HSI_STATUS IsHomedOld(bool& bHomed);
HSI_STATUS GetSpeedXyz(int AxisNum, double& Speed);
HSI_STATUS SetSpeedXyz(double Speed);
HSI_STATUS GetFocusSpeed(double& Speed);
@@ -192,10 +190,8 @@ public:
HSI_STATUS SetAccelerationXyz(double AccelX, double AccelY, double AccelZ);
HSI_STATUS GetSpeedR(double& Speed);
HSI_STATUS SetSpeedR(double Speed);
HSI_STATUS GetScaleResolution(double& _ScaleX, double& _ScaleY, double& _ScaleZ);
HSI_STATUS SetScaleResolution(double _ScaleX, double _ScaleY, double _ScaleZ);
HSI_STATUS GetDeadBand(double& DeadbandX, double& DeadbandY, double& DeadbandZ, double& DeadbandR);
HSI_STATUS GetRefreshDeadBand(double& Deadband);
/**
@@ -206,7 +202,6 @@ public:
*/
HSI_STATUS Jog(UINT AxisTypes, double Speed);
HSI_STATUS JogOld(UINT AxisTypes, double Speed);
HSI_STATUS JoyStick(UINT AxisTypes, long Speed);
/**
* \brief
@@ -215,7 +210,6 @@ public:
HSI_STATUS StopJog();
HSI_STATUS StopJogOld();
HSI_STATUS StopJogEx(UINT AxisTypes);
HSI_STATUS GetPositionEncPrfMulti(UINT AxisTypes, double* EncPos, double* PrfPos, int Count);
/**
* \brief 获取轴当前位置
@@ -228,7 +222,6 @@ public:
*/
HSI_STATUS GetPositionXyz(UINT AxisTypes, double& PositionX, double& PositionY, double& PositionZ, double& Time);
HSI_STATUS GetPositionXyzOld(UINT AxisTypes, double& PositionX, double& PositionY, double& PositionZ, double& Time);
HSI_STATUS GetPositionXyzaProbe(UINT AxisTypes, double& PositionX, double& PositionY, double& PositionZ,
double& PositionA);
HSI_STATUS GetEncoderXyz(long* lEncoderVal);
@@ -241,7 +234,7 @@ public:
* \param motionParam
* \return
*/
HSI_STATUS HSI_Motion::GetMotorParam(int AXIS, double motionParam[5]);
HSI_STATUS HSI_Motion::GetMotorParam(int AXIS, double* motionParam[5]);
/**
* \brief 运行到指定位置
@@ -256,17 +249,21 @@ public:
HSI_STATUS SetPositionXyz(UINT AxisTypes, double PositionX, double PositionY, double PositionZ,
HSI_MOTION_MOVE_TYPE eType, double dFlyRadius);
HSI_STATUS SetPositionXyzOld(UINT AxisTypes, double PositionX, double PositionY, double PositionZ,
HSI_MOTION_MOVE_TYPE eType, double dFlyRadius);
HSI_MOTION_MOVE_TYPE eType, double dFlyRadius);
HSI_STATUS SetPositionXyza(UINT AxisTypes, double PositionX, double PositionY, double PositionZ, double PositionA,
HSI_MOTION_MOVE_TYPE eType, double dFlyRadius);
HSI_STATUS SetPositionXyzCache(UINT AxisTypes, HSI_MOTION_MOVE_TYPE eType, int DataCount, Point* CacheData);
HSI_STATUS GetPositionR(UINT AxisTypes, double& PositionR, double& Time);
HSI_STATUS SetPositionR(UINT AxisTypes, double PositionR, HSI_MOTION_AXIS_R_MOVE_TYPE DirectionType, bool bWait);
HSI_STATUS SetCircleInterpolate(double PositionX, double PositionY, double PositionZ);//圆弧插补
HSI_STATUS SetCircleInterpolate(double PositionX, double PositionY, double PositionZ); //圆弧插补
HSI_STATUS Load_EF3_Motion_Inifile(CString GoogolIniFile);
HSI_STATUS Load_EF3_Config_Inifile(CString GoogolIniFile);
/**
* \brief
* \return
*/
HSI_STATUS AbortMotion();
HSI_STATUS AbortMotionOld();
HSI_STATUS GetDIO(UINT IOChannel, UINT& _Status);
HSI_STATUS SetDIO(UINT IOChannel, UINT _Status);
@@ -278,7 +275,6 @@ public:
*/
HSI_STATUS Shutdown() override;
//HSI_STATUS ShutdownOld() override;
HSI_STATUS IsSupportedEx(UINT AxisTypes, UINT& Types);
HSI_STATUS StartupEx(UINT AxisTypes, bool bHome);
HSI_STATUS GetScaleResolutionEx(UINT AxisTypes, double& Scale);
@@ -290,7 +286,6 @@ public:
HSI_STATUS SetSpeedEx(UINT AxisTypes, double Speed);
HSI_STATUS GetAccelerationEx(UINT AxisTypes, double& Accel);
HSI_STATUS SetAccelerationEx(UINT AxisTypes, double Accel);
HSI_STATUS SetTriggerLight(int triggleNum, int delayLighting, int delayLightBefor, int triggleMode,
double* Intensities);
HSI_STATUS DCCPPStartPoint(double* startPoint);
@@ -298,10 +293,8 @@ public:
HSI_STATUS DCCScanStart();
HSI_STATUS DCCScanStop();
HSI_STATUS DCCForLightPlate();
HSI_STATUS IOStep(bool RunSts);
HSI_STATUS IOprogram(byte* SendData, int length);
HSI_STATUS FindOriginTest(bool state);
HSI_STATUS StartPlcJob(int* CamerasDis, int* BinsDis, int SubArea, int filterTime1, int filterTime2,
@@ -476,7 +469,7 @@ public:
bool bCircleRun; //圆弧插补
int iCircleRunPnt[5]; //圆弧插补时的圆心位置
int jogAxisNum; //jog运动的轴号
int jogspeed;//jog 运动的速度
int jogspeed; //jog 运动的速度
bool jogMoving;
bool jogDirFlag;
bool m_bEmergencyState;
@@ -502,7 +495,7 @@ public:
int SpCompleteTEnd;
int SpTimeCount;
bool SetPotionRunEnd;
double PntToPntDistance;
double PntToPntDistance; //欧氏距离
double m_LockPos[5];
double m_ijk[5];
@@ -538,10 +531,6 @@ public:
static unsigned __stdcall m_Thread(LPVOID pThis);
void CreateThread();
void CloseThread();
/**
* \brief
*
*/
void UpdateMotionState();
void UpdateMotionStateOld();
void UpdateMotionStateEx();
@@ -574,6 +563,11 @@ public:
int& AccCurve, int& DecCurve);
void HomeJogGearsChoice(int AxisType, int JogGears, int& DriveSpeed, int& StartSpeed, int& AccLine, int& DecLine,
int& AccCurve, int& DecCurve);
/**
* \brief
* \param AxisTypes
* \return
*/
short AxisConvertIndex(UINT AxisTypes);
short IndexConvertAxis(int Index);
double LimitOver(UINT AxisTypes, double& LimitPos);
@@ -592,7 +586,7 @@ private:
int iMotionDirection;
int begin_position[5]; //外触发到初始点需要发送的脉冲数
HINSTANCE m_Hinst;
E_SO7_MOTION_TYPE CurrentMotionState;
E_SO7_MOTION_TYPE CurrentMotionState; //当前运动状态
E_SO7_MOTION_READ_TYPE CurrentReadDataType;
//网口通信添加
HSI_HexagonMI_EF3/SevenOcean/CMMIO_SERIAL.CPP
+1 -1
View File
@@ -843,7 +843,7 @@ int CPSerial::ProgramPort(int Port, int Baud, char Parity, int Bits, int StopBit
}
else
{
TRACE(_T("CmmIO> Port was not OPEN ProgramPort not done \n"));
TRACE(_T("CmmIO> Port was not OPEN, Program Port not done!\n"));
Ok = TRUE;
}
}
HSI_HexagonMI_EF3/logger.cpp
+96 -20
View File
@@ -21,6 +21,7 @@ void CLogger::SendAtTime(const TCHAR* buffer)
_ftprintf(m_File,_T("%s"), csTime);
_ftprintf(m_File, _T("%s\r\n"), buffer);
}
LeaveCriticalSection(&m_lockLogger);
}
@@ -39,6 +40,7 @@ void CLogger::Send(LPCTSTR format, ...)
{
_ftprintf(m_File, m_Str);
}
va_end(list);
LeaveCriticalSection(&m_lockLogger);
}
@@ -59,6 +61,7 @@ void CLogger::SendAndFlush(LPCTSTR format, ...)
if (m_FileName.GetLength() > 0)
m_File = _wfsopen(m_FileName, _T("at"), _SH_DENYWR);
}
va_end(list);
LeaveCriticalSection(&m_lockLogger);
}
@@ -71,16 +74,17 @@ void CLogger::SendAndFlushWithTime(LPCTSTR format, ...)
}
if (!m_File)
{
m_File = _wfsopen(m_FileName, _T("at"), _SH_DENYNO);
m_File = _wfsopen(m_FileName, _T("at"), _SH_DENYNO); //追加
}
if (m_File)
{
int length = 0;
va_list list;
//在C中，当无法列出传递函数的所有实参的类型和数目时，可以用省略号指定参数表
va_start(list, format);
length = vswprintf_s(m_Str2, 5000, format, list);
CTime _cTime = CTime::GetCurrentTime();
CString csTime = _cTime.Format("[%m/%d %H:%M:%S");
CString csTime = _cTime.Format("[%Y%m/%d %H:%M:%S");
struct _timeb timebuffer;
_ftime64_s(&timebuffer);
if (m_File)
@@ -96,7 +100,14 @@ void CLogger::SendAndFlushWithTime(LPCTSTR format, ...)
fclose(m_File);
m_File = nullptr;
}
va_end(list);
}
/*放弃当前线程对锁定部分的所有权。一旦锁定部分的所有权被放弃，那么请求访问临界区的下一个线程，将可以对锁定部分进行操作。每一个调用EnterCriticalSection的线程，都应该调用一次LeaveCriticalSection。
使一个线程可以使用一个临界段对象来进行互斥同步。这个过程需要优先创建一个临界区结构体变量（分配使用内存）。在使用临界区之前, 待操作临界区的进程必须调用InitializeCriticalSection 或者 InitializeCriticalSectionAndSpinCount函数来初始化临界区。
一个线程使用EnterCriticalSection 或TryEnterCriticalSection函数来获得关键部分对象的所有权时，该线程必须在离开临界区时调用LeaveCriticalSection。*/
LeaveCriticalSection(&m_lockLogger);
}
@@ -117,31 +128,96 @@ void CLogger::SendAndFlushPerMode(LPCTSTR format, ...)
if (m_FileName.GetLength() > 0)
m_File = _wfsopen(m_FileName, _T("at"), _SH_DENYWR);
}
va_end(list);
LeaveCriticalSection(&m_lockLogger);
}
//void Getfilepath(const char* path, const char* filename, char* filepath)
//{
// strcpy(filepath, path);
// if (filepath[strlen(path) - 1] != '/')
// strcat(filepath, "/");
// strcat(filepath, filename);
// printf("path is = %s\n", filepath);
//}
//删除当前目录下txt文件
void DeleteOldFile()
{
}
string convertToString(char* a, int size)
string ConvertCharToString(char* a, int size)
{
int i;
string s = "";
for (i = 0; i < size; i++) {
for (i = 0; i < size; i++)
{
s = s + a[i];
}
return s;
}
}
//删除指定目录以及目录下的所有文件
#include <iostream>
#include <io.h>
#include <direct.h>
using namespace std;
void DelFiles(string path)
{
//在目录后面加上"\\*.*"进行第一次搜索
string newDir = path + "\\*.*";
//用于查找的句柄
intptr_t handle;
struct _finddata_t fileinfo;
//第一次查找
handle = _findfirst(newDir.c_str(), &fileinfo);
if (handle == -1)
{
cout << "无文件" << endl;
system("pause");
return;
}
do
{
if (fileinfo.attrib & _A_SUBDIR)
{
//如果为文件夹，加上文件夹路径，再次遍历
if (strcmp(fileinfo.name, ".") == 0 || strcmp(fileinfo.name, "..") == 0)
continue;
// 在目录后面加上"\\"和搜索到的目录名进行下一次搜索
newDir = path + "\\" + fileinfo.name;
DelFiles(newDir.c_str()); //先遍历删除文件夹下的文件，再删除空的文件夹
cout << newDir.c_str() << endl;
if (_rmdir(newDir.c_str()) == 0)
{
//删除空文件夹
cout << "delete empty dir success" << endl;
}
else
{
cout << "delete empty dir error" << endl;
}
}
else
{
string file_path = path + "\\" + fileinfo.name;
cout << file_path.c_str() << endl;
if (remove(file_path.c_str()) == 0)
{
//删除文件
cout << "delete file success" << endl;
}
else
{
cout << "delete file error" << endl;
}
}
}
while (!_findnext(handle, &fileinfo));
_findclose(handle);
return;
}
string ByteArrayToString(byte byteArray[])
{
char str[sizeof(byteArray) + 1];
memcpy(str, byteArray, sizeof(byteArray));
str[sizeof(byteArray)] = '\0';
return str;
}
HSI_HexagonMI_EF3/logger.h
+18 -5
View File
@@ -10,12 +10,18 @@
#include <stdio.h>
#include <stdarg.h>
using namespace std;
using namespace std;
const long LOGINIT = 0x0001;
const long LOGACTIONS = 0x0002;
const long LOGCOMM = 0x0004;
const long LOGFLUSH = 0x0008;
extern string ConvertCharToString(char* a, int size);
extern void DelFiles(string path);
extern string ByteArrayToString(byte byteArray[]);
class CLogger
{
public:
@@ -37,6 +43,13 @@ public:
tmpPath.Empty();
Path = tmpPath;
}
else //文件夹不为空
{
TRACE0("Log File Path: \n", CStringA());
//DelFiles(CT2A(m_Name.GetString()));
}
m_FileName = Path + m_Name; //_T("\\SO7_SSILog.txt");
m_lLogMask = 0;
InitializeCriticalSection(&m_lockLogger);
@@ -53,8 +66,8 @@ public:
void SendAndFlush(LPCTSTR, ...);
void SendAndFlushPerMode(LPCTSTR, ...);
void SendAndFlushWithTime(LPCTSTR, ...);
void DeleteOldFile();
bool IsEnabledLog/* = false*/; //是否启用日志
CString m_FileName;
@@ -62,8 +75,8 @@ public:
FILE* m_File;
_TCHAR m_Str[20000];
_TCHAR m_Str2[20000];
CRITICAL_SECTION m_lockLogger;
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.17 / 20:19 "
#define HSI_FILE_CSDESCRIPTION _T("2022.10.17 / 20:19 ")
#define HSI_FILE_DESCRIPTION "2022.10.19 / 20:17 "
#define HSI_FILE_CSDESCRIPTION _T("2022.10.19 / 20:17 ")