diff --git a/HSI_HexagonMI_EF3/HSI.cpp b/HSI_HexagonMI_EF3/HSI.cpp
index 6fc61a9..c164a2e 100644
--- a/HSI_HexagonMI_EF3/HSI.cpp
+++ b/HSI_HexagonMI_EF3/HSI.cpp
@@ -145,9 +145,8 @@ HSI_API HSI_STATUS WINAPI HSI_SHUTDOWM()
////////////////////////////////////////////////////////////////////////////////
// MOTION API 运动相关API
///////////////////////////////////////////////////////////////////////////////
-#pragma region Motion API
+#pragma region Motion API Phase 1
-#pragma region 第一阶段实现的API功能
//===========================================================================
HSI_API HSI_STATUS WINAPI HSI_MOTION_STARTUP(bool bHome)
{
@@ -378,12 +377,6 @@ HSI_API HSI_STATUS WINAPI HSI_MOTION_IS_SUPPORTED(UINT& Types)
}
return rStatus;
}
-#pragma endregion
-
-#define USE_Motion_API
-#ifdef USE_Motion_API
-
-#pragma region 后续考虑
//===========================================================================
HSI_API HSI_STATUS WINAPI HSI_MOTION_GET_FOCUSSPEED_XYZ(double& Speed)
@@ -415,6 +408,15 @@ HSI_API HSI_STATUS WINAPI HSI_MOTION_SET_FOCUSSPEED_XYZ(double Speed)
return rStatus;
}
+
+#pragma endregion
+
+#define USE_Motion_API
+#ifdef USE_Motion_API
+
+#pragma region 后续考虑
+
+
//===========================================================================
HSI_API HSI_STATUS WINAPI HSI_MOTION_SET_SPEED_XYZ(double Speed)
{
diff --git a/HSI_HexagonMI_EF3/HSI_HexagonMI_EF3.vcxproj b/HSI_HexagonMI_EF3/HSI_HexagonMI_EF3.vcxproj
index e8d4191..1d4b2f5 100644
--- a/HSI_HexagonMI_EF3/HSI_HexagonMI_EF3.vcxproj
+++ b/HSI_HexagonMI_EF3/HSI_HexagonMI_EF3.vcxproj
@@ -122,6 +122,9 @@ copy "$(TargetDir)$(ProjectName).dll" "D:\HSI_Sevenocean_EF3.dll"
Create
+
+
+
diff --git a/HSI_HexagonMI_EF3/HSI_HexagonMI_EF3.vcxproj.filters b/HSI_HexagonMI_EF3/HSI_HexagonMI_EF3.vcxproj.filters
index 207a525..790b4b0 100644
--- a/HSI_HexagonMI_EF3/HSI_HexagonMI_EF3.vcxproj.filters
+++ b/HSI_HexagonMI_EF3/HSI_HexagonMI_EF3.vcxproj.filters
@@ -22,4 +22,7 @@
+
+
+
\ No newline at end of file
diff --git a/HSI_HexagonMI_EF3/HSI_Motion.cpp b/HSI_HexagonMI_EF3/HSI_Motion.cpp
index a04d278..6820100 100644
--- a/HSI_HexagonMI_EF3/HSI_Motion.cpp
+++ b/HSI_HexagonMI_EF3/HSI_Motion.cpp
@@ -64,10 +64,9 @@ int HSI_Motion::bRunGlueDispenser = THREAD_PAUSED;
//===========================================================================
HANDLE HSI_Motion::m_ThreadTCP_Id = nullptr;
int HSI_Motion::m_ThreadTCP_State = TCPIP_THREAD_PAUSED;
-SOCKET m_socket[4] = {0};
+SOCKET m_socket[4] = { 0 };
-
- void HSI_Motion::ErrorsHandler()
+void HSI_Motion::ErrorsHandler()
{
if (handleACS != ACSC_INVALID)
{
@@ -76,17 +75,16 @@ SOCKET m_socket[4] = {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
+ ErrorCode, // error code
+ ErrorStr, // buffer for the error explanation
+ 255, // available buffer length
+ &Received // number of actually received bytes
))
{
ErrorStr[Received] = '\0';
printf("Motion Error: %d [%s]\n", ErrorCode, ErrorStr);
g_pLogger->SendAndFlushWithTime(L"[ACS Motion] Motion Error,Code: %d, %S\n", ErrorCode, ErrorStr);
-
//向上层发送错误信息
sEvenProp.Init();
sEvenProp.EventType = HSI_EVENT_FUNCTION;
@@ -126,7 +124,7 @@ HSI_Motion::HSI_Motion()
iMotionDirection = 1;
m_InputStatus = 0;
m_ForStatus = 0;
- m_setPositionDelay = 0; //设置定位超时
+ m_setPositionDelay = 0; //设置定位超时
m_setPositionPrecision = 0; //设置定位精度
m_IsUse_HSICompensation = 0; //是否启用HSI进行定位补偿 0为不启用 1为启用 默认为0
m_Compensation_Pluse = 20; //补偿脉冲数
@@ -161,7 +159,7 @@ HSI_Motion::HSI_Motion()
m_IsUseJerk = 0; //是否启用急停 0为不启用 1为启用
t_start = 0; //获取jog运行的开始时间
t_end = 0; //获取jog运行的结束时间
- m_isOKGlint = 0; //是否开启ok/ng 闪烁
+ m_isOKGlint = 0; //是否开启ok/ng 闪烁
m_selectedIndex = 0;
m_setPositionNum = 5;
m_axisStatus = 0; //运动各轴的状态
@@ -189,7 +187,7 @@ HSI_Motion::HSI_Motion()
m_axisDirZ = 0; //探针运动时Z轴的运动方向
m_probeSeekSpeed = 0;
- bUseGlueDispenser = false; //是否开启点胶
+ bUseGlueDispenser = false; //是否开启点胶
m_iGlueStartSpeed = 1;
m_iGlueDriveSpeed = 1;
m_iGlueAccSpeed = 1;
@@ -215,7 +213,7 @@ HSI_Motion::HSI_Motion()
for (int i = 0; i < 4; i++)
{
- m_rockerHStartSpeed[i] = 5; //摇杆XYZ轴高初始速度
+ m_rockerHStartSpeed[i] = 5; //摇杆XYZ轴高初始速度
m_rockerHDriveSpeed[i] = 20; //摇杆XYZ轴高驱动速度
m_rockerLStartSpeed[i] = 2; //摇杆XYZ轴低初始速度
m_rockerLDriveSpeed[i] = 10; //摇杆XYZ轴低驱动速度
@@ -271,7 +269,7 @@ HSI_Motion::HSI_Motion()
m_Home_AddJogGears[i] = 4;
m_Home_DecJogGears[i] = 3;
m_SetPotion_StartSpeed[i] = 20;
-
+
m_SetPotion_Line[i] = 150;
m_SetPotion_Buffer[i] = 40;
m_SetPotion_AccCurve[i] = 0;
@@ -362,12 +360,12 @@ bool HSI_Motion::PortInit(int iSerialComPort, int iBuadRate)
sprintf_s(buf, "COM%d", iSerialComPort);
CString comName(buf);
hCom = CreateFile(comName,
- GENERIC_READ | GENERIC_WRITE, //允许读和写
- 0, //独占方式,串口必须为0
- nullptr,
- OPEN_EXISTING, //打开而不是创建，串口必须为打开
- 0, //同步方式,同步执行时，函数直到操作完成后才返回
- nullptr); //串口必须为NULL
+ GENERIC_READ | GENERIC_WRITE, //允许读和写
+ 0, //独占方式,串口必须为0
+ nullptr,
+ OPEN_EXISTING, //打开而不是创建，串口必须为打开
+ 0, //同步方式,同步执行时，函数直到操作完成后才返回
+ nullptr); //串口必须为NULL
if (hCom != (HANDLE)-1)
{
PurgeComm(hCom, PURGE_TXCLEAR | PURGE_RXCLEAR);
@@ -451,7 +449,6 @@ HSI_STATUS HSI_Motion::Startup(HWND _hWnd, bool _bOfflineOnly)
}
#endif // OFFLINE
-
m_SO7_Serial.SetTimeouts(1000, 1000);
m_bConnected = true;
g_pLogger->SendAndFlushWithTime(L"[Startup] Serial: [COM%d] is open success\n", m_EF3COMPort);
@@ -495,7 +492,7 @@ HSI_STATUS HSI_Motion::Startup(HWND _hWnd, bool _bOfflineOnly)
}
#endif
//使能电机
- int Axes[] = {ACSC_AXIS_1, ACSC_AXIS_0, ACSC_AXIS_8, -1}; //根据电气层面定义，这里的0对应X轴，1对应Y轴，8对应Z轴
+ int Axes[] = { ACSC_AXIS_1, ACSC_AXIS_0, ACSC_AXIS_8, -1 }; //根据电气层面定义，这里的0对应X轴，1对应Y轴，8对应Z轴
if (!acsc_EnableM(handleACS, Axes, nullptr))
{
g_pLogger->SendAndFlushWithTime(L"[ACS Motion] ACS Motors Enable Failed!\n");
@@ -509,7 +506,6 @@ HSI_STATUS HSI_Motion::Startup(HWND _hWnd, bool _bOfflineOnly)
m_bACSConnected = true;
g_pLogger->SendAndFlushWithTime(L"[ACS Motion] ACS Initialization Success!\n");
-
//获取ACS 控制器版本号, 待测试，2.70
char Firmware[256];
int Received;
@@ -528,11 +524,11 @@ HSI_STATUS HSI_Motion::Startup(HWND _hWnd, bool _bOfflineOnly)
//获取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)));*/
+ 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)));
+ HIBYTE(HIWORD(Ver)), LOBYTE(HIWORD(Ver)), HIBYTE(LOWORD(Ver)),
+ LOBYTE(LOWORD(Ver)));
}
else
{
@@ -545,13 +541,13 @@ HSI_STATUS HSI_Motion::Startup(HWND _hWnd, bool _bOfflineOnly)
}
// 04 05 0F 4A 04 00 04 00 00 00 00 00 00 00 00 00
- //
+ //
//01 01 0F 04 00 00 00 00 00 00 00 00 00 00 00 00
//01 01 0F 04 00 00 00 00 00 00 00 00 00 00 00 00
- //
+ //
//01 01 01 07 02 01 01 00 00 00 00 00 00 00 00 00
//01 09 01 01 01 07 07 00 00 00 07 04 00 00 00 00
- //
+ //
//01 01 01 05 01 07 07 00 00 00 07 04 00 00 00 00
//01 01 02 05 01 07 07 00 00 00 07 04 00 00 00 00
//01 01 04 05 01 07 07 00 00 00 07 04 00 00 00 00
@@ -649,11 +645,11 @@ 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
+ // m_cSendData[15] = m_StartInputPort & 0xff; //外部启动端口号 L
//}
//m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
//Sleep(10);
@@ -762,7 +758,7 @@ HSI_STATUS HSI_Motion::Startup(HWND _hWnd, bool _bOfflineOnly)
g_pLogger->SendAndFlushWithTime(L"[Startup] SetpositionXyz Enable\n");
//读取EF3数据状态线程
- //CreateThreadData();
+ //CreateThreadData();
//SetEvent(m_hTriggerEventData);
//m_Thread_StateData = HSI_THREAD_RUNNING;
//g_pLogger->SendAndFlushWithTime(L"[Startup] Read EF3 Status Run\n");
@@ -770,7 +766,7 @@ HSI_STATUS HSI_Motion::Startup(HWND _hWnd, bool _bOfflineOnly)
//JOG运行到软限位的运动调节线程
//if (m_DeviceType != 3)
//{
- // CreateThreadJOGStop();
+ // CreateThreadJOGStop();
// SetEvent(m_hTriggerEventJOGStop);
// m_Thread_StateJOGStop = HSI_THREAD_PAUSED;
//}
@@ -796,7 +792,7 @@ HSI_STATUS HSI_Motion::Startup(HWND _hWnd, bool _bOfflineOnly)
g_pLogger->SendAndFlushWithTime(L"[Startup] Out\n");
//启用EF3锁存模式设置，定时模式
- if (m_IsUseEF3 )
+ if (m_IsUseEF3)
{
//设置锁存频率 1秒钟（）
m_cSendData[0] = 0x01;
@@ -828,15 +824,15 @@ HSI_STATUS HSI_Motion::Startup(HWND _hWnd, bool _bOfflineOnly)
//===========================================================================
/**
* \brief 获取EF3固件版本
- * \param version
- * \return
+ * \param version
+ * \return
*/
HSI_STATUS HSI_Motion::GetFirmwareVersion(byte* version)
{
m_Thread_StateData = HSI_THREAD_PAUSED;
Sleep(3);
int waite_count = 0;
- unsigned char senddata[8] = {0};
+ unsigned char senddata[8] = { 0 };
//senddata[0] = 0x04;
//senddata[1] = 0x03;
@@ -898,8 +894,8 @@ HSI_STATUS HSI_Motion::GetFirmwareVersion(byte* version)
//===========================================================================
/**
* \brief 回家
- * \param bHomed
- * \return
+ * \param bHomed
+ * \return
*/
HSI_STATUS HSI_Motion::HomeMachineOld(bool bHomed)
{
@@ -943,7 +939,7 @@ HSI_STATUS HSI_Motion::HomeMachineOld(bool bHomed)
//根据当前位置，保存位置，求解相对运动
//回家后，设置正负限位
CurrentHomeMachineState = E_EF3_HOME_ING;
- int GetHomePos[5] = {0};
+ int GetHomePos[5] = { 0 };
if (m_SO7_Serial.m_RecvData[0] == 2)
{
if (m_IsHavePattern & 0x01 == 0x01)
@@ -982,7 +978,7 @@ HSI_STATUS HSI_Motion::HomeMachineOld(bool bHomed)
return HSI_STATUS_FAILED;
}
- int SavePos[5] = {0};
+ int SavePos[5] = { 0 };
if (m_SO7_Serial.m_RecvData[0] == 2)
{
SavePos[1] = (m_SO7_Serial.m_RecvData[41] << 24 | m_SO7_Serial.m_RecvData[42] << 16 | m_SO7_Serial.
@@ -994,7 +990,7 @@ HSI_STATUS HSI_Motion::HomeMachineOld(bool bHomed)
SavePos[4] = (m_SO7_Serial.m_RecvData[53] << 24 | m_SO7_Serial.m_RecvData[54] << 16 | m_SO7_Serial.
m_RecvData[55] << 8 | m_SO7_Serial.m_RecvData[56]);
}
- double PrinfPos[5] = {0};
+ double PrinfPos[5] = { 0 };
PrinfPos[1] = SavePos[1] * m_Resolution[1];
PrinfPos[2] = SavePos[2] * m_Resolution[2];
PrinfPos[3] = SavePos[3] * m_Resolution[3];
@@ -1007,7 +1003,7 @@ HSI_STATUS HSI_Motion::HomeMachineOld(bool bHomed)
L"[HomeMachine] PrinfPos[1] = %.4f,PrinfPos[2] = %.4f,PrinfPos[3] = %.4f,PrinfPos[4] = %.4f\n", PrinfPos[1],
PrinfPos[2], PrinfPos[3], PrinfPos[4]);
- int LastPos[5] = {0};
+ int LastPos[5] = { 0 };
if (m_SO7_Serial.m_RecvData[0] == 2)
{
LastPos[1] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial.m_RecvData[
@@ -1021,7 +1017,7 @@ HSI_STATUS HSI_Motion::HomeMachineOld(bool bHomed)
}
m_Thread_State = HSI_THREAD_RUNNING;
g_pLogger->SendAndFlushWithTime(L"[HomeMachine] SetPositionXyz\n");
- double PrinfMovePos[5] = {0};
+ double PrinfMovePos[5] = { 0 };
PrinfMovePos[1] = (GetHomePos[1] - SavePos[1]) * m_Resolution[1]; //求解相对运动位置
PrinfMovePos[2] = (GetHomePos[2] - SavePos[2]) * m_Resolution[2]; //相对运动，目标位置和现在位置求移动距离
PrinfMovePos[3] = (GetHomePos[3] - SavePos[3]) * m_Resolution[3];
@@ -1040,7 +1036,7 @@ HSI_STATUS HSI_Motion::HomeMachineOld(bool bHomed)
}
Sleep(20);
SetPositionXyza(0, PrinfMovePos[1], PrinfMovePos[2], PrinfMovePos[3], PrinfMovePos[4], HSI_MOTION_MOVE_WAIT,
- 0);
+ 0);
}
CurrentHomeMachineState = E_EF3_HOME_FINISHED;
for (int AxisTypes = 1; AxisTypes <= 8;)
@@ -1133,21 +1129,20 @@ HSI_STATUS HSI_Motion::HomeMachine(bool bHomed)
//等待运动结束 ，方式1
/* acsc_WaitMotionEnd(handleACS, ACSC_AXIS_1, INFINITE);
g_pLogger->SendAndFlushWithTime(L"[HomeMachine] X homed\n");
-
+
acsc_WaitMotionEnd(handleACS, ACSC_AXIS_0, INFINITE);
g_pLogger->SendAndFlushWithTime(L"[HomeMachine] Y homed\n");
-
+
acsc_WaitMotionEnd(handleACS, ACSC_AXIS_4, INFINITE);
g_pLogger->SendAndFlushWithTime(L"[HomeMachine] Z homed\n");*/
- //等待运动结束 ，方式2
+ //等待运动结束 ，方式2
do
{
//再次读取回家标志位，或者上个动作完成回调
IsHomed(home);
Sleep(200);
- }
- while (!home);
+ } while (!home);
if (m_IsUseEF3) //启用EF3锁存功能，需要再回家完成后，设置锁存板的位置
{
@@ -1163,7 +1158,6 @@ HSI_STATUS HSI_Motion::HomeMachine(bool bHomed)
m_cSendData[1] = 0x04;
m_WriteByte = Send_Command(0, (const char*)m_cSendData, 2);
Sleep(5);
-
}
//回家表示改为1
@@ -1196,7 +1190,7 @@ HSI_STATUS HSI_Motion::HomeJog(short AxisNumber, short Dir, bool Wait)
//===========================================================================
/**
* \brief 取消限位，设置初始速度，加减速等参数
- * \return
+ * \return
*/
HSI_STATUS HSI_Motion::HomeFindIndex()
{
@@ -1241,7 +1235,7 @@ HSI_STATUS HSI_Motion::HomeFindIndex()
m_cSendData[2] = AxisTypes;
m_cSendData[3] = HOME_LIMIT_SPEED_ACC_DEC;
HomeJogGearsChoice(i, m_Home_AddJogGears[i], DriveSpeed, StartSpeed, AccLine, DecLine, AccCurve,
- DecCurve);
+ DecCurve);
m_cSendData[4] = (StartSpeed & 0xff);
m_cSendData[5] = ((StartSpeed >> 8) & 0xff);
@@ -1280,7 +1274,7 @@ HSI_STATUS HSI_Motion::HomeFindIndex()
m_cSendData[2] = AxisTypes;
m_cSendData[3] = HOME_ORG_SPEED_ACC_DEC;
HomeJogGearsChoice(i, m_Home_DecJogGears[i], DriveSpeed, StartSpeed, AccLine, DecLine, AccCurve,
- DecCurve);
+ DecCurve);
m_cSendData[4] = (StartSpeed & 0xff);
m_cSendData[5] = ((StartSpeed >> 8) & 0xff);
m_cSendData[6] = ((StartSpeed >> 16) & 0xff);
@@ -1366,30 +1360,29 @@ HSI_STATUS HSI_Motion::HomeFindIndex()
}
Count++;
g_pLogger->SendAndFlushWithTime(L"[HomeFindIndex] Going Home Count=%d\n", Count);
- }
- while (true);
+ } while (true);
char MessageHome[100] = "";
bool bHomed = true;
if ((m_SO7_Serial.m_RecvData[38] & 0x01) == 0 && m_Home_Machine_Axis[1] == 1)
{
/* strcat_s 是系统的安全函数，微软在 2005 后建议用一系统所谓安全的函数，这中间就有 strcat_s 取代了 strcat ，原来 strcat 函数，没有方法来保证有效的缓冲区尺寸，所以它只能假定缓冲足够大来容纳要拷贝的字符串, 容易产生程序崩溃。而strcat_s函数能很好的规避这个问题*/
- strcat_s(MessageHome, 30, "1、");
+ strcat_s(MessageHome, 30, "1");
bHomed = false;
}
if ((m_SO7_Serial.m_RecvData[38] & 0x02) == 0 && m_Home_Machine_Axis[2] == 1)
{
- strcat_s(MessageHome, 30, "2、");
+ strcat_s(MessageHome, 30, "2");
bHomed = false;
}
if ((m_SO7_Serial.m_RecvData[38] & 0x04) == 0 && m_Home_Machine_Axis[3] == 1)
{
- strcat_s(MessageHome, 30, "3、");
+ strcat_s(MessageHome, 30, "3");
bHomed = false;
}
if ((m_SO7_Serial.m_RecvData[38] & 0x08) == 0 && m_Home_Machine_Axis[4] == 1)
{
- strcat_s(MessageHome, 30, "4、");
+ strcat_s(MessageHome, 30, "4");
bHomed = false;
}
if (!bHomed)
@@ -1412,7 +1405,7 @@ HSI_STATUS HSI_Motion::HomeFindIndex()
//===========================================================================
void HSI_Motion::HomeJogGearsChoice(int AxisTypes, int JogGears, int& DriveSpeed, int& StartSpeed, int& AccLine,
- int& DecLine, int& AccCurve, int& DecCurve)
+ int& DecLine, int& AccCurve, int& DecCurve)
{
DriveSpeed = m_JogDriveSpeed[AxisTypes][4 - JogGears];
StartSpeed = m_JogStartSpeed[AxisTypes][4 - JogGears];
@@ -1450,7 +1443,7 @@ HSI_STATUS HSI_Motion::IsHomedOld(bool& bHomed)
if (g_pHSI_Motion)
{
g_pLogger->SendAndFlushWithTime(L"[IsHomed] In\n");
- short isHomed[5] = {1, 1, 1, 1, 1};
+ short isHomed[5] = { 1, 1, 1, 1, 1 };
//int Count = 0;
//if (bHomed == true)//定位是增大判断精度
//{
@@ -1521,8 +1514,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] = {0, 1, 1, 1, 1}; //暂定只有一个回家标志位，即全部回家完成，没有按单个轴回家来看
-
+ int isHomed[5] = { 0, 1, 1, 1, 1 }; //暂定只有一个回家标志位，即全部回家完成，没有按单个轴回家来看
//所有轴都不需要回家
if (m_Home_Machine_Axis[1] == 0 && m_Home_Machine_Axis[2] == 0 && m_Home_Machine_Axis[3] == 0 &&
@@ -1536,8 +1528,8 @@ HSI_STATUS HSI_Motion::IsHomed(bool& bHomed)
// 判断是否需要回家，读取ACS控制器回家标志位，来判断本次上电是否已经回过家 ， 1：已经回过家，0：未回过家
if (!acsc_ReadInteger(handleACS, ACSC_NONE, "YAW_HOME_DONE", ACSC_NONE, ACSC_NONE, ACSC_NONE, ACSC_NONE,
- isHomed,
- nullptr))
+ isHomed,
+ nullptr))
{
g_pLogger->SendAndFlushWithTime(L"[IsHomed] ACS Read ISHOMED Flag Error\n");
ErrorsHandler();
@@ -1548,7 +1540,7 @@ HSI_STATUS HSI_Motion::IsHomed(bool& bHomed)
}
g_pLogger->SendAndFlushWithTime(L"[IsHomed] ACS Read YAW_HOME_DONE X:[%d] Y:[%d] Z:[%d]\n", isHomed[0],
- isHomed[1], isHomed[2]);
+ isHomed[1], isHomed[2]);
//如果各个轴标志位 已经回过家
if (isHomed[0] == 1 && isHomed[1] == 1 && isHomed[2] == 1 && isHomed[3] == 1)
@@ -1562,7 +1554,6 @@ HSI_STATUS HSI_Motion::IsHomed(bool& bHomed)
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");
}
@@ -1627,7 +1618,7 @@ HSI_STATUS HSI_Motion::ZeroPos(bool bZeroPos)
* \brief JOG模式
* \param AxisTypes 单轴
* \param Speed 速度，Speed > 0 正移动
- * \return
+ * \return
*/
HSI_STATUS HSI_Motion::JogOld(UINT AxisTypes, double Speed)
{
@@ -1694,9 +1685,9 @@ HSI_STATUS HSI_Motion::JogOld(UINT AxisTypes, double Speed)
{
return rStatus;
}
- double now_pos[5] = {0};
- double Prf_pos[5] = {0};
- double limitpos[4] = {0};
+ double now_pos[5] = { 0 };
+ double Prf_pos[5] = { 0 };
+ double limitpos[4] = { 0 };
int RemainPul = 0;
int limitSDPul = 0;
double time;
@@ -1936,7 +1927,6 @@ HSI_STATUS HSI_Motion::Jog(UINT AxisTypes, double Speed)
int DecCurve(1);
int JogSpeed(1);
bool bJOGDir = Speed > 0 ? true : false; //运动方向
-
jogDirFlag = bJOGDir;
m_Thread_State = HSI_THREAD_PAUSED;
@@ -1946,7 +1936,7 @@ HSI_STATUS HSI_Motion::Jog(UINT AxisTypes, double Speed)
//软限位
g_pLogger->SendAndFlushWithTime(
L"[Jog] Limit Enable, Axis = %d, m_P_Work_Limit = %f,m_N_Work_Limit = %f\n",
- AxisTypes,m_P_Work_Limit[AxisTypes], m_N_Work_Limit[AxisTypes]);
+ AxisTypes, m_P_Work_Limit[AxisTypes], m_N_Work_Limit[AxisTypes]);
}
else
{
@@ -1962,9 +1952,9 @@ HSI_STATUS HSI_Motion::Jog(UINT AxisTypes, double Speed)
}
//设置 JOG运动参数 加减速 JOG_SPEED_ACC_DEC
- double now_pos[5] = {0};
- double Prf_pos[5] = {0};
- double limitpos[4] = {0};
+ double now_pos[5] = { 0 };
+ double Prf_pos[5] = { 0 };
+ double limitpos[4] = { 0 };
int RemainPul = 0;
int limitSDPul = 0;
double time;
@@ -1974,13 +1964,13 @@ HSI_STATUS HSI_Motion::Jog(UINT AxisTypes, double Speed)
g_pLogger->SendAndFlushWithTime(
L"[Jog] Speed: [%d], DriveSpeed: [%d],AccLine: [%d], DecLine: [%d] AccCurve: [%d], DecCurve: [%d],\n",
- Speed, DriveSpeed, AccLine, DecLine,AccCurve, DecCurve );
+ Speed, DriveSpeed, AccLine, DecLine, AccCurve, DecCurve);
//转到真实ACS平台轴号，并开始执行
byte AxisNumber = static_cast(AxisConvertIndex(AxisTypes)); //Jog
- double motionParam[5] = { DriveSpeed,AccLine , DecLine, AccCurve,DecCurve }; //速度，加速度，减速度，Kill, jerk
+ double motionParam[5] = { DriveSpeed,AccLine , DecLine, AccCurve,DecCurve }; //速度，加速度，减速度，Kill, jerk
SetSingleAxisMotionParams(AxisNumber, motionParam);
-
+
// 急停判断
if ((StartSpeed < 250) && (DriveSpeed < 6))
{
@@ -2011,19 +2001,15 @@ HSI_STATUS HSI_Motion::Jog(UINT AxisTypes, double Speed)
DriveSpeed = DriveSpeed * (-1); // Negative direction : Using - (minus) velocity //正方向，或 负方向
}
-
if (!acsc_Jog(handleACS, 0, AxisNumber, DriveSpeed, nullptr))
{
- printf("[Jog] 轴[%d] [%s] 方向移动失败", AxisTypes, bJOGDir ? "正" : "负");
- g_pLogger->SendAndFlushWithTime(L"[Jog] failed, Aixs:[%d] JOGDir:[%S]\n", AxisTypes,
- bJOGDir ? "Positive" : "Negative");
+ g_pLogger->SendAndFlushWithTime(L"[Jog] 方向移动失败, 轴:[%d] JOGDir:[%S]\n", AxisTypes, bJOGDir ? "Positive" : "Negative");
ErrorsHandler();
}
-
jogMoving = true;
g_pLogger->SendAndFlushWithTime(L"[Jog] Out, AxisNumber = %d, DriveSpeed = %d AccCurve:[%d] DecCurve:[%d]\n", AxisNumber, DriveSpeed,
- AccLine, AccLine);
+ AccLine, AccLine);
}
return rStatus;
}
@@ -2095,9 +2081,9 @@ HSI_STATUS HSI_Motion::JoyStick(UINT AxisTypes, long Speed)
{
return rStatus;
}
- double now_pos[5] = {0};
- double Prf_pos[5] = {0};
- double limitpos[4] = {0};
+ double now_pos[5] = { 0 };
+ double Prf_pos[5] = { 0 };
+ double limitpos[4] = { 0 };
int RemainPul = 0;
int limitSDPul = 0;
double time;
@@ -2105,7 +2091,7 @@ HSI_STATUS HSI_Motion::JoyStick(UINT AxisTypes, long Speed)
GetPositionEncPrfMulti(1, now_pos, Prf_pos, 1);
long lSpeed = abs(Speed);
if (!abs(SpeedPercentJoyStick(AxisNumber, lSpeed, DriveSpeed, StartSpeed, AccLine, DecLine, AccCurve,
- DecCurve)))
+ DecCurve)))
{
return rStatus;
}
@@ -2326,7 +2312,7 @@ HSI_STATUS HSI_Motion::JoyStick(UINT AxisTypes, long Speed)
//===========================================================================
/**
* \brief 停止Jog运动
- * \return
+ * \return
*/
HSI_STATUS HSI_Motion::StopJogOld()
{
@@ -2344,7 +2330,7 @@ HSI_STATUS HSI_Motion::StopJogOld()
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;
@@ -2390,7 +2376,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)
@@ -2416,7 +2402,7 @@ HSI_STATUS HSI_Motion::StopJog()
m_WriteByte = Send_Command(0, (const char*)m_SendJogData, m_SendDataLength);
}*/
- int Axes[] = {ACSC_AXIS_1, ACSC_AXIS_0, ACSC_AXIS_8, -1};
+ int Axes[] = { ACSC_AXIS_1, ACSC_AXIS_0, ACSC_AXIS_8, -1 };
if (handleACS != ACSC_INVALID)
{
if (!acsc_HaltM(handleACS, Axes, nullptr)) //停止JOG运动
@@ -2435,7 +2421,6 @@ HSI_STATUS HSI_Motion::StopJog()
return rStatus;
}
-
//===========================================================================
HSI_STATUS HSI_Motion::StopJogEx(UINT AxisTypes)
{
@@ -2454,7 +2439,7 @@ HSI_STATUS HSI_Motion::StopJogEx(UINT AxisTypes)
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;
@@ -2506,11 +2491,11 @@ int HSI_Motion::P2P(short AxisNumber, long Pos, double Speed, double Acc)
//===========================================================================
/**
* \brief 对比现在编码器位置和期望位置, position给上层用的, HSI内部用的是EncPrf
- * \param AxisTypes
- * \param EncPos
- * \param PrfPos
- * \param Count
- * \return
+ * \param AxisTypes
+ * \param EncPos
+ * \param PrfPos
+ * \param Count
+ * \return
*/
HSI_STATUS HSI_Motion::GetPositionEncPrfMultiOld(UINT AxisTypes, double* EncPos, double* PrfPos, int Count)
{
@@ -2524,48 +2509,48 @@ HSI_STATUS HSI_Motion::GetPositionEncPrfMultiOld(UINT AxisTypes, double* EncPos,
{
EncPos[1] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial.m_RecvData[
3] << 8 | m_SO7_Serial.m_RecvData[4]) * m_Resolution[1];
- EncPos[2] = (m_SO7_Serial.m_RecvData[5] << 24 | m_SO7_Serial.m_RecvData[6] << 16 | m_SO7_Serial.m_RecvData[
- 7] << 8 | m_SO7_Serial.m_RecvData[8]) * m_Resolution[2];
- EncPos[3] = (m_SO7_Serial.m_RecvData[9] << 24 | m_SO7_Serial.m_RecvData[10] << 16 | m_SO7_Serial.m_RecvData[
- 11] << 8 | m_SO7_Serial.m_RecvData[12]) * m_Resolution[3];
- EncPos[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | m_SO7_Serial.m_RecvData
- [15] << 8 | m_SO7_Serial.m_RecvData[16]) * m_Resolution[4];
+ EncPos[2] = (m_SO7_Serial.m_RecvData[5] << 24 | m_SO7_Serial.m_RecvData[6] << 16 | m_SO7_Serial.m_RecvData[
+ 7] << 8 | m_SO7_Serial.m_RecvData[8]) * m_Resolution[2];
+ EncPos[3] = (m_SO7_Serial.m_RecvData[9] << 24 | m_SO7_Serial.m_RecvData[10] << 16 | m_SO7_Serial.m_RecvData[
+ 11] << 8 | m_SO7_Serial.m_RecvData[12]) * m_Resolution[3];
+ EncPos[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | m_SO7_Serial.m_RecvData
+ [15] << 8 | m_SO7_Serial.m_RecvData[16]) * m_Resolution[4];
- PrfPos[1] = (m_SO7_Serial.m_RecvData[17] << 24 | m_SO7_Serial.m_RecvData[18] << 16 | m_SO7_Serial.m_RecvData
- [19] << 8 | m_SO7_Serial.m_RecvData[20]) * m_Resolution[1];
- PrfPos[2] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial.m_RecvData
- [23] << 8 | m_SO7_Serial.m_RecvData[24]) * m_Resolution[2];
- PrfPos[3] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial.m_RecvData
- [27] << 8 | m_SO7_Serial.m_RecvData[28]) * m_Resolution[3];
- PrfPos[4] = (m_SO7_Serial.m_RecvData[29] << 24 | m_SO7_Serial.m_RecvData[30] << 16 | m_SO7_Serial.m_RecvData
- [31] << 8 | m_SO7_Serial.m_RecvData[32]) * m_Resolution[4];
+ PrfPos[1] = (m_SO7_Serial.m_RecvData[17] << 24 | m_SO7_Serial.m_RecvData[18] << 16 | m_SO7_Serial.m_RecvData
+ [19] << 8 | m_SO7_Serial.m_RecvData[20]) * m_Resolution[1];
+ PrfPos[2] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial.m_RecvData
+ [23] << 8 | m_SO7_Serial.m_RecvData[24]) * m_Resolution[2];
+ PrfPos[3] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial.m_RecvData
+ [27] << 8 | m_SO7_Serial.m_RecvData[28]) * m_Resolution[3];
+ PrfPos[4] = (m_SO7_Serial.m_RecvData[29] << 24 | m_SO7_Serial.m_RecvData[30] << 16 | m_SO7_Serial.m_RecvData
+ [31] << 8 | m_SO7_Serial.m_RecvData[32]) * m_Resolution[4];
- if (m_IsHavePattern & 0x01)
- m_EncPos[1] = EncPos[1];
- else
- m_EncPos[1] = PrfPos[1];
- if (m_IsHavePattern & 0x02)
- m_EncPos[2] = EncPos[2];
- else
- m_EncPos[2] = PrfPos[2];
- if (m_IsHavePattern & 0x04)
- m_EncPos[3] = EncPos[3];
- else
- m_EncPos[3] = PrfPos[3];
- if (m_IsHavePattern & 0x08)
- m_EncPos[4] = EncPos[4];
- else
- m_EncPos[4] = PrfPos[4];
+ if (m_IsHavePattern & 0x01)
+ m_EncPos[1] = EncPos[1];
+ else
+ m_EncPos[1] = PrfPos[1];
+ if (m_IsHavePattern & 0x02)
+ m_EncPos[2] = EncPos[2];
+ else
+ m_EncPos[2] = PrfPos[2];
+ if (m_IsHavePattern & 0x04)
+ m_EncPos[3] = EncPos[3];
+ else
+ m_EncPos[3] = PrfPos[3];
+ if (m_IsHavePattern & 0x08)
+ m_EncPos[4] = EncPos[4];
+ else
+ m_EncPos[4] = PrfPos[4];
- m_PrfPos[1] = PrfPos[1];
- m_PrfPos[2] = PrfPos[2];
- m_PrfPos[3] = PrfPos[3];
- m_PrfPos[4] = PrfPos[4];
+ m_PrfPos[1] = PrfPos[1];
+ m_PrfPos[2] = PrfPos[2];
+ m_PrfPos[3] = PrfPos[3];
+ m_PrfPos[4] = PrfPos[4];
- //begin_position[1] = EncPos[1] / m_Resolution[1];
- //begin_position[2] = EncPos[2] / m_Resolution[1];
- //begin_position[3] = EncPos[3] / m_Resolution[1];
- //begin_position[4] = EncPos[4] / m_Resolution[1];
+ //begin_position[1] = EncPos[1] / m_Resolution[1];
+ //begin_position[2] = EncPos[2] / m_Resolution[1];
+ //begin_position[3] = EncPos[3] / m_Resolution[1];
+ //begin_position[4] = EncPos[4] / m_Resolution[1];
}
else
{
@@ -2596,48 +2581,48 @@ HSI_STATUS HSI_Motion::GetPositionEncPrfMulti(UINT AxisTypes, double* EncPos, do
{
EncPos[1] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial.m_RecvData[
3] << 8 | m_SO7_Serial.m_RecvData[4]) * m_Resolution[1];
- EncPos[2] = (m_SO7_Serial.m_RecvData[5] << 24 | m_SO7_Serial.m_RecvData[6] << 16 | m_SO7_Serial.m_RecvData[
- 7] << 8 | m_SO7_Serial.m_RecvData[8]) * m_Resolution[2];
- EncPos[3] = (m_SO7_Serial.m_RecvData[9] << 24 | m_SO7_Serial.m_RecvData[10] << 16 | m_SO7_Serial.m_RecvData[
- 11] << 8 | m_SO7_Serial.m_RecvData[12]) * m_Resolution[3];
- EncPos[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | m_SO7_Serial.m_RecvData
- [15] << 8 | m_SO7_Serial.m_RecvData[16]) * m_Resolution[4];
+ EncPos[2] = (m_SO7_Serial.m_RecvData[5] << 24 | m_SO7_Serial.m_RecvData[6] << 16 | m_SO7_Serial.m_RecvData[
+ 7] << 8 | m_SO7_Serial.m_RecvData[8]) * m_Resolution[2];
+ EncPos[3] = (m_SO7_Serial.m_RecvData[9] << 24 | m_SO7_Serial.m_RecvData[10] << 16 | m_SO7_Serial.m_RecvData[
+ 11] << 8 | m_SO7_Serial.m_RecvData[12]) * m_Resolution[3];
+ EncPos[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | m_SO7_Serial.m_RecvData
+ [15] << 8 | m_SO7_Serial.m_RecvData[16]) * m_Resolution[4];
- PrfPos[1] = (m_SO7_Serial.m_RecvData[17] << 24 | m_SO7_Serial.m_RecvData[18] << 16 | m_SO7_Serial.m_RecvData
- [19] << 8 | m_SO7_Serial.m_RecvData[20]) * m_Resolution[1];
- PrfPos[2] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial.m_RecvData
- [23] << 8 | m_SO7_Serial.m_RecvData[24]) * m_Resolution[2];
- PrfPos[3] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial.m_RecvData
- [27] << 8 | m_SO7_Serial.m_RecvData[28]) * m_Resolution[3];
- PrfPos[4] = (m_SO7_Serial.m_RecvData[29] << 24 | m_SO7_Serial.m_RecvData[30] << 16 | m_SO7_Serial.m_RecvData
- [31] << 8 | m_SO7_Serial.m_RecvData[32]) * m_Resolution[4];
+ PrfPos[1] = (m_SO7_Serial.m_RecvData[17] << 24 | m_SO7_Serial.m_RecvData[18] << 16 | m_SO7_Serial.m_RecvData
+ [19] << 8 | m_SO7_Serial.m_RecvData[20]) * m_Resolution[1];
+ PrfPos[2] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial.m_RecvData
+ [23] << 8 | m_SO7_Serial.m_RecvData[24]) * m_Resolution[2];
+ PrfPos[3] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial.m_RecvData
+ [27] << 8 | m_SO7_Serial.m_RecvData[28]) * m_Resolution[3];
+ PrfPos[4] = (m_SO7_Serial.m_RecvData[29] << 24 | m_SO7_Serial.m_RecvData[30] << 16 | m_SO7_Serial.m_RecvData
+ [31] << 8 | m_SO7_Serial.m_RecvData[32]) * m_Resolution[4];
- if (m_IsHavePattern & 0x01)
- m_EncPos[1] = EncPos[1];
- else
- m_EncPos[1] = PrfPos[1];
- if (m_IsHavePattern & 0x02)
- m_EncPos[2] = EncPos[2];
- else
- m_EncPos[2] = PrfPos[2];
- if (m_IsHavePattern & 0x04)
- m_EncPos[3] = EncPos[3];
- else
- m_EncPos[3] = PrfPos[3];
- if (m_IsHavePattern & 0x08)
- m_EncPos[4] = EncPos[4];
- else
- m_EncPos[4] = PrfPos[4];
+ if (m_IsHavePattern & 0x01)
+ m_EncPos[1] = EncPos[1];
+ else
+ m_EncPos[1] = PrfPos[1];
+ if (m_IsHavePattern & 0x02)
+ m_EncPos[2] = EncPos[2];
+ else
+ m_EncPos[2] = PrfPos[2];
+ if (m_IsHavePattern & 0x04)
+ m_EncPos[3] = EncPos[3];
+ else
+ m_EncPos[3] = PrfPos[3];
+ if (m_IsHavePattern & 0x08)
+ m_EncPos[4] = EncPos[4];
+ else
+ m_EncPos[4] = PrfPos[4];
- m_PrfPos[1] = PrfPos[1];
- m_PrfPos[2] = PrfPos[2];
- m_PrfPos[3] = PrfPos[3];
- m_PrfPos[4] = PrfPos[4];
+ m_PrfPos[1] = PrfPos[1];
+ m_PrfPos[2] = PrfPos[2];
+ m_PrfPos[3] = PrfPos[3];
+ m_PrfPos[4] = PrfPos[4];
- //begin_position[1] = EncPos[1] / m_Resolution[1];
- //begin_position[2] = EncPos[2] / m_Resolution[1];
- //begin_position[3] = EncPos[3] / m_Resolution[1];
- //begin_position[4] = EncPos[4] / m_Resolution[1];
+ //begin_position[1] = EncPos[1] / m_Resolution[1];
+ //begin_position[2] = EncPos[2] / m_Resolution[1];
+ //begin_position[3] = EncPos[3] / m_Resolution[1];
+ //begin_position[4] = EncPos[4] / m_Resolution[1];
}
else
{
@@ -2659,15 +2644,15 @@ HSI_STATUS HSI_Motion::GetPositionEncPrfMulti(UINT AxisTypes, double* EncPos, do
//===========================================================================
/**
* \brief 获取轴当前运动位置
- * \param AxisTypes
- * \param PositionX
- * \param PositionY
- * \param PositionZ
+ * \param AxisTypes
+ * \param PositionX
+ * \param PositionY
+ * \param PositionZ
* \param Time 耗时
- * \return
+ * \return
*/
HSI_STATUS HSI_Motion::GetPositionXyzOld(UINT AxisTypes, double& PositionX, double& PositionY, double& PositionZ,
- double& Time)
+ double& Time)
{
auto rStatus = HSI_STATUS_NORMAL;
UNREFERENCED_PARAMETER(AxisTypes);
@@ -2741,7 +2726,7 @@ HSI_STATUS HSI_Motion::GetPositionXyzOld(UINT AxisTypes, double& PositionX, doub
}
HSI_STATUS HSI_Motion::GetPositionXyz(UINT AxisTypes, double& PositionX, double& PositionY, double& PositionZ,
- double& Time)
+ double& Time)
{
auto rStatus = HSI_STATUS_NORMAL;
//UNREFERENCED_PARAMETER(AxisTypes)的意思就是告诉编译器，
@@ -2822,7 +2807,7 @@ HSI_STATUS HSI_Motion::GetEncoderXyzOld(long* lEncoderVal)//原读取编码器
if (m_DeviceType != 1)
{
if (m_IsHavePattern & 0x01)
- {
+ {
lEncoderVal[0] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial
.m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]);
g_pLogger->SendAndFlushWithTime(L"[GetEncoderXyz] lEncoderVal[0] %ld \n", lEncoderVal[0]);
@@ -2840,7 +2825,7 @@ HSI_STATUS HSI_Motion::GetEncoderXyzOld(long* lEncoderVal)//原读取编码器
g_pLogger->SendAndFlushWithTime(L"[GetEncoderXyz] lEncoderVal[1] %ld \n", lEncoderVal[1]);
}
else
- {
+ {
lEncoderVal[1] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 |
m_SO7_Serial.m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]);
g_pLogger->SendAndFlushWithTime(L"[GetEncoderXyz] lEncoderVal[01] %ld \n", lEncoderVal[1]);
@@ -2852,7 +2837,7 @@ HSI_STATUS HSI_Motion::GetEncoderXyzOld(long* lEncoderVal)//原读取编码器
g_pLogger->SendAndFlushWithTime(L"[GetEncoderXyz] lEncoderVal[2] %ld \n", lEncoderVal[2]);
}
else
- {
+ {
lEncoderVal[2] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 |
m_SO7_Serial.m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]);
g_pLogger->SendAndFlushWithTime(L"[GetEncoderXyz] lEncoderVal[02] %ld \n", lEncoderVal[2]);
@@ -2890,8 +2875,8 @@ HSI_STATUS HSI_Motion::GetEncoderXyz(long* lEncoderVal)//原读取编码器值
{
lEncoderVal[0] = m_EncPos[1] / m_Resolution[1];//PositionX
lEncoderVal[1] = m_EncPos[2] / m_Resolution[1];// PositionY;
- lEncoderVal[2] = m_EncPos[3]/ m_Resolution[1]; // PositionZ;
-
+ lEncoderVal[2] = m_EncPos[3] / m_Resolution[1]; // PositionZ;
+
g_pLogger->SendAndFlushWithTime(L"[GetEncoderXyz] lEncoderVal[0]= %ld,EncoderVal[1]= %ld,,EncoderVal[2]= %ld, \n", lEncoderVal[0], lEncoderVal[1], lEncoderVal[2]);
}
//else
@@ -2906,7 +2891,7 @@ HSI_STATUS HSI_Motion::GetEncoderXyz(long* lEncoderVal)//原读取编码器值
//===========================================================================
HSI_STATUS HSI_Motion::GetPositionXyzaProbe(UINT AxisTypes, double& PositionX, double& PositionY, double& PositionZ,
- double& PositionA)
+ double& PositionA)
{
auto rStatus = HSI_STATUS_NORMAL;
UNREFERENCED_PARAMETER(AxisTypes);
@@ -2923,590 +2908,586 @@ HSI_STATUS HSI_Motion::GetPositionXyzaProbe(UINT AxisTypes, double& PositionX, d
//===========================================================================
/**
* \brief 设置多轴运动到指定位置
- * \param AxisTypes
- * \param PositionX
- * \param PositionY
- * \param PositionZ
- * \param eType
- * \param dFlyRadius
- * \return
+ * \param AxisTypes
+ * \param PositionX
+ * \param PositionY
+ * \param PositionZ
+ * \param eType
+ * \param dFlyRadius
+ * \return
*/
-HSI_STATUS HSI_Motion::SetPositionXyzOld(UINT AxisTypes, double PositionX, double PositionY, double PositionZ,
- HSI_MOTION_MOVE_TYPE eType, double dFlyRadius)
-{
- WaitForSingleObject(g_WR_ToMove_Mutex, INFINITE);
- auto rStatus = HSI_STATUS_NORMAL;
- if (g_pHSI_Motion)
- {
- g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] In\n");
- g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Pos[1] = %.4f,Pos[2] = %.4f,Pos[3] = %.4f\n", PositionX,
- PositionY, PositionZ);
- unsigned char send_pos_data[64] = {0};
- axis_start = 0;
- unsigned char direct_pos = 0;
- unsigned char xyzAxis = 0;
- //如果状态非 运动中，设置为运动中
- if (CurrentMotionState != E_SO7_MOTION_MOVETO)
- {
- CurrentMotionState = E_SO7_MOTION_MOVETO;
- LimitOver(HSI_MOTION_AXIS_X, PositionX);
- LimitOver(HSI_MOTION_AXIS_Y, PositionY);
- LimitOver(HSI_MOTION_AXIS_Z, PositionZ);
- LimitOver(HSI_MOTION_AXIS_R, m_PositionA);
-
- m_PosThread[1] = PositionX; //SetpositionXyz的目标位置
- m_PosThread[2] = PositionY;
- m_PosThread[3] = PositionZ;
- m_PosThread[4] = m_PositionA;
-
- targetpos_n[1] = PositionX;
- targetpos_n[2] = PositionY;
- targetpos_n[3] = PositionZ;
- targetpos_n[4] = m_PositionA;
-
- int Pos_t[5] = {0};
- int Pos_n[5] = {0};
- int Pos[5] = {0};
- int NowPos[5] = {0};
- int target_pos[5] = {0};
-
- if (m_SO7_Serial.m_RecvData[0] == 2)
- {
- //根据设备类型获取现在位置
- if (m_DeviceType != 1) //设备类型非三激光
- {
- if (m_IsHavePattern & 0x01 == 0x01)
- NowPos[1] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial.
- m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]);
- else
- NowPos[1] = (m_SO7_Serial.m_RecvData[17] << 24 | m_SO7_Serial.m_RecvData[18] << 16 |
- m_SO7_Serial.m_RecvData[19] << 8 | m_SO7_Serial.m_RecvData[20]);
- if (m_IsHavePattern & 0x02)
- NowPos[2] = (m_SO7_Serial.m_RecvData[5] << 24 | m_SO7_Serial.m_RecvData[6] << 16 | m_SO7_Serial.
- m_RecvData[7] << 8 | m_SO7_Serial.m_RecvData[8]);
- else
- NowPos[2] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 |
- m_SO7_Serial.m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]);
- if (m_IsHavePattern & 0x04)
- NowPos[3] = (m_SO7_Serial.m_RecvData[9] << 24 | m_SO7_Serial.m_RecvData[10] << 16 | m_SO7_Serial
- .m_RecvData[11] << 8 | m_SO7_Serial.m_RecvData[12]);
- else
- NowPos[3] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 |
- m_SO7_Serial.m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]);
- if (m_IsHavePattern & 0x08)
- NowPos[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 |
- m_SO7_Serial.m_RecvData[15] << 8 | m_SO7_Serial.m_RecvData[16]);
- else
- NowPos[4] = (m_SO7_Serial.m_RecvData[29] << 24 | m_SO7_Serial.m_RecvData[30] << 16 |
- m_SO7_Serial.m_RecvData[31] << 8 | m_SO7_Serial.m_RecvData[32]);
- }
- else
- {
- NowPos[1] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial.
- m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]);
- NowPos[2] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial.
- m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]);
- NowPos[3] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial.
- m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]);
- NowPos[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | m_SO7_Serial.
- m_RecvData[15] << 8 | m_SO7_Serial.m_RecvData[16]);
- }
- //NowPos[1] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial.m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]);
- //NowPos[2] = (m_SO7_Serial.m_RecvData[5] << 24 | m_SO7_Serial.m_RecvData[6] << 16 | m_SO7_Serial.m_RecvData[7] << 8 | m_SO7_Serial.m_RecvData[8]);
- //NowPos[3] = (m_SO7_Serial.m_RecvData[9] << 24 | m_SO7_Serial.m_RecvData[10] << 16 | m_SO7_Serial.m_RecvData[11] << 8 | m_SO7_Serial.m_RecvData[12]);
- //NowPos[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | m_SO7_Serial.m_RecvData[15] << 8 | m_SO7_Serial.m_RecvData[16]);
-
- Pos_t[1] = (m_SO7_Serial.m_RecvData[17] << 24 | m_SO7_Serial.m_RecvData[18] << 16 | m_SO7_Serial.
- m_RecvData[19] << 8 | m_SO7_Serial.m_RecvData[20]);
- Pos_t[2] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial.
- m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]);
- Pos_t[3] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial.
- m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]);
- Pos_t[4] = (m_SO7_Serial.m_RecvData[29] << 24 | m_SO7_Serial.m_RecvData[30] << 16 | m_SO7_Serial.
- m_RecvData[31] << 8 | m_SO7_Serial.m_RecvData[32]);
- }
- else
- {
- /* 编译器隐式执行的任何类型转换都可以由static_cast显式完成。
- static_cast可以用来将枚举类型转换成整型，或者整型转换成浮点型*/
- Pos_t[1] = NowPos[1] = static_cast(m_EncPos[1] / m_Resolution[1]);
- Pos_t[2] = NowPos[2] = static_cast(m_EncPos[2] / m_Resolution[2]);
- Pos_t[3] = NowPos[3] = static_cast(m_EncPos[3] / m_Resolution[3]);
- Pos_t[4] = NowPos[4] = static_cast(m_EncPos[4] / m_Resolution[4]);
- }
-
- if (m_motorType & 0x01) //步进电机
- Pos[1] = static_cast(PositionX / m_Resolution[1]) - NowPos[1];
- else
- Pos[1] = static_cast(PositionX / m_Resolution[1]) - Pos_t[1];
- if (m_motorType & 0x02) //步进电机
- Pos[2] = static_cast(PositionX / m_Resolution[2]) - NowPos[2];
- else
- Pos[2] = static_cast(PositionY / m_Resolution[2]) - Pos_t[2];
- if (m_motorType & 0x04) //步进电机
- Pos[3] = static_cast(PositionX / m_Resolution[3]) - NowPos[3];
- else
- Pos[3] = static_cast(PositionZ / m_Resolution[3]) - Pos_t[3];
- if (m_motorType & 0x08) //步进电机
- Pos[4] = static_cast(PositionX / m_Resolution[4]) - NowPos[4];
- else
- Pos[4] = static_cast(m_PositionA / m_Resolution[4]) - Pos_t[4];
-
- /*if (m_motorType==1)
- {
- if (m_IsUse_HSICompensation)
- {
-
- }
- else
- {
- Pos[1] = (int)(PositionX / m_Resolution[1]) - NowPos[1];
- Pos[2] = (int)(PositionY / m_Resolution[2]) - NowPos[2];
- Pos[3] = (int)(PositionZ / m_Resolution[3]) - NowPos[3];
- Pos[4] = (int)(m_PositionA / m_Resolution[4]) - NowPos[4];
- }
- }
- else
- {
- Pos_n[1] = (int)(targetpos_n[1] / m_Resolution[1]) - NowPos[1];
- Pos_n[2] = (int)(targetpos_n[2] / m_Resolution[2]) - NowPos[2];
- Pos_n[3] = (int)(targetpos_n[3] / m_Resolution[3]) - NowPos[3];
-
- Pos_t[1] = (int)(targetpos_n[1] / m_Resolution[1]) - (int)(targetpos_l[1] / m_Resolution[1]);
- Pos_t[2] = (int)(targetpos_n[2] / m_Resolution[2]) - (int)(targetpos_l[2] / m_Resolution[1]);
- Pos_t[3] = (int)(targetpos_n[3] / m_Resolution[3]) - (int)(targetpos_l[3] / m_Resolution[1]);
-
- if (m_IsUse_HSICompensation)
- {
- for (int k = 1; k < 4; k++)
- {
- if (abs(Pos_n[k] - Pos_t[k]) > m_Compensation_Pluse)
- {
- Pos[k] = Pos_n[k];
- }
- else
- {
- Pos[k] = Pos_t[k];
- }
- if (abs(Pos[k]) < m_Compensation_Pluse)
- {
- Pos[k] = 0;
- }
- }
- }
- else
- {
- Pos[1] = (int)(PositionX / m_Resolution[1]) - NowPos[1];
- Pos[2] = (int)(PositionY / m_Resolution[2]) - NowPos[2];
- Pos[3] = (int)(PositionZ / m_Resolution[3]) - NowPos[3];
- Pos[4] = (int)(m_PositionA / m_Resolution[4]) - NowPos[4];
- }
- }*/
- target_pos[1] = static_cast(PositionX / m_Resolution[1]); //计算到目标位置
- target_pos[2] = static_cast(PositionY / m_Resolution[2]);
- target_pos[3] = static_cast(PositionZ / m_Resolution[3]);
- target_pos[4] = static_cast(m_PositionA / m_Resolution[4]);
-
- begin_position[1] = target_pos[1]; //将目标位置设置为 开始位置
- begin_position[2] = target_pos[2];
- begin_position[3] = target_pos[3];
- begin_position[4] = target_pos[4];
-
- float scale[4] = {0};
- int Stepdriverspeed[5] = {0};
- int StepStartspeed[5] = {0};
- int StepAcc[5] = {0};
- //if (Pos[1] > 0) direct_pos |= 0x01;
- //if (Pos[2] > 0) direct_pos |= 0x02;
- //if (Pos[3] > 0) direct_pos |= 0x04;
- if (abs(Pos[1]) > 2) axis_start |= 0x01;
- if (abs(Pos[2]) > 2) axis_start |= 0x02;
- if (abs(Pos[3]) > 2) axis_start |= 0x04;
- int MaxPos = abs(Pos[1]);
- for (int i = 2; i < 5; i++)
- {
- if (MaxPos < abs(Pos[i]))
- {
- MaxPos = abs(Pos[i]);
- }
- }
- //MaxPos = abs(MaxPos);
- scale[0] = abs(Pos[1]) / static_cast(MaxPos);
- scale[1] = abs(Pos[2]) / static_cast(MaxPos);
- scale[2] = abs(Pos[3]) / static_cast(MaxPos);
- scale[3] = abs(Pos[4]) / static_cast(MaxPos);
-
- g_pLogger->SendAndFlushWithTime(
- L"[SetPositionXyzNowPos] Pos[1] = %.4f,Pos[2] = %.4f,Pos[3] = %.4f,Pos[4] = %.4f\n",
- NowPos[1] * m_Resolution[1], NowPos[2] * m_Resolution[2], NowPos[3] * m_Resolution[3],
- NowPos[4] * m_Resolution[4]);
- g_pLogger->SendAndFlushWithTime(
- L"[SetPositionXyzTagPos] Pos[1] = %.4f,Pos[2] = %.4f,Pos[3] = %.4f,Pos[4] = %.4f\n", PositionX,
- PositionY, PositionZ, m_PositionA);
-
-
- int stepinterpolation = 0x03;
- //if (!m_motorType)
- //{
- // /*if ((stepinterpolation & 0x01) && m_IsUseManualRunin)
- // {
- // Stepdriverspeed[1] = m_SetPotion_DriveSpeed[1] * scale[0];
- // if (Stepdriverspeed[1] < m_SetPotion_StartSpeed[1])
- // {
- // Stepdriverspeed[1] = m_SetPotion_StartSpeed[1];
- // }
- // }
- // else
- // {
- // Stepdriverspeed[1] = m_SetPotion_DriveSpeed[1];
- // }
- // if ((stepinterpolation & 0x02) && m_IsUseManualRunin)
- // {
- // Stepdriverspeed[2] = m_SetPotion_DriveSpeed[2] * scale[1];
- // if (Stepdriverspeed[2] < m_SetPotion_StartSpeed[2])
- // {
- // Stepdriverspeed[2] = m_SetPotion_StartSpeed[2];
- // }
- // }
- // else
- // {
- // Stepdriverspeed[2] = m_SetPotion_DriveSpeed[2];
- // }
- // if ((stepinterpolation & 0x04) && m_IsUseManualRunin)
- // {
- // Stepdriverspeed[3] = m_SetPotion_DriveSpeed[3] * scale[2];
- // if (Stepdriverspeed[3] < m_SetPotion_StartSpeed[3])
- // {
- // Stepdriverspeed[3] = m_SetPotion_DriveSpeed[3];
- // }
- // }
- // else
- // {
- // Stepdriverspeed[3] = m_SetPotion_DriveSpeed[3];
- // }
- // if ((stepinterpolation & 0x08) && m_IsUseManualRunin)
- // {
- // Stepdriverspeed[4] = m_SetPotion_DriveSpeed[4] * scale[3];
- // if (Stepdriverspeed[4] < m_SetPotion_StartSpeed[4])
- // {
- // Stepdriverspeed[4] = m_SetPotion_DriveSpeed[4];
- // }
- // }
- // else
- // {
- // Stepdriverspeed[4] = m_SetPotion_DriveSpeed[4];
- // }*/
- // for (int i = 1; i < 3; i++)
- // {
- // if (abs(Pos[i]) < m_stepPosition_Load[0])
- // {
- // StepStartspeed[i] = m_stepPosition_L_speed[0];
- // Stepdriverspeed[i] = m_stepPosition_H_speed[0];
- // StepAcc[i] = m_stepPosition_acc[0];
- // }
- // else if (abs(Pos[i]) < m_stepPosition_Load[1])
- // {
- // StepStartspeed[i] = m_stepPosition_L_speed[1];
- // Stepdriverspeed[i] = m_stepPosition_H_speed[1];
- // StepAcc[i] = m_stepPosition_acc[1];
- // }
- // else if (abs(Pos[i]) < m_stepPosition_Load[2])
- // {
- // StepStartspeed[i] = m_stepPosition_L_speed[2];
- // Stepdriverspeed[i] = m_stepPosition_H_speed[2];
- // StepAcc[i] = m_stepPosition_acc[2];
- // }
- // else if (abs(Pos[i]) < m_stepPosition_Load[3])
- // {
- // StepStartspeed[i] = m_stepPosition_L_speed[3];
- // Stepdriverspeed[i] = m_stepPosition_H_speed[3];
- // StepAcc[i] = m_stepPosition_acc[3];
- // }
- // else if (abs(Pos[i]) < m_stepPosition_Load[4])
- // {
- // StepStartspeed[i] = m_stepPosition_L_speed[4];
- // Stepdriverspeed[i] = m_stepPosition_H_speed[4];
- // StepAcc[i] = m_stepPosition_acc[4];
- // }
- // else if (abs(Pos[i]) < m_stepPosition_Load[5])
- // {
- // StepStartspeed[i] = m_stepPosition_L_speed[5];
- // Stepdriverspeed[i] = m_stepPosition_H_speed[5];
- // StepAcc[i] = m_stepPosition_acc[5];
- // }
- // else if (abs(Pos[i]) < m_stepPosition_Load[6])
- // {
- // StepStartspeed[i] = m_stepPosition_L_speed[6];
- // Stepdriverspeed[i] = m_stepPosition_H_speed[6];
- // StepAcc[i] = m_stepPosition_acc[6];
- // }
- // else if (abs(Pos[i]) < m_stepPosition_Load[7])
- // {
- // StepStartspeed[i] = m_stepPosition_L_speed[7];
- // Stepdriverspeed[i] = m_stepPosition_H_speed[7];
- // StepAcc[i] = m_stepPosition_acc[7];
- // }
- // else if (abs(Pos[i]) < m_stepPosition_Load[8])
- // {
- // StepStartspeed[i] = m_stepPosition_L_speed[8];
- // Stepdriverspeed[i] = m_stepPosition_H_speed[8];
- // StepAcc[i] = m_stepPosition_acc[8];
- // }
- // else if (abs(Pos[i]) < m_stepPosition_Load[9])
- // {
- // StepStartspeed[i] = m_stepPosition_L_speed[9];
- // Stepdriverspeed[i] = m_stepPosition_H_speed[9];
- // StepAcc[i] = m_stepPosition_acc[9];
- // }
- // else
- // {
- // StepStartspeed[i] = m_SetPotion_StartSpeed[i];
- // Stepdriverspeed[i] = m_SetPotion_DriveSpeed[i];
- // StepAcc[i] = m_SetPotion_Line[i];
- // }
- // }
- // StepStartspeed[3] = m_SetPotion_StartSpeed[3];
- // StepAcc[3] = m_SetPotion_Line[3];
- // Stepdriverspeed[3] = m_SetPotion_DriveSpeed[3];
- //}
-
- int axisCount = 4;
- if (fourthAxisFlag) //第4轴
- {
- fourthAxisFlag = false;
- if (Pos[4] > 0) direct_pos |= 0x08;
- if (abs(Pos[4]) > 20) axis_start |= 0x08;
- axisCount = 5;
- xyzAxis = AXIS_XYZU;
- }
- else
- xyzAxis = AXIS_XYZ;
-
- //设置运动参数
- for (int i = 1; i < axisCount; i++)
- {
- int time_out_send = 0;
-
- send_pos_data[0] = CT_MOTOR;
- send_pos_data[1] = CT_MOTOR_SET;
- send_pos_data[2] = 1 << (i - 1);
- send_pos_data[3] = POSITION_SPEED_ACC_DEC_POS;
- /*if (m_motorType == 1)
- {*/
- send_pos_data[4] = (m_SetPotion_StartSpeed[i] & 0xff);
- send_pos_data[5] = ((m_SetPotion_StartSpeed[i] >> 8) & 0xff);
- send_pos_data[6] = ((m_SetPotion_StartSpeed[i] >> 16) & 0xff);
- send_pos_data[7] = ((m_SetPotion_StartSpeed[i] >> 24) & 0xff);
- if ((xyzAxis == AXIS_XYZU) && (m_IsUseFourthSpeed == 1))
- {
- send_pos_data[8] = (m_SetPotion_DriveSpeed[4] & 0xff);
- send_pos_data[9] = ((m_SetPotion_DriveSpeed[4] >> 8) & 0xff);
- send_pos_data[10] = ((m_SetPotion_DriveSpeed[4] >> 16) & 0xff);
- send_pos_data[11] = ((m_SetPotion_DriveSpeed[4] >> 24) & 0xff);
- }
- else
- {
- send_pos_data[8] = (m_SetPotion_DriveSpeed[i] & 0xff);
- send_pos_data[9] = ((m_SetPotion_DriveSpeed[i] >> 8) & 0xff);
- send_pos_data[10] = ((m_SetPotion_DriveSpeed[i] >> 16) & 0xff);
- send_pos_data[11] = ((m_SetPotion_DriveSpeed[i] >> 24) & 0xff);
- }
- send_pos_data[12] = (m_SetPotion_Line[i] & 0xff);
- send_pos_data[13] = ((m_SetPotion_Line[i] >> 8) & 0xff);
- send_pos_data[14] = ((m_SetPotion_Line[i] >> 16) & 0xff);
- send_pos_data[15] = ((m_SetPotion_Line[i] >> 24) & 0xff);
-
- send_pos_data[16] = (m_SetPotion_Buffer[i] & 0xff);
- send_pos_data[17] = ((m_SetPotion_Buffer[i] >> 8) & 0xff);
- send_pos_data[18] = ((m_SetPotion_Buffer[i] >> 16) & 0xff);
- send_pos_data[19] = ((m_SetPotion_Buffer[i] >> 24) & 0xff);
-
- send_pos_data[28] = (target_pos[i] & 0xff);
- send_pos_data[29] = ((target_pos[i] >> 8) & 0xff);
- send_pos_data[30] = ((target_pos[i] >> 16) & 0xff);
- send_pos_data[31] = ((target_pos[i] >> 24) & 0xff);
-
- send_pos_data[32] = (Pos[i] & 0xff);
- send_pos_data[33] = ((Pos[i] >> 8) & 0xff);
- send_pos_data[34] = ((Pos[i] >> 16) & 0xff);
- send_pos_data[35] = ((Pos[i] >> 24) & 0xff);
-
- g_pLogger->SendAndFlushWithTime(
- L"[SetPositionXyz] m_SetPotion_StartSpeed[%d] = %d,m_SetPotion_DriveSpeed[%d] = %d\n", i,
- m_SetPotion_StartSpeed[i], i, m_SetPotion_DriveSpeed[i]);
- //send_pos_data[32] = 0x21;
- /*}*/
-
- /*else
- {
- send_pos_data[4] = (StepStartspeed[i] & 0xff);
- send_pos_data[5] = ((StepStartspeed[i] >> 8) & 0xff);
- send_pos_data[6] = ((StepStartspeed[i] >> 16) & 0xff);
- send_pos_data[7] = ((StepStartspeed[i] >> 24) & 0xff);
-
-
- send_pos_data[8] = (Stepdriverspeed[i] & 0xff);
- send_pos_data[9] = ((Stepdriverspeed[i] >> 8) & 0xff);
- send_pos_data[10] = ((Stepdriverspeed[i] >> 16) & 0xff);
- send_pos_data[11] = ((Stepdriverspeed[i] >> 24) & 0xff);
-
- send_pos_data[12] = (StepAcc[i] & 0xff);
- send_pos_data[13] = ((StepAcc[i] >> 8) & 0xff);
- send_pos_data[14] = ((StepAcc[i] >> 16) & 0xff);
- send_pos_data[15] = ((StepAcc[i] >> 24) & 0xff);
-
- send_pos_data[16] = (m_SetPotion_Buffer[i] & 0xff);
- send_pos_data[17] = ((m_SetPotion_Buffer[i] >> 8) & 0xff);
- send_pos_data[18] = ((m_SetPotion_Buffer[i] >> 16) & 0xff);
- send_pos_data[19] = ((m_SetPotion_Buffer[i] >> 24) & 0xff);
-
- send_pos_data[28] = (target_pos[i] & 0xff);
- send_pos_data[29] = ((target_pos[i] >> 8) & 0xff);
- send_pos_data[30] = ((target_pos[i] >> 16) & 0xff);
- send_pos_data[31] = ((target_pos[i] >> 24) & 0xff);
-
- send_pos_data[32] = (Pos[i] & 0xff);
- send_pos_data[33] = ((Pos[i] >> 8) & 0xff);
- send_pos_data[34] = ((Pos[i] >> 16) & 0xff);
- send_pos_data[35] = ((Pos[i] >> 24) & 0xff);
-
- g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] m_SetPotion_StartSpeed[%d] = %d,m_SetPotion_DriveSpeed[%d] = %d\n", i, m_SetPotion_StartSpeed[i], i, m_SetPotion_DriveSpeed[i]);
-
- }*/
- m_WriteByte = Send_Command(0, (const char*)send_pos_data, m_SendDataLength);
- Sleep(6);
- }
-
- //清除缓存位置
- while (m_SO7_Serial.m_RecvData[39])
- {
- send_pos_data[0] = CT_ORDER;
- send_pos_data[1] = CT_POSFLAG_CLEAR;
- m_WriteByte = Send_Command(0, (const char*)send_pos_data, m_SendDataLength);
- Sleep(10);
- }
-
- Sleep(10);
- //启动插补和定位功能
- /* if (m_motorType == 1)
- {
- send_pos_data[0] = CT_MOTOR;
- send_pos_data[1] = CT_START_POSITION;
- send_pos_data[2] = xyzAxis;
- send_pos_data[3] = 0x53;
- send_pos_data[4] = INTERPOLATION;
- }
- else
- {*/
- send_pos_data[0] = CT_MOTOR;
- send_pos_data[1] = CT_START_POSITION;
- send_pos_data[2] = axis_start;
- send_pos_data[3] = 0x53;
- send_pos_data[4] = m_motorType & 0xff;
- /* }*/
- for (int j = 1; j < axisCount; j++)
- {
- send_pos_data[9 + 4 * (j - 1)] = (target_pos[j] & 0xff);
- send_pos_data[10 + 4 * (j - 1)] = ((target_pos[j] >> 8) & 0xff);
- send_pos_data[11 + 4 * (j - 1)] = ((target_pos[j] >> 16) & 0xff);
- send_pos_data[12 + 4 * (j - 1)] = ((target_pos[j] >> 24) & 0xff);
- }
- //send_pos_data[25] = direct_pos;
- int stepmotoracc = 0;
- stepmotoracc = CaculateStepMotorACC(Pos[1], m_SetPotion_Line[1], 10);
- send_pos_data[25] = stepmotoracc;
- stepmotoracc = CaculateStepMotorACC(Pos[2], m_SetPotion_Line[2], 10);
- send_pos_data[26] = stepmotoracc;
- stepmotoracc = CaculateStepMotorACC(Pos[3], m_SetPotion_Line[3], 10);
- send_pos_data[27] = stepmotoracc;
- stepmotoracc = CaculateStepMotorACC(Pos[4], m_SetPotion_Line[4], 10);
- /*send_pos_data[25] = m_SetPotion_Line[1];
- send_pos_data[26] = m_SetPotion_Line[2];
- send_pos_data[27] = m_SetPotion_Line[3];
- send_pos_data[28] = m_SetPotion_Line[4];*/
-
- send_pos_data[29] = m_SpeedAdjustPeriod[1];
- send_pos_data[30] = m_SpeedAdjustPeriod[2];
- send_pos_data[31] = m_SpeedAdjustPeriod[3];
- send_pos_data[32] = m_SpeedAdjustPeriod[4];
-
- for (size_t i = 1; i < 5; i++)
- {
- Stepdriverspeed[i] = m_SetPotion_DriveSpeed[i] * m_Resolution[i] * 50;
- }
-
- send_pos_data[33] = (Stepdriverspeed[1] >> 8) & 0xff;
- send_pos_data[34] = Stepdriverspeed[1] & 0xff;
- send_pos_data[35] = (Stepdriverspeed[2] >> 8) & 0xff;
- send_pos_data[36] = Stepdriverspeed[2] & 0xff;
- send_pos_data[37] = (Stepdriverspeed[3] >> 8) & 0xff;
- send_pos_data[38] = Stepdriverspeed[3] & 0xff;
- send_pos_data[39] = (Stepdriverspeed[4] >> 8) & 0xff;
- send_pos_data[40] = Stepdriverspeed[4] & 0xff;
-
-
- if (bCircleRun) //圆弧插补
- {
- bCircleRun = false;
- send_pos_data[1] = CT_CIRCLERUN_POSITION;
- send_pos_data[2] = 0x03;
- send_pos_data[4] = CIRCLER;
- for (size_t i = 1; i < 3; i++)
- {
- iCircleRunPnt[i] = iCircleRunPnt[i] - NowPos[i];
- send_pos_data[26 + 4 * (i - 1)] = (iCircleRunPnt[i] & 0xff);
- send_pos_data[27 + 4 * (i - 1)] = ((iCircleRunPnt[i] >> 8) & 0xff);
- send_pos_data[28 + 4 * (i - 1)] = ((iCircleRunPnt[i] >> 16) & 0xff);
- send_pos_data[29 + 4 * (i - 1)] = ((iCircleRunPnt[i] >> 24) & 0xff);
- }
- }
- g_IsClose = false;
- m_WriteByte = Send_Command(0, (const char*)send_pos_data, m_SendDataLength);
- set_start = GetTickCount();
- Sleep(3); //
- if (eType == HSI_MOTION_MOVE_NOWAIT)
- {
- g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Nowait SetEvent\n");
- g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Nowait move!\n");
- g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Out Success Nowait\n");
- m_IsExMotion = 0;
- bRunGlueDispenser = HSI_THREAD_PAUSED;
- m_Thread_State = HSI_THREAD_RUNNING;
- SetEvent(m_hTriggerEvent);
- }
- if (eType == HSI_MOTION_MOVE_WAIT)
- {
- g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Out Success Wait Mode\n");
- m_Thread_State = HSI_THREAD_RUNNING;
- m_IsExMotion = 0;
- UpdateMotionState();
- m_Thread_State = HSI_THREAD_PAUSED;
- }
- }
- else
- {
- g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] HSI_STATUS_MOTION_MOVING\n");
- g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Out\n");
- rStatus = HSI_STATUS_MOTION_MOVING;
- }
- g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Out\n");
- targetpos_l[1] = PositionX;
- targetpos_l[2] = PositionY;
- targetpos_l[3] = PositionZ;
- }
- ReleaseMutex(g_WR_ToMove_Mutex);
- return rStatus;
-}
+//HSI_STATUS HSI_Motion::SetPositionXyzOld(UINT AxisTypes, double PositionX, double PositionY, double PositionZ,
+// HSI_MOTION_MOVE_TYPE eType, double dFlyRadius)
+//{
+// WaitForSingleObject(g_WR_ToMove_Mutex, INFINITE);
+// auto rStatus = HSI_STATUS_NORMAL;
+// if (g_pHSI_Motion)
+// {
+// g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] In\n");
+// g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Pos[1] = %.4f,Pos[2] = %.4f,Pos[3] = %.4f\n", PositionX,PositionY, PositionZ);
+// unsigned char send_pos_data[64] = { 0 };
+// axis_start = 0;
+// unsigned char direct_pos = 0;
+// unsigned char xyzAxis = 0;
+// //如果状态非 运动中，设置为运动中
+// if (CurrentMotionState != E_SO7_MOTION_MOVETO)
+// {
+// CurrentMotionState = E_SO7_MOTION_MOVETO;
+// LimitOver(HSI_MOTION_AXIS_X, PositionX);
+// LimitOver(HSI_MOTION_AXIS_Y, PositionY);
+// LimitOver(HSI_MOTION_AXIS_Z, PositionZ);
+// LimitOver(HSI_MOTION_AXIS_R, m_PositionA);
+//
+// m_PosThread[1] = PositionX; //SetpositionXyz的目标位置
+// m_PosThread[2] = PositionY;
+// m_PosThread[3] = PositionZ;
+// m_PosThread[4] = m_PositionA;
+//
+// targetpos_n[1] = PositionX;
+// targetpos_n[2] = PositionY;
+// targetpos_n[3] = PositionZ;
+// targetpos_n[4] = m_PositionA;
+//
+// int Pos_t[5] = { 0 };
+// int Pos_n[5] = { 0 };
+// int Pos[5] = { 0 };
+// int NowPos[5] = { 0 };
+// int target_pos[5] = { 0 };
+//
+// if (m_SO7_Serial.m_RecvData[0] == 2)
+// {
+// //根据设备类型获取现在位置
+// if (m_DeviceType != 1) //设备类型非三激光
+// {
+// if (m_IsHavePattern & 0x01 == 0x01)
+// NowPos[1] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial.
+// m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]);
+// else
+// NowPos[1] = (m_SO7_Serial.m_RecvData[17] << 24 | m_SO7_Serial.m_RecvData[18] << 16 |
+// m_SO7_Serial.m_RecvData[19] << 8 | m_SO7_Serial.m_RecvData[20]);
+// if (m_IsHavePattern & 0x02)
+// NowPos[2] = (m_SO7_Serial.m_RecvData[5] << 24 | m_SO7_Serial.m_RecvData[6] << 16 | m_SO7_Serial.
+// m_RecvData[7] << 8 | m_SO7_Serial.m_RecvData[8]);
+// else
+// NowPos[2] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 |
+// m_SO7_Serial.m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]);
+// if (m_IsHavePattern & 0x04)
+// NowPos[3] = (m_SO7_Serial.m_RecvData[9] << 24 | m_SO7_Serial.m_RecvData[10] << 16 | m_SO7_Serial
+// .m_RecvData[11] << 8 | m_SO7_Serial.m_RecvData[12]);
+// else
+// NowPos[3] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 |
+// m_SO7_Serial.m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]);
+// if (m_IsHavePattern & 0x08)
+// NowPos[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 |
+// m_SO7_Serial.m_RecvData[15] << 8 | m_SO7_Serial.m_RecvData[16]);
+// else
+// NowPos[4] = (m_SO7_Serial.m_RecvData[29] << 24 | m_SO7_Serial.m_RecvData[30] << 16 |
+// m_SO7_Serial.m_RecvData[31] << 8 | m_SO7_Serial.m_RecvData[32]);
+// }
+// else
+// {
+// NowPos[1] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial.
+// m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]);
+// NowPos[2] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial.
+// m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]);
+// NowPos[3] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial.
+// m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]);
+// NowPos[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | m_SO7_Serial.
+// m_RecvData[15] << 8 | m_SO7_Serial.m_RecvData[16]);
+// }
+// //NowPos[1] = (m_SO7_Serial.m_RecvData[1] << 24 | m_SO7_Serial.m_RecvData[2] << 16 | m_SO7_Serial.m_RecvData[3] << 8 | m_SO7_Serial.m_RecvData[4]);
+// //NowPos[2] = (m_SO7_Serial.m_RecvData[5] << 24 | m_SO7_Serial.m_RecvData[6] << 16 | m_SO7_Serial.m_RecvData[7] << 8 | m_SO7_Serial.m_RecvData[8]);
+// //NowPos[3] = (m_SO7_Serial.m_RecvData[9] << 24 | m_SO7_Serial.m_RecvData[10] << 16 | m_SO7_Serial.m_RecvData[11] << 8 | m_SO7_Serial.m_RecvData[12]);
+// //NowPos[4] = (m_SO7_Serial.m_RecvData[13] << 24 | m_SO7_Serial.m_RecvData[14] << 16 | m_SO7_Serial.m_RecvData[15] << 8 | m_SO7_Serial.m_RecvData[16]);
+//
+// Pos_t[1] = (m_SO7_Serial.m_RecvData[17] << 24 | m_SO7_Serial.m_RecvData[18] << 16 | m_SO7_Serial.
+// m_RecvData[19] << 8 | m_SO7_Serial.m_RecvData[20]);
+// Pos_t[2] = (m_SO7_Serial.m_RecvData[21] << 24 | m_SO7_Serial.m_RecvData[22] << 16 | m_SO7_Serial.
+// m_RecvData[23] << 8 | m_SO7_Serial.m_RecvData[24]);
+// Pos_t[3] = (m_SO7_Serial.m_RecvData[25] << 24 | m_SO7_Serial.m_RecvData[26] << 16 | m_SO7_Serial.
+// m_RecvData[27] << 8 | m_SO7_Serial.m_RecvData[28]);
+// Pos_t[4] = (m_SO7_Serial.m_RecvData[29] << 24 | m_SO7_Serial.m_RecvData[30] << 16 | m_SO7_Serial.
+// m_RecvData[31] << 8 | m_SO7_Serial.m_RecvData[32]);
+// }
+// else
+// {
+// /* 编译器隐式执行的任何类型转换都可以由static_cast显式完成。
+// static_cast可以用来将枚举类型转换成整型，或者整型转换成浮点型*/
+// Pos_t[1] = NowPos[1] = static_cast(m_EncPos[1] / m_Resolution[1]);
+// Pos_t[2] = NowPos[2] = static_cast(m_EncPos[2] / m_Resolution[2]);
+// Pos_t[3] = NowPos[3] = static_cast(m_EncPos[3] / m_Resolution[3]);
+// Pos_t[4] = NowPos[4] = static_cast(m_EncPos[4] / m_Resolution[4]);
+// }
+//
+// if (m_motorType & 0x01) //步进电机
+// Pos[1] = static_cast(PositionX / m_Resolution[1]) - NowPos[1];
+// else
+// Pos[1] = static_cast(PositionX / m_Resolution[1]) - Pos_t[1];
+// if (m_motorType & 0x02) //步进电机
+// Pos[2] = static_cast(PositionX / m_Resolution[2]) - NowPos[2];
+// else
+// Pos[2] = static_cast(PositionY / m_Resolution[2]) - Pos_t[2];
+// if (m_motorType & 0x04) //步进电机
+// Pos[3] = static_cast(PositionX / m_Resolution[3]) - NowPos[3];
+// else
+// Pos[3] = static_cast(PositionZ / m_Resolution[3]) - Pos_t[3];
+// if (m_motorType & 0x08) //步进电机
+// Pos[4] = static_cast(PositionX / m_Resolution[4]) - NowPos[4];
+// else
+// Pos[4] = static_cast(m_PositionA / m_Resolution[4]) - Pos_t[4];
+//
+// /*if (m_motorType==1)
+// {
+// if (m_IsUse_HSICompensation)
+// {
+// }
+// else
+// {
+// Pos[1] = (int)(PositionX / m_Resolution[1]) - NowPos[1];
+// Pos[2] = (int)(PositionY / m_Resolution[2]) - NowPos[2];
+// Pos[3] = (int)(PositionZ / m_Resolution[3]) - NowPos[3];
+// Pos[4] = (int)(m_PositionA / m_Resolution[4]) - NowPos[4];
+// }
+// }
+// else
+// {
+// Pos_n[1] = (int)(targetpos_n[1] / m_Resolution[1]) - NowPos[1];
+// Pos_n[2] = (int)(targetpos_n[2] / m_Resolution[2]) - NowPos[2];
+// Pos_n[3] = (int)(targetpos_n[3] / m_Resolution[3]) - NowPos[3];
+//
+// Pos_t[1] = (int)(targetpos_n[1] / m_Resolution[1]) - (int)(targetpos_l[1] / m_Resolution[1]);
+// Pos_t[2] = (int)(targetpos_n[2] / m_Resolution[2]) - (int)(targetpos_l[2] / m_Resolution[1]);
+// Pos_t[3] = (int)(targetpos_n[3] / m_Resolution[3]) - (int)(targetpos_l[3] / m_Resolution[1]);
+//
+// if (m_IsUse_HSICompensation)
+// {
+// for (int k = 1; k < 4; k++)
+// {
+// if (abs(Pos_n[k] - Pos_t[k]) > m_Compensation_Pluse)
+// {
+// Pos[k] = Pos_n[k];
+// }
+// else
+// {
+// Pos[k] = Pos_t[k];
+// }
+// if (abs(Pos[k]) < m_Compensation_Pluse)
+// {
+// Pos[k] = 0;
+// }
+// }
+// }
+// else
+// {
+// Pos[1] = (int)(PositionX / m_Resolution[1]) - NowPos[1];
+// Pos[2] = (int)(PositionY / m_Resolution[2]) - NowPos[2];
+// Pos[3] = (int)(PositionZ / m_Resolution[3]) - NowPos[3];
+// Pos[4] = (int)(m_PositionA / m_Resolution[4]) - NowPos[4];
+// }
+// }*/
+// target_pos[1] = static_cast(PositionX / m_Resolution[1]); //计算到目标位置
+// target_pos[2] = static_cast(PositionY / m_Resolution[2]);
+// target_pos[3] = static_cast(PositionZ / m_Resolution[3]);
+// target_pos[4] = static_cast(m_PositionA / m_Resolution[4]);
+//
+// begin_position[1] = target_pos[1]; //将目标位置设置为 开始位置
+// begin_position[2] = target_pos[2];
+// begin_position[3] = target_pos[3];
+// begin_position[4] = target_pos[4];
+//
+// float scale[4] = { 0 };
+// int Stepdriverspeed[5] = { 0 };
+// int StepStartspeed[5] = { 0 };
+// int StepAcc[5] = { 0 };
+// //if (Pos[1] > 0) direct_pos |= 0x01;
+// //if (Pos[2] > 0) direct_pos |= 0x02;
+// //if (Pos[3] > 0) direct_pos |= 0x04;
+// if (abs(Pos[1]) > 2) axis_start |= 0x01;
+// if (abs(Pos[2]) > 2) axis_start |= 0x02;
+// if (abs(Pos[3]) > 2) axis_start |= 0x04;
+// int MaxPos = abs(Pos[1]);
+// for (int i = 2; i < 5; i++)
+// {
+// if (MaxPos < abs(Pos[i]))
+// {
+// MaxPos = abs(Pos[i]);
+// }
+// }
+// //MaxPos = abs(MaxPos);
+// scale[0] = abs(Pos[1]) / static_cast(MaxPos);
+// scale[1] = abs(Pos[2]) / static_cast(MaxPos);
+// scale[2] = abs(Pos[3]) / static_cast(MaxPos);
+// scale[3] = abs(Pos[4]) / static_cast(MaxPos);
+//
+// g_pLogger->SendAndFlushWithTime(
+// L"[SetPositionXyzNowPos] Pos[1] = %.4f,Pos[2] = %.4f,Pos[3] = %.4f,Pos[4] = %.4f\n",
+// NowPos[1] * m_Resolution[1], NowPos[2] * m_Resolution[2], NowPos[3] * m_Resolution[3],
+// NowPos[4] * m_Resolution[4]);
+// g_pLogger->SendAndFlushWithTime(
+// L"[SetPositionXyzTagPos] Pos[1] = %.4f,Pos[2] = %.4f,Pos[3] = %.4f,Pos[4] = %.4f\n", PositionX,
+// PositionY, PositionZ, m_PositionA);
+//
+// int stepinterpolation = 0x03;
+// //if (!m_motorType)
+// //{
+// // /*if ((stepinterpolation & 0x01) && m_IsUseManualRunin)
+// // {
+// // Stepdriverspeed[1] = m_SetPotion_DriveSpeed[1] * scale[0];
+// // if (Stepdriverspeed[1] < m_SetPotion_StartSpeed[1])
+// // {
+// // Stepdriverspeed[1] = m_SetPotion_StartSpeed[1];
+// // }
+// // }
+// // else
+// // {
+// // Stepdriverspeed[1] = m_SetPotion_DriveSpeed[1];
+// // }
+// // if ((stepinterpolation & 0x02) && m_IsUseManualRunin)
+// // {
+// // Stepdriverspeed[2] = m_SetPotion_DriveSpeed[2] * scale[1];
+// // if (Stepdriverspeed[2] < m_SetPotion_StartSpeed[2])
+// // {
+// // Stepdriverspeed[2] = m_SetPotion_StartSpeed[2];
+// // }
+// // }
+// // else
+// // {
+// // Stepdriverspeed[2] = m_SetPotion_DriveSpeed[2];
+// // }
+// // if ((stepinterpolation & 0x04) && m_IsUseManualRunin)
+// // {
+// // Stepdriverspeed[3] = m_SetPotion_DriveSpeed[3] * scale[2];
+// // if (Stepdriverspeed[3] < m_SetPotion_StartSpeed[3])
+// // {
+// // Stepdriverspeed[3] = m_SetPotion_DriveSpeed[3];
+// // }
+// // }
+// // else
+// // {
+// // Stepdriverspeed[3] = m_SetPotion_DriveSpeed[3];
+// // }
+// // if ((stepinterpolation & 0x08) && m_IsUseManualRunin)
+// // {
+// // Stepdriverspeed[4] = m_SetPotion_DriveSpeed[4] * scale[3];
+// // if (Stepdriverspeed[4] < m_SetPotion_StartSpeed[4])
+// // {
+// // Stepdriverspeed[4] = m_SetPotion_DriveSpeed[4];
+// // }
+// // }
+// // else
+// // {
+// // Stepdriverspeed[4] = m_SetPotion_DriveSpeed[4];
+// // }*/
+// // for (int i = 1; i < 3; i++)
+// // {
+// // if (abs(Pos[i]) < m_stepPosition_Load[0])
+// // {
+// // StepStartspeed[i] = m_stepPosition_L_speed[0];
+// // Stepdriverspeed[i] = m_stepPosition_H_speed[0];
+// // StepAcc[i] = m_stepPosition_acc[0];
+// // }
+// // else if (abs(Pos[i]) < m_stepPosition_Load[1])
+// // {
+// // StepStartspeed[i] = m_stepPosition_L_speed[1];
+// // Stepdriverspeed[i] = m_stepPosition_H_speed[1];
+// // StepAcc[i] = m_stepPosition_acc[1];
+// // }
+// // else if (abs(Pos[i]) < m_stepPosition_Load[2])
+// // {
+// // StepStartspeed[i] = m_stepPosition_L_speed[2];
+// // Stepdriverspeed[i] = m_stepPosition_H_speed[2];
+// // StepAcc[i] = m_stepPosition_acc[2];
+// // }
+// // else if (abs(Pos[i]) < m_stepPosition_Load[3])
+// // {
+// // StepStartspeed[i] = m_stepPosition_L_speed[3];
+// // Stepdriverspeed[i] = m_stepPosition_H_speed[3];
+// // StepAcc[i] = m_stepPosition_acc[3];
+// // }
+// // else if (abs(Pos[i]) < m_stepPosition_Load[4])
+// // {
+// // StepStartspeed[i] = m_stepPosition_L_speed[4];
+// // Stepdriverspeed[i] = m_stepPosition_H_speed[4];
+// // StepAcc[i] = m_stepPosition_acc[4];
+// // }
+// // else if (abs(Pos[i]) < m_stepPosition_Load[5])
+// // {
+// // StepStartspeed[i] = m_stepPosition_L_speed[5];
+// // Stepdriverspeed[i] = m_stepPosition_H_speed[5];
+// // StepAcc[i] = m_stepPosition_acc[5];
+// // }
+// // else if (abs(Pos[i]) < m_stepPosition_Load[6])
+// // {
+// // StepStartspeed[i] = m_stepPosition_L_speed[6];
+// // Stepdriverspeed[i] = m_stepPosition_H_speed[6];
+// // StepAcc[i] = m_stepPosition_acc[6];
+// // }
+// // else if (abs(Pos[i]) < m_stepPosition_Load[7])
+// // {
+// // StepStartspeed[i] = m_stepPosition_L_speed[7];
+// // Stepdriverspeed[i] = m_stepPosition_H_speed[7];
+// // StepAcc[i] = m_stepPosition_acc[7];
+// // }
+// // else if (abs(Pos[i]) < m_stepPosition_Load[8])
+// // {
+// // StepStartspeed[i] = m_stepPosition_L_speed[8];
+// // Stepdriverspeed[i] = m_stepPosition_H_speed[8];
+// // StepAcc[i] = m_stepPosition_acc[8];
+// // }
+// // else if (abs(Pos[i]) < m_stepPosition_Load[9])
+// // {
+// // StepStartspeed[i] = m_stepPosition_L_speed[9];
+// // Stepdriverspeed[i] = m_stepPosition_H_speed[9];
+// // StepAcc[i] = m_stepPosition_acc[9];
+// // }
+// // else
+// // {
+// // StepStartspeed[i] = m_SetPotion_StartSpeed[i];
+// // Stepdriverspeed[i] = m_SetPotion_DriveSpeed[i];
+// // StepAcc[i] = m_SetPotion_Line[i];
+// // }
+// // }
+// // StepStartspeed[3] = m_SetPotion_StartSpeed[3];
+// // StepAcc[3] = m_SetPotion_Line[3];
+// // Stepdriverspeed[3] = m_SetPotion_DriveSpeed[3];
+// //}
+//
+// int axisCount = 4;
+// if (fourthAxisFlag) //第4轴
+// {
+// fourthAxisFlag = false;
+// if (Pos[4] > 0) direct_pos |= 0x08;
+// if (abs(Pos[4]) > 20) axis_start |= 0x08;
+// axisCount = 5;
+// xyzAxis = AXIS_XYZU;
+// }
+// else
+// {
+// xyzAxis = AXIS_XYZ;
+// }
+//
+// //设置运动参数
+// for (int i = 1; i < axisCount; i++)
+// {
+// int time_out_send = 0;
+//
+// send_pos_data[0] = CT_MOTOR;
+// send_pos_data[1] = CT_MOTOR_SET;
+// send_pos_data[2] = 1 << (i - 1);
+// send_pos_data[3] = POSITION_SPEED_ACC_DEC_POS;
+// /*if (m_motorType == 1)
+// {*/
+// send_pos_data[4] = (m_SetPotion_StartSpeed[i] & 0xff);
+// send_pos_data[5] = ((m_SetPotion_StartSpeed[i] >> 8) & 0xff);
+// send_pos_data[6] = ((m_SetPotion_StartSpeed[i] >> 16) & 0xff);
+// send_pos_data[7] = ((m_SetPotion_StartSpeed[i] >> 24) & 0xff);
+// if ((xyzAxis == AXIS_XYZU) && (m_IsUseFourthSpeed == 1))
+// {
+// send_pos_data[8] = (m_SetPotion_DriveSpeed[4] & 0xff);
+// send_pos_data[9] = ((m_SetPotion_DriveSpeed[4] >> 8) & 0xff);
+// send_pos_data[10] = ((m_SetPotion_DriveSpeed[4] >> 16) & 0xff);
+// send_pos_data[11] = ((m_SetPotion_DriveSpeed[4] >> 24) & 0xff);
+// }
+// else
+// {
+// send_pos_data[8] = (m_SetPotion_DriveSpeed[i] & 0xff);
+// send_pos_data[9] = ((m_SetPotion_DriveSpeed[i] >> 8) & 0xff);
+// send_pos_data[10] = ((m_SetPotion_DriveSpeed[i] >> 16) & 0xff);
+// send_pos_data[11] = ((m_SetPotion_DriveSpeed[i] >> 24) & 0xff);
+// }
+// send_pos_data[12] = (m_SetPotion_Line[i] & 0xff);
+// send_pos_data[13] = ((m_SetPotion_Line[i] >> 8) & 0xff);
+// send_pos_data[14] = ((m_SetPotion_Line[i] >> 16) & 0xff);
+// send_pos_data[15] = ((m_SetPotion_Line[i] >> 24) & 0xff);
+//
+// send_pos_data[16] = (m_SetPotion_Buffer[i] & 0xff);
+// send_pos_data[17] = ((m_SetPotion_Buffer[i] >> 8) & 0xff);
+// send_pos_data[18] = ((m_SetPotion_Buffer[i] >> 16) & 0xff);
+// send_pos_data[19] = ((m_SetPotion_Buffer[i] >> 24) & 0xff);
+//
+// send_pos_data[28] = (target_pos[i] & 0xff);
+// send_pos_data[29] = ((target_pos[i] >> 8) & 0xff);
+// send_pos_data[30] = ((target_pos[i] >> 16) & 0xff);
+// send_pos_data[31] = ((target_pos[i] >> 24) & 0xff);
+//
+// send_pos_data[32] = (Pos[i] & 0xff);
+// send_pos_data[33] = ((Pos[i] >> 8) & 0xff);
+// send_pos_data[34] = ((Pos[i] >> 16) & 0xff);
+// send_pos_data[35] = ((Pos[i] >> 24) & 0xff);
+//
+// g_pLogger->SendAndFlushWithTime(
+// L"[SetPositionXyz] m_SetPotion_StartSpeed[%d] = %d,m_SetPotion_DriveSpeed[%d] = %d\n", i,
+// m_SetPotion_StartSpeed[i], i, m_SetPotion_DriveSpeed[i]);
+// //send_pos_data[32] = 0x21;
+// /*}*/
+//
+// /*else
+// {
+// send_pos_data[4] = (StepStartspeed[i] & 0xff);
+// send_pos_data[5] = ((StepStartspeed[i] >> 8) & 0xff);
+// send_pos_data[6] = ((StepStartspeed[i] >> 16) & 0xff);
+// send_pos_data[7] = ((StepStartspeed[i] >> 24) & 0xff);
+//
+// send_pos_data[8] = (Stepdriverspeed[i] & 0xff);
+// send_pos_data[9] = ((Stepdriverspeed[i] >> 8) & 0xff);
+// send_pos_data[10] = ((Stepdriverspeed[i] >> 16) & 0xff);
+// send_pos_data[11] = ((Stepdriverspeed[i] >> 24) & 0xff);
+//
+// send_pos_data[12] = (StepAcc[i] & 0xff);
+// send_pos_data[13] = ((StepAcc[i] >> 8) & 0xff);
+// send_pos_data[14] = ((StepAcc[i] >> 16) & 0xff);
+// send_pos_data[15] = ((StepAcc[i] >> 24) & 0xff);
+//
+// send_pos_data[16] = (m_SetPotion_Buffer[i] & 0xff);
+// send_pos_data[17] = ((m_SetPotion_Buffer[i] >> 8) & 0xff);
+// send_pos_data[18] = ((m_SetPotion_Buffer[i] >> 16) & 0xff);
+// send_pos_data[19] = ((m_SetPotion_Buffer[i] >> 24) & 0xff);
+//
+// send_pos_data[28] = (target_pos[i] & 0xff);
+// send_pos_data[29] = ((target_pos[i] >> 8) & 0xff);
+// send_pos_data[30] = ((target_pos[i] >> 16) & 0xff);
+// send_pos_data[31] = ((target_pos[i] >> 24) & 0xff);
+//
+// send_pos_data[32] = (Pos[i] & 0xff);
+// send_pos_data[33] = ((Pos[i] >> 8) & 0xff);
+// send_pos_data[34] = ((Pos[i] >> 16) & 0xff);
+// send_pos_data[35] = ((Pos[i] >> 24) & 0xff);
+//
+// g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] m_SetPotion_StartSpeed[%d] = %d,m_SetPotion_DriveSpeed[%d] = %d\n", i, m_SetPotion_StartSpeed[i], i, m_SetPotion_DriveSpeed[i]);
+// }*/
+// m_WriteByte = Send_Command(0, (const char*)send_pos_data, m_SendDataLength);
+// Sleep(6);
+// }
+//
+// //清除缓存位置
+// while (m_SO7_Serial.m_RecvData[39])
+// {
+// send_pos_data[0] = CT_ORDER;
+// send_pos_data[1] = CT_POSFLAG_CLEAR;
+// m_WriteByte = Send_Command(0, (const char*)send_pos_data, m_SendDataLength);
+// Sleep(10);
+// }
+//
+// Sleep(10);
+// //启动插补和定位功能
+// /* if (m_motorType == 1)
+// {
+// send_pos_data[0] = CT_MOTOR;
+// send_pos_data[1] = CT_START_POSITION;
+// send_pos_data[2] = xyzAxis;
+// send_pos_data[3] = 0x53;
+// send_pos_data[4] = INTERPOLATION;
+// }
+// else
+// {*/
+// send_pos_data[0] = CT_MOTOR;
+// send_pos_data[1] = CT_START_POSITION;
+// send_pos_data[2] = axis_start;
+// send_pos_data[3] = 0x53;
+// send_pos_data[4] = m_motorType & 0xff;
+// /* }*/
+// for (int j = 1; j < axisCount; j++)
+// {
+// send_pos_data[9 + 4 * (j - 1)] = (target_pos[j] & 0xff);
+// send_pos_data[10 + 4 * (j - 1)] = ((target_pos[j] >> 8) & 0xff);
+// send_pos_data[11 + 4 * (j - 1)] = ((target_pos[j] >> 16) & 0xff);
+// send_pos_data[12 + 4 * (j - 1)] = ((target_pos[j] >> 24) & 0xff);
+// }
+// //send_pos_data[25] = direct_pos;
+// int stepmotoracc = 0;
+// stepmotoracc = CaculateStepMotorACC(Pos[1], m_SetPotion_Line[1], 10);
+// send_pos_data[25] = stepmotoracc;
+// stepmotoracc = CaculateStepMotorACC(Pos[2], m_SetPotion_Line[2], 10);
+// send_pos_data[26] = stepmotoracc;
+// stepmotoracc = CaculateStepMotorACC(Pos[3], m_SetPotion_Line[3], 10);
+// send_pos_data[27] = stepmotoracc;
+// stepmotoracc = CaculateStepMotorACC(Pos[4], m_SetPotion_Line[4], 10);
+// /*send_pos_data[25] = m_SetPotion_Line[1];
+// send_pos_data[26] = m_SetPotion_Line[2];
+// send_pos_data[27] = m_SetPotion_Line[3];
+// send_pos_data[28] = m_SetPotion_Line[4];*/
+//
+// send_pos_data[29] = m_SpeedAdjustPeriod[1];
+// send_pos_data[30] = m_SpeedAdjustPeriod[2];
+// send_pos_data[31] = m_SpeedAdjustPeriod[3];
+// send_pos_data[32] = m_SpeedAdjustPeriod[4];
+//
+// for (size_t i = 1; i < 5; i++)
+// {
+// Stepdriverspeed[i] = m_SetPotion_DriveSpeed[i] * m_Resolution[i] * 50;
+// }
+//
+// send_pos_data[33] = (Stepdriverspeed[1] >> 8) & 0xff;
+// send_pos_data[34] = Stepdriverspeed[1] & 0xff;
+// send_pos_data[35] = (Stepdriverspeed[2] >> 8) & 0xff;
+// send_pos_data[36] = Stepdriverspeed[2] & 0xff;
+// send_pos_data[37] = (Stepdriverspeed[3] >> 8) & 0xff;
+// send_pos_data[38] = Stepdriverspeed[3] & 0xff;
+// send_pos_data[39] = (Stepdriverspeed[4] >> 8) & 0xff;
+// send_pos_data[40] = Stepdriverspeed[4] & 0xff;
+//
+// if (bCircleRun) //圆弧插补
+// {
+// bCircleRun = false;
+// send_pos_data[1] = CT_CIRCLERUN_POSITION;
+// send_pos_data[2] = 0x03;
+// send_pos_data[4] = CIRCLER;
+// for (size_t i = 1; i < 3; i++)
+// {
+// iCircleRunPnt[i] = iCircleRunPnt[i] - NowPos[i];
+// send_pos_data[26 + 4 * (i - 1)] = (iCircleRunPnt[i] & 0xff);
+// send_pos_data[27 + 4 * (i - 1)] = ((iCircleRunPnt[i] >> 8) & 0xff);
+// send_pos_data[28 + 4 * (i - 1)] = ((iCircleRunPnt[i] >> 16) & 0xff);
+// send_pos_data[29 + 4 * (i - 1)] = ((iCircleRunPnt[i] >> 24) & 0xff);
+// }
+// }
+// g_IsClose = false;
+// m_WriteByte = Send_Command(0, (const char*)send_pos_data, m_SendDataLength);
+// set_start = GetTickCount();
+// Sleep(3); //
+// if (eType == HSI_MOTION_MOVE_NOWAIT)
+// {
+// g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Nowait SetEvent\n");
+// g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Nowait move!\n");
+// g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Out Success Nowait\n");
+// m_IsExMotion = 0;
+// bRunGlueDispenser = HSI_THREAD_PAUSED;
+// m_Thread_State = HSI_THREAD_RUNNING;
+// SetEvent(m_hTriggerEvent);
+// }
+// if (eType == HSI_MOTION_MOVE_WAIT)
+// {
+// g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Out Success Wait Mode\n");
+// m_Thread_State = HSI_THREAD_RUNNING;
+// m_IsExMotion = 0;
+// UpdateMotionState();
+// m_Thread_State = HSI_THREAD_PAUSED;
+// }
+// }
+// else
+// {
+// g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] HSI_STATUS_MOTION_MOVING\n");
+// g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Out\n");
+// rStatus = HSI_STATUS_MOTION_MOVING;
+// }
+// g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] Out\n");
+// targetpos_l[1] = PositionX;
+// targetpos_l[2] = PositionY;
+// targetpos_l[3] = PositionZ;
+// }
+// ReleaseMutex(g_WR_ToMove_Mutex);
+// return rStatus;
+//}
HSI_STATUS HSI_Motion::SetPositionXyz(UINT AxisTypes, double PositionX, double PositionY, double PositionZ,
- HSI_MOTION_MOVE_TYPE eType, double dFlyRadius)
+ HSI_MOTION_MOVE_TYPE eType, double dFlyRadius)
{
WaitForSingleObject(g_WR_ToMove_Mutex, INFINITE);
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] In\n");
- g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] AxisTypes = %d, PositionX = %.4f,PositionY = %.4f,PositionZ = %.4f\n", AxisTypes,PositionX, PositionY, PositionZ);
+ g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] AxisTypes = %d, PositionX = %.4f,PositionY = %.4f,PositionZ = %.4f\n", AxisTypes, PositionX, PositionY, PositionZ);
axis_start = 0;
unsigned char direct_pos = 0;
@@ -3514,7 +3495,7 @@ HSI_STATUS HSI_Motion::SetPositionXyz(UINT AxisTypes, double PositionX, double P
if (CurrentMotionState != E_SO7_MOTION_MOVETO) //当前运动状态
{
CurrentMotionState = E_SO7_MOTION_MOVETO;
-
+
//限位功能
LimitOver(HSI_MOTION_AXIS_X, PositionX);
LimitOver(HSI_MOTION_AXIS_Y, PositionY);
@@ -3531,9 +3512,8 @@ HSI_STATUS HSI_Motion::SetPositionXyz(UINT AxisTypes, double PositionX, double P
m_PosThread[4] = m_PositionA;
//目标位置
- g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] PositionX = %.4f, PositionY = %.4f, PositionZ = %.4f\n", PositionX,PositionY, PositionZ);
+ g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] PositionX = %.4f, PositionY = %.4f, PositionZ = %.4f\n", PositionX, PositionY, PositionZ);
-
//设置速度，对应配置文件中定位合成速度 SET_POTION_DRIVESPEED_1
g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] DriveSpeed[0] = %d, DriveSpeed[1] = %d, DriveSpeed[2] = %d, DriveSpeed[3] = %d, DriveSpeed[4] = %d\n",
m_SetPotion_DriveSpeed[0], m_SetPotion_DriveSpeed[1], m_SetPotion_DriveSpeed[2], m_SetPotion_DriveSpeed[3], m_SetPotion_DriveSpeed[4]);
@@ -3546,25 +3526,24 @@ HSI_STATUS HSI_Motion::SetPositionXyz(UINT AxisTypes, double PositionX, double P
m_SetPotion_DriveSpeed[1] * 100,
m_SetPotion_DriveSpeed[1] * 100
};
-
+
SetSingleAxisMotionParams(ACSC_AXIS_1, X_SetmotionParam);//设置X轴定位速度
m_SetPotion_DriveSpeed[ACSC_AXIS_1] = X_SetmotionParam[0];// 记录X轴速度
g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] 设置Y轴定位速度 %d\n", m_SetPotion_DriveSpeed[2]);
- double Y_SetmotionParam[5] = {
+ double Y_SetmotionParam[5] = {
m_SetPotion_DriveSpeed[2],
- m_SetPotion_DriveSpeed[2] * 10 ,
- m_SetPotion_DriveSpeed[2] * 10,
+ m_SetPotion_DriveSpeed[2] * 10 ,
+ m_SetPotion_DriveSpeed[2] * 10,
m_SetPotion_DriveSpeed[2] * 100,
m_SetPotion_DriveSpeed[2] * 100
};
-
SetSingleAxisMotionParams(ACSC_AXIS_0, Y_SetmotionParam);//设置Y轴定位速度
m_SetPotion_DriveSpeed[ACSC_AXIS_0] = Y_SetmotionParam[0];// 记录Y轴速度
g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] 设置Z轴定位速度 %d\n", m_SetPotion_DriveSpeed[3]);
- double Z_SetmotionParam[5] = {
+ double Z_SetmotionParam[5] = {
m_SetPotion_DriveSpeed[3], //速度
m_SetPotion_DriveSpeed[3] * 10 ,//加速度
m_SetPotion_DriveSpeed[3] * 10, //减速度
@@ -3588,14 +3567,13 @@ HSI_STATUS HSI_Motion::SetPositionXyz(UINT AxisTypes, double PositionX, double P
}
//开始运动到指定位置，多轴运动
- double Points[] = {PositionX, PositionY, PositionZ}; //目标位置点
+ double Points[] = { PositionX, PositionY, PositionZ }; //目标位置点
if (!acsc_ToPointM(handleACS, 0, Axes, Points, nullptr)) //移动到绝对位置
{
g_pLogger->SendAndFlushWithTime(L"[SetPositionXyz] ACS Multi Motion Error\n");
ErrorsHandler();
}
-
//状态更新
if (eType == HSI_MOTION_MOVE_NOWAIT) //非等待
{
@@ -3657,16 +3635,16 @@ int HSI_Motion::CaculateStepMotorACC(int pos, int maxacc, int minacc)
//版权声明：本文为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 func3(void); //利用全局变量返回数组
+//void func2(uchar* s); //利用指针返回数组
+//uchar* func1(); //利用指针函数返回数组
//void func0(uchar*& r); //利用引用返回数组
/**
* \brief 获取单轴运动参数
- * \param AXIS
- * \param motionParam
- * \return
+ * \param AXIS
+ * \param motionParam
+ * \return
*/
HSI_STATUS HSI_Motion::GetSingleAxisParam(int AXIS, double motionParam[5])
{
@@ -3713,7 +3691,7 @@ HSI_STATUS HSI_Motion::GetSingleAxisParam(int AXIS, double motionParam[5])
//打印数组 motionParam[5]
/* g_pLogger->SendAndFlushWithTime(L"[GetSingleAxisParam] AXIS = %d, Vel = %.4f, ACC = %.4f, DCC = %.4f, KillDec = %.4f, Jerk = %.4f\n",
AXIS,motionParam[0], motionParam[1], motionParam[2], motionParam[3], motionParam[4]);*/
- //g_pLogger->SendAndFlushWithTime(L"[GetSingleAxisParam] Out\n");
+ //g_pLogger->SendAndFlushWithTime(L"[GetSingleAxisParam] Out\n");
}
return rStatus;
@@ -3766,7 +3744,7 @@ HSI_STATUS HSI_Motion::SetSingleAxisParam(int AXIS, double motionParam[5]) //设
//打印 motionParam[5]
g_pLogger->SendAndFlushWithTime(L"[SetSingleAxisParam] AXIS = %d, Vel = %.4f, ACC = %.4f, DCC = %.4f, KillDec = %.4f, Jerk = %.4f\n",
- AXIS,motionParam[0], motionParam[1], motionParam[2], motionParam[3], motionParam[4]);
+ AXIS, motionParam[0], motionParam[1], motionParam[2], motionParam[3], motionParam[4]);
//g_pLogger->SendAndFlushWithTime(L"[SetSingleAxisParam] Out\n");
}
@@ -3776,7 +3754,7 @@ HSI_STATUS HSI_Motion::SetSingleAxisParam(int AXIS, double motionParam[5]) //设
//===========================================================================
HSI_STATUS HSI_Motion::SetPositionXyza(UINT AxisTypes, double PositionX, double PositionY, double PositionZ,
- double PositionA, HSI_MOTION_MOVE_TYPE eType, double dFlyRadius)
+ double PositionA, HSI_MOTION_MOVE_TYPE eType, double dFlyRadius)
{
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
@@ -3805,9 +3783,9 @@ HSI_STATUS HSI_Motion::SetPositionXyzCache(UINT AxisTypes, HSI_MOTION_MOVE_TYPE
//===========================================================================
/**
* \brief 获取缓存点
- * \param CacheData
- * \param DataCount
- * \return
+ * \param CacheData
+ * \param DataCount
+ * \return
*/
HSI_STATUS HSI_Motion::GetPositionXyzCache(unsigned char* CacheData, int& DataCount)
{
@@ -3819,7 +3797,7 @@ HSI_STATUS HSI_Motion::GetPositionXyzCache(unsigned char* CacheData, int& DataCo
if (m_SO7_Serial.IsOpen())
{
//发送获取点数量命令 0x01 0x05
- unsigned char m_cSendData[8] = {0};
+ unsigned char m_cSendData[8] = { 0 };
m_cSendData[0] = 0x01;
m_cSendData[1] = 0x05;
m_WriteByte = Send_Command(0, (const char*)m_cSendData, 2, 8); //期望回复8个字节，其中 4-7字节为点数量
@@ -3836,12 +3814,12 @@ HSI_STATUS HSI_Motion::GetPositionXyzCache(unsigned char* CacheData, int& DataCo
//详细 https://blog.csdn.net/hebbely/article/details/79577880
g_pLogger->SendAndFlushWithTime(L"[GetPositionXyzCache] DataCount：%d, %s\n", DataCount,
- m_SO7_Serial.HexToStr((const char*)m_SO7_Serial.m_RecvData, 8));
+ m_SO7_Serial.HexToStr((const char*)m_SO7_Serial.m_RecvData, 8));
}
else //查询失败
{
g_pLogger->SendAndFlushWithTime(L"[GetPositionXyzCache] DataCount：%d, %s\n", DataCount,
- m_SO7_Serial.HexToStr((const char*)m_SO7_Serial.m_RecvData, 8));
+ m_SO7_Serial.HexToStr((const char*)m_SO7_Serial.m_RecvData, 8));
}
}
@@ -3865,9 +3843,9 @@ HSI_STATUS HSI_Motion::GetPositionXyzCache(unsigned char* CacheData, int& DataCo
m_SO7_Serial.m_RecvData[0] = 0xff;
memcpy(CacheData, m_SO7_Serial.m_RecvData, m_SO7_Serial.m_iRecvBytes); //返回内容
- DataCount = m_SO7_Serial.m_iRecvBytes ; //返回点数
+ DataCount = m_SO7_Serial.m_iRecvBytes; //返回点数
g_pLogger->SendAndFlushWithTime(L"[GetPositionXyzCache] m_iRecvBytes ok, Points: %d\n",
- m_SO7_Serial.m_iRecvBytes);
+ m_SO7_Serial.m_iRecvBytes);
m_SO7_Serial.m_iRecvState = FALSE; //将接收标志重置
rStatus = HSI_STATUS_NORMAL;
}
@@ -3886,10 +3864,10 @@ HSI_STATUS HSI_Motion::GetPositionXyzCache(unsigned char* CacheData, int& DataCo
//===========================================================================
/**
* \brief 圆弧插补
- * \param PositionX
- * \param PositionY
- * \param PositionZ
- * \return
+ * \param PositionX
+ * \param PositionY
+ * \param PositionZ
+ * \return
*/
HSI_STATUS HSI_Motion::SetCircleInterpolate(double PositionX, double PositionY, double PositionZ)
{
@@ -3909,7 +3887,7 @@ HSI_STATUS HSI_Motion::SetCircleInterpolate(double PositionX, double PositionY,
//===========================================================================
/**
* \brief 探针接口
- * \param RetractManDist
+ * \param RetractManDist
*/
void HSI_Motion::ProbeRetractManDist(int RetractManDist)
{
@@ -3940,7 +3918,7 @@ HSI_STATUS HSI_Motion::JogProbe(UINT AxisTypes, double Speed)
m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
Sleep(10);
}
- unsigned char send_probe_data[64] = {0};
+ unsigned char send_probe_data[64] = { 0 };
unsigned char motionAxis = 0;
unsigned char motionDir = 0;
for (size_t i = 0; i < 3; i++)
@@ -3986,7 +3964,7 @@ HSI_STATUS HSI_Motion::JogProbe(UINT AxisTypes, double Speed)
m_Thread_State = HSI_THREAD_PAUSED;
JogSpeed = abs(SpeedPercent(AxisNumber, Speed, DriveSpeed, StartSpeed, AccLine, DecLine, AccCurve,
- DecCurve));
+ DecCurve));
send_probe_data[0] = CT_MOTOR;
send_probe_data[1] = CT_MOTOR_SET;
send_probe_data[2] = AxisNumber;
@@ -4045,8 +4023,8 @@ HSI_STATUS HSI_Motion::JogProbe(UINT AxisTypes, double Speed)
//===========================================================================
/**
* \brief 读取配置
- * \param GoogolIniFile
- * \return
+ * \param GoogolIniFile
+ * \return
*/
HSI_STATUS HSI_Motion::Load_EF3_Motion_Inifile(CString GoogolIniFile)
{
@@ -4056,8 +4034,8 @@ HSI_STATUS HSI_Motion::Load_EF3_Motion_Inifile(CString GoogolIniFile)
CString csAppPath = GoogolIniFile;
USES_CONVERSION;
CString temp = L"";
- CString strGear[5] = {L"GEAR0_", L"GEAR1_", L"GEAR2_", L"GEAR3_", L"GEAR4_"};
- CString axisNum[5] = {L"0", L"1", L"2", L"3", L"4"};
+ CString strGear[5] = { L"GEAR0_", L"GEAR1_", L"GEAR2_", L"GEAR3_", L"GEAR4_" };
+ CString axisNum[5] = { L"0", L"1", L"2", L"3", L"4" };
//判断Log目录是否存在，不存在就创建
if (CreateDirectory(m_AppPath + L"\\Log", nullptr))
{
@@ -4081,78 +4059,79 @@ HSI_STATUS HSI_Motion::Load_EF3_Motion_Inifile(CString GoogolIniFile)
m_precisionCount[i] =
GetPrivateProfileInt(L"PRECISION", L"PRECISION_COUNT_" + axisNum[i], 14000, csAppPath); //回家误差脉冲个数
m_precisionTime[i] = static_cast(GetPrivateProfileInt(L"PRECISION", L"PRECISION_TIME_" + axisNum[i],
- 14000,
- csAppPath)); //超时时间(0.1ms)
+ 14000,
+ csAppPath)); //超时时间(0.1ms)
GetPrivateProfileString(L"RESOLUTION", L"SCALE_RESOLUTION_" + axisNum[i], L"0.0004",
- temp.GetBufferSetLength(50), 50, csAppPath);
+ temp.GetBufferSetLength(50), 50, csAppPath);
m_Resolution[i] = (atof(T2A(temp)));//分辨率
GetPrivateProfileString(L"LIMIT", L"NEG_WORKING_LIMIT_" + axisNum[i], L"-40", temp.GetBufferSetLength(50),
- 50, csAppPath);
+ 50, csAppPath);
m_N_Work_Limit[i] = (atof(T2A(temp))); //各轴负限位
GetPrivateProfileString(L"LIMIT", L"POS_WORKING_LIMIT_" + axisNum[i], L"160", temp.GetBufferSetLength(50),
- 50, csAppPath);
+ 50, csAppPath);
m_P_Work_Limit[i] = (atof(T2A(temp)));//各轴正限位
//打印正负限位
g_pLogger->SendAndFlushWithTime(L"[Load_EF3_Motion_Inifile] m_N_Work_Limit[%d]: %.2f, m_P_Work_Limit[%d]: %.2f\n",
- i, m_N_Work_Limit[i], i, m_P_Work_Limit[i]);
+ i, m_N_Work_Limit[i], i, m_P_Work_Limit[i]);
m_Home_Time[i] = static_cast(GetPrivateProfileInt(L"HOME", L"HOME_TIME_" + axisNum[i], 1500,
- csAppPath)); //回家超时时间(0.1ms)
+ csAppPath)); //回家超时时间(0.1ms)
m_Home_AddJogGears[i] = GetPrivateProfileInt(L"HOME", L"HOME_ADD_JOGCHOICE_" + axisNum[i], 100, csAppPath);
m_Home_DecJogGears[i] = GetPrivateProfileInt(L"HOME", L"HOME_DEC_JOGCHOICE_" + axisNum[i], 100, csAppPath);
m_SetPotion_Count[i] = GetPrivateProfileInt(L"SET_POSITION_SPEED", L"SET_POSITION_COUNT_" + axisNum[i], 10,
- csAppPath);
+ csAppPath);
m_SetPotion_DriveSpeed[i] = GetPrivateProfileInt(L"SET_POSITION_SPEED",
- L"SET_POTION_DRIVESPEED_" + axisNum[i], 100, csAppPath);
+ L"SET_POTION_DRIVESPEED_" + axisNum[i], 100, csAppPath);
//m_SetPotion_DriveSpeed[i] = m_SetPotion_DriveSpeed[i] / (m_Resolution[i] * 50);
//打印定位合成速度
g_pLogger->SendAndFlushWithTime(L"[Load_EF3_Motion_Inifile] m_SetPotion_DriveSpeed[%d]: %d\n", i,
- m_SetPotion_DriveSpeed[i]);
+ m_SetPotion_DriveSpeed[i]);
m_SetPotion_Line[i] = GetPrivateProfileInt(L"SET_POSITION_SPEED", L"SET_POTION_LINE_" + axisNum[i], 100,
- csAppPath);
+ csAppPath);
m_SetPotion_StartSpeed[i] = GetPrivateProfileInt(L"SET_POSITION_SPEED",
- L"SET_POTION_STARTSPEED_" + axisNum[i], 100, csAppPath);
+ L"SET_POTION_STARTSPEED_" + axisNum[i], 100, csAppPath);
m_SetPotion_StartSpeed[i] = m_SetPotion_StartSpeed[i] / (m_Resolution[i] * 500);
m_SetPotion_Buffer[i] = GetPrivateProfileInt(L"SET_POSITION_SPEED", L"SET_POTION_BUFFER_" + axisNum[i], 105,
- csAppPath);
+ csAppPath);
m_LogIsOpen[i] = GetPrivateProfileInt(L"LOG", L"LOG_IS_OPEN_" + axisNum[i], 0, csAppPath);
m_Home_Machine_Axis[i] = GetPrivateProfileInt(L"HOME", L"HOME_MACHINE_AXIS_" + axisNum[i], 0, csAppPath);
m_Home_Pos_Axis[i] = GetPrivateProfileInt(L"HOME", L"HOME_POS_AXIS_" + axisNum[i], 0, csAppPath);
m_StopJogMode[i] = GetPrivateProfileInt(L"JOG_SPEED", L"JOG_STOP_MODE_" + axisNum[i], 0, csAppPath);
m_SpeedAdjustPeriod[i] = GetPrivateProfileInt(L"SET_POSITION_SPEED", L"SPEED_ADJUSTMENT_" + axisNum[i], 1,
- csAppPath);
+ csAppPath);
m_SpeedMax[i] = GetPrivateProfileInt(L"SET_POSITION_SPEED", L"SPEED_RUNMAX_" + axisNum[i], 150, csAppPath);
}
- for (int i = 0; i < 5; i++) // i 轴
+ for (int i = 0; i < 5; i++) // i 轴
{
for (int j = 1; j < 5; j++) //j 档位
{
GetPrivateProfileString(L"JOG_SPEED", L"JOG_DRIVESPEED_" + strGear[i] + axisNum[j], L"10",
- temp.GetBufferSetLength(50), 10, csAppPath);
+ temp.GetBufferSetLength(50), 10, csAppPath);
float speed = (atof(T2A(temp)));
//printf("spedd:%.2F %S\n",speed, L"JOG_DRIVESPEED_" + strGear[i] + axisNum[j]); //调试打印
/*m_JogDriveSpeed[j][i] = GetPrivateProfileInt(L"JOG_SPEED", L"JOG_DRIVESPEED_" , 10, csAppPath);*/
//m_JogDriveSpeed[j][i] = speed / (m_Resolution[j] * 50);//速度转换为脉冲
m_JogDriveSpeed[j][i] = speed; //直接读取速度
g_pLogger->SendAndFlushWithTime(L"[Load_EF3_Motion_Inifile] m_JogDriveSpeed[%d][%d]: %ld %ld\n", j, i,
- speed, m_JogDriveSpeed[j][i]); //打印配置文件 档位速度
+ speed, m_JogDriveSpeed[j][i]); //打印配置文件 档位速度
GetPrivateProfileString(L"JOG_SPEED", L"JOG_STARTSPEED_" + strGear[i] + axisNum[j], L"10",
- temp.GetBufferSetLength(50), 10, csAppPath);
+ temp.GetBufferSetLength(50), 10, csAppPath);
speed = (atof(T2A(temp)));
m_JogStartSpeed[j][i] = GetPrivateProfileInt(L"JOG_SPEED", L"JOG_STARTSPEED_" + strGear[i] + axisNum[j],
- 10, csAppPath);
+ 10, csAppPath);
//m_JogStartSpeed[j][i] = speed / (m_Resolution[j] * 50);
m_JogAccLine[j][i] = GetPrivateProfileInt(L"JOG_SPEED", L"JOG_LINE_" + strGear[i] + axisNum[j], 10,
- csAppPath);
+ csAppPath);
m_JogDecLine[j][i] = GetPrivateProfileInt(L"JOG_SPEED", L"JOG_LINE_" + strGear[i] + axisNum[j], 10,
- csAppPath);
+ csAppPath);
}
}
m_Jog_Auto_Focus = m_JogDriveSpeed[3][4];
+ g_pLogger->SendAndFlushWithTime(L"[Load_EF3_Motion_Inifile] m_Jog_Auto_Focus: %f\n", m_Jog_Auto_Focus); //记录自动对焦速度
m_Home_Machine_Axis[1] = GetPrivateProfileInt(L"HOME", L"HOME_MACHINE_AXIS_1", 1, csAppPath);
m_Home_Machine_Axis[2] = GetPrivateProfileInt(L"HOME", L"HOME_MACHINE_AXIS_2", 1, csAppPath);
m_Home_Machine_Axis[3] = GetPrivateProfileInt(L"HOME", L"HOME_MACHINE_AXIS_3", 1, csAppPath);
@@ -4181,52 +4160,52 @@ HSI_STATUS HSI_Motion::Load_EF3_Motion_Inifile(CString GoogolIniFile)
m_iSpeedType = GetPrivateProfileInt(L"SET_SPEED", L"SPEEDTYPE", 0, csAppPath);
- CString comNum[5] = {L"0", L"1", L"2", L"3"};
+ CString comNum[5] = { L"0", L"1", L"2", L"3" };
for (int i = 0; i < 4; i++)
{
GetPrivateProfileString(L"ROCKER_SPEED", L"ROCKER_HSTARTSPEED_" + comNum[i], L"5",
- temp.GetBufferSetLength(50), 10, csAppPath);
+ temp.GetBufferSetLength(50), 10, csAppPath);
float speed = (atof(T2A(temp)));
m_rockerHStartSpeed[i] = speed / (m_Resolution[i] * 50);
GetPrivateProfileString(L"ROCKER_SPEED", L"ROCKER_HDRIVESPEED_" + comNum[i], L"20",
- temp.GetBufferSetLength(50), 10, csAppPath);
+ temp.GetBufferSetLength(50), 10, csAppPath);
speed = (atof(T2A(temp)));
m_rockerHDriveSpeed[i] = speed / (m_Resolution[i] * 50);
GetPrivateProfileString(L"ROCKER_SPEED", L"ROCKER_LSTARTSPEED_" + comNum[i], L"2",
- temp.GetBufferSetLength(50), 10, csAppPath);
+ temp.GetBufferSetLength(50), 10, csAppPath);
speed = (atof(T2A(temp)));
m_rockerLStartSpeed[i] = speed / (m_Resolution[i] * 50);
GetPrivateProfileString(L"ROCKER_SPEED", L"ROCKER_LDRIVESPEED_" + comNum[i], L"10",
- temp.GetBufferSetLength(50), 10, csAppPath);
+ temp.GetBufferSetLength(50), 10, csAppPath);
speed = (atof(T2A(temp)));
m_rockerLDriveSpeed[i] = speed / (m_Resolution[i] * 50);
GetPrivateProfileString(L"ROCKER_SPEED", L"ROCKER_ASPEED_" + comNum[i], L"100", temp.GetBufferSetLength(50),
- 10, csAppPath);
+ 10, csAppPath);
m_rockerASpeed[i] = (atoi(T2A(temp)));
GetPrivateProfileString(L"ROCKER_SPEED", L"ROCKER_DSPEED_" + comNum[i], L"100", temp.GetBufferSetLength(50),
- 10, csAppPath);
+ 10, csAppPath);
m_rockerDSpeed[i] = (atoi(T2A(temp)));
}
- CString strAxis[10] = {L"0", L"1", L"2", L"3", L"4", L"5", L"6", L"7", L"8", L"9"};
+ CString strAxis[10] = { L"0", L"1", L"2", L"3", L"4", L"5", L"6", L"7", L"8", L"9" };
for (int i = 0; i < 10; i++)
{
m_stepPosition_L_speed[i] = GetPrivateProfileInt(L"STEPMOTORPOSITIONINIT",
- L"StepPositionLSpeed_" + strAxis[i], 1, csAppPath);
+ L"StepPositionLSpeed_" + strAxis[i], 1, csAppPath);
m_stepPosition_L_speed[i] = m_stepPosition_L_speed[i] / (m_Resolution[1] * 50);
m_stepPosition_H_speed[i] = GetPrivateProfileInt(L"STEPMOTORPOSITIONINIT",
- L"StepPositionHSpeed_" + strAxis[i], 1, csAppPath);
+ L"StepPositionHSpeed_" + strAxis[i], 1, csAppPath);
m_stepPosition_H_speed[i] = m_stepPosition_H_speed[i] / (m_Resolution[1] * 50);
m_stepPosition_Load[i] = GetPrivateProfileInt(L"STEPMOTORPOSITIONINIT", L"StepPositionLoad_" + strAxis[i],
- 1, csAppPath);
+ 1, csAppPath);
m_stepPosition_Load[i] = m_stepPosition_Load[i] / (m_Resolution[1]);
m_stepPosition_acc[i] = GetPrivateProfileInt(L"STEPMOTORPOSITIONINIT", L"StepPositionAcc_" + strAxis[i], 1,
- csAppPath);
+ csAppPath);
}
- CString SubArea[8] = {L"0", L"1", L"2", L"3", L"4", L"5", L"6", L"7"};
+ CString SubArea[8] = { L"0", L"1", L"2", L"3", L"4", L"5", L"6", L"7" };
for (int i = 0; i < 8; i++)
{
m_SixEightSubArea[i] = GetPrivateProfileInt(L"SIXEIGHTSUBAREA", L"SIXEIGHT_SUBARE_" + SubArea[i], 0,
- csAppPath);
+ csAppPath);
}
}
return rStatus;
@@ -4315,11 +4294,11 @@ HSI_STATUS HSI_Motion::Load_EF3_Config_Inifile(CString GoogolIniFile)
m_UseAxisNum = GetPrivateProfileInt(L"DEVICE", L"USE_AXISNUM", 0, csAppPath);
m_iJoyStick = GetPrivateProfileInt(L"JOYSTICK", L"JOY_STICK_TYPE", 0, csAppPath);
- CString comNum[5] = {L"0", L"1", L"2", L"3"};
+ CString comNum[5] = { L"0", L"1", L"2", L"3" };
for (int i = 0; i < 4; i++)
{
GetPrivateProfileString(L"IPADDRESS", L"IP_ADDRESS_" + comNum[i], L"192.168.0.1",
- temp.GetBufferSetLength(50), 50, csAppPath);
+ temp.GetBufferSetLength(50), 50, csAppPath);
m_IsOpenTCPIP[i] = temp;
temp = temp.Trim();
int iNext = temp.Find(':');
@@ -4339,10 +4318,10 @@ HSI_STATUS HSI_Motion::Load_EF3_Config_Inifile(CString GoogolIniFile)
//===========================================================================
/**
* \brief 读取/设置光栅尺精度
- * \param _ScaleX
- * \param _ScaleY
- * \param _ScaleZ
- * \return
+ * \param _ScaleX
+ * \param _ScaleY
+ * \param _ScaleZ
+ * \return
*/
HSI_STATUS HSI_Motion::GetScaleResolution(double& _ScaleX, double& _ScaleY, double& _ScaleZ)
{
@@ -4460,7 +4439,6 @@ void HSI_Motion::UpdateMotionState()
acsc_WaitMotionEnd(handleACS, ACSC_AXIS_8, INFINITE);
printf("\nMotion end\n");
-
//回家误差
unsigned int tempPrecision[5] = {
0, m_precisionCount[1], m_precisionCount[2], m_precisionCount[3], m_precisionCount[4]
@@ -4472,15 +4450,14 @@ void HSI_Motion::UpdateMotionState()
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};
-
+ double prfpos[5] = { 0 };
interpolationflag = true;
if (interpolationflag && m_motorType)
{
while (Count < m_SetPotion_Count[1]) //到位次数判断, 读配置文件240
{
- Sleep(2); //延时2毫秒，1秒 =1000 毫秒
+ Sleep(2); //延时2毫秒，1秒 =1000 毫秒
//获取当前位置
GetPositionXyz(HSI_MOTION_AXIS_ALL, prfpos[1], prfpos[2], prfpos[3], prfpos[0]);
@@ -4494,8 +4471,8 @@ void HSI_Motion::UpdateMotionState()
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]))
+ 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)
@@ -4573,7 +4550,7 @@ void HSI_Motion::UpdateMotionState()
}
switch (CurrentMotionState)
{
- case E_SO7_MOTION_MOVETO:
+ case E_SO7_MOTION_MOVETO:
{
g_pLogger->SendAndFlushWithTime(
L"[UpdateMotionState] Nowait CurrentMotionState E_SO7_MOTION_MOVETO\n");
@@ -4583,7 +4560,7 @@ void HSI_Motion::UpdateMotionState()
SendMsgMotionFinished(); //定位完成
break;
}
- default:
+ default:
{
g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Nowait CurrentMotionState default\n");
break;
@@ -4647,8 +4624,7 @@ void HSI_Motion::UpdateMotionStateOld()
timeoutflag = true;
break;
}
- }
- while (true);
+ } 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]
@@ -4659,7 +4635,7 @@ void HSI_Motion::UpdateMotionStateOld()
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};
+ double prfpos[5] = { 0 };
//GetPositionEncPrfMulti(1, m_EncPos, m_PrfPos, 4);
//double EncPos[5] = { 0.0 };
@@ -4673,8 +4649,8 @@ void HSI_Motion::UpdateMotionStateOld()
//目标位置 和当前位置 小于回家误差脉冲数
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]))
+ 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)
@@ -4735,7 +4711,7 @@ void HSI_Motion::UpdateMotionStateOld()
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]);
+ prfpos[1], prfpos[2], prfpos[3]);
if (timeoutflag) //定位超时
{
g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Nowait Timeout\n");
@@ -4749,20 +4725,20 @@ void HSI_Motion::UpdateMotionStateOld()
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;
- }
+ {
+ 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] Nowait CurrentMotionState default\n");
+ break;
+ }
}
}
g_pLogger->SendAndFlushWithTime(L"[UpdateMotionState] Out\n");
@@ -4928,9 +4904,9 @@ void HSI_Motion::UpdateMotionStateProbe()
{
Sleep(200);
hitFlag = false;
- double ProbeHitCompletePos[4] = {0};
+ double ProbeHitCompletePos[4] = { 0 };
GetPositionXyz(0, ProbeHitCompletePos[0], ProbeHitCompletePos[1], ProbeHitCompletePos[2],
- ProbeHitCompletePos[3]);
+ ProbeHitCompletePos[3]);
m_ijk[0] = ProbeHitCompletePos[0];
m_ijk[1] = ProbeHitCompletePos[1];
m_ijk[2] = ProbeHitCompletePos[2];
@@ -5029,10 +5005,10 @@ HSI_STATUS HSI_Motion::SpecialMotorMove(short AxisNumber, double Position)
//========================IO===================================================
/**
- * \brief
- * \param IOChannel
- * \param _Status
- * \return
+ * \brief
+ * \param IOChannel
+ * \param _Status
+ * \return
*/
HSI_STATUS HSI_Motion::GetDIOOld(UINT IOChannel, UINT& _Status)
{
@@ -5142,17 +5118,16 @@ HSI_STATUS HSI_Motion::GetDIO(UINT IOChannel, UINT& _Status)
//-----------TEST Begin------------------
_Status = 0;
-
//-----------TEST End------------------
return rStatus;
}
//===========================================================================
/**
- * \brief
- * \param IOChannel
- * \param _Status
- * \return
+ * \brief
+ * \param IOChannel
+ * \param _Status
+ * \return
*/
HSI_STATUS HSI_Motion::SetDIOOld(UINT IOChannel, UINT _Status)
{
@@ -5283,7 +5258,7 @@ HSI_STATUS HSI_Motion::GetAxisStatus(int* _Status)
//===========================================================================
/**
* \brief 暂停和关闭
- * \return
+ * \return
*/
HSI_STATUS HSI_Motion::AbortMotionOld() //需要运动实现
{
@@ -5332,7 +5307,7 @@ HSI_STATUS HSI_Motion::AbortMotion() //需要运动实现
g_IsClose = true;
g_pLogger->SendAndFlushWithTime(L"[AbortMotion] In\n");
- int Axes[] = {ACSC_AXIS_1, ACSC_AXIS_0, ACSC_AXIS_8, -1};
+ int Axes[] = { ACSC_AXIS_1, ACSC_AXIS_0, ACSC_AXIS_8, -1 };
if (handleACS != ACSC_INVALID)
{
if (!acsc_HaltM(handleACS, Axes, nullptr)) //停止JOG运动
@@ -5349,7 +5324,7 @@ HSI_STATUS HSI_Motion::AbortMotion() //需要运动实现
//===========================================================================
/**
* \brief 关闭
- * \return
+ * \return
*/
HSI_STATUS HSI_Motion::Shutdown()
{
@@ -5417,21 +5392,21 @@ HSI_STATUS HSI_Motion::Shutdown()
//===========================================================================
/**
* \brief 触发灯光
- * \param triggleNum
- * \param delayLighting
- * \param delayLightBefor
- * \param triggleMode
- * \param Intensities
- * \return
+ * \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)
+ double* Intensities)
{
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
setLightFlag = true;
- unsigned char m_SetTriggerLightData[64] = {0};
+ unsigned char m_SetTriggerLightData[64] = { 0 };
m_SetTriggerLightData[0] = CT_LIGHT;
m_SetTriggerLightData[1] = 0x03;
m_SetTriggerLightData[2] = triggleNum;
@@ -5489,8 +5464,8 @@ HSI_STATUS HSI_Motion::SetTriggerLight(int triggleNum, int delayLighting, int de
//===========================================================================
/**
* \brief 硬件触发拍照
- * \param startPoint
- * \return
+ * \param startPoint
+ * \return
*/
HSI_STATUS HSI_Motion::DCCPPStartPoint(double* startPoint)
{
@@ -5529,7 +5504,6 @@ HSI_STATUS HSI_Motion::DCCScanSetDataWithPoints(UINT AxisTypes, double speed, Po
// 其他处理逻辑
// ...
-
return rStatus;
}
@@ -5543,7 +5517,7 @@ HSI_STATUS HSI_Motion::DCCScanSetDataOld(UINT AxisTypes, HSI_SCAN_MOTION_TYPE eT
//触发的位置为相对位置,用法一般是移动到起点位置，再开始设置触发位置(相对位置)，最终设置终点位置
g_pLogger->SendAndFlushWithTime(L"[DCCScanSetData] In\n");
int axisNum;
- unsigned char m_SendDCCData[64] = {0};
+ unsigned char m_SendDCCData[64] = { 0 };
m_SendDCCData[0] = CT_MOTOR;
m_SendDCCData[1] = CT_TRIGGER_DATA;
m_SendDCCData[2] = 0x01;
@@ -5573,379 +5547,379 @@ HSI_STATUS HSI_Motion::DCCScanSetDataOld(UINT AxisTypes, HSI_SCAN_MOTION_TYPE eT
switch (eType)
{
case HSI_SCAN_MOTION_SPEC_LOCA:
+ {
+ if (dTrigDis[0] > 0.0001)
{
- if (dTrigDis[0] > 0.0001)
+ iMotionDirection = 0;
+ }
+ else
+ {
+ iMotionDirection = 1;
+ }
+
+ switch (iaxisNum)
+ {
+ case AXIS_X:
+ {
+ if (!m_IsUsePPS)
{
- iMotionDirection = 0;
+ begin_position[1] = begin_position[1] + dTrigDis[0] / m_Resolution[axisNum];
}
else
{
- iMotionDirection = 1;
+ if (iMotionDirection == 0)
+ {
+ begin_position[1] = begin_position[1] - 20;
+ }
+ else
+ {
+ begin_position[1] = begin_position[1] + 20;
+ }
}
-
- switch (iaxisNum)
+ if (iMotionDirection == 0)
{
- case AXIS_X:
- {
- if (!m_IsUsePPS)
- {
- begin_position[1] = begin_position[1] + dTrigDis[0] / m_Resolution[axisNum];
- }
- else
- {
- if (iMotionDirection == 0)
- {
- begin_position[1] = begin_position[1] - 20;
- }
- else
- {
- begin_position[1] = begin_position[1] + 20;
- }
- }
- if (iMotionDirection == 0)
- {
- limit = 1000.0;
- }
- else
- {
- limit = -1000.0;
- }
- break;
- }
- case AXIS_Y:
- {
- if (!m_IsUsePPS)
- {
- begin_position[2] = begin_position[2] + dTrigDis[0] / m_Resolution[axisNum];
- }
- else
- {
- if (iMotionDirection == 0)
- {
- begin_position[2] = begin_position[2] - 20;
- }
- else
- {
- begin_position[2] = begin_position[2] + 20;
- }
- }
- if (iMotionDirection == 0)
- {
- limit = 1000.0;
- }
- else
- {
- limit = -1000.0;
- }
- break;
- }
- case AXIS_Z:
- {
- if (!m_IsUsePPS)
- {
- begin_position[3] = begin_position[3] + dTrigDis[0] / m_Resolution[axisNum];
- }
- else
- {
- if (iMotionDirection == 0)
- {
- begin_position[3] = begin_position[3] - 20;
- }
- else
- {
- begin_position[3] = begin_position[3] + 20;
- }
- }
- if (iMotionDirection == 0)
- {
- limit = 1000.0;
- }
- else
- {
- limit = -1000.0;
- }
- break;
- }
- default:
- break;
+ limit = 1000.0;
}
-
- iScanMotionType = HSI_SCAN_MOTION_SPEC_LOCA;
- if (lTrigNumber > 100)
+ else
{
- return HSI_STATUS_FAILED;
- }
- int num = lTrigNumber / 14;
- int i = 0;
- for (i = 0; i < num; i++)
- {
- m_SendDCCData[3] = i;
- for (size_t j = 14 * i, z = 4; j < 14 * (i + 1); ++j)
- {
- m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) & 0xff;
- m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 8) & 0xff;
- m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 16) & 0xff;
- m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 24;
- }
- m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
- Sleep(1);
- }
- if ((num > 0) && (lTrigNumber % 14 != 0))
- {
- m_SendDCCData[3] = i;
- int j = 0, z = 0;
- for (j = 14 * i, z = 4; j < lTrigNumber; ++j)
- {
- m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) & 0xff;
- m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 8) & 0xff;
- m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 16) & 0xff;
- m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 24;
- }
- m_SendDCCData[z++] = static_cast(limit / m_Resolution[axisNum]) & 0xff;
- m_SendDCCData[z++] = (static_cast(limit / m_Resolution[axisNum]) >> 8) & 0xff;
- m_SendDCCData[z++] = (static_cast(limit / m_Resolution[axisNum]) >> 16) & 0xff;
- m_SendDCCData[z++] = static_cast(limit / m_Resolution[axisNum]) >> 24;
- m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
- Sleep(10);
- }
- else if (num == 0)
- {
- m_SendDCCData[3] = 0;
- int j = 0, z = 0;
- for (j = 0, z = 4; j < lTrigNumber; ++j)
- {
- m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) & 0xff;
- m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 8) & 0xff;
- m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 16) & 0xff;
- m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 24;
- }
- m_SendDCCData[z++] = static_cast(limit / m_Resolution[axisNum]) & 0xff;
- m_SendDCCData[z++] = (static_cast(limit / m_Resolution[axisNum]) >> 8) & 0xff;
- m_SendDCCData[z++] = (static_cast(limit / m_Resolution[axisNum]) >> 16) & 0xff;
- m_SendDCCData[z++] = static_cast(limit / m_Resolution[axisNum]) >> 24;
- m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
- Sleep(10);
+ limit = -1000.0;
}
break;
}
+ case AXIS_Y:
+ {
+ if (!m_IsUsePPS)
+ {
+ begin_position[2] = begin_position[2] + dTrigDis[0] / m_Resolution[axisNum];
+ }
+ else
+ {
+ if (iMotionDirection == 0)
+ {
+ begin_position[2] = begin_position[2] - 20;
+ }
+ else
+ {
+ begin_position[2] = begin_position[2] + 20;
+ }
+ }
+ if (iMotionDirection == 0)
+ {
+ limit = 1000.0;
+ }
+ else
+ {
+ limit = -1000.0;
+ }
+ break;
+ }
+ case AXIS_Z:
+ {
+ if (!m_IsUsePPS)
+ {
+ begin_position[3] = begin_position[3] + dTrigDis[0] / m_Resolution[axisNum];
+ }
+ else
+ {
+ if (iMotionDirection == 0)
+ {
+ begin_position[3] = begin_position[3] - 20;
+ }
+ else
+ {
+ begin_position[3] = begin_position[3] + 20;
+ }
+ }
+ if (iMotionDirection == 0)
+ {
+ limit = 1000.0;
+ }
+ else
+ {
+ limit = -1000.0;
+ }
+ break;
+ }
+ default:
+ break;
+ }
+
+ iScanMotionType = HSI_SCAN_MOTION_SPEC_LOCA;
+ if (lTrigNumber > 100)
+ {
+ return HSI_STATUS_FAILED;
+ }
+ int num = lTrigNumber / 14;
+ int i = 0;
+ for (i = 0; i < num; i++)
+ {
+ m_SendDCCData[3] = i;
+ for (size_t j = 14 * i, z = 4; j < 14 * (i + 1); ++j)
+ {
+ m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) & 0xff;
+ m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 8) & 0xff;
+ m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 16) & 0xff;
+ m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 24;
+ }
+ m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
+ Sleep(1);
+ }
+ if ((num > 0) && (lTrigNumber % 14 != 0))
+ {
+ m_SendDCCData[3] = i;
+ int j = 0, z = 0;
+ for (j = 14 * i, z = 4; j < lTrigNumber; ++j)
+ {
+ m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) & 0xff;
+ m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 8) & 0xff;
+ m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 16) & 0xff;
+ m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 24;
+ }
+ m_SendDCCData[z++] = static_cast(limit / m_Resolution[axisNum]) & 0xff;
+ m_SendDCCData[z++] = (static_cast(limit / m_Resolution[axisNum]) >> 8) & 0xff;
+ m_SendDCCData[z++] = (static_cast(limit / m_Resolution[axisNum]) >> 16) & 0xff;
+ m_SendDCCData[z++] = static_cast(limit / m_Resolution[axisNum]) >> 24;
+ m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
+ Sleep(10);
+ }
+ else if (num == 0)
+ {
+ m_SendDCCData[3] = 0;
+ int j = 0, z = 0;
+ for (j = 0, z = 4; j < lTrigNumber; ++j)
+ {
+ m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) & 0xff;
+ m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 8) & 0xff;
+ m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 16) & 0xff;
+ m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 24;
+ }
+ m_SendDCCData[z++] = static_cast(limit / m_Resolution[axisNum]) & 0xff;
+ m_SendDCCData[z++] = (static_cast(limit / m_Resolution[axisNum]) >> 8) & 0xff;
+ m_SendDCCData[z++] = (static_cast(limit / m_Resolution[axisNum]) >> 16) & 0xff;
+ m_SendDCCData[z++] = static_cast(limit / m_Resolution[axisNum]) >> 24;
+ m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
+ Sleep(10);
+ }
+ break;
+ }
case HSI_SCAN_MOTION_EQ_DIS:
+ {
+ if (dTrigDis[0] > 0.0001)
{
- if (dTrigDis[0] > 0.0001)
+ iMotionDirection = 0;
+ }
+ else
+ {
+ iMotionDirection = 1;
+ }
+ switch (iaxisNum)
+ {
+ case AXIS_X:
+ {
+ if (!m_IsUsePPS)
{
- iMotionDirection = 0;
+ begin_position[1] = begin_position[1] + dTrigDis[0] / m_Resolution[axisNum];
}
else
{
- iMotionDirection = 1;
+ if (iMotionDirection == 0)
+ {
+ begin_position[1] = begin_position[1] - 20;
+ }
+ else
+ {
+ begin_position[1] = begin_position[1] + 20;
+ }
}
- switch (iaxisNum)
+ if (iMotionDirection == 0)
{
- case AXIS_X:
- {
- if (!m_IsUsePPS)
- {
- begin_position[1] = begin_position[1] + dTrigDis[0] / m_Resolution[axisNum];
- }
- else
- {
- if (iMotionDirection == 0)
- {
- begin_position[1] = begin_position[1] - 20;
- }
- else
- {
- begin_position[1] = begin_position[1] + 20;
- }
- }
- if (iMotionDirection == 0)
- {
- limit = 1000;
- }
- else
- {
- limit = -1000;
- }
- break;
- }
- case AXIS_Y:
- {
- if (!m_IsUsePPS)
- {
- begin_position[2] = begin_position[2] + dTrigDis[0] / m_Resolution[axisNum];
- }
- else
- {
- if (iMotionDirection == 0)
- {
- begin_position[2] = begin_position[2] - 20;
- }
- else
- {
- begin_position[2] = begin_position[2] + 20;
- }
- }
- if (iMotionDirection == 0)
- {
- limit = 1000;
- }
- else
- {
- limit = -1000;
- }
- break;
- }
- case AXIS_Z:
- {
- if (!m_IsUsePPS)
- {
- begin_position[3] = begin_position[3] + dTrigDis[0] / m_Resolution[axisNum];
- }
- else
- {
- if (iMotionDirection == 0)
- {
- begin_position[3] = begin_position[3] - 20;
- }
- else
- {
- begin_position[3] = begin_position[3] + 20;
- }
- }
- if (iMotionDirection == 0)
- {
- limit = 1000;
- }
- else
- {
- limit = -1000;
- }
- break;
- }
- default:
- break;
+ limit = 1000;
+ }
+ else
+ {
+ limit = -1000;
}
- iScanMotionType = HSI_SCAN_MOTION_EQ_DIS;
- m_SendDCCData[3] = 0x00;
- m_SendDCCData[4] = static_cast(dTrigDis[0] / m_Resolution[axisNum]) & 0xff;
- m_SendDCCData[5] = (static_cast(dTrigDis[0] / m_Resolution[axisNum]) >> 8) & 0xff;
- m_SendDCCData[6] = (static_cast(dTrigDis[0] / m_Resolution[axisNum]) >> 16) & 0xff;
- m_SendDCCData[7] = static_cast(dTrigDis[0] / m_Resolution[axisNum]) >> 24;
- m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
- Sleep(10);
break;
}
+ case AXIS_Y:
+ {
+ if (!m_IsUsePPS)
+ {
+ begin_position[2] = begin_position[2] + dTrigDis[0] / m_Resolution[axisNum];
+ }
+ else
+ {
+ if (iMotionDirection == 0)
+ {
+ begin_position[2] = begin_position[2] - 20;
+ }
+ else
+ {
+ begin_position[2] = begin_position[2] + 20;
+ }
+ }
+ if (iMotionDirection == 0)
+ {
+ limit = 1000;
+ }
+ else
+ {
+ limit = -1000;
+ }
+ break;
+ }
+ case AXIS_Z:
+ {
+ if (!m_IsUsePPS)
+ {
+ begin_position[3] = begin_position[3] + dTrigDis[0] / m_Resolution[axisNum];
+ }
+ else
+ {
+ if (iMotionDirection == 0)
+ {
+ begin_position[3] = begin_position[3] - 20;
+ }
+ else
+ {
+ begin_position[3] = begin_position[3] + 20;
+ }
+ }
+ if (iMotionDirection == 0)
+ {
+ limit = 1000;
+ }
+ else
+ {
+ limit = -1000;
+ }
+ break;
+ }
+ default:
+ break;
+ }
+ iScanMotionType = HSI_SCAN_MOTION_EQ_DIS;
+ m_SendDCCData[3] = 0x00;
+ m_SendDCCData[4] = static_cast(dTrigDis[0] / m_Resolution[axisNum]) & 0xff;
+ m_SendDCCData[5] = (static_cast(dTrigDis[0] / m_Resolution[axisNum]) >> 8) & 0xff;
+ m_SendDCCData[6] = (static_cast(dTrigDis[0] / m_Resolution[axisNum]) >> 16) & 0xff;
+ m_SendDCCData[7] = static_cast(dTrigDis[0] / m_Resolution[axisNum]) >> 24;
+ m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
+ Sleep(10);
+ break;
+ }
case HSI_SCAN_MOTION_EQ_DIS_II:
+ {
+ if (dTrigDis[1] > 0.0001)
{
- if (dTrigDis[1] > 0.0001)
+ iMotionDirection = 0;
+ }
+ else
+ {
+ iMotionDirection = 1;
+ }
+
+ switch (iaxisNum)
+ {
+ case AXIS_X:
+ {
+ if (!m_IsUsePPS)
{
- iMotionDirection = 0;
+ begin_position[1] = begin_position[1] + dTrigDis[0] / m_Resolution[axisNum];
}
else
{
- iMotionDirection = 1;
+ if (iMotionDirection == 0)
+ {
+ begin_position[1] = begin_position[1] - 20;
+ }
+ else
+ {
+ begin_position[1] = begin_position[1] + 20;
+ }
}
-
- switch (iaxisNum)
+ if (iMotionDirection == 0)
{
- case AXIS_X:
- {
- if (!m_IsUsePPS)
- {
- begin_position[1] = begin_position[1] + dTrigDis[0] / m_Resolution[axisNum];
- }
- else
- {
- if (iMotionDirection == 0)
- {
- begin_position[1] = begin_position[1] - 20;
- }
- else
- {
- begin_position[1] = begin_position[1] + 20;
- }
- }
- if (iMotionDirection == 0)
- {
- limit = 1000;
- }
- else
- {
- limit = -1000;
- }
- break;
- }
- case AXIS_Y:
- {
- if (!m_IsUsePPS)
- {
- begin_position[2] = begin_position[2] + dTrigDis[0] / m_Resolution[axisNum];
- }
- else
- {
- if (iMotionDirection == 0)
- {
- begin_position[2] = begin_position[2] - 20;
- }
- else
- {
- begin_position[2] = begin_position[2] + 20;
- }
- }
- if (iMotionDirection == 0)
- {
- limit = 1000;
- }
- else
- {
- limit = -1000;
- }
- break;
- }
- case AXIS_Z:
- {
- if (!m_IsUsePPS)
- {
- begin_position[3] = begin_position[3] + dTrigDis[0] / m_Resolution[axisNum];
- }
- else
- {
- if (iMotionDirection == 0)
- {
- begin_position[3] = begin_position[3] - 20;
- }
- else
- {
- begin_position[3] = begin_position[3] + 20;
- }
- }
- if (iMotionDirection == 0)
- {
- limit = 1000;
- }
- else
- {
- limit = -1000;
- }
- break;
- }
- default:
- break;
+ limit = 1000;
}
-
- iScanMotionType = HSI_SCAN_MOTION_EQ_DIS_II;
- m_SendDCCData[3] = 0x00;
- int i = 0, j = 0;
- for (i = 0, j = 4; i < 2; ++i)
+ else
{
- m_SendDCCData[j++] = static_cast(dTrigDis[i] / m_Resolution[axisNum]) & 0xff;
- m_SendDCCData[j++] = (static_cast(dTrigDis[i] / m_Resolution[axisNum]) >> 8) & 0xff;
- m_SendDCCData[j++] = (static_cast(dTrigDis[i] / m_Resolution[axisNum]) >> 16) & 0xff;
- m_SendDCCData[j++] = static_cast(dTrigDis[i] / m_Resolution[axisNum]) >> 24;
+ limit = -1000;
}
- m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
- Sleep(10);
break;
}
+ case AXIS_Y:
+ {
+ if (!m_IsUsePPS)
+ {
+ begin_position[2] = begin_position[2] + dTrigDis[0] / m_Resolution[axisNum];
+ }
+ else
+ {
+ if (iMotionDirection == 0)
+ {
+ begin_position[2] = begin_position[2] - 20;
+ }
+ else
+ {
+ begin_position[2] = begin_position[2] + 20;
+ }
+ }
+ if (iMotionDirection == 0)
+ {
+ limit = 1000;
+ }
+ else
+ {
+ limit = -1000;
+ }
+ break;
+ }
+ case AXIS_Z:
+ {
+ if (!m_IsUsePPS)
+ {
+ begin_position[3] = begin_position[3] + dTrigDis[0] / m_Resolution[axisNum];
+ }
+ else
+ {
+ if (iMotionDirection == 0)
+ {
+ begin_position[3] = begin_position[3] - 20;
+ }
+ else
+ {
+ begin_position[3] = begin_position[3] + 20;
+ }
+ }
+ if (iMotionDirection == 0)
+ {
+ limit = 1000;
+ }
+ else
+ {
+ limit = -1000;
+ }
+ break;
+ }
+ default:
+ break;
+ }
+
+ iScanMotionType = HSI_SCAN_MOTION_EQ_DIS_II;
+ m_SendDCCData[3] = 0x00;
+ int i = 0, j = 0;
+ for (i = 0, j = 4; i < 2; ++i)
+ {
+ m_SendDCCData[j++] = static_cast(dTrigDis[i] / m_Resolution[axisNum]) & 0xff;
+ m_SendDCCData[j++] = (static_cast(dTrigDis[i] / m_Resolution[axisNum]) >> 8) & 0xff;
+ m_SendDCCData[j++] = (static_cast(dTrigDis[i] / m_Resolution[axisNum]) >> 16) & 0xff;
+ m_SendDCCData[j++] = static_cast(dTrigDis[i] / m_Resolution[axisNum]) >> 24;
+ }
+ m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
+ Sleep(10);
+ break;
+ }
default:
break;
}
@@ -5963,9 +5937,9 @@ HSI_STATUS HSI_Motion::DCCScanSetData(UINT AxisTypes, HSI_SCAN_MOTION_TYPE eType
{
//触发的位置为相对位置,用法一般是移动到起点位置，再开始设置触发位置(相对位置)，最终设置终点位置
g_pLogger->SendAndFlushWithTime(L"[DCCScanSetData] In AxisTypes:%d eType:%d lTrigNumber:%d dTrigDis:ld\n",
- AxisTypes, eType, lTrigNumber, dTrigDis);
+ AxisTypes, eType, lTrigNumber, dTrigDis);
int axisNum;
- unsigned char m_SendDCCData[64] = {0};
+ unsigned char m_SendDCCData[64] = { 0 };
m_SendDCCData[0] = CT_MOTOR;
m_SendDCCData[1] = CT_TRIGGER_DATA;
m_SendDCCData[2] = 0x01;
@@ -5995,387 +5969,387 @@ HSI_STATUS HSI_Motion::DCCScanSetData(UINT AxisTypes, HSI_SCAN_MOTION_TYPE eType
switch (eType)
{
case HSI_SCAN_MOTION_SPEC_LOCA:
+ {
+ if (dTrigDis[0] > 0.0001)
{
- if (dTrigDis[0] > 0.0001)
+ iMotionDirection = 0;
+ }
+ else
+ {
+ iMotionDirection = 1;
+ }
+
+ switch (iaxisNum)
+ {
+ case AXIS_X:
+ {
+ if (!m_IsUsePPS)
{
- iMotionDirection = 0;
+ begin_position[1] = begin_position[1] + dTrigDis[0] / m_Resolution[axisNum];
}
else
{
- iMotionDirection = 1;
+ if (iMotionDirection == 0)
+ {
+ begin_position[1] = begin_position[1] - 20;
+ }
+ else
+ {
+ begin_position[1] = begin_position[1] + 20;
+ }
}
-
- switch (iaxisNum)
+ if (iMotionDirection == 0)
{
- case AXIS_X:
- {
- if (!m_IsUsePPS)
- {
- begin_position[1] = begin_position[1] + dTrigDis[0] / m_Resolution[axisNum];
- }
- else
- {
- if (iMotionDirection == 0)
- {
- begin_position[1] = begin_position[1] - 20;
- }
- else
- {
- begin_position[1] = begin_position[1] + 20;
- }
- }
- if (iMotionDirection == 0)
- {
- limit = 1000.0;
- }
- else
- {
- limit = -1000.0;
- }
- break;
- }
- case AXIS_Y:
- {
- if (!m_IsUsePPS)
- {
- begin_position[2] = begin_position[2] + dTrigDis[0] / m_Resolution[axisNum];
- }
- else
- {
- if (iMotionDirection == 0)
- {
- begin_position[2] = begin_position[2] - 20;
- }
- else
- {
- begin_position[2] = begin_position[2] + 20;
- }
- }
- if (iMotionDirection == 0)
- {
- limit = 1000.0;
- }
- else
- {
- limit = -1000.0;
- }
- break;
- }
- case AXIS_Z:
- {
- if (!m_IsUsePPS)
- {
- begin_position[3] = begin_position[3] + dTrigDis[0] / m_Resolution[axisNum];
- }
- else
- {
- if (iMotionDirection == 0)
- {
- begin_position[3] = begin_position[3] - 20;
- }
- else
- {
- begin_position[3] = begin_position[3] + 20;
- }
- }
- if (iMotionDirection == 0)
- {
- limit = 1000.0;
- }
- else
- {
- limit = -1000.0;
- }
- break;
- }
- default:
- break;
+ limit = 1000.0;
}
-
- iScanMotionType = HSI_SCAN_MOTION_SPEC_LOCA;
- if (lTrigNumber > 100)
+ else
{
- return HSI_STATUS_FAILED;
- }
- int num = lTrigNumber / 14;
- int i = 0;
- for (i = 0; i < num; i++)
- {
- m_SendDCCData[3] = i;
- for (size_t j = 14 * i, z = 4; j < 14 * (i + 1); ++j)
- {
- m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) & 0xff;
- m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 8) & 0xff;
- m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 16) & 0xff;
- m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 24;
- }
- //m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
- Sleep(1);
- }
- if ((num > 0) && (lTrigNumber % 14 != 0))
- {
- m_SendDCCData[3] = i;
- int j = 0, z = 0;
- for (j = 14 * i, z = 4; j < lTrigNumber; ++j)
- {
- m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) & 0xff;
- m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 8) & 0xff;
- m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 16) & 0xff;
- m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 24;
- }
- m_SendDCCData[z++] = static_cast(limit / m_Resolution[axisNum]) & 0xff;
- m_SendDCCData[z++] = (static_cast(limit / m_Resolution[axisNum]) >> 8) & 0xff;
- m_SendDCCData[z++] = (static_cast(limit / m_Resolution[axisNum]) >> 16) & 0xff;
- m_SendDCCData[z++] = static_cast(limit / m_Resolution[axisNum]) >> 24;
- //m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
- Sleep(10);
- }
- else if (num == 0)
- {
- m_SendDCCData[3] = 0;
- int j = 0, z = 0;
- for (j = 0, z = 4; j < lTrigNumber; ++j)
- {
- m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) & 0xff;
- m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 8) & 0xff;
- m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 16) & 0xff;
- m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 24;
- }
- m_SendDCCData[z++] = static_cast(limit / m_Resolution[axisNum]) & 0xff;
- m_SendDCCData[z++] = (static_cast(limit / m_Resolution[axisNum]) >> 8) & 0xff;
- m_SendDCCData[z++] = (static_cast(limit / m_Resolution[axisNum]) >> 16) & 0xff;
- m_SendDCCData[z++] = static_cast(limit / m_Resolution[axisNum]) >> 24;
- //m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
- Sleep(10);
+ limit = -1000.0;
}
break;
}
+ case AXIS_Y:
+ {
+ if (!m_IsUsePPS)
+ {
+ begin_position[2] = begin_position[2] + dTrigDis[0] / m_Resolution[axisNum];
+ }
+ else
+ {
+ if (iMotionDirection == 0)
+ {
+ begin_position[2] = begin_position[2] - 20;
+ }
+ else
+ {
+ begin_position[2] = begin_position[2] + 20;
+ }
+ }
+ if (iMotionDirection == 0)
+ {
+ limit = 1000.0;
+ }
+ else
+ {
+ limit = -1000.0;
+ }
+ break;
+ }
+ case AXIS_Z:
+ {
+ if (!m_IsUsePPS)
+ {
+ begin_position[3] = begin_position[3] + dTrigDis[0] / m_Resolution[axisNum];
+ }
+ else
+ {
+ if (iMotionDirection == 0)
+ {
+ begin_position[3] = begin_position[3] - 20;
+ }
+ else
+ {
+ begin_position[3] = begin_position[3] + 20;
+ }
+ }
+ if (iMotionDirection == 0)
+ {
+ limit = 1000.0;
+ }
+ else
+ {
+ limit = -1000.0;
+ }
+ break;
+ }
+ default:
+ break;
+ }
+
+ iScanMotionType = HSI_SCAN_MOTION_SPEC_LOCA;
+ if (lTrigNumber > 100)
+ {
+ return HSI_STATUS_FAILED;
+ }
+ int num = lTrigNumber / 14;
+ int i = 0;
+ for (i = 0; i < num; i++)
+ {
+ m_SendDCCData[3] = i;
+ for (size_t j = 14 * i, z = 4; j < 14 * (i + 1); ++j)
+ {
+ m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) & 0xff;
+ m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 8) & 0xff;
+ m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 16) & 0xff;
+ m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 24;
+ }
+ //m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
+ Sleep(1);
+ }
+ if ((num > 0) && (lTrigNumber % 14 != 0))
+ {
+ m_SendDCCData[3] = i;
+ int j = 0, z = 0;
+ for (j = 14 * i, z = 4; j < lTrigNumber; ++j)
+ {
+ m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) & 0xff;
+ m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 8) & 0xff;
+ m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 16) & 0xff;
+ m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 24;
+ }
+ m_SendDCCData[z++] = static_cast(limit / m_Resolution[axisNum]) & 0xff;
+ m_SendDCCData[z++] = (static_cast(limit / m_Resolution[axisNum]) >> 8) & 0xff;
+ m_SendDCCData[z++] = (static_cast(limit / m_Resolution[axisNum]) >> 16) & 0xff;
+ m_SendDCCData[z++] = static_cast(limit / m_Resolution[axisNum]) >> 24;
+ //m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
+ Sleep(10);
+ }
+ else if (num == 0)
+ {
+ m_SendDCCData[3] = 0;
+ int j = 0, z = 0;
+ for (j = 0, z = 4; j < lTrigNumber; ++j)
+ {
+ m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) & 0xff;
+ m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 8) & 0xff;
+ m_SendDCCData[z++] = (static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 16) & 0xff;
+ m_SendDCCData[z++] = static_cast(dTrigDis[j] / m_Resolution[axisNum]) >> 24;
+ }
+ m_SendDCCData[z++] = static_cast(limit / m_Resolution[axisNum]) & 0xff;
+ m_SendDCCData[z++] = (static_cast(limit / m_Resolution[axisNum]) >> 8) & 0xff;
+ m_SendDCCData[z++] = (static_cast(limit / m_Resolution[axisNum]) >> 16) & 0xff;
+ m_SendDCCData[z++] = static_cast(limit / m_Resolution[axisNum]) >> 24;
+ //m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
+ Sleep(10);
+ }
+ break;
+ }
case HSI_SCAN_MOTION_EQ_DIS:
+ {
+ if (dTrigDis[0] > 0.0001)
{
- if (dTrigDis[0] > 0.0001)
+ iMotionDirection = 0;
+ }
+ else
+ {
+ iMotionDirection = 1;
+ }
+ switch (iaxisNum)
+ {
+ case AXIS_X:
+ {
+ if (!m_IsUsePPS)
{
- iMotionDirection = 0;
+ begin_position[1] = begin_position[1] + dTrigDis[0] / m_Resolution[axisNum];
}
else
{
- iMotionDirection = 1;
+ if (iMotionDirection == 0)
+ {
+ begin_position[1] = begin_position[1] - 20;
+ }
+ else
+ {
+ begin_position[1] = begin_position[1] + 20;
+ }
}
- switch (iaxisNum)
+ if (iMotionDirection == 0)
{
- case AXIS_X:
- {
- if (!m_IsUsePPS)
- {
- begin_position[1] = begin_position[1] + dTrigDis[0] / m_Resolution[axisNum];
- }
- else
- {
- if (iMotionDirection == 0)
- {
- begin_position[1] = begin_position[1] - 20;
- }
- else
- {
- begin_position[1] = begin_position[1] + 20;
- }
- }
- if (iMotionDirection == 0)
- {
- limit = 1000;
- }
- else
- {
- limit = -1000;
- }
- break;
- }
- case AXIS_Y:
- {
- if (!m_IsUsePPS)
- {
- begin_position[2] = begin_position[2] + dTrigDis[0] / m_Resolution[axisNum];
- }
- else
- {
- if (iMotionDirection == 0)
- {
- begin_position[2] = begin_position[2] - 20;
- }
- else
- {
- begin_position[2] = begin_position[2] + 20;
- }
- }
- if (iMotionDirection == 0)
- {
- limit = 1000;
- }
- else
- {
- limit = -1000;
- }
- break;
- }
- case AXIS_Z:
- {
- if (!m_IsUsePPS)
- {
- begin_position[3] = begin_position[3] + dTrigDis[0] / m_Resolution[axisNum];
- }
- else
- {
- if (iMotionDirection == 0)
- {
- begin_position[3] = begin_position[3] - 20;
- }
- else
- {
- begin_position[3] = begin_position[3] + 20;
- }
- }
- if (iMotionDirection == 0)
- {
- limit = 1000;
- }
- else
- {
- limit = -1000;
- }
- break;
- }
- default:
- break;
+ limit = 1000;
+ }
+ else
+ {
+ limit = -1000;
}
- iScanMotionType = HSI_SCAN_MOTION_EQ_DIS;
- m_SendDCCData[3] = 0x00;
- m_SendDCCData[4] = static_cast(dTrigDis[0] / m_Resolution[axisNum]) & 0xff;
- m_SendDCCData[5] = (static_cast(dTrigDis[0] / m_Resolution[axisNum]) >> 8) & 0xff;
- m_SendDCCData[6] = (static_cast(dTrigDis[0] / m_Resolution[axisNum]) >> 16) & 0xff;
- m_SendDCCData[7] = static_cast(dTrigDis[0] / m_Resolution[axisNum]) >> 24;
- //m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
- Sleep(10);
break;
}
+ case AXIS_Y:
+ {
+ if (!m_IsUsePPS)
+ {
+ begin_position[2] = begin_position[2] + dTrigDis[0] / m_Resolution[axisNum];
+ }
+ else
+ {
+ if (iMotionDirection == 0)
+ {
+ begin_position[2] = begin_position[2] - 20;
+ }
+ else
+ {
+ begin_position[2] = begin_position[2] + 20;
+ }
+ }
+ if (iMotionDirection == 0)
+ {
+ limit = 1000;
+ }
+ else
+ {
+ limit = -1000;
+ }
+ break;
+ }
+ case AXIS_Z:
+ {
+ if (!m_IsUsePPS)
+ {
+ begin_position[3] = begin_position[3] + dTrigDis[0] / m_Resolution[axisNum];
+ }
+ else
+ {
+ if (iMotionDirection == 0)
+ {
+ begin_position[3] = begin_position[3] - 20;
+ }
+ else
+ {
+ begin_position[3] = begin_position[3] + 20;
+ }
+ }
+ if (iMotionDirection == 0)
+ {
+ limit = 1000;
+ }
+ else
+ {
+ limit = -1000;
+ }
+ break;
+ }
+ default:
+ break;
+ }
+ iScanMotionType = HSI_SCAN_MOTION_EQ_DIS;
+ m_SendDCCData[3] = 0x00;
+ m_SendDCCData[4] = static_cast(dTrigDis[0] / m_Resolution[axisNum]) & 0xff;
+ m_SendDCCData[5] = (static_cast(dTrigDis[0] / m_Resolution[axisNum]) >> 8) & 0xff;
+ m_SendDCCData[6] = (static_cast(dTrigDis[0] / m_Resolution[axisNum]) >> 16) & 0xff;
+ m_SendDCCData[7] = static_cast(dTrigDis[0] / m_Resolution[axisNum]) >> 24;
+ //m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
+ Sleep(10);
+ break;
+ }
case HSI_SCAN_MOTION_EQ_DIS_II:
+ {
+ if (dTrigDis[1] > 0.0001)
{
- if (dTrigDis[1] > 0.0001)
+ iMotionDirection = 0;
+ }
+ else
+ {
+ iMotionDirection = 1;
+ }
+
+ switch (iaxisNum)
+ {
+ case AXIS_X:
+ {
+ if (!m_IsUsePPS)
{
- iMotionDirection = 0;
+ begin_position[1] = begin_position[1] + dTrigDis[0] / m_Resolution[axisNum];
}
else
{
- iMotionDirection = 1;
+ if (iMotionDirection == 0)
+ {
+ begin_position[1] = begin_position[1] - 20;
+ }
+ else
+ {
+ begin_position[1] = begin_position[1] + 20;
+ }
}
-
- switch (iaxisNum)
+ if (iMotionDirection == 0)
{
- case AXIS_X:
- {
- if (!m_IsUsePPS)
- {
- begin_position[1] = begin_position[1] + dTrigDis[0] / m_Resolution[axisNum];
- }
- else
- {
- if (iMotionDirection == 0)
- {
- begin_position[1] = begin_position[1] - 20;
- }
- else
- {
- begin_position[1] = begin_position[1] + 20;
- }
- }
- if (iMotionDirection == 0)
- {
- limit = 1000;
- }
- else
- {
- limit = -1000;
- }
- break;
- }
- case AXIS_Y:
- {
- if (!m_IsUsePPS)
- {
- begin_position[2] = begin_position[2] + dTrigDis[0] / m_Resolution[axisNum];
- }
- else
- {
- if (iMotionDirection == 0)
- {
- begin_position[2] = begin_position[2] - 20;
- }
- else
- {
- begin_position[2] = begin_position[2] + 20;
- }
- }
- if (iMotionDirection == 0)
- {
- limit = 1000;
- }
- else
- {
- limit = -1000;
- }
- break;
- }
- case AXIS_Z:
- {
- if (!m_IsUsePPS)
- {
- begin_position[3] = begin_position[3] + dTrigDis[0] / m_Resolution[axisNum];
- }
- else
- {
- if (iMotionDirection == 0)
- {
- begin_position[3] = begin_position[3] - 20;
- }
- else
- {
- begin_position[3] = begin_position[3] + 20;
- }
- }
- if (iMotionDirection == 0)
- {
- limit = 1000;
- }
- else
- {
- limit = -1000;
- }
- break;
- }
- default:
- break;
+ limit = 1000;
}
-
- iScanMotionType = HSI_SCAN_MOTION_EQ_DIS_II;
- m_SendDCCData[3] = 0x00;
- int i = 0, j = 0;
- for (i = 0, j = 4; i < 2; ++i)
+ else
{
- m_SendDCCData[j++] = static_cast(dTrigDis[i] / m_Resolution[axisNum]) & 0xff;
- m_SendDCCData[j++] = (static_cast(dTrigDis[i] / m_Resolution[axisNum]) >> 8) & 0xff;
- m_SendDCCData[j++] = (static_cast(dTrigDis[i] / m_Resolution[axisNum]) >> 16) & 0xff;
- m_SendDCCData[j++] = static_cast(dTrigDis[i] / m_Resolution[axisNum]) >> 24;
+ limit = -1000;
}
- //m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
- Sleep(10);
break;
}
+ case AXIS_Y:
+ {
+ if (!m_IsUsePPS)
+ {
+ begin_position[2] = begin_position[2] + dTrigDis[0] / m_Resolution[axisNum];
+ }
+ else
+ {
+ if (iMotionDirection == 0)
+ {
+ begin_position[2] = begin_position[2] - 20;
+ }
+ else
+ {
+ begin_position[2] = begin_position[2] + 20;
+ }
+ }
+ if (iMotionDirection == 0)
+ {
+ limit = 1000;
+ }
+ else
+ {
+ limit = -1000;
+ }
+ break;
+ }
+ case AXIS_Z:
+ {
+ if (!m_IsUsePPS)
+ {
+ begin_position[3] = begin_position[3] + dTrigDis[0] / m_Resolution[axisNum];
+ }
+ else
+ {
+ if (iMotionDirection == 0)
+ {
+ begin_position[3] = begin_position[3] - 20;
+ }
+ else
+ {
+ begin_position[3] = begin_position[3] + 20;
+ }
+ }
+ if (iMotionDirection == 0)
+ {
+ limit = 1000;
+ }
+ else
+ {
+ limit = -1000;
+ }
+ break;
+ }
+ default:
+ break;
+ }
+
+ iScanMotionType = HSI_SCAN_MOTION_EQ_DIS_II;
+ m_SendDCCData[3] = 0x00;
+ int i = 0, j = 0;
+ for (i = 0, j = 4; i < 2; ++i)
+ {
+ m_SendDCCData[j++] = static_cast(dTrigDis[i] / m_Resolution[axisNum]) & 0xff;
+ m_SendDCCData[j++] = (static_cast(dTrigDis[i] / m_Resolution[axisNum]) >> 8) & 0xff;
+ m_SendDCCData[j++] = (static_cast(dTrigDis[i] / m_Resolution[axisNum]) >> 16) & 0xff;
+ m_SendDCCData[j++] = static_cast(dTrigDis[i] / m_Resolution[axisNum]) >> 24;
+ }
+ //m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
+ Sleep(10);
+ break;
+ }
default:
break;
}
//启动定时锁存的同时，启动扫描 外部IO
- unsigned char m_cSendData[8] = {0};
+ unsigned char m_cSendData[8] = { 0 };
- //清空EF3缓存Flash
+ //清空EF3缓存Flash
m_cSendData[0] = 0x01;
m_cSendData[1] = 0x04;
m_WriteByte = Send_Command(0, (const char*)m_cSendData, 2);
@@ -6395,7 +6369,7 @@ HSI_STATUS HSI_Motion::DCCScanStartOld()
{
if (!setLightFlag)
{
- unsigned char m_SetTriggerLightData[64] = {0};
+ unsigned char m_SetTriggerLightData[64] = { 0 };
m_SetTriggerLightData[0] = CT_LIGHT;
m_SetTriggerLightData[1] = 0x03;
m_SetTriggerLightData[53] = 0;
@@ -6404,7 +6378,7 @@ HSI_STATUS HSI_Motion::DCCScanStartOld()
}
setLightFlag = false;
g_pLogger->SendAndFlushWithTime(L"[DCCScanStart] In\n");
- unsigned char m_SendDCCData[64] = {0};
+ unsigned char m_SendDCCData[64] = { 0 };
m_SendDCCData[0] = CT_MOTOR;
m_SendDCCData[1] = CT_TRIGGER_DATA;
m_SendDCCData[2] = 0x00;
@@ -6457,7 +6431,7 @@ HSI_STATUS HSI_Motion::DCCScanStart()
{
if (!setLightFlag)
{
- unsigned char m_SetTriggerLightData[64] = {0};
+ unsigned char m_SetTriggerLightData[64] = { 0 };
m_SetTriggerLightData[0] = CT_LIGHT;
m_SetTriggerLightData[1] = 0x03;
m_SetTriggerLightData[53] = 0;
@@ -6466,7 +6440,7 @@ HSI_STATUS HSI_Motion::DCCScanStart()
}
setLightFlag = false;
g_pLogger->SendAndFlushWithTime(L"[DCCScanStart] In\n");
- unsigned char m_SendDCCData[64] = {0};
+ unsigned char m_SendDCCData[64] = { 0 };
m_SendDCCData[0] = CT_MOTOR;
m_SendDCCData[1] = CT_TRIGGER_DATA;
m_SendDCCData[2] = 0x00;
@@ -6508,7 +6482,7 @@ HSI_STATUS HSI_Motion::DCCScanStart()
//m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
//启动定时扫描，此处清空旧缓存值
- unsigned char m_cSendData[8] = {0};
+ unsigned char m_cSendData[8] = { 0 };
m_cSendData[0] = 0x01;
m_cSendData[1] = 0x04;
m_WriteByte = Send_Command(0, (const char*)m_cSendData, 2, 2);
@@ -6519,7 +6493,6 @@ HSI_STATUS HSI_Motion::DCCScanStart()
m_cSendData[1] = 0x02;
m_WriteByte = Send_Command(0, (const char*)m_cSendData, 2, 2);
-
g_pLogger->SendAndFlushWithTime(L"[DCCScanStart] Out\n");
m_IsUsePPS = false;
}
@@ -6535,7 +6508,7 @@ HSI_STATUS HSI_Motion::DCCScanStopOld()
if (g_pHSI_Motion)
{
g_pLogger->SendAndFlushWithTime(L"[DCCScanStop] In\n");
- unsigned char m_SendDCCData[64] = {0};
+ unsigned char m_SendDCCData[64] = { 0 };
m_SendDCCData[0] = CT_MOTOR;
m_SendDCCData[1] = CT_TRIGGER_DATA;
m_SendDCCData[2] = 0x02;
@@ -6554,7 +6527,7 @@ HSI_STATUS HSI_Motion::DCCScanStop()
if (g_pHSI_Motion)
{
g_pLogger->SendAndFlushWithTime(L"[DCCScanStop] In\n");
- unsigned char m_SendDCCData[64] = {0};
+ unsigned char m_SendDCCData[64] = { 0 };
m_SendDCCData[0] = 0x01;
m_SendDCCData[1] = 0x03;
m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, 2, 2);
@@ -6603,16 +6576,16 @@ HSI_STATUS HSI_Motion::DCCForLightPlate()
//===========================================================================
/**
* \brief 转盘
- * \param CamerasDis
- * \param BinsDis
- * \param SubArea
- * \param filterTime1
- * \param filterTime2
- * \param pluseSumDis
- * \return
+ * \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)
+ int pluseSumDis)
{
g_pLogger->SendAndFlushWithTime(L"[StartPlcJob] In\n");
auto rStatus = HSI_STATUS_NORMAL;
@@ -6625,7 +6598,7 @@ HSI_STATUS HSI_Motion::StartPlcJob(int* CamerasDis, int* BinsDis, int SubArea, i
sEvenProp.EventType = HSI_EVENT_ERROR;
sEvenProp.EventID = HSI_EVENT_MOTION;
sEvenProp.EventResponse = HSI_EVENT_RESPONSE_OK;
- strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "开始前请确认当前位置大于零！");
+ strcpy_s(sEvenProp.EventData, HSI_MaxStringLength, "开始前请确认当前位置大于零");
EventCallback(sEvenProp);
return HSI_STATUS_FAILED;
}
@@ -6722,21 +6695,21 @@ HSI_STATUS HSI_Motion::GetTriggleCount(int* nCount, int& nArea)
<< 8 | m_SO7_Serial.m_RecvData[9];
nCount[2] = m_SO7_Serial.m_RecvData[10] << 24 | m_SO7_Serial.m_RecvData[11] << 16 | m_SO7_Serial.m_RecvData[
12] << 8 | m_SO7_Serial.m_RecvData[13];
- nCount[3] = m_SO7_Serial.m_RecvData[14] << 24 | m_SO7_Serial.m_RecvData[15] << 16 | m_SO7_Serial.m_RecvData[
- 16] << 8 | m_SO7_Serial.m_RecvData[17];
- nCount[4] = m_SO7_Serial.m_RecvData[18] << 24 | m_SO7_Serial.m_RecvData[19] << 16 | m_SO7_Serial.m_RecvData[
- 20] << 8 | m_SO7_Serial.m_RecvData[21];
- nCount[5] = m_SO7_Serial.m_RecvData[22] << 24 | m_SO7_Serial.m_RecvData[23] << 16 | m_SO7_Serial.m_RecvData[
- 24] << 8 | m_SO7_Serial.m_RecvData[25];
- nCount[6] = m_SO7_Serial.m_RecvData[26] << 24 | m_SO7_Serial.m_RecvData[27] << 16 | m_SO7_Serial.m_RecvData[
- 28] << 8 | m_SO7_Serial.m_RecvData[29];
- nCount[7] = m_SO7_Serial.m_RecvData[30] << 24 | m_SO7_Serial.m_RecvData[31] << 16 | m_SO7_Serial.m_RecvData[
- 32] << 8 | m_SO7_Serial.m_RecvData[33];
- nCount[8] = m_SO7_Serial.m_RecvData[34] << 24 | m_SO7_Serial.m_RecvData[35] << 16 | m_SO7_Serial.m_RecvData[
- 36] << 8 | m_SO7_Serial.m_RecvData[37];
- nCount[9] = m_SO7_Serial.m_RecvData[38] << 24 | m_SO7_Serial.m_RecvData[39] << 16 | m_SO7_Serial.m_RecvData[
- 40] << 8 | m_SO7_Serial.m_RecvData[41];
- g_pLogger->SendAndFlushWithTime(L"[GetTriggleCount] nArea = %d\n", nArea);
+ nCount[3] = m_SO7_Serial.m_RecvData[14] << 24 | m_SO7_Serial.m_RecvData[15] << 16 | m_SO7_Serial.m_RecvData[
+ 16] << 8 | m_SO7_Serial.m_RecvData[17];
+ nCount[4] = m_SO7_Serial.m_RecvData[18] << 24 | m_SO7_Serial.m_RecvData[19] << 16 | m_SO7_Serial.m_RecvData[
+ 20] << 8 | m_SO7_Serial.m_RecvData[21];
+ nCount[5] = m_SO7_Serial.m_RecvData[22] << 24 | m_SO7_Serial.m_RecvData[23] << 16 | m_SO7_Serial.m_RecvData[
+ 24] << 8 | m_SO7_Serial.m_RecvData[25];
+ nCount[6] = m_SO7_Serial.m_RecvData[26] << 24 | m_SO7_Serial.m_RecvData[27] << 16 | m_SO7_Serial.m_RecvData[
+ 28] << 8 | m_SO7_Serial.m_RecvData[29];
+ nCount[7] = m_SO7_Serial.m_RecvData[30] << 24 | m_SO7_Serial.m_RecvData[31] << 16 | m_SO7_Serial.m_RecvData[
+ 32] << 8 | m_SO7_Serial.m_RecvData[33];
+ nCount[8] = m_SO7_Serial.m_RecvData[34] << 24 | m_SO7_Serial.m_RecvData[35] << 16 | m_SO7_Serial.m_RecvData[
+ 36] << 8 | m_SO7_Serial.m_RecvData[37];
+ nCount[9] = m_SO7_Serial.m_RecvData[38] << 24 | m_SO7_Serial.m_RecvData[39] << 16 | m_SO7_Serial.m_RecvData[
+ 40] << 8 | m_SO7_Serial.m_RecvData[41];
+ g_pLogger->SendAndFlushWithTime(L"[GetTriggleCount] nArea = %d\n", nArea);
}
}
return rStatus;
@@ -6746,12 +6719,12 @@ HSI_STATUS HSI_Motion::GetTriggleCount(int* nCount, int& nArea)
//点胶
//===========================================================================
HSI_STATUS HSI_Motion::GluePPSpnts(int* ppsDir, double* gluePPSPos, int* delayLightBefore, int* lightTime,
- double* lightData, int num)
+ double* lightData, int num)
{
auto rStatus = HSI_STATUS_NORMAL;
if (g_pHSI_Motion)
{
- unsigned char send_gluePPS_data[64] = {0};
+ unsigned char send_gluePPS_data[64] = { 0 };
send_gluePPS_data[0] = CT_GLUEDISPENSER;
send_gluePPS_data[1] = 0x01; //擦除指令
m_WriteByte = Send_Command(0, (const char*)send_gluePPS_data, m_SendDataLength);
@@ -6792,29 +6765,29 @@ HSI_STATUS HSI_Motion::GluePPSpnts(int* ppsDir, double* gluePPSPos, int* delayLi
switch (saveDir)
{
case 0:
- {
- axisNum = 1;
- dirnum = 2;
- break;
- }
+ {
+ axisNum = 1;
+ dirnum = 2;
+ break;
+ }
case 1:
- {
- axisNum = 1;
- dirnum = 1;
- break;
- }
+ {
+ axisNum = 1;
+ dirnum = 1;
+ break;
+ }
case 2:
- {
- axisNum = 2;
- dirnum = 2;
- break;
- }
+ {
+ axisNum = 2;
+ dirnum = 2;
+ break;
+ }
case 3:
- {
- axisNum = 2;
- dirnum = 1;
- break;
- }
+ {
+ axisNum = 2;
+ dirnum = 1;
+ break;
+ }
default:
break;
}
@@ -6831,53 +6804,53 @@ HSI_STATUS HSI_Motion::GluePPSpnts(int* ppsDir, double* gluePPSPos, int* delayLi
switch (dirType)
{
case 0:
- {
- c++;
- loadFeet[0] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]);
- loadFeet[1] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]);
- loadFeet[2] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]);
- send_gluePPS_data[j++] = (loadFeet[0] & 0xff);
- send_gluePPS_data[j++] = ((loadFeet[0] >> 8) & 0xff);
- send_gluePPS_data[j++] = ((loadFeet[0] >> 16) & 0xff);
- send_gluePPS_data[j++] = ((loadFeet[0] >> 24) & 0xff);
- break;
- }
+ {
+ c++;
+ loadFeet[0] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]);
+ loadFeet[1] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]);
+ loadFeet[2] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]);
+ send_gluePPS_data[j++] = (loadFeet[0] & 0xff);
+ send_gluePPS_data[j++] = ((loadFeet[0] >> 8) & 0xff);
+ send_gluePPS_data[j++] = ((loadFeet[0] >> 16) & 0xff);
+ send_gluePPS_data[j++] = ((loadFeet[0] >> 24) & 0xff);
+ break;
+ }
case 1:
- {
- c++;
- loadFeet[0] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]);
- loadFeet[1] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]);
- loadFeet[2] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]);
- send_gluePPS_data[j++] = (loadFeet[0] & 0xff);
- send_gluePPS_data[j++] = ((loadFeet[0] >> 8) & 0xff);
- send_gluePPS_data[j++] = ((loadFeet[0] >> 16) & 0xff);
- send_gluePPS_data[j++] = ((loadFeet[0] >> 24) & 0xff);
- break;
- }
+ {
+ c++;
+ loadFeet[0] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]);
+ loadFeet[1] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]);
+ loadFeet[2] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]);
+ send_gluePPS_data[j++] = (loadFeet[0] & 0xff);
+ send_gluePPS_data[j++] = ((loadFeet[0] >> 8) & 0xff);
+ send_gluePPS_data[j++] = ((loadFeet[0] >> 16) & 0xff);
+ send_gluePPS_data[j++] = ((loadFeet[0] >> 24) & 0xff);
+ break;
+ }
case 2:
- {
- c++;
- loadFeet[0] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]);
- loadFeet[1] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]);
- loadFeet[2] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]);
- send_gluePPS_data[j++] = (loadFeet[1] & 0xff);
- send_gluePPS_data[j++] = ((loadFeet[1] >> 8) & 0xff);
- send_gluePPS_data[j++] = ((loadFeet[1] >> 16) & 0xff);
- send_gluePPS_data[j++] = ((loadFeet[1] >> 24) & 0xff);
- break;
- }
+ {
+ c++;
+ loadFeet[0] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]);
+ loadFeet[1] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]);
+ loadFeet[2] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]);
+ send_gluePPS_data[j++] = (loadFeet[1] & 0xff);
+ send_gluePPS_data[j++] = ((loadFeet[1] >> 8) & 0xff);
+ send_gluePPS_data[j++] = ((loadFeet[1] >> 16) & 0xff);
+ send_gluePPS_data[j++] = ((loadFeet[1] >> 24) & 0xff);
+ break;
+ }
case 3:
- {
- c++;
- loadFeet[0] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]);
- loadFeet[1] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]);
- loadFeet[2] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]);
- send_gluePPS_data[j++] = (loadFeet[1] & 0xff);
- send_gluePPS_data[j++] = ((loadFeet[1] >> 8) & 0xff);
- send_gluePPS_data[j++] = ((loadFeet[1] >> 16) & 0xff);
- send_gluePPS_data[j++] = ((loadFeet[1] >> 24) & 0xff);
- break;
- }
+ {
+ c++;
+ loadFeet[0] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]);
+ loadFeet[1] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]);
+ loadFeet[2] = static_cast(gluePPSPos[posIndex++] / m_Resolution[1]);
+ send_gluePPS_data[j++] = (loadFeet[1] & 0xff);
+ send_gluePPS_data[j++] = ((loadFeet[1] >> 8) & 0xff);
+ send_gluePPS_data[j++] = ((loadFeet[1] >> 16) & 0xff);
+ send_gluePPS_data[j++] = ((loadFeet[1] >> 24) & 0xff);
+ break;
+ }
default:
break;
}
@@ -6894,29 +6867,29 @@ HSI_STATUS HSI_Motion::GluePPSpnts(int* ppsDir, double* gluePPSPos, int* delayLi
switch (saveDir)
{
case 0:
- {
- axisNum = 1;
- dirnum = 2;
- break;
- }
+ {
+ axisNum = 1;
+ dirnum = 2;
+ break;
+ }
case 1:
- {
- axisNum = 1;
- dirnum = 1;
- break;
- }
+ {
+ axisNum = 1;
+ dirnum = 1;
+ break;
+ }
case 2:
- {
- axisNum = 2;
- dirnum = 2;
- break;
- }
+ {
+ axisNum = 2;
+ dirnum = 2;
+ break;
+ }
case 3:
- {
- axisNum = 2;
- dirnum = 1;
- break;
- }
+ {
+ axisNum = 2;
+ dirnum = 1;
+ break;
+ }
default:
break;
}
@@ -6942,29 +6915,29 @@ HSI_STATUS HSI_Motion::GluePPSpnts(int* ppsDir, double* gluePPSPos, int* delayLi
switch (saveDir)
{
case 0:
- {
- axisNum = 1;
- dirnum = 2;
- break;
- }
+ {
+ axisNum = 1;
+ dirnum = 2;
+ break;
+ }
case 1:
- {
- axisNum = 1;
- dirnum = 1;
- break;
- }
+ {
+ axisNum = 1;
+ dirnum = 1;
+ break;
+ }
case 2:
- {
- axisNum = 2;
- dirnum = 2;
- break;
- }
+ {
+ axisNum = 2;
+ dirnum = 2;
+ break;
+ }
case 3:
- {
- axisNum = 2;
- dirnum = 1;
- break;
- }
+ {
+ axisNum = 2;
+ dirnum = 1;
+ break;
+ }
default:
break;
}
@@ -6975,8 +6948,8 @@ HSI_STATUS HSI_Motion::GluePPSpnts(int* ppsDir, double* gluePPSPos, int* delayLi
}
#pragma endregion
- unsigned char send_data_light2[64] = {0};
- unsigned char send_data_delay[64] = {0};
+ unsigned char send_data_light2[64] = { 0 };
+ unsigned char send_data_delay[64] = { 0 };
c = 0;
GlueDispenserindexNum = 0;
int ldIndex = 0;
@@ -7097,7 +7070,7 @@ HSI_STATUS HSI_Motion::GlueDispenser(int* index, int* cirdirection, double* glue
if (g_pHSI_Motion)
{
GluerunCount = 0;
- unsigned char send_glue_data[64] = {0};
+ unsigned char send_glue_data[64] = { 0 };
send_glue_data[0] = CT_ORDER;
send_glue_data[1] = CT_GLUEDISPENSER_CLEAR;
send_glue_data[2] = 0x02;
@@ -7115,418 +7088,418 @@ HSI_STATUS HSI_Motion::GlueDispenser(int* index, int* cirdirection, double* glue
switch (index[i])
{
case 0:
+ {
+ glueaxis = 0;
+ if (i == 0)
{
- glueaxis = 0;
- if (i == 0)
+ GluerunCount++;
+ endGluepos[0] = gluePos[posIndex++];
+ endGluepos[1] = gluePos[posIndex++];
+ endGluepos[2] = gluePos[posIndex++];
+ endGluepos[3] = gluePos[posIndex++];
+ saveGluepos[0] = endGluepos[0];
+ saveGluepos[1] = endGluepos[1];
+ saveGluepos[2] = endGluepos[2];
+ saveGluepos[3] = endGluepos[3];
+ loadFeet[0] = static_cast(endGluepos[0] / m_Resolution[1]);
+ loadFeet[1] = static_cast(endGluepos[1] / m_Resolution[2]);
+ loadFeet[2] = static_cast(endGluepos[2] / m_Resolution[3]);
+ loadFeet[3] = static_cast(endGluepos[3] / m_Resolution[4]);
+ if (abs(loadFeet[0]) > 20) glueaxis |= 0x01;
+ if (abs(loadFeet[1]) > 20) glueaxis |= 0x02;
+ if (abs(loadFeet[2]) > 20) glueaxis |= 0x04;
+ if (abs(loadFeet[3]) > 20) glueaxis |= 0x08;
+ send_glue_data[3] = glueaxis;
+ send_glue_data[4] = 0x01;
+ send_glue_data[5] = 0x00;
+ int j = 6;
+ for (size_t k = 0; k < 4; k++)
{
- GluerunCount++;
- endGluepos[0] = gluePos[posIndex++];
- endGluepos[1] = gluePos[posIndex++];
- endGluepos[2] = gluePos[posIndex++];
- endGluepos[3] = gluePos[posIndex++];
- saveGluepos[0] = endGluepos[0];
- saveGluepos[1] = endGluepos[1];
- saveGluepos[2] = endGluepos[2];
- saveGluepos[3] = endGluepos[3];
- loadFeet[0] = static_cast(endGluepos[0] / m_Resolution[1]);
- loadFeet[1] = static_cast(endGluepos[1] / m_Resolution[2]);
- loadFeet[2] = static_cast(endGluepos[2] / m_Resolution[3]);
- loadFeet[3] = static_cast(endGluepos[3] / m_Resolution[4]);
- if (abs(loadFeet[0]) > 20) glueaxis |= 0x01;
- if (abs(loadFeet[1]) > 20) glueaxis |= 0x02;
- if (abs(loadFeet[2]) > 20) glueaxis |= 0x04;
- if (abs(loadFeet[3]) > 20) glueaxis |= 0x08;
- send_glue_data[3] = glueaxis;
- send_glue_data[4] = 0x01;
- send_glue_data[5] = 0x00;
- int j = 6;
- for (size_t k = 0; k < 4; k++)
- {
- send_glue_data[j++] = (loadFeet[k] & 0xff);
- send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff);
- send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff);
- send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff);
- }
- send_glue_data[0] = CT_MOTOR;
- send_glue_data[1] = CT_GLUEDISPENSER_SET;
- int len = j - 3;
- send_glue_data[2] = len;
- m_WriteByte = Send_Command(0, (const char*)send_glue_data, m_SendDataLength);
- Sleep(20);
+ send_glue_data[j++] = (loadFeet[k] & 0xff);
+ send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff);
+ send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff);
+ send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff);
}
- else
- {
- GluerunCount++;
- endGluepos[0] = gluePos[posIndex++];
- endGluepos[1] = gluePos[posIndex++];
- endGluepos[2] = gluePos[posIndex++];
- endGluepos[3] = gluePos[posIndex++];
- startGluepos[0] = saveGluepos[0];
- startGluepos[1] = saveGluepos[1];
- startGluepos[2] = saveGluepos[2];
- startGluepos[3] = saveGluepos[3];
- saveGluepos[0] = endGluepos[0];
- saveGluepos[1] = endGluepos[1];
- saveGluepos[2] = endGluepos[2];
- saveGluepos[3] = endGluepos[3];
- loadFeet[0] = static_cast((endGluepos[0] - startGluepos[0]) / m_Resolution[1]);
- loadFeet[1] = static_cast((endGluepos[1] - startGluepos[1]) / m_Resolution[2]);
- loadFeet[2] = static_cast((endGluepos[2] - startGluepos[2]) / m_Resolution[3]);
- loadFeet[3] = static_cast((endGluepos[3] - startGluepos[3]) / m_Resolution[4]);
- if (abs(loadFeet[0]) > 20) glueaxis |= 0x01;
- if (abs(loadFeet[1]) > 20) glueaxis |= 0x02;
- if (abs(loadFeet[2]) > 20) glueaxis |= 0x04;
- if (abs(loadFeet[3]) > 20) glueaxis |= 0x08;
- send_glue_data[3] = glueaxis;
- send_glue_data[4] = 0x01;
- send_glue_data[5] = 0x00;
- int j = 6;
- for (size_t k = 0; k < 4; k++)
- {
- send_glue_data[j++] = (loadFeet[k] & 0xff);
- send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff);
- send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff);
- send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff);
- }
- send_glue_data[0] = CT_MOTOR;
- send_glue_data[1] = CT_GLUEDISPENSER_SET;
- int len = j - 3;
- send_glue_data[2] = len;
- m_WriteByte = Send_Command(0, (const char*)send_glue_data, m_SendDataLength);
- Sleep(20);
- }
- break;
+ send_glue_data[0] = CT_MOTOR;
+ send_glue_data[1] = CT_GLUEDISPENSER_SET;
+ int len = j - 3;
+ send_glue_data[2] = len;
+ m_WriteByte = Send_Command(0, (const char*)send_glue_data, m_SendDataLength);
+ Sleep(20);
}
+ else
+ {
+ GluerunCount++;
+ endGluepos[0] = gluePos[posIndex++];
+ endGluepos[1] = gluePos[posIndex++];
+ endGluepos[2] = gluePos[posIndex++];
+ endGluepos[3] = gluePos[posIndex++];
+ startGluepos[0] = saveGluepos[0];
+ startGluepos[1] = saveGluepos[1];
+ startGluepos[2] = saveGluepos[2];
+ startGluepos[3] = saveGluepos[3];
+ saveGluepos[0] = endGluepos[0];
+ saveGluepos[1] = endGluepos[1];
+ saveGluepos[2] = endGluepos[2];
+ saveGluepos[3] = endGluepos[3];
+ loadFeet[0] = static_cast((endGluepos[0] - startGluepos[0]) / m_Resolution[1]);
+ loadFeet[1] = static_cast((endGluepos[1] - startGluepos[1]) / m_Resolution[2]);
+ loadFeet[2] = static_cast((endGluepos[2] - startGluepos[2]) / m_Resolution[3]);
+ loadFeet[3] = static_cast((endGluepos[3] - startGluepos[3]) / m_Resolution[4]);
+ if (abs(loadFeet[0]) > 20) glueaxis |= 0x01;
+ if (abs(loadFeet[1]) > 20) glueaxis |= 0x02;
+ if (abs(loadFeet[2]) > 20) glueaxis |= 0x04;
+ if (abs(loadFeet[3]) > 20) glueaxis |= 0x08;
+ send_glue_data[3] = glueaxis;
+ send_glue_data[4] = 0x01;
+ send_glue_data[5] = 0x00;
+ int j = 6;
+ for (size_t k = 0; k < 4; k++)
+ {
+ send_glue_data[j++] = (loadFeet[k] & 0xff);
+ send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff);
+ send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff);
+ send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff);
+ }
+ send_glue_data[0] = CT_MOTOR;
+ send_glue_data[1] = CT_GLUEDISPENSER_SET;
+ int len = j - 3;
+ send_glue_data[2] = len;
+ m_WriteByte = Send_Command(0, (const char*)send_glue_data, m_SendDataLength);
+ Sleep(20);
+ }
+ break;
+ }
case 1:
+ {
+ if (i == 0)
{
- if (i == 0)
+ GluerunCount++;
+ endGluepos[0] = gluePos[posIndex++];
+ endGluepos[1] = gluePos[posIndex++];
+ endGluepos[2] = gluePos[posIndex++];
+ endGluepos[3] = gluePos[posIndex++];
+ saveGluepos[0] = endGluepos[0];
+ saveGluepos[1] = endGluepos[1];
+ saveGluepos[2] = endGluepos[2];
+ saveGluepos[3] = endGluepos[3];
+ loadFeet[0] = static_cast(endGluepos[0] / m_Resolution[1]);
+ loadFeet[1] = static_cast(endGluepos[1] / m_Resolution[2]);
+ loadFeet[2] = static_cast(endGluepos[2] / m_Resolution[3]);
+ loadFeet[3] = static_cast(endGluepos[3] / m_Resolution[4]);
+ if (abs(loadFeet[0]) > 20) glueaxis |= 0x01;
+ if (abs(loadFeet[1]) > 20) glueaxis |= 0x02;
+ if (abs(loadFeet[2]) > 20) glueaxis |= 0x04;
+ if (abs(loadFeet[3]) > 20) glueaxis |= 0x08;
+ send_glue_data[3] = glueaxis;
+ send_glue_data[4] = 0x01;
+ send_glue_data[5] = 0x00;
+ int j = 6;
+ for (size_t k = 0; k < 4; k++)
{
- GluerunCount++;
- endGluepos[0] = gluePos[posIndex++];
- endGluepos[1] = gluePos[posIndex++];
- endGluepos[2] = gluePos[posIndex++];
- endGluepos[3] = gluePos[posIndex++];
- saveGluepos[0] = endGluepos[0];
- saveGluepos[1] = endGluepos[1];
- saveGluepos[2] = endGluepos[2];
- saveGluepos[3] = endGluepos[3];
- loadFeet[0] = static_cast(endGluepos[0] / m_Resolution[1]);
- loadFeet[1] = static_cast(endGluepos[1] / m_Resolution[2]);
- loadFeet[2] = static_cast(endGluepos[2] / m_Resolution[3]);
- loadFeet[3] = static_cast(endGluepos[3] / m_Resolution[4]);
- if (abs(loadFeet[0]) > 20) glueaxis |= 0x01;
- if (abs(loadFeet[1]) > 20) glueaxis |= 0x02;
- if (abs(loadFeet[2]) > 20) glueaxis |= 0x04;
- if (abs(loadFeet[3]) > 20) glueaxis |= 0x08;
- send_glue_data[3] = glueaxis;
- send_glue_data[4] = 0x01;
- send_glue_data[5] = 0x00;
- int j = 6;
- for (size_t k = 0; k < 4; k++)
- {
- send_glue_data[j++] = (loadFeet[k] & 0xff);
- send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff);
- send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff);
- send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff);
- }
- GluerunCount++;
- glueaxis = 0;
- endGluepos[0] = gluePos[posIndex++];
- endGluepos[1] = gluePos[posIndex++];
- endGluepos[2] = gluePos[posIndex++];
- endGluepos[3] = gluePos[posIndex++];
- startGluepos[0] = saveGluepos[0];
- startGluepos[1] = saveGluepos[1];
- startGluepos[2] = saveGluepos[2];
- startGluepos[3] = saveGluepos[3];
- saveGluepos[0] = endGluepos[0];
- saveGluepos[1] = endGluepos[1];
- saveGluepos[2] = endGluepos[2];
- saveGluepos[3] = endGluepos[3];
- loadFeet[0] = static_cast((endGluepos[0] - startGluepos[0]) / m_Resolution[1]);
- loadFeet[1] = static_cast((endGluepos[1] - startGluepos[1]) / m_Resolution[2]);
- loadFeet[2] = static_cast((endGluepos[2] - startGluepos[2]) / m_Resolution[3]);
- loadFeet[3] = static_cast((endGluepos[3] - startGluepos[3]) / m_Resolution[4]);
- if (abs(loadFeet[0]) > 20) glueaxis |= 0x01;
- if (abs(loadFeet[1]) > 20) glueaxis |= 0x02;
- if (abs(loadFeet[2]) > 20) glueaxis |= 0x04;
- if (abs(loadFeet[3]) > 20) glueaxis |= 0x08;
- send_glue_data[j++] = glueaxis;
- send_glue_data[j++] = 0x02;
- send_glue_data[j++] = 0x01;
- for (size_t k = 0; k < 4; k++)
- {
- send_glue_data[j++] = (loadFeet[k] & 0xff);
- send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff);
- send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff);
- send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff);
- }
- send_glue_data[0] = CT_MOTOR;
- send_glue_data[1] = CT_GLUEDISPENSER_SET;
- int len = j - 3;
- send_glue_data[2] = len;
- m_WriteByte = Send_Command(0, (const char*)send_glue_data, m_SendDataLength);
- Sleep(20);
+ send_glue_data[j++] = (loadFeet[k] & 0xff);
+ send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff);
+ send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff);
+ send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff);
}
- else
+ GluerunCount++;
+ glueaxis = 0;
+ endGluepos[0] = gluePos[posIndex++];
+ endGluepos[1] = gluePos[posIndex++];
+ endGluepos[2] = gluePos[posIndex++];
+ endGluepos[3] = gluePos[posIndex++];
+ startGluepos[0] = saveGluepos[0];
+ startGluepos[1] = saveGluepos[1];
+ startGluepos[2] = saveGluepos[2];
+ startGluepos[3] = saveGluepos[3];
+ saveGluepos[0] = endGluepos[0];
+ saveGluepos[1] = endGluepos[1];
+ saveGluepos[2] = endGluepos[2];
+ saveGluepos[3] = endGluepos[3];
+ loadFeet[0] = static_cast((endGluepos[0] - startGluepos[0]) / m_Resolution[1]);
+ loadFeet[1] = static_cast((endGluepos[1] - startGluepos[1]) / m_Resolution[2]);
+ loadFeet[2] = static_cast((endGluepos[2] - startGluepos[2]) / m_Resolution[3]);
+ loadFeet[3] = static_cast((endGluepos[3] - startGluepos[3]) / m_Resolution[4]);
+ if (abs(loadFeet[0]) > 20) glueaxis |= 0x01;
+ if (abs(loadFeet[1]) > 20) glueaxis |= 0x02;
+ if (abs(loadFeet[2]) > 20) glueaxis |= 0x04;
+ if (abs(loadFeet[3]) > 20) glueaxis |= 0x08;
+ send_glue_data[j++] = glueaxis;
+ send_glue_data[j++] = 0x02;
+ send_glue_data[j++] = 0x01;
+ for (size_t k = 0; k < 4; k++)
{
- GluerunCount++;
- glueaxis = 0;
- endGluepos[0] = gluePos[posIndex++];
- endGluepos[1] = gluePos[posIndex++];
- endGluepos[2] = gluePos[posIndex++];
- endGluepos[3] = gluePos[posIndex++];
- startGluepos[0] = saveGluepos[0];
- startGluepos[1] = saveGluepos[1];
- startGluepos[2] = saveGluepos[2];
- startGluepos[3] = saveGluepos[3];
- saveGluepos[0] = endGluepos[0];
- saveGluepos[1] = endGluepos[1];
- saveGluepos[2] = endGluepos[2];
- saveGluepos[3] = endGluepos[3];
- loadFeet[0] = static_cast((endGluepos[0] - startGluepos[0]) / m_Resolution[1]);
- loadFeet[1] = static_cast((endGluepos[1] - startGluepos[1]) / m_Resolution[2]);
- loadFeet[2] = static_cast((endGluepos[2] - startGluepos[2]) / m_Resolution[3]);
- loadFeet[3] = static_cast((endGluepos[3] - startGluepos[3]) / m_Resolution[4]);
- if (abs(loadFeet[0]) > 20) glueaxis |= 0x01;
- if (abs(loadFeet[1]) > 20) glueaxis |= 0x02;
- if (abs(loadFeet[2]) > 20) glueaxis |= 0x04;
- if (abs(loadFeet[3]) > 20) glueaxis |= 0x08;
- send_glue_data[3] = glueaxis;
- send_glue_data[4] = 0x02;
- send_glue_data[5] = 0x00;
- int j = 6;
- for (size_t k = 0; k < 4; k++)
- {
- send_glue_data[j++] = (loadFeet[k] & 0xff);
- send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff);
- send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff);
- send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff);
- }
- GluerunCount++;
- glueaxis = 0;
- endGluepos[0] = gluePos[posIndex++];
- endGluepos[1] = gluePos[posIndex++];
- endGluepos[2] = gluePos[posIndex++];
- endGluepos[3] = gluePos[posIndex++];
- startGluepos[0] = saveGluepos[0];
- startGluepos[1] = saveGluepos[1];
- startGluepos[2] = saveGluepos[2];
- startGluepos[3] = saveGluepos[3];
- saveGluepos[0] = endGluepos[0];
- saveGluepos[1] = endGluepos[1];
- saveGluepos[2] = endGluepos[2];
- saveGluepos[3] = endGluepos[3];
- loadFeet[0] = static_cast((endGluepos[0] - startGluepos[0]) / m_Resolution[1]);
- loadFeet[1] = static_cast((endGluepos[1] - startGluepos[1]) / m_Resolution[2]);
- loadFeet[2] = static_cast((endGluepos[2] - startGluepos[2]) / m_Resolution[3]);
- loadFeet[3] = static_cast((endGluepos[3] - startGluepos[3]) / m_Resolution[4]);
- if (abs(loadFeet[0]) > 20) glueaxis |= 0x01;
- if (abs(loadFeet[1]) > 20) glueaxis |= 0x02;
- if (abs(loadFeet[2]) > 20) glueaxis |= 0x04;
- if (abs(loadFeet[3]) > 20) glueaxis |= 0x08;
- send_glue_data[j++] = glueaxis;
- send_glue_data[j++] = 0x02;
- send_glue_data[j++] = 0x01;
- for (size_t k = 0; k < 4; k++)
- {
- send_glue_data[j++] = (loadFeet[k] & 0xff);
- send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff);
- send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff);
- send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff);
- }
- send_glue_data[0] = CT_MOTOR;
- send_glue_data[1] = CT_GLUEDISPENSER_SET;
- int len = j - 3;
- send_glue_data[2] = len;
- m_WriteByte = Send_Command(0, (const char*)send_glue_data, m_SendDataLength);
- Sleep(20);
+ send_glue_data[j++] = (loadFeet[k] & 0xff);
+ send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff);
+ send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff);
+ send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff);
}
- break;
+ send_glue_data[0] = CT_MOTOR;
+ send_glue_data[1] = CT_GLUEDISPENSER_SET;
+ int len = j - 3;
+ send_glue_data[2] = len;
+ m_WriteByte = Send_Command(0, (const char*)send_glue_data, m_SendDataLength);
+ Sleep(20);
}
+ else
+ {
+ GluerunCount++;
+ glueaxis = 0;
+ endGluepos[0] = gluePos[posIndex++];
+ endGluepos[1] = gluePos[posIndex++];
+ endGluepos[2] = gluePos[posIndex++];
+ endGluepos[3] = gluePos[posIndex++];
+ startGluepos[0] = saveGluepos[0];
+ startGluepos[1] = saveGluepos[1];
+ startGluepos[2] = saveGluepos[2];
+ startGluepos[3] = saveGluepos[3];
+ saveGluepos[0] = endGluepos[0];
+ saveGluepos[1] = endGluepos[1];
+ saveGluepos[2] = endGluepos[2];
+ saveGluepos[3] = endGluepos[3];
+ loadFeet[0] = static_cast((endGluepos[0] - startGluepos[0]) / m_Resolution[1]);
+ loadFeet[1] = static_cast((endGluepos[1] - startGluepos[1]) / m_Resolution[2]);
+ loadFeet[2] = static_cast((endGluepos[2] - startGluepos[2]) / m_Resolution[3]);
+ loadFeet[3] = static_cast((endGluepos[3] - startGluepos[3]) / m_Resolution[4]);
+ if (abs(loadFeet[0]) > 20) glueaxis |= 0x01;
+ if (abs(loadFeet[1]) > 20) glueaxis |= 0x02;
+ if (abs(loadFeet[2]) > 20) glueaxis |= 0x04;
+ if (abs(loadFeet[3]) > 20) glueaxis |= 0x08;
+ send_glue_data[3] = glueaxis;
+ send_glue_data[4] = 0x02;
+ send_glue_data[5] = 0x00;
+ int j = 6;
+ for (size_t k = 0; k < 4; k++)
+ {
+ send_glue_data[j++] = (loadFeet[k] & 0xff);
+ send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff);
+ send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff);
+ send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff);
+ }
+ GluerunCount++;
+ glueaxis = 0;
+ endGluepos[0] = gluePos[posIndex++];
+ endGluepos[1] = gluePos[posIndex++];
+ endGluepos[2] = gluePos[posIndex++];
+ endGluepos[3] = gluePos[posIndex++];
+ startGluepos[0] = saveGluepos[0];
+ startGluepos[1] = saveGluepos[1];
+ startGluepos[2] = saveGluepos[2];
+ startGluepos[3] = saveGluepos[3];
+ saveGluepos[0] = endGluepos[0];
+ saveGluepos[1] = endGluepos[1];
+ saveGluepos[2] = endGluepos[2];
+ saveGluepos[3] = endGluepos[3];
+ loadFeet[0] = static_cast((endGluepos[0] - startGluepos[0]) / m_Resolution[1]);
+ loadFeet[1] = static_cast((endGluepos[1] - startGluepos[1]) / m_Resolution[2]);
+ loadFeet[2] = static_cast((endGluepos[2] - startGluepos[2]) / m_Resolution[3]);
+ loadFeet[3] = static_cast((endGluepos[3] - startGluepos[3]) / m_Resolution[4]);
+ if (abs(loadFeet[0]) > 20) glueaxis |= 0x01;
+ if (abs(loadFeet[1]) > 20) glueaxis |= 0x02;
+ if (abs(loadFeet[2]) > 20) glueaxis |= 0x04;
+ if (abs(loadFeet[3]) > 20) glueaxis |= 0x08;
+ send_glue_data[j++] = glueaxis;
+ send_glue_data[j++] = 0x02;
+ send_glue_data[j++] = 0x01;
+ for (size_t k = 0; k < 4; k++)
+ {
+ send_glue_data[j++] = (loadFeet[k] & 0xff);
+ send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff);
+ send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff);
+ send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff);
+ }
+ send_glue_data[0] = CT_MOTOR;
+ send_glue_data[1] = CT_GLUEDISPENSER_SET;
+ int len = j - 3;
+ send_glue_data[2] = len;
+ m_WriteByte = Send_Command(0, (const char*)send_glue_data, m_SendDataLength);
+ Sleep(20);
+ }
+ break;
+ }
case 3:
+ {
+ if (i == 0)
{
- if (i == 0)
+ GluerunCount++;
+ endGluepos[0] = gluePos[posIndex++];
+ endGluepos[1] = gluePos[posIndex++];
+ endGluepos[2] = gluePos[posIndex++];
+ endGluepos[3] = gluePos[posIndex++];
+ saveGluepos[0] = endGluepos[0];
+ saveGluepos[1] = endGluepos[1];
+ saveGluepos[2] = endGluepos[2];
+ saveGluepos[3] = endGluepos[3];
+ loadFeet[0] = static_cast(endGluepos[0] / m_Resolution[1]);
+ loadFeet[1] = static_cast(endGluepos[1] / m_Resolution[2]);
+ loadFeet[2] = static_cast(endGluepos[2] / m_Resolution[3]);
+ loadFeet[3] = static_cast(endGluepos[3] / m_Resolution[4]);
+ if (abs(loadFeet[0]) > 20) glueaxis |= 0x01;
+ if (abs(loadFeet[1]) > 20) glueaxis |= 0x02;
+ if (abs(loadFeet[2]) > 20) glueaxis |= 0x04;
+ if (abs(loadFeet[3]) > 20) glueaxis |= 0x08;
+ send_glue_data[3] = glueaxis;
+ send_glue_data[4] = 0x01;
+ send_glue_data[5] = 0x00;
+ int j = 6;
+ for (size_t k = 0; k < 4; k++)
{
- GluerunCount++;
- endGluepos[0] = gluePos[posIndex++];
- endGluepos[1] = gluePos[posIndex++];
- endGluepos[2] = gluePos[posIndex++];
- endGluepos[3] = gluePos[posIndex++];
- saveGluepos[0] = endGluepos[0];
- saveGluepos[1] = endGluepos[1];
- saveGluepos[2] = endGluepos[2];
- saveGluepos[3] = endGluepos[3];
- loadFeet[0] = static_cast(endGluepos[0] / m_Resolution[1]);
- loadFeet[1] = static_cast(endGluepos[1] / m_Resolution[2]);
- loadFeet[2] = static_cast(endGluepos[2] / m_Resolution[3]);
- loadFeet[3] = static_cast(endGluepos[3] / m_Resolution[4]);
- if (abs(loadFeet[0]) > 20) glueaxis |= 0x01;
- if (abs(loadFeet[1]) > 20) glueaxis |= 0x02;
- if (abs(loadFeet[2]) > 20) glueaxis |= 0x04;
- if (abs(loadFeet[3]) > 20) glueaxis |= 0x08;
- send_glue_data[3] = glueaxis;
- send_glue_data[4] = 0x01;
- send_glue_data[5] = 0x00;
- int j = 6;
- for (size_t k = 0; k < 4; k++)
- {
- send_glue_data[j++] = (loadFeet[k] & 0xff);
- send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff);
- send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff);
- send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff);
- }
- GluerunCount++;
- glueaxis = 0;
- endGluepos[0] = gluePos[posIndex++];
- endGluepos[1] = gluePos[posIndex++];
- endGluepos[2] = gluePos[posIndex++];
- endGluepos[3] = gluePos[posIndex++];
- startGluepos[0] = saveGluepos[0];
- startGluepos[1] = saveGluepos[1];
- startGluepos[2] = saveGluepos[2];
- startGluepos[3] = saveGluepos[3];
- circlepnt[0] = static_cast((gluePos[posIndex++] - startGluepos[0]) / m_Resolution[1]);
- circlepnt[1] = static_cast((gluePos[posIndex++] - startGluepos[1]) / m_Resolution[2]);
- circlepnt[2] = static_cast((gluePos[posIndex++] - startGluepos[2]) / m_Resolution[3]);
- circlepnt[3] = static_cast((gluePos[posIndex++] - startGluepos[3]) / m_Resolution[4]);
- saveGluepos[0] = endGluepos[0];
- saveGluepos[1] = endGluepos[1];
- saveGluepos[2] = endGluepos[2];
- saveGluepos[3] = endGluepos[3];
- loadFeet[0] = static_cast((endGluepos[0] - startGluepos[0]) / m_Resolution[1]);
- loadFeet[1] = static_cast((endGluepos[1] - startGluepos[1]) / m_Resolution[2]);
- loadFeet[2] = static_cast((endGluepos[2] - startGluepos[2]) / m_Resolution[3]);
- loadFeet[3] = static_cast((endGluepos[3] - startGluepos[3]) / m_Resolution[4]);
- if (abs(loadFeet[0]) > 20) glueaxis |= 0x01;
- if (abs(loadFeet[1]) > 20) glueaxis |= 0x02;
- if (abs(loadFeet[2]) > 20) glueaxis |= 0x04;
- if (abs(loadFeet[3]) > 20) glueaxis |= 0x08;
- send_glue_data[j++] = 0x03;
- if (cirdirection[i] == 0)
- {
- send_glue_data[j++] = 0x04;
- }
- else
- {
- send_glue_data[j++] = 0x03;
- }
- send_glue_data[j++] = 0x01;
- for (size_t k = 0; k < 4; k++)
- {
- send_glue_data[j++] = (loadFeet[k] & 0xff);
- send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff);
- send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff);
- send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff);
- }
- for (size_t k = 0; k < 4; k++)
- {
- send_glue_data[j++] = (circlepnt[k] & 0xff);
- send_glue_data[j++] = ((circlepnt[k] >> 8) & 0xff);
- send_glue_data[j++] = ((circlepnt[k] >> 16) & 0xff);
- send_glue_data[j++] = ((circlepnt[k] >> 24) & 0xff);
- }
- send_glue_data[0] = CT_MOTOR;
- send_glue_data[1] = CT_GLUEDISPENSER_SET;
- int len = j - 3;
- send_glue_data[2] = len;
- m_WriteByte = Send_Command(0, (const char*)send_glue_data, m_SendDataLength);
- Sleep(20);
+ send_glue_data[j++] = (loadFeet[k] & 0xff);
+ send_glue_data[j++] = ((loadFeet[k] >> 8) & 0xff);
+ send_glue_data[j++] = ((loadFeet[k] >> 16) & 0xff);
+ send_glue_data[j++] = ((loadFeet[k] >> 24) & 0xff);
+ }
+ GluerunCount++;
+ glueaxis = 0;
+ endGluepos[0] = gluePos[posIndex++];
+ endGluepos[1] = gluePos[posIndex++];
+ endGluepos[2] = gluePos[posIndex++];
+ endGluepos[3] = gluePos[posIndex++];
+ startGluepos[0] = saveGluepos[0];
+ startGluepos[1] = saveGluepos[1];
+ startGluepos[2] = saveGluepos[2];
+ startGluepos[3] = saveGluepos[3];
+ circlepnt[0] = static_cast((gluePos[posIndex++] - startGluepos[0]) / m_Resolution[1]);
+ circlepnt[1] = static_cast((gluePos[posIndex++] - startGluepos[1]) / m_Resolution[2]);
+ circlepnt[2] = static_cast((gluePos[posIndex++] - startGluepos[2]) / m_Resolution[3]);
+ circlepnt[3] = static_cast((gluePos[posIndex++] - startGluepos[3]) / m_Resolution[4]);
+ saveGluepos[0] = endGluepos[0];
+ saveGluepos[1] = endGluepos[1];
+ saveGluepos[2] = endGluepos[2];
+ saveGluepos[3] = endGluepos[3];
+ loadFeet[0] = static_cast((endGluepos[0] - startGluepos[0]) / m_Resolution[1]);
+ loadFeet[1] = static_cast((endGluepos[1] - startGluepos[1]) / m_Resolution[2]);
+ loadFeet[2] = static_cast((endGluepos[2] - startGluepos[2]) / m_Resolution[3]);
+ loadFeet[3] = static_cast((endGluepos[3] - startGluepos[3]) / m_Resolution[4]);
+ if (abs(loadFeet[0]) > 20) glueaxis |= 0x01;
+ if (abs(loadFeet[1]) > 20) glueaxis |= 0x02;
+ if (abs(loadFeet[2]) > 20) glueaxis |= 0x04;
+ if (abs(loadFeet[3]) > 20) glueaxis |= 0x08;
+ send_glue_data[j++] = 0x03;
+ if (cirdirection[i] == 0)
+ {
+ send_glue_data[j++] = 0x04;
}
else
{
- GluerunCount++;
- glueaxis = 0;
- endGluepos[0] = gluePos[posIndex++];
- endGluepos[1] = gluePos[posIndex++];
- endGluepos[2] = gluePos[posIndex++];
- endGluepos[3] = gluePos[posIndex++];
- startGluepos[0] = saveGluepos[0];
- startGluepos[1] = saveGluepos[1];
- startGluepos[2] = saveGluepos[2];
- startGluepos[3] = saveGluepos[3];
- saveGluepos[0] = endGluepos[0];
- saveGluepos[1] = endGluepos[1];
- saveGluepos[2] = endGluepos[2];
- saveGluepos[3] = endGluepos[3];
- loadFeet[0] = static_cast((endGluepos[0] - startGluepos[0]) / m_Resolution[1]);
- loadFeet[1] = static_cast((endGluepos[1] - startGluepos[1]) / m_Resolution[2]);
- loadFeet[2] = static_cast