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

#0023: 对焦功能调试通过，新增Jog和PTP运动切换前，增加 Halt

Browse Source
This commit is contained in:
zhengxuan.zhang
2025-03-05 17:37:29 +08:00
parent 3925b2c3df
commit 9f3ee84467
3 changed files with 80 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
HSI_HexagonMI_EF3/HSI_Motion.cpp
+65 -9
View File
@@ -100,6 +100,7 @@ void HSI_Motion::ErrorsHandler()
g_pLogger->SendAndFlushWithTime(L"[ACS Motion] ACS Communication handle is invalid\n");
}
};
//===========================================================================
//运动类构造函数，涉及 运动控制参数、插补、回家、摇杆、日志等配置信息的加载
HSI_Motion::HSI_Motion()
@@ -205,6 +206,7 @@ HSI_Motion::HSI_Motion()
SetPotionRunEnd = false;
PntToPntDistance = 0.0;
m_IsLightDebug = 0; //是否不回家也能调试灯光 0为不启用 1为启用 默认为0
for (int i = 0; i < 4; i++)
{
m_IsOpenTCPIP[i] = ""; //可提供的tcp通信的ip
@@ -360,6 +362,9 @@ HSI_Motion::HSI_Motion()
g_RW_Data_Mutex = CreateMutex(nullptr, FALSE, nullptr);
g_WR_ToMove_Mutex = CreateMutex(nullptr, FALSE, nullptr);
g_Lock_JogAndTrigger = CreateMutex(nullptr, FALSE, nullptr);
// 初始化当前运动模式
m_CurrentMode = JogMode;
}
//===========================================================================
@@ -2023,6 +2028,8 @@ HSI_STATUS HSI_Motion::Jog(UINT AxisTypes, double Speed)
g_pLogger->SendAndFlushWithTime(L"[ACS Motion] ACS Motors Enable Success!\n");
}
SwitchMode(JogMode); //切换模式,设置当前模式为JOG模式
//开始JOG运动
if (!bJOGDir)
{
@@ -2430,6 +2437,8 @@ HSI_STATUS HSI_Motion::StopJog()
m_WriteByte = Send_Command(0, (const char*)m_SendJogData, m_SendDataLength);
}*/
SwitchMode(JogMode); //设置当前模式为JOG模式
//int Axes[] = { ACSC_AXIS_1, ACSC_AXIS_0, ACSC_AXIS_8, -1 };
if (handleACS != ACSC_INVALID)
{
@@ -3594,6 +3603,9 @@ HSI_STATUS HSI_Motion::SetPositionXyz(UINT AxisTypes, double PositionX, double P
g_pLogger->SendAndFlushWithTime(L"[ACS Motion] ACS Motors Enable Success!\n");
}
SwitchMode(PTPMode); //设置当前模式为JOG模式
//开始运动到指定位置，多轴运动
double Points[] = { PositionX, PositionY, PositionZ }; //目标位置点
if (!acsc_ToPointM(handleACS, 0, ACSAxisNumbers, Points, nullptr)) //移动到绝对位置
@@ -5364,6 +5376,11 @@ HSI_STATUS HSI_Motion::AbortMotion() //需要运动实现
ErrorsHandler();
}
}
// 增加一个阻塞延时 500ms
Sleep(500);
g_pLogger->SendAndFlushWithTime(L"[AbortMotion] Out\n");
}
return rStatus;
@@ -7919,6 +7936,39 @@ HSI_STATUS HSI_Motion::SetSpeedXyz(double Speed)
return rStatus;
}
//===========================================================================
// 切换运动模式函数
void HSI_Motion::SwitchMode(MotionMode newMode)
{
g_pLogger->SendAndFlushWithTime(L"[SwitchMode] In ================================ \n");
// 打印新模式
g_pLogger->SendAndFlushWithTime(L"[SwitchMode] New mode: %d\n", newMode);
// 打印当前模式
g_pLogger->SendAndFlushWithTime(L"[SwitchMode] Current mode: %d\n", m_CurrentMode);
if (m_CurrentMode != newMode)
{
// 根据新模式进行准备工作
//switch (newMode)
//{
//case MODE_A:
// PrepareForModeA();
// break;
//case MODE_B:
// PrepareForModeB();
// break;
//}
AbortMotion(); //模式切换前先停止运动
// 更新当前模式
m_CurrentMode = newMode;
g_pLogger->SendAndFlushWithTime(L"Switched to new mode: %d\n", newMode);
}
g_pLogger->SendAndFlushWithTime(L"[SwitchMode] Out ================================\n");
}
//===========================================================================
//HSI_STATUS HSI_Motion::GetFocusSpeedOld(double& Speed)
//{
@@ -7941,18 +7991,18 @@ HSI_STATUS HSI_Motion::GetFocusSpeed(double& Speed)
auto rStatus = HSI_STATUS_NORMAL;
// 方式1
//Speed = m_Jog_Auto_Focus;
Speed = m_Jog_Auto_Focus;
g_pLogger->SendAndFlushWithTime(L"[GetFocusSpeed] m_Jog_Auto_Focus = %f\n", m_Jog_Auto_Focus);
// 方式2，从底层查询
// 正常这里要读取z轴的速度，但是这里直接返回m_Jog_Auto_Focus
double getMotionParam[5] = { 0 };
GetSingleAxisParam(ACSAxisNumbers[2], getMotionParam); //获取单轴运动参数
g_pLogger->SendAndFlushWithTime(
L"[GetFocusSpeed] Axis= %d, Velocity = %.2f, Acceleration= %.2f, Deceleration= %.2f, KillDeceleration= %.2f, Jerk= %.2f\n",
ACSAxisNumbers[2], getMotionParam[0], getMotionParam[1], getMotionParam[2], getMotionParam[3], getMotionParam[4]);
//double getMotionParam[5] = { 0 };
//GetSingleAxisParam(ACSAxisNumbers[2], getMotionParam); //获取单轴运动参数
//g_pLogger->SendAndFlushWithTime(
// L"[GetFocusSpeed] Axis= %d, Velocity = %.2f, Acceleration= %.2f, Deceleration= %.2f, KillDeceleration= %.2f, Jerk= %.2f\n",
// ACSAxisNumbers[2], getMotionParam[0], getMotionParam[1], getMotionParam[2], getMotionParam[3], getMotionParam[4]);
Speed = getMotionParam[0]; //从getMotionParam[0]中获取速度
//Speed = getMotionParam[0]; //从getMotionParam[0]中获取速度
g_pLogger->SendAndFlushWithTime(L"[GetFocusSpeed] AxisNum = %d, Speed=%.2f\n", ACSAxisNumbers[2], Speed);
return rStatus;
}
@@ -7979,7 +8029,7 @@ HSI_STATUS HSI_Motion::GetFocusSpeed(double& Speed)
HSI_STATUS HSI_Motion::SetFocusSpeed(double Speed)
{
auto rStatus = HSI_STATUS_NORMAL;
g_pLogger->SendAndFlushWithTime(L"[SetFocusSpeed] In \n");
g_pLogger->SendAndFlushWithTime(L"[SetFocusSpeed] In ================================\n");
g_pLogger->SendAndFlushWithTime(L"[SetFocusSpeed] Speed = %f\n", Speed);
m_Jog_Auto_Focus = Speed;
g_pLogger->SendAndFlushWithTime(L"[SetFocusSpeed] m_Jog_Auto_Focus= %f\n", m_Jog_Auto_Focus);
@@ -7997,7 +8047,13 @@ HSI_STATUS HSI_Motion::SetFocusSpeed(double Speed)
SetSingleAxisMotionParams(ACSAxisNumbers[2], Z_SetmotionParam);//设置Z轴定位速度
m_SetPotion_DriveSpeed[ACSAxisNumbers[2]] = Z_SetmotionParam[0]; //记录Z轴速度
g_pLogger->SendAndFlushWithTime(L"[SetFocusSpeed] Out \n");
for (int j = 0;j < 9;j++)
{
//写日志
g_pLogger->SendAndFlushWithTime(L"m_SetPotion_DriveSpeed[%d] = %d\n", j, m_SetPotion_DriveSpeed[j]);
}
g_pLogger->SendAndFlushWithTime(L"[SetFocusSpeed] Out ================================\n");
return rStatus;
}
HSI_HexagonMI_EF3/HSI_Motion.h
+13 -1
View File
@@ -145,6 +145,15 @@ struct Point3D {
double z;
};
// 定义运动模式枚举类型
enum MotionMode
{
JogMode,
PTPMode,
};
class HSI_Motion : public HSI
{
public:
@@ -184,6 +193,8 @@ public:
*/
HSI_STATUS GetSpeedXyz(int AxisNum, double& Speed);
HSI_STATUS GetSpeedXyzOld(int AxisNum, double& Speed);
void SwitchMode(MotionMode newMode);
/**
* \brief 设置单轴运动速度
* \param AxisNum
@@ -582,7 +593,8 @@ public:
int IS_DEBUG; //是否启用调试模式
// 变量来存储当前的运动模式
MotionMode m_CurrentMode;
BOOL Send_Command(int com, const char* _SendData, DWORD _SendDataLength);
BOOL Send_Command(int com, const char* _SendData, DWORD SendDataLength, int expectType);// 带期望返回值的发送命令
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 "2025.03.03 / 15:05 "
#define HSI_FILE_CSDESCRIPTION _T("2025.03.03 / 15:05 ")
#define HSI_FILE_DESCRIPTION "2025.03.05 / 16:50 "
#define HSI_FILE_CSDESCRIPTION _T("2025.03.05 / 16:50 ")