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

平台阶段性结束

Browse Source 
1、新增了加速度，减速度，运行速度读配置档位
2、优化了串口通讯，待继续调试
3、测试用例，新增了 DCCScanStart() 和 DCCScanStop()的调试
This commit is contained in:
zhengxuan.zhang
2022-11-25 10:51:04 +08:00
parent 341aaa08ff
commit b94d85b393
9 changed files with 453 additions and 220 deletions
Download Patch File Download Diff File Expand all files Collapse all files
HSI_HexagonMI_EF3/HSI_Motion.cpp
+159 -131
View File
@@ -81,7 +81,7 @@ void ErrorsHandler()
{
ErrorStr[Received] = '\0';
printf("Motion Error: %d [%s]\n", ErrorCode, ErrorStr);
g_pLogger->SendAndFlushWithTime(L"[ACS Motion] Motion Error %d%S\n", ErrorCode, ErrorStr);
g_pLogger->SendAndFlushWithTime(L"[ACS Motion] Motion Error,Code: %d, %S\n", ErrorCode, ErrorStr);
}
}
else
@@ -777,18 +777,19 @@ HSI_STATUS HSI_Motion::Startup(HWND _hWnd, bool _bOfflineOnly)
//if (m_IsUseRocker == 1)
{
//软件清空
m_cSendData[0] = 0x01;
m_cSendData[1] = 0x06;
m_WriteByte = Send_Command(0, (const char*)m_cSendData, 2);
Sleep(5);
//m_cSendData[0] = 0x01;
//m_cSendData[1] = 0x06;
//m_WriteByte = Send_Command(0, (const char*)m_cSendData, 2);
//Sleep(10);
//设置锁存频率 1秒钟（）
m_cSendData[0] = 0x01;
m_cSendData[1] = 0x01;
m_cSendData[2] = 0x02;
m_cSendData[3] = 0x03;
m_cSendData[3] = 0x03; // 10000 （0x27 0x10）对应1秒，3-4字节表示锁存周期
m_cSendData[4] = 0xE8;
m_WriteByte = Send_Command(0, (const char*)m_cSendData, 5);
Sleep(5);
Sleep(10);
g_pLogger->SendAndFlushWithTime(L"[Startup] Set EF3 Timing latch Success\n");
}
@@ -1935,113 +1936,113 @@ HSI_STATUS HSI_Motion::Jog(UINT AxisTypes, double Speed)
L"[Jog] Speed: [%d], DriveSpeed: [%d], AccCurve: [%d], AccLine: [%d], DecCurve: [%d], DecLine: [%d]\n",
Speed, DriveSpeed, AccCurve, AccLine, DecCurve, DecLine);
if (m_DeviceType != 3)
{
if (AxisTypes == AXIS_X /*&& m_motorType & 0x01*/)
{
if (!bJOGDir) //负方向
{
RemainPul = static_cast<int>(now_pos[1] / m_Resolution[1]) - static_cast<int>(m_N_Work_Limit[1] /
m_Resolution[1]);
limitSDPul = (DriveSpeed - StartSpeed) * 13;
if ((RemainPul < limitSDPul * 2) && (RemainPul > 0))
{
float SpeedRatio = limitSDPul * 2 / RemainPul;
DriveSpeed = DriveSpeed / SpeedRatio;
if (DriveSpeed < StartSpeed)
{
DriveSpeed = StartSpeed;
}
}
}
else
{
RemainPul = static_cast<int>(m_P_Work_Limit[1] / m_Resolution[1]) - static_cast<int>(now_pos[1] /
m_Resolution[1]);
limitSDPul = (DriveSpeed - StartSpeed) * 13;
if ((RemainPul < limitSDPul * 2) && (RemainPul > 0))
{
float SpeedRatio = limitSDPul * 2 / RemainPul;
DriveSpeed = DriveSpeed / SpeedRatio;
if (DriveSpeed < StartSpeed)
{
DriveSpeed = StartSpeed;
}
}
}
}
else if (AxisTypes == AXIS_Y /*&& m_motorType & 0x02*/)
{
if (!bJOGDir) //负方向
{
RemainPul = static_cast<int>(now_pos[2] / m_Resolution[2]) - static_cast<int>(m_N_Work_Limit[2] /
m_Resolution[2]);
limitSDPul = (DriveSpeed - StartSpeed) * 13;
if ((RemainPul < limitSDPul * 2) && (RemainPul > 0))
{
float SpeedRatio = 1 + limitSDPul * 2 / RemainPul;
DriveSpeed = DriveSpeed / SpeedRatio;
if (DriveSpeed < StartSpeed)
{
DriveSpeed = StartSpeed;
}
}
}
else
{
RemainPul = static_cast<int>(m_P_Work_Limit[2] / m_Resolution[2]) - static_cast<int>(now_pos[2] /
m_Resolution[2]);
limitSDPul = (DriveSpeed - StartSpeed) * 13;
if ((RemainPul < limitSDPul * 2) && (RemainPul > 0))
{
float SpeedRatio = limitSDPul * 2 / RemainPul;
DriveSpeed = DriveSpeed / SpeedRatio;
if (DriveSpeed < StartSpeed)
{
DriveSpeed = StartSpeed;
}
}
}
}
if (AxisTypes == AXIS_Z /*&& m_motorType & 0x04*/)
{
if (!bJOGDir) //负方向
{
RemainPul = static_cast<int>(now_pos[3] / m_Resolution[3]) - static_cast<int>(m_N_Work_Limit[3] /
m_Resolution[3]);
limitSDPul = (DriveSpeed - StartSpeed) * 13;
if ((RemainPul < limitSDPul * 2) && (RemainPul > 0))
{
float SpeedRatio = limitSDPul * 2 / RemainPul;
DriveSpeed = DriveSpeed / SpeedRatio;
if (DriveSpeed < StartSpeed)
{
DriveSpeed = StartSpeed;
}
}
}
else
{
RemainPul = static_cast<int>(m_P_Work_Limit[3] / m_Resolution[3]) - static_cast<int>(now_pos[3] /
m_Resolution[3]);
limitSDPul = (DriveSpeed - StartSpeed) * 13;
if ((RemainPul < limitSDPul * 2) && (RemainPul > 0))
{
float SpeedRatio = limitSDPul * 2 / RemainPul;
DriveSpeed = DriveSpeed / SpeedRatio;
if (DriveSpeed < StartSpeed)
{
DriveSpeed = StartSpeed;
}
}
}
}
}
//if (m_DeviceType != 3)
//{
// if (AxisTypes == AXIS_X /*&& m_motorType & 0x01*/)
// {
// if (!bJOGDir) //负方向
// {
// RemainPul = static_cast<int>(now_pos[1] / m_Resolution[1]) - static_cast<int>(m_N_Work_Limit[1] /
// m_Resolution[1]);
// limitSDPul = (DriveSpeed - StartSpeed) * 13;
// if ((RemainPul < limitSDPul * 2) && (RemainPul > 0))
// {
// float SpeedRatio = limitSDPul * 2 / RemainPul;
// DriveSpeed = DriveSpeed / SpeedRatio;
// if (DriveSpeed < StartSpeed)
// {
// DriveSpeed = StartSpeed;
// }
// }
// }
// else
// {
// RemainPul = static_cast<int>(m_P_Work_Limit[1] / m_Resolution[1]) - static_cast<int>(now_pos[1] /
// m_Resolution[1]);
// limitSDPul = (DriveSpeed - StartSpeed) * 13;
// if ((RemainPul < limitSDPul * 2) && (RemainPul > 0))
// {
// float SpeedRatio = limitSDPul * 2 / RemainPul;
// DriveSpeed = DriveSpeed / SpeedRatio;
// if (DriveSpeed < StartSpeed)
// {
// DriveSpeed = StartSpeed;
// }
// }
// }
// }
// else if (AxisTypes == AXIS_Y /*&& m_motorType & 0x02*/)
// {
// if (!bJOGDir) //负方向
// {
// RemainPul = static_cast<int>(now_pos[2] / m_Resolution[2]) - static_cast<int>(m_N_Work_Limit[2] /
// m_Resolution[2]);
// limitSDPul = (DriveSpeed - StartSpeed) * 13;
// if ((RemainPul < limitSDPul * 2) && (RemainPul > 0))
// {
// float SpeedRatio = 1 + limitSDPul * 2 / RemainPul;
// DriveSpeed = DriveSpeed / SpeedRatio;
// if (DriveSpeed < StartSpeed)
// {
// DriveSpeed = StartSpeed;
// }
// }
// }
// else
// {
// RemainPul = static_cast<int>(m_P_Work_Limit[2] / m_Resolution[2]) - static_cast<int>(now_pos[2] /
// m_Resolution[2]);
// limitSDPul = (DriveSpeed - StartSpeed) * 13;
// if ((RemainPul < limitSDPul * 2) && (RemainPul > 0))
// {
// float SpeedRatio = limitSDPul * 2 / RemainPul;
// DriveSpeed = DriveSpeed / SpeedRatio;
// if (DriveSpeed < StartSpeed)
// {
// DriveSpeed = StartSpeed;
// }
// }
// }
// }
// if (AxisTypes == AXIS_Z /*&& m_motorType & 0x04*/)
// {
// if (!bJOGDir) //负方向
// {
// RemainPul = static_cast<int>(now_pos[3] / m_Resolution[3]) - static_cast<int>(m_N_Work_Limit[3] /
// m_Resolution[3]);
// limitSDPul = (DriveSpeed - StartSpeed) * 13;
// if ((RemainPul < limitSDPul * 2) && (RemainPul > 0))
// {
// float SpeedRatio = limitSDPul * 2 / RemainPul;
// DriveSpeed = DriveSpeed / SpeedRatio;
// if (DriveSpeed < StartSpeed)
// {
// DriveSpeed = StartSpeed;
// }
// }
// }
// else
// {
// RemainPul = static_cast<int>(m_P_Work_Limit[3] / m_Resolution[3]) - static_cast<int>(now_pos[3] /
// m_Resolution[3]);
// limitSDPul = (DriveSpeed - StartSpeed) * 13;
// if ((RemainPul < limitSDPul * 2) && (RemainPul > 0))
// {
// float SpeedRatio = limitSDPul * 2 / RemainPul;
// DriveSpeed = DriveSpeed / SpeedRatio;
// if (DriveSpeed < StartSpeed)
// {
// DriveSpeed = StartSpeed;
// }
// }
// }
// }
//}
//速度限制
g_pLogger->SendAndFlushWithTime(
L"[Jog] StartSpeed: [%d], DriveSpeed: [%d], AccCurve: [%d], AccLine: [%d], DecCurve: [%d], DecLine: [%d]\n",
StartSpeed, DriveSpeed, AccCurve, AccLine, DecCurve, DecLine);
L"[Jog] DriveSpeed: [%d], AccCurve: [%d], AccLine: [%d], DecCurve: [%d], DecLine: [%d]\n",
DriveSpeed, AccCurve, AccLine, DecCurve, DecLine);
// 急停判断
if ((StartSpeed < 250) && (DriveSpeed < 6))
@@ -2057,16 +2058,33 @@ HSI_STATUS HSI_Motion::Jog(UINT AxisTypes, double Speed)
double motionParam[5] = {0};
GetMotorParam(jogAxisNum, motionParam);
g_pLogger->SendAndFlushWithTime(
L"[Jog] Axis= %d, Velocity = %.2f, Acceleration= %.2f, Deceleration= %.2f, KillDeceleration= %.2f, Jerk= %.2f\n",
L"[Jog] Axis= %d,Current Velocity = %.2f, Acceleration= %.2f, Deceleration= %.2f, KillDeceleration= %.2f, Jerk= %.2f\n",
jogAxisNum, motionParam[0], motionParam[1], motionParam[2], motionParam[3], motionParam[4]);
//开始JOG运动
if (!bJOGDir)
{
StartSpeed = StartSpeed * (-1); // Negative direction : Using - (minus) velocity
DriveSpeed = DriveSpeed * (-1); // Negative direction : Using - (minus) velocity
}
//int acsDirection = bJOGDir ? ACSC_POSITIVE_DIRECTION : ACSC_NEGATIVE_DIRECTION; //正方向，或 负方向
if (!acsc_Jog(handleACS, 0, jogAxisNum, StartSpeed, nullptr))
//设置加速度
if(!acsc_SetAccelerationImm(handleACS, jogAxisNum, AccLine, nullptr))
{
g_pLogger->SendAndFlushWithTime(L"[Jog] ACS SetAccelerationImm failed, Aixs:[%d] AccCurve:[%d]\n", jogAxisNum,
AccLine);
ErrorsHandler();
}
//设置减速度
if(!acsc_SetDeceleration(handleACS, jogAxisNum, AccLine, nullptr))
{
g_pLogger->SendAndFlushWithTime(L"[Jog] ACS SetDeceleration failed, Aixs:[%d] DecCurve:[%d]\n", jogAxisNum,
AccLine);
ErrorsHandler();
}
if (!acsc_Jog(handleACS, 0, jogAxisNum, DriveSpeed, nullptr))
{
printf("[Jog] 轴[%d] [%s] 方向移动失败", AxisTypes, bJOGDir ? "正" : "负");
g_pLogger->SendAndFlushWithTime(L"[Jog] failed, Aixs:[%d] JOGDir:[%S]\n", AxisTypes,
@@ -2076,7 +2094,7 @@ HSI_STATUS HSI_Motion::Jog(UINT AxisTypes, double Speed)
jogMoving = true;
g_pLogger->SendAndFlushWithTime(L"[Jog] Out, StartSpeed = %d\n", StartSpeed);
g_pLogger->SendAndFlushWithTime(L"[Jog] Out, DriveSpeed = %d AccCurve:[%d] DecCurve:[%d]\n", DriveSpeed, AccLine, AccLine);
}
return rStatus;
}
@@ -3956,9 +3974,10 @@ HSI_STATUS HSI_Motion::Load_EF3_Motion_Inifile(CString GoogolIniFile)
temp.GetBufferSetLength(50), 10, csAppPath);
float speed = (atof(T2A(temp)));
/*m_JogDriveSpeed[j][i] = GetPrivateProfileInt(L"JOG_SPEED", L"JOG_DRIVESPEED_" , 10, csAppPath);*/
m_JogDriveSpeed[j][i] = speed / (m_Resolution[j] * 50);
g_pLogger->SendAndFlushWithTime(L"[Load_EF3_Motion_Inifile] m_JogDriveSpeed[%d][%d]: %.4f %ld\n", i, j,
speed, m_JogDriveSpeed[i][j]); //打印配置文件 档位速度
//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", i, j,
speed, m_JogDriveSpeed[j][i]); //打印配置文件 档位速度
GetPrivateProfileString(L"JOG_SPEED", L"JOG_STARTSPEED_" + strGear[i] + axisNum[j], L"10",
temp.GetBufferSetLength(50), 10, csAppPath);
@@ -5057,7 +5076,7 @@ HSI_STATUS HSI_Motion::SetDIO(UINT IOChannel, UINT _Status)
m_cSendData[1] = 0x02;
m_cSendData[2] = (_Status >> 8) & 0xff;
m_cSendData[3] = _Status & 0xff;
m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
//m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
Sleep(5);
}
g_pLogger->SendAndFlushWithTime(L"[SetDIO] Out\n");
@@ -5170,7 +5189,7 @@ HSI_STATUS HSI_Motion::Shutdown()
{
memset(m_cSendData, 0x00, 64);
m_cSendData[0] = CT_SOFTSTOP;
m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
//m_WriteByte = Send_Command(0, (const char*)m_cSendData, m_SendDataLength);
Sleep(5);
}
@@ -5882,7 +5901,7 @@ HSI_STATUS HSI_Motion::DCCScanSetData(UINT AxisTypes, HSI_SCAN_MOTION_TYPE eType
m_SendDCCData[z++] = (static_cast<long>(dTrigDis[j] / m_Resolution[axisNum]) >> 16) & 0xff;
m_SendDCCData[z++] = static_cast<long>(dTrigDis[j] / m_Resolution[axisNum]) >> 24;
}
m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
//m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
Sleep(1);
}
if ((num > 0) && (lTrigNumber % 14 != 0))
@@ -5900,7 +5919,7 @@ HSI_STATUS HSI_Motion::DCCScanSetData(UINT AxisTypes, HSI_SCAN_MOTION_TYPE eType
m_SendDCCData[z++] = (static_cast<long>(limit / m_Resolution[axisNum]) >> 8) & 0xff;
m_SendDCCData[z++] = (static_cast<long>(limit / m_Resolution[axisNum]) >> 16) & 0xff;
m_SendDCCData[z++] = static_cast<long>(limit / m_Resolution[axisNum]) >> 24;
m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
//m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
Sleep(10);
}
else if (num == 0)
@@ -5918,7 +5937,7 @@ HSI_STATUS HSI_Motion::DCCScanSetData(UINT AxisTypes, HSI_SCAN_MOTION_TYPE eType
m_SendDCCData[z++] = (static_cast<long>(limit / m_Resolution[axisNum]) >> 8) & 0xff;
m_SendDCCData[z++] = (static_cast<long>(limit / m_Resolution[axisNum]) >> 16) & 0xff;
m_SendDCCData[z++] = static_cast<long>(limit / m_Resolution[axisNum]) >> 24;
m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
//m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
Sleep(10);
}
break;
@@ -6025,7 +6044,7 @@ HSI_STATUS HSI_Motion::DCCScanSetData(UINT AxisTypes, HSI_SCAN_MOTION_TYPE eType
m_SendDCCData[5] = (static_cast<long>(dTrigDis[0] / m_Resolution[axisNum]) >> 8) & 0xff;
m_SendDCCData[6] = (static_cast<long>(dTrigDis[0] / m_Resolution[axisNum]) >> 16) & 0xff;
m_SendDCCData[7] = static_cast<long>(dTrigDis[0] / m_Resolution[axisNum]) >> 24;
m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
//m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
Sleep(10);
break;
}
@@ -6137,14 +6156,22 @@ HSI_STATUS HSI_Motion::DCCScanSetData(UINT AxisTypes, HSI_SCAN_MOTION_TYPE eType
m_SendDCCData[j++] = (static_cast<long>(dTrigDis[i] / m_Resolution[axisNum]) >> 16) & 0xff;
m_SendDCCData[j++] = static_cast<long>(dTrigDis[i] / m_Resolution[axisNum]) >> 24;
}
m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
g_pLogger->SendAndFlushWithTime(L"[DCCScanSetData] Send_Command: %s\n", (const char*)m_SendDCCData);
//m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
Sleep(10);
break;
}
default:
break;
}
//启动定时锁存的同时，启动扫描 外部IO
unsigned char m_cSendData[8] = { 0 };
//清空EF3缓存Flash
m_cSendData[0] = 0x01;
m_cSendData[1] = 0x04;
m_WriteByte = Send_Command(0, (const char*)m_cSendData, 2);
Sleep(20);
g_pLogger->SendAndFlushWithTime(L"[DCCScanSetData] Out\n");
}
ReleaseMutex(g_WR_ToMove_Mutex);
@@ -6226,7 +6253,7 @@ HSI_STATUS HSI_Motion::DCCScanStart()
m_SetTriggerLightData[0] = CT_LIGHT;
m_SetTriggerLightData[1] = 0x03;
m_SetTriggerLightData[53] = 0;
m_WriteByte = Send_Command(0, (const char*)m_SetTriggerLightData, m_SendDataLength);
//m_WriteByte = Send_Command(0, (const char*)m_SetTriggerLightData, m_SendDataLength);
Sleep(1);
}
setLightFlag = false;
@@ -6268,12 +6295,13 @@ HSI_STATUS HSI_Motion::DCCScanStart()
break;
}
g_pLogger->SendAndFlushWithTime(
L"[DCCScanStart] iaxisNum:%d, iTriggleNum:%d, iMotionDirection:%d,begin_position:%d \n", iaxisNum,
iTriggleNum, iMotionDirection, begin_position);
L"[DCCScanStart] iaxisNum:%d, iTriggleNum:%d, iMotionDirection:%d, begin_position:%d \n", iaxisNum,
iTriggleNum, iMotionDirection, begin_position[1]);
//m_WriteByte = Send_Command(0, (const char*)m_SendDCCData, m_SendDataLength);
//启动定时锁存的同时，启动扫描 外部IO
unsigned char m_cSendData[8] = {0};
unsigned char m_cSendData[64] = {0};
m_cSendData[0] = 0x01;
m_cSendData[1] = 0x02;
m_WriteByte = Send_Command(0, (const char*)m_cSendData, 2);
@@ -8802,7 +8830,7 @@ BOOL HSI_Motion::Send_Command(int com, const char* _SendData, DWORD SendDataLeng
}
//打印串口返回值
Sleep(10);
Sleep(5);
m_SO7_Serial.OnReceive();
}
ReleaseMutex(g_RW_Data_Mutex);