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

重写 HomeMachine 、IsHomed、GetPositionXyz 三个函数

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