|
01 ZMC432-V2運動控制器介紹
ZMC432-V2高性能多軸運動控制器是一款兼容EtherCAT總線和脈沖型的獨立式運動控制器，自帶6軸本地差分脈沖軸，最多可擴展至32軸，能實現總線軸+脈沖軸混合插補的多軸運動控制場合。同時支持正運動遠程顯示功能，能提供網絡組態(tài)顯示，可實時監(jiān)控和調整參數配置。
ZMC432-V2硬件功能特性：
（1）支持32軸運動控制（脈沖+EtherCAT總線），EtherCAT最小通訊周期可達125us；
（2）24路通用輸入、12路通用輸出，2路模擬量輸出(DA)，其中包括2路高速輸入和2路高速輸出；
（3）6路差分脈沖軸輸出，總線軸、脈沖軸可混合插補；
（4）內置多項實時性運動控制功能，例如視覺飛拍、多維PSO、高速位置鎖存，多軸同步運行等；
（5）可通過EtherCAT擴展模塊進行IO硬件資源擴展，可擴展至4096個隔離輸入口和4096個隔離輸出口；
（6）具備豐富的運動控制功能，如點位運動、電子凸輪、直線插補、圓弧插補、連續(xù)軌跡加工；
（7）支持掉電檢測、掉電存儲，多種程序加密方式，能夠有效防止系統故障，保護項目工程文件數據，并提高系統的可靠性；
（8）通過純國產IDE開發(fā)環(huán)境RTSys進行項目開發(fā)，可實時仿真、在線跟蹤以及診斷與調試，簡便易用，支持多種高級上位機語言聯合編程進行二次開發(fā)。
02 內容回顧
回顧上期推文，我們對CAD導圖和小線段速度前瞻的C#Demo中的CAD導圖模塊進行了大致的介紹（詳情點擊→C#運動控制開源(一):CAD導圖和小線段速度前瞻的優(yōu)化之CAD導圖），本期推文我們將對參數設置和前瞻優(yōu)化以及運動指令下發(fā)這三個功能進行介紹。
參數設置主要是設置對應軸參數和加工參數。
前瞻優(yōu)化的作用尤為關鍵，若CAD導圖軌跡為復雜異形軌跡，導出后往往會生成大量小線段。這時候想要保證機臺加工高效平穩(wěn)，則需要通過運動前瞻算法對軌跡進行一定程度的平滑，并且在拐彎點合理降速，曲線段整體也要合理降速，保證各分軸速度連續(xù)不出現速度突變。
在前瞻優(yōu)化過后則需要用對應函數把軌跡下發(fā)到控制器中運行。
03 C#使用運動函數庫和軌跡平滑庫進行參數設置和前瞻優(yōu)化
正運動技術提供開放的Zmotion，ZAuxdll運動函數庫和專門的ZTrackSmooth前瞻平滑庫，可以對CAD等獲得的軌跡進行前瞻優(yōu)化，通過運動函數庫下發(fā)到控制器中執(zhí)行，保證運行的高效平穩(wěn)。
1.在VS2019菜單“文件”→“新建”→“項目”，啟動創(chuàng)建項目向導。
2.選擇開發(fā)語言為“C#”和Windows窗體應用程序，點擊下一步。
3.配置好項目名稱和位置，以及相應框架，點擊創(chuàng)建。
4.找到廠家提供的光盤資料里面的C#函數庫，路徑如下（64位庫為例）。
進入廠商提供的光盤資料，找到zauxdll.dll，zmotion.dll和Zmcaux.cs這三個庫文件。庫文件路徑：【00光盤資料】→【04PC函數】→【01PC函數庫V2.1】→【Windows平臺】→【C#】→【64位】→【庫文件】。
5.將廠商提供的C#的庫文件以及相關文件復制到新建的項目中。
（1）將zmcaux.cs文件復制到新建的項目里面中。
（2）將zauxdll.dll和zmotion.dll文件放入bin\debug文件夾中。
（3）將Zmcaux.cs文件添加進項目中。右鍵項目名稱，選擇添加，再選擇現有項，選擇Zmcaux.cs文件。
6.雙擊Form1.cs里面的Form1，出現代碼編輯界面，在文件開頭寫入using cszmcaux。
7.至此，項目新建完成，可進行C#項目開發(fā)。
例程界面如下：
前瞻優(yōu)化下發(fā)運動流程：
這次推文介紹到的參數設置，前瞻優(yōu)化，運動指令下發(fā)的內容基本都放在了ZmotionCard.cs中，方便大家學習。
功能一：軸參數配置以及單軸測試
步驟1：連接控制器并初始化參數
ZAux_OpenEth(string ipaddr, out IntPtr phandle) //網口連接控制器
ZAux_Direct_SetUnits(IntPtr handle, int iaxis, float fValue) //設置脈沖當量
ZAux_Direct_SetSpeed(IntPtr handle, int iaxis, float fValue) //設置速度
ZAux_Direct_SetDecel(IntPtr handle, int iaxis, float fValue) //設置加速度
ZAux_Direct_SetSramp(IntPtr handle, int iaxis, float fValue) //設置加減速時間ZAux_Direct_SetFastDec(IntPtr handle, int iaxis, float iValue) //設置快減減速度
正運動的所有函數都可以接受函數返回值，非0即是錯誤碼，可以用錯誤碼判斷函數下發(fā)情況
通過ZAux_OpenEth連接控制器，然后讀取對應參數設置到控制器中，具體代碼實現:
#region
初始化運動控制控卡
public bool ZmcCardIni(string strIpAddress)
{
//讀取運動控制參數
ReadPara();
int iret = 0;
//打開運動控制器(ip"192.168.0.11" 正運動控制卡 ; ip"127.0.0.1" 正運動仿真器)
iret += zmcaux.ZAux_OpenEth(strIpAddress, out g_handle); CommandHandler("ZAux_OpenEth", iret);
//設置初始軸參數
iret += AxisParaSet();
//返回結果：如果運動控制指令返回值為0，初始化成功
return iret == 0 ? m_bInited = true : m_bInited = false;
}
#endregion
#region
設置軸參數
public int AxisParaSet()
{
int iret = 0;
int[] axisnum = { m_nAxis_X, m_nAxis_Y, m_nAxis_Z, m_nAxis_U };
float[] units = { (float)(m_nAxis_X_OnePul * m_dAxis_X_Ratio / m_dAxis_X_Lead), (float)(m_nAxis_Y_OnePul * m_dAxis_Y_Ratio / m_dAxis_Y_Lead), (float)(m_nAxis_Z_OnePul * m_dAxis_Z_Ratio / m_dAxis_Z_Lead), (float)(m_nAxis_U_OnePul * m_dAxis_U_Ratio / m_dAxis_U_Lead) }; //脈沖當量,單位pulse
float[] speed = { (float)m_dAxis_X_NormalSpeed, (float)m_dAxis_Y_NormalSpeed, (float)m_dAxis_Z_NormalSpeed, (float)m_dAxis_U_NormalSpeed }; //初始速度,units/s
float[] accel = { (float)m_dAxis_X_Acc, (float)m_dAxis_Y_Acc, (float)m_dAxis_Z_Acc, (float)m_dAxis_U_Acc }; //初始加速度,units/s2
float[] decel = { (float)m_dAxis_X_Dec, (float)m_dAxis_Y_Acc, (float)m_dAxis_Z_Acc, (float)m_dAxis_U_Acc }; //初始減速度,units/s2
float[] sramp = { m_nAxis_X_Sramp, m_nAxis_Y_Sramp, m_nAxis_Z_Sramp, m_nAxis_U_Sramp }; //加減速時間
float[] fslimit = { (float)m_dAxis_X_Fs, (float)m_dAxis_Y_Fs, (float)m_dAxis_Z_Fs, (float)m_dAxis_U_Fs }; //正軟限位
float[] rslimit = { (float)m_dAxis_X_Rs, (float)m_dAxis_Y_Rs, (float)m_dAxis_Z_Rs, (float)m_dAxis_U_Rs }; //負軟限位
int[] datumin = { m_nAxis_X_DatumIn, m_nAxis_Y_DatumIn, m_nAxis_Z_DatumIn, m_nAxis_U_DatumIn }; //原點in
int[] fwdin = { m_nAxis_X_FwdIn, m_nAxis_Y_FwdIn, m_nAxis_Z_FwdIn, m_nAxis_U_FwdIn }; //正限位in
int[] revin = { m_nAxis_X_RevIn, m_nAxis_Y_RevIn, m_nAxis_Z_RevIn, m_nAxis_U_RevIn }; //負限位in
float[] fastdec = { (float)m_dAxis_X_FastDec, (float)m_dAxis_Y_FastDec, (float)m_dAxis_Z_FastDec, (float)m_dAxis_U_FastDec }; //初始急停減速度,units/s2
float[] creep = { (float)m_dAxis_X_Creep, (float)m_dAxis_Y_Creep, (float)m_dAxis_Z_Creep, (float)m_dAxis_U_Creep }; //初始回原爬行速度,units/s
//若句柄g_handle為0說明鏈接控制卡失敗,返回值sRtn為非0錯誤碼。
if (g_handle != (IntPtr)0)
{
for (int i = 0; i < m_AxisNum; i++)
{
//設置各軸的運動參數
iret += zmcaux.ZAux_Direct_SetUnits(g_handle, axisnum[i], units[i]); CommandHandler("ZAux_Direct_SetUnits", iret);
iret += zmcaux.ZAux_Direct_SetSpeed(g_handle, axisnum[i], speed[i]); CommandHandler("ZAux_Direct_SetSpeed", iret);
iret += zmcaux.ZAux_Direct_SetAccel(g_handle, axisnum[i], accel[i]); CommandHandler("ZAux_Direct_SetAccel", iret);
iret += zmcaux.ZAux_Direct_SetDecel(g_handle, axisnum[i], decel[i]); CommandHandler("ZAux_Direct_SetDecel", iret);
iret += zmcaux.ZAux_Direct_SetSramp(g_handle, axisnum[i], sramp[i]); CommandHandler("ZAux_Direct_SetSramp", iret);
iret += zmcaux.ZAux_Direct_SetFastDec(g_handle, axisnum[i], fastdec[i]); CommandHandler("ZAux_Direct_SetFastDec", iret);
//設置各軸的正限位、負限位、原點、報警信號 所對應的輸入口
iret += zmcaux.ZAux_Direct_SetFsLimit(g_handle, axisnum[i], fslimit[i]); CommandHandler("ZAux_Direct_SetFsLimit", iret);
iret += zmcaux.ZAux_Direct_SetRsLimit(g_handle, axisnum[i], rslimit[i]); CommandHandler("ZAux_Direct_SetRsLimit", iret);
iret += zmcaux.ZAux_Direct_SetFwdIn(g_handle, axisnum[i], fwdin[i]); CommandHandler("ZAux_Direct_SetFwdIn", iret);
iret += zmcaux.ZAux_Direct_SetRevIn(g_handle, axisnum[i], revin[i]); CommandHandler("ZAux_Direct_SetRevIn", iret);
iret += zmcaux.ZAux_Direct_SetDatumIn(g_handle, axisnum[i], datumin[i]); CommandHandler("ZAux_Direct_SetDatumIn", iret);
}
}
return iret;
}
#endregion
?步驟2：單軸測試
ZAux_Direct_Single_Vmove(IntPtr handle, int iaxis, int idir) //單軸持續(xù)運動ZAux_Direct_Single_Move(IntPtr handle, int iaxis, float fdistance) //單軸相對運動ZAux_Direct_Single_MoveAbs(IntPtr handle, int iaxis, float fdistance)//絕對運動ZAux_Direct_Single_Cancel(IntPtr handle, int iaxis, int imode)//單軸停止
ZAux_Direct_SetFsLimit(IntPtr handle, int iaxis, float fValue) //設置正向軟限位
ZAux_Direct_SetRsLimit(IntPtr handle, int iaxis, float fValue) //設置負向軟限位
ZAux_Direct_SetFwdIn(IntPtr handle, int iaxis, int iValue) //設置正硬限位映射
ZAux_Direct_SetRevIn(IntPtr handle, int iaxis, int iValue) //設置負硬限位映射
ZAux_Direct_SetDatumIn(IntPtr handle, int iaxis, int iValue) //設置原點映射
ZAux_Direct_Single_Datum(IntPtr handle, int iaxis, int imode) //控制器回零
ZAux_BusCmd_Datum(IntPtr handle, UInt32 iaxis, UInt32 homemode) //驅動器回零
設置好參數后可以通過按鈕調用對應單軸軸控功能進行測試，具體代碼如下:
#region
軸持續(xù)運動(不阻塞)
public int AxisVmove(int nAxis, bool bdir, float speed)
{
//判斷控制卡是否初始化成功
if (!m_bInited)
return -1; //-1代表初始化未成功
int iret = 0;
iret += zmcaux.ZAux_Direct_SetSpeed(g_handle, nAxis, speed); CommandHandler("ZAux_Direct_SetSpeed", iret);
if (bdir) //正向
{
iret += zmcaux.ZAux_Direct_Single_Vmove(g_handle, nAxis, 1); CommandHandler("ZAux_Direct_Single_Vmove", iret);
}
else //負向
{
iret += zmcaux.ZAux_Direct_Single_Vmove(g_handle, nAxis, -1); CommandHandler("ZAux_Direct_Single_Vmove", iret);
}
return iret;
}
#endregion
#region
軸相對運動(不阻塞)
public int AxisMove(int nAxis, float speed, float dis)
{ //判斷控制卡是否初始化成功
if (!m_bInited)
return -1; //-1代表初始化未成功
int iret = 0;
iret += zmcaux.ZAux_Direct_SetSpeed(g_handle, nAxis, speed); CommandHandler("ZAux_Direct_SetSpeed", iret);
iret += zmcaux.ZAux_Direct_Single_Move(g_handle, nAxis, dis); CommandHandler("ZAux_Direct_Single_Move", iret);
return iret;
}
#endregion
#region
軸絕對運動(不阻塞)
public int AxisMoveAbs(int nAxis, float speed, float dis)
{
//判斷控制卡是否初始化成功
if (!m_bInited)
return -1; //-1代表初始化未成功
int iret = 0;
iret += zmcaux.ZAux_Direct_SetSpeed(g_handle, nAxis, speed); CommandHandler("ZAux_Direct_SetSpeed", iret);
iret += zmcaux.ZAux_Direct_Single_MoveAbs(g_handle, nAxis, dis); CommandHandler("ZAux_Direct_Single_Move", iret);
return iret;
}
#endregion
#region
單軸停止(不阻塞)
public int AxisCancel(int nAxis)
{
//判斷控制卡是否初始化成功
if (!m_bInited)
return -1; //-1代表初始化未成功
int iret = 0;
iret += zmcaux.ZAux_Direct_Single_Cancel(g_handle, nAxis, 2); CommandHandler("ZAux_Direct_Single_Cancel", iret);
return iret;}#endregion#region檢測軸是否正常運動完成
public bool CheckAxisIdle(int imaxaxises, int[] piAxislist)
{
//判斷控制卡是否初始化成功
if (!m_bInited)
return false; //-1代表初始化未成功
int iret = 0;
for (int i = 0; i < imaxaxises; i++)
{
int idle = 0;
while (true)
{
iret = zmcaux.ZAux_Direct_GetIfIdle(g_handle, piAxislist[i], ref idle); CommandHandler("ZAux_Direct_GetIfIdle", iret);
if (iret != 0) return false;
if (idle == -1) break;
}
}
return true;
}
#endregion
#region軸單獨回原(下發(fā)進緩沖,程序不阻塞)
public int AxisHome(int nAxis, int homemode, float homespeed, float homecreep, bool ifbushome, float homeoffset)
{
//判斷控制卡是否初始化成功
if (!m_bInited)
return -1; //-1代表初始化未成功
int iret = 0;
StringBuilder cmdbuffack = new StringBuilder(2048);
iret += zmcaux.ZAux_Direct_SetSpeed(g_handle, nAxis, homespeed); CommandHandler("ZAux_Direct_SetSpeed", iret);
iret += zmcaux.ZAux_Direct_SetCreep(g_handle, nAxis, homecreep); CommandHandler("ZAux_Direct_SetCreep", iret);
iret += zmcaux.ZAux_DirectCommand(g_handle, $"DATUM_OFFSET({nAxis}) = {homeoffset}", cmdbuffack, 2048); CommandHandler("ZAux_DirectCommand", iret);
if (!ifbushome)
{
//控制器回零
iret += zmcaux.ZAux_Direct_Single_Datum(g_handle, nAxis, homemode); CommandHandler("ZAux_Direct_Single_Datum", iret);
}
else
{
//驅動器回零
iret += zmcaux.ZAux_BusCmd_Datum(g_handle, (uint)nAxis, (uint)homemode); CommandHandler("ZAux_BusCmd_Datum", iret);
}
return iret;
}
#endregion
功能二：前瞻參數設置
?步驟1：設置控制器前瞻參數
之前推文也有介紹過，控制器的運動前瞻主要通過Corner_mode進行設置�？刂破鞯那罢爸饕幸韵聨讉�功能:
（一）Corner_Mode設置
1.拐角減速
拐角減速功能解決的問題是：當指令間夾角過大時，如果仍以較大速度運行，會在夾角處產生較大的機械沖擊，軌跡偏離。
控制器會對指令間軌跡變化的夾角進行提前識別，比較其與減速/停止角的大小關系，提前決定是否進行減速，保證在指令連接處平穩(wěn)過渡。
如圖，OA過渡AB段位置時角度小于減速角度則，S1-S2段不進行減速，AB過渡BC段時角度大于減速角度則進行減速處理過渡過程如S2-S3段，BC過渡CD段角度大于停止角度速度需要降到零如S3-S4段位置處理。執(zhí)行效果如下：
（1）未開啟拐角減速
（2）開啟拐角減速
→達到減速角度，未達到停止角度，部分減速。
→達到停止角度，完全減速。
2.小圓限速
小圓限速功能用于處理在運行軌跡中可能運行圓弧軌跡擬合成的小圓，由于角度偏轉較大導致出現軌跡偏轉，因此在這種位置需要進行速度限制的處理。開啟小圓限速，小圓半徑超過限速半徑的時候不會對速度限制，小圓半徑小于限速半徑的時候則會開始對速度進行限制。
3.自動倒角
自動倒角功能一般是用于拐角處按照一定的倒角半徑進行軌跡的弧度化處理，使速度變化更平滑。如圖所示：
（1）未開啟倒角
（2）開啟倒角
（二）適用于小線段應用的新平滑指令(高系列控制器支持)
針對連續(xù)小線段應用，可以開啟zsmooth_mode平滑速度曲線模式，效果顯著。
1.啟用平滑速度曲線模式
①zsmooth_start()
②zsmooth_end()
上述兩個basic指令是用于開啟和關閉新的平滑模式，這兩個指令都是進入緩沖的，記得一定要調用zsmooth_end指令，否則可能導致最后幾個小運動段位于等待狀態(tài)�？梢蕴崆跋日{用一次zsmooth_end，以防上次沒有正常關閉。
在PC程序里調用的話可以用Execute函數發(fā)送字符串，注意：zsmooth_start()是作用于某個軸的，如果前面沒有base指令的話，就要用axis指令指定軸號，用法應該是zsmooth_start() axis(xxx)，而不是zsmooth_start(xxx)。下圖execute示例主軸為軸0。
開啟新的平滑模式后，zsmooth的效果就非常強。數值越大越平滑，但如果小線段點間隔過大，并且zsmooth也很大，可能會造成運動軌跡變形。如果出現變形，要么減小zsmooth，或者減少小線段的間隔。
2.設置CORNER_ACCEL
CORNER_ACCEL類似之前的小圓限速，默認值0不生效，設置數值后替換FULL_SP_RADIUS。
用于在曲率較大的地方去合理降速。這個拐彎加速度的大致理解為v=sqrt(a * r)，這個a就是拐彎加速度。
3.設置JERK
JERK用來控制加加速度大小，可以讓合成速度的不平滑處更平滑，也會約束空跑階段的加速度大小。默認值0不生效，設置值后替換SRAMP。
相關函數：
ZAux_Direct_SetCornerMode(IntPtr handle, int iaxis, int pfValue)
ZAux_Direct_SetMerge(IntPtr handle, int iaxis, int iValue)
ZAux_Direct_SetFullSpRadius(IntPtr handle, int iaxis, float fValue)
ZAux_Direct_SetZsmooth(IntPtr handle, int iaxis, float fValue)
ZAux_Direct_SetDecelAngle(IntPtr handle, int iaxis, float fValue)
ZAux_Direct_SetStopAngle(IntPtr handle, int iaxis, float fValue)
ZAux_Direct_SetStartMoveSpeed(IntPtr handle, int iaxis, float fValue)
ZAux_Direct_SetEndMoveSpeed(IntPtr handle, int iaxis, float fValue)
在運動開始前設置好控制器運動前瞻參數，具體代碼如下
#region
控制器前瞻參數初始化
public bool RunParaIni()
{
//判斷控制卡是否初始化成功
if (!m_bInited)
return false; //-1代表初始化未成功
int iret = 0;
//控制器前瞻參數設置到主軸上
iret += zmcaux.ZAux_Direct_SetCornerMode(g_handle, m_nAxis_X, m_nCornerMode); CommandHandler("ZAux_Direct_SetCornerMode", iret); //Corner_Mode
iret += zmcaux.ZAux_Direct_SetMerge(g_handle, m_nAxis_X, 1); CommandHandler("ZAux_Direct_SetMerge", iret); //打開連續(xù)插補
iret += zmcaux.ZAux_Direct_SetFullSpRadius(g_handle, m_nAxis_X, m_fLimitR); CommandHandler("ZAux_Direct_SetFullSpRadius", iret); //設置小圓限速半徑
StringBuilder cmdbuffack = new StringBuilder(2048);
iret += zmcaux.ZAux_Execute(g_handle, "SPLIMIT_RADIUS(" + m_nAxis_X.ToString() + ") = " + m_fLimitRMinSp.ToString(), cmdbuffack, 2048); CommandHandler("ZAux_Execute", iret);//小圓限速最小限速 iret += zmcaux.ZAux_Direct_SetZsmooth(g_handle, m_nAxis_X, m_fZsmooth); CommandHandler("ZAux_Direct_SetZsmooth", iret); //倒角半徑
iret += zmcaux.ZAux_Direct_SetDecelAngle(g_handle, m_nAxis_X, (float)((m_fDecelAngle / 180) * Math.PI)); CommandHandler("ZAux_Direct_SetDecelAngle", iret); //減速角
iret += zmcaux.ZAux_Direct_SetStopAngle(g_handle, m_nAxis_X, (float)((m_fStopAngle / 180) * Math.PI)); CommandHandler("ZAux_Direct_SetStopAngle", iret); //停止角
iret += zmcaux.ZAux_Direct_SetStartMoveSpeed(g_handle, m_nAxis_X, m_fXYSpeed); CommandHandler("ZAux_Direct_SetStartMoveSpeed", iret); //起始速度設置成加工速度
iret += zmcaux.ZAux_Direct_SetEndMoveSpeed(g_handle, m_nAxis_X, m_fXYSpeed); CommandHandler("ZAux_Direct_SetEndMoveSpeed", iret); //結束速度設置成加工速度
if (iret != 0) return false;
return true;
}
#endregion
?步驟2：設置ZTrackSmooth平滑參數
在cad導出小線段數組后，可以通過ZTrackSmooth.dll庫中提供的ZTS_ContinueSmoothAndSpeed函數進行運動前瞻規(guī)劃，它的作用主要是通過給定參數用樣條方式去連續(xù)平滑軌跡，并且在拐彎點和曲線段合理降速。
主要函數：
public static extern Int32 ZTS_ContinueSmoothAndSpeed2(IntPtr pzmcHandle, double[] pDataPoints, UInt32 dataPointsNum, byte axisCnt
, double disErr, double splineDisPrecision, double cornerAccel, double maxSpeed, double[] plimitR
, double[] plimitRSp, UInt32 limitRCnt, out IntPtr pSmoothPoints
, out IntPtr parryLine, out IntPtr parryForceSp, out IntPtr parryLmtSp, out IntPtr parryR,ref UInt32 pSmoothPointsNum)
● pDataPoints：傳入的cad導出的小線段數組。
● pSmoothPoints：平滑過后的坐標點集。
● parryLmtSp：輸出每個點限速(mm/s)，對應每條線段，用movelimit運行，<=0就不用設置//。
● parryForceSp：輸出的每段force_speed運行速度。
● disErr：參考誤差系數（設置越大，拐點處軌跡誤差越大，默認可輸入1）。用來控制高級平滑算法平滑后的軌跡與原軌跡之間最大參考誤差，當此參數設置越大，平滑后的軌跡距離原軌跡的最大誤差也越大，同時平滑效果越好，運動時的速度可以達到更快，曲率大的地方過彎速度可以更快，反之則是相反的效果。
同一段軌跡：
此參數是設置為1.0
此參數設3.0
● splineDisPrecision：樣條最小線段的參考長度（設置的越小，樣條拆分越細）默認可輸入0.1。用來平滑高級平滑后的軌跡是原來離散點的參考倍數，默認0.1即可，代表10倍。此參數設置的倍數，平滑后的軌跡越密集，最小設置為4倍(0.25)，最大建議不要設置超過20(0.05)。
設置為4倍：
設置為10倍：
● plimitR：限速半徑數組，不同的限速半徑對應不同的限速值。
● plimitRSp：限速半徑速度數組，和限速半徑數組對應。
● limitRCnt：限速半徑的個數。用來根據曲率半徑對應的限制速度進行曲線段限速。和控制器底層的小圓限速區(qū)別在于：
①小圓限速只需要設置最大限速半徑，限制速度和最小速度，然后根據實際圓速度=限制速度*實際半徑/限速最大半徑進行線性限速；
②該函數可以通過設置分段限速，應對上面小圓限速效果不好的時候，更可以根據實際機臺效果進行設置，比如設置限速半徑和對應限速值為:
獲得CAD軌跡數據后可以通過ZTS_ContinueSmoothAndSpeed2進行前瞻優(yōu)化，然后按照一定格式下發(fā)進控制器，具體代碼如下:
#region
ZTrackSmooth函數平滑下發(fā)
public int SmoothByZTrackDown(double[] originalPos,ref float StartRDpos)
{
//判斷控制卡是否初始化成功
if (!m_bInited)
return -1; //-1代表初始化未成功
int iret = 0;
UInt32 pSmoothPointsNum = 0;
IntPtr pSmoothPoints = (IntPtr)0;
IntPtr parryLine = (IntPtr)0;
IntPtr parryForceSp = (IntPtr)0;
IntPtr parryLmtSp = (IntPtr)0;
IntPtr parryR = (IntPtr)0;
ZTrackSmooth.ZTS_Delete(pSmoothPoints);
ZTrackSmooth.ZTS_Delete(parryLine);
ZTrackSmooth.ZTS_Delete(parryForceSp);
ZTrackSmooth.ZTS_Delete(parryLmtSp);
ZTrackSmooth.ZTS_Delete(parryR);
//使用函數平滑
if (m_dLimitRArray.Length > 0) //有設置非線性小圓限速
iret = ZTrackSmooth.ZTS_ContinueSmoothAndSpeed2(g_handle, originalPos, Convert.ToUInt16(originalPos.Length / 2), 2, m_dDiserr, m_dSplineDis, m_dCornerAccel, m_fXYSpeed, m_dLimitRArray, m_dLimitRSpArray, (uint)m_dLimitRArray.Length, out pSmoothPoints, out parryLine, out parryForceSp, out parryLmtSp, out parryR, ref pSmoothPointsNum);
else
iret = ZTrackSmooth.ZTS_ContinueSmoothAndSpeed2(g_handle, originalPos, Convert.ToUInt16(originalPos.Length / 2), 2, m_dDiserr, m_dSplineDis, m_dCornerAccel, m_fXYSpeed, new double[] { 0.1, m_fLimitR }, new double[] { m_fXYSpeed, m_fXYSpeed }, 2, out pSmoothPoints, out parryLine, out parryForceSp, out parryLmtSp, out parryR, ref pSmoothPointsNum);
//提出平滑后的軌跡
double[] SmoothPos = new double[pSmoothPointsNum * 2];
double[] SmoothSpeed = new double[pSmoothPointsNum];
double[] SmoothLimSp = new double[pSmoothPointsNum];
double[] SmoothR = new double[pSmoothPointsNum];
Marshal.Copy(pSmoothPoints, SmoothPos, 0, (int)(pSmoothPointsNum * 2));
Marshal.Copy(parryForceSp, SmoothSpeed, 0, (int)(pSmoothPointsNum));
Marshal.Copy(parryLmtSp, SmoothLimSp, 0, (int)(pSmoothPointsNum));
Marshal.Copy(parryR, SmoothR, 0, (int)(pSmoothPointsNum));
StringBuilder cmdbuffack = new StringBuilder(2048);
double rdpos = StartRDpos;
//下發(fā)平滑軌跡
for (uint i = 0; i < pSmoothPointsNum; i++)
{
float[] targetpos = new float[2];
targetpos[0] = (float)SmoothPos[i * 2 + 0];
targetpos[1] = (float)SmoothPos[i * 2 + 1];
float runspeed = m_fXYSpeed;
float limitspeed = m_fXYSpeed;
runspeed = (float)SmoothSpeed[i];
limitspeed = (float)SmoothLimSp[i];
//計算該點需要轉的角度值
if (i < pSmoothPointsNum - 2 && i >= 0)
{
double moveangle = CalculateAngleWithX((float)SmoothPos[(i) * 2 + 0], (float)SmoothPos[(i) * 2 + 1], (float)SmoothPos[(i + 1) * 2 + 0], (float)SmoothPos[(i + 1) * 2 + 1]);
rdpos = MoveR(moveangle, StartRDpos);
}
StartRDpos = (float)rdpos;
if (m_b3FileDown) Z3pFile_LineString(targetpos[0], targetpos[1], rdpos, runspeed, limitspeed, i);
else Command_LineString(targetpos[0], targetpos[1], rdpos, runspeed, limitspeed, i);
}
ZTrackSmooth.ZTS_Delete(pSmoothPoints);
ZTrackSmooth.ZTS_Delete(parryLine);
ZTrackSmooth.ZTS_Delete(parryForceSp);
ZTrackSmooth.ZTS_Delete(parryLmtSp);
ZTrackSmooth.ZTS_Delete(parryR);
return iret;
}
#endregion
功能三：運動軌跡下發(fā)
?步驟1：函數形式批量下發(fā)
如果用傳統的PC調用API指令方式下發(fā)運動方式執(zhí)行，指令調用速度可能遠長于運動執(zhí)行時間，造成運動不連續(xù)，控制效果不理想的情況。
正運動的Zaux_DirectCommand函數可以自定義封裝下發(fā)批量運動指令，實現效率的提升。
相關函數：
ZAux_DirectCommand(IntPtr handle, string pszCommand, StringBuilder psResponse, UInt32 uiResponseLength)
ZAux_Execute(IntPtr handle, string pszCommand, StringBuilder psResponse, UInt32 uiResponseLength)
對應下發(fā)代碼：
#region
自定義封裝函數批量下發(fā)
public void CommandIni()
{
StringBuilder cmdbuffack = new StringBuilder(2048);
//選擇軸組
int iret = zmcaux.ZAux_DirectCommand(g_handle, $"BASE({m_nAxis_X},{m_nAxis_Y},{m_nAxis_Z},{m_nAxis_U})\r\n", cmdbuffack, 2048);
CommandHandler("ZAux_DirectCommand", iret);
}
//生成閉合曲線起點 運動加落刀
public void Command_StartString(double Xpos, double Ypos, double angle, ref float CurRDpos)
{
StringBuilder cmdbuffack = new StringBuilder(2048);
StringBuilder cmdstr = new StringBuilder(2048);
int iret = 0;
//空移參數設置
cmdstr.Append($"MOVE_PARA(SPEED,{m_nAxis_X},{m_fCMoveSpeed})\r\n"); //設置空移速度
cmdstr.Append($"MOVE_PARA(ACCEL,{m_nAxis_X},{m_fCMoveAccel})\r\n"); //設置空移加速度
cmdstr.Append($"MOVE_PARA(DECEL,{m_nAxis_X},{m_fCMoveDecel})\r\n"); //設置空移減速度
cmdstr.Append($"MOVE_PARA(JERK,{m_nAxis_X},{m_fCMoveJerk})\r\n"); //設置空移JERK
cmdstr.Append($"MOVE_PARA(SRAMP,{m_nAxis_X},{m_fCMoveSramp})\r\n"); //設置空移SRAMP
cmdstr.Append($"FORCE_SPEED={m_fCMoveSpeed}\r\n"); //設置空移速度
double rDpos;
rDpos = MoveR(angle, CurRDpos); //計算起始角度
CurRDpos = (float)rDpos;
//空移到起點
cmdstr.Append($"MOVEABSSP({Math.Round(Xpos, 4)},{Math.Round(Ypos, 4)},{m_fZSafePos},{rDpos})\r\n");
while (true)
{
int idle = 0;
iret = zmcaux.ZAux_Direct_GetIfIdle(g_handle, m_nAxis_X, ref idle);
CommandHandler("ZAux_Direct_GetIfIdle", iret);
if (idle != 0)
break;
}
//Z軸參數設置
cmdstr.Append($"MOVE_PARA(SPEED,{m_nAxis_X},{m_fZSpeed})\r\n"); //設置空移速度
cmdstr.Append($"MOVE_PARA(ACCEL,{m_nAxis_X},{m_fZAccel})\r\n"); //設置空移加速度
cmdstr.Append($"MOVE_PARA(DECEL,{m_nAxis_X},{m_fZDecel})\r\n"); //設置空移減速度
cmdstr.Append($"MOVE_PARA(JERK,{m_nAxis_X},{m_fZJerk})\r\n"); //設置空移JERK
cmdstr.Append($"MOVE_PARA(SRAMP,{m_nAxis_X},{m_fZSramp})\r\n"); //設置空移SRAMP
cmdstr.Append($"FORCE_SPEED={m_fZSpeed}\r\n"); //設置空移速度
//落刀
cmdstr.Append($"MOVEABSSP({Math.Round(Xpos, 4)},{Math.Round(Ypos, 4)},{m_fZDownPos},{rDpos})\r\n");
while (true)
{
int idle = 0;
iret = zmcaux.ZAux_Direct_GetIfIdle(g_handle, m_nAxis_X, ref idle);
CommandHandler("ZAux_Direct_GetIfIdle", iret);
if (idle != 0)break;
}
//落刀延時
//切割軌跡起始參數設置
if (m_bZsmoothMode == true)
{
//cmdstr.Append($"ZSMOOTH_START()AXIS({m_nAxis_X})\r\n"); //設置多段合一的小線段模式
iret = zmcaux.ZAux_Execute(g_handle, $"ZSMOOTH_START()AXIS({m_nAxis_X})\r\n", cmdbuffack, 2048);CommandHandler("ZAux_Execute", iret);
}
cmdstr.Append($"MOVE_PARA(ACCEL,{m_nAxis_X},{m_fXYAccel})\r\n"); //設置切割加速度
cmdstr.Append($"MOVE_PARA(DECEL,{m_nAxis_X},{m_fXYDecel})\r\n"); //設置切割減速度
cmdstr.Append($"MOVE_PARA(JERK,{m_nAxis_X},{m_fXYJerk})\r\n"); //設置切割JERK
cmdstr.Append($"MOVE_PARA(SRAMP,{m_nAxis_X},{m_fXYSramp})\r\n"); //設置切割SRAMP
cmdstr.Append($"MOVE_PARA(SPEED,{m_nAxis_X},{m_fXYSpeed})\r\n"); //設置切割速度
cmdstr.Append($"FORCE_SPEED={m_fXYSpeed}\r\n"); //設置切割速度
m_dSetSpeed = m_fXYSpeed;
//判斷緩沖區(qū)是否有剩余
while (true)
{
int showbuff = 0;
zmcaux.ZAux_Direct_GetRemain_LineBuffer(g_handle, m_nAxis_X, ref showbuff);
if (showbuff > 128)break;
}
iret = zmcaux.ZAux_DirectCommand(g_handle, cmdstr.ToString(), cmdbuffack, 2048);
CommandHandler("ZAux_DirectCommand", iret);
}
//生成切割段字符串
public void Command_LineString(double Xpos, double Ypos, double angle, float runspeed, float lmtspeed, uint num)
{
StringBuilder cmdbuffack = new StringBuilder(2048);
StringBuilder cmdstr = new StringBuilder(2048);
//移動命令
if (m_dSetSpeed != runspeed) //避免每條重復寫速度，減小3次文件內容
{
m_dSetSpeed = runspeed;
cmdstr.Append($"MOVE_PARA(SPEED,{m_nAxis_X},{runspeed})\r\n"); //設置速度
cmdstr.Append($"FORCE_SPEED={runspeed}\r\n"); //設置速度
}
cmdstr.Append($"MOVEABSSP({Math.Round(Xpos, 4)},{Math.Round(Ypos, 4)},{m_fZDownPos},{angle})\r\n");
//判斷緩沖區(qū)是否有剩余
while (true)
{
int showbuff = 0;
zmcaux.ZAux_Direct_GetRemain_LineBuffer(g_handle, m_nAxis_X, ref showbuff);
if (showbuff > 128)break;
}
int iret = zmcaux.ZAux_DirectCommand(g_handle, cmdstr.ToString(), cmdbuffack, 2048);
CommandHandler("ZAux_DirectCommand", iret);
if (num > 0)
{
if (lmtspeed > 0)
{
if (lmtspeed < runspeed - 1) //cmdstr.Append($"MOVELIMIT({lmtspeed} - 1)\r\n");
iret = zmcaux.ZAux_Direct_MoveLimit(g_handle, m_nAxis_X, lmtspeed - 1); CommandHandler("ZAux_Direct_MoveLimit", iret);
}
}
}
}
//生成閉合曲線起點 抬刀
public void Command_EndString(double Xpos, double Ypos)
{
StringBuilder cmdbuffack = new StringBuilder(2048);
StringBuilder cmdstr = new StringBuilder(2048);
int iret = 0;
if (m_bZsmoothMode == true)
{
//cmdstr.Append($"ZSMOOTH_END()AXIS({m_nAxis_X})\r\n"); //設置多段合一的小線段模式
iret = zmcaux.ZAux_Execute(g_handle, $"ZSMOOTH_END()AXIS({m_nAxis_X})\r\n", cmdbuffack, 2048); CommandHandler("ZAux_Execute", iret);
}
while (true)
{
int idle = 0;
iret = zmcaux.ZAux_Direct_GetIfIdle(g_handle, m_nAxis_X, ref idle);CommandHandler("ZAux_Direct_GetIfIdle", iret);
if (idle != 0) break;
}
//Z軸參數設置
cmdstr.Append($"MOVE_PARA(SPEED,{m_nAxis_X},{m_fZSpeed})\r\n"); //設置Z速度
cmdstr.Append($"MOVE_PARA(ACCEL,{m_nAxis_X},{m_fZAccel})\r\n"); //設置Z加速度
cmdstr.Append($"MOVE_PARA(DECEL,{m_nAxis_X},{m_fZDecel})\r\n"); //設置Z減速度
cmdstr.Append($"MOVE_PARA(JERK,{m_nAxis_X},{m_fZJerk})\r\n"); //設置ZJERK
cmdstr.Append($"MOVE_PARA(SRAMP,{m_nAxis_X},{m_fZSramp})\r\n"); //設置ZSRAMP
cmdstr.Append($"FORCE_SPEED={m_fZSpeed}\r\n"); //設置Z速度
//抬刀
cmdstr.Append($"MOVEABSSP({Math.Round(Xpos, 4)},{Math.Round(Ypos, 4)},{m_fZSafePos})\r\n");
while (true)
{
int idle = 0;
iret = zmcaux.ZAux_Direct_GetIfIdle(g_handle, m_nAxis_X, ref idle);
CommandHandler("ZAux_Direct_GetIfIdle", iret);
if (idle != 0)break;
}
//判斷緩沖區(qū)是否有剩余
while (true)
{
int showbuff = 0;
zmcaux.ZAux_Direct_GetRemain_LineBuffer(g_handle, m_nAxis_X, ref showbuff);
if (showbuff > 128)break;
}
iret = zmcaux.ZAux_DirectCommand(g_handle, cmdstr.ToString(), cmdbuffack, 2048);CommandHandler("ZAux_DirectCommand", iret);
}
#endregion
?步驟2：通過三次文件下發(fā)(高系列控制器)
另外,正運動高系列控制器也可以通過支持3次文件加載的方式來減低PC交互，軌跡轉換生成3次文件，批量的加載到控制內獨立運行即可。
相關函數：
ZAux_3FileRamDownBegin(IntPtr handle, int ifile3num, ref UInt32 premainbyte, string File3Name)
ZAux_3FileRamGetRemainSpace(IntPtr handle, int ifile3num, ref UInt32 premainbyte)
ZAux_3FileRamDownPart(IntPtr handle, int ifile3num, StringBuilder pbuffer, UInt32 buffsize, ref UInt32 premainbyte)
ZAux_Run3FileRam(IntPtr handle, int ifile3num, int itasknum)
ZAux_3FileRamDownEnd(IntPtr handle, int ifile3num)
三次文件下發(fā)具體代碼:
#region
三次文件下發(fā)
// ZPJ3次文件運行初始化
public void Z3pFile_Init()
{
int iret;
uint iRemainSpace = 0;
iret = zmcaux.
ZAux_3FileRamDownBegin(g_handle, Z3P_FILE_NUM, ref iRemainSpace, "ZmcScan.z3p"); //開啟3次文件動態(tài)加載
m_Str3File.Length = 0;
m_bStartFile3Flag = false;
m_Str3File.Append($"BASE({m_nAxis_X},{m_nAxis_Y},{m_nAxis_Z},{m_nAxis_U})\r\n"); //設置運動
}
//生成閉合曲線起點 運動加落刀public void Z3pFile_StartString(double Xpos, double Ypos, double angle,ref float CurRDpos)
{
StringBuilder cmdbuffack = new StringBuilder(2048);
//空移參數設置
m_Str3File.Append($"MOVE_PARA(SPEED,{m_nAxis_X},{m_fCMoveSpeed})\r\n"); //設置空移速度
m_Str3File.Append($"MOVE_PARA(ACCEL,{m_nAxis_X},{m_fCMoveAccel})\r\n"); //設置空移加速度
m_Str3File.Append($"MOVE_PARA(DECEL,{m_nAxis_X},{m_fCMoveDecel})\r\n"); //設置空移減速度
m_Str3File.Append($"MOVE_PARA(JERK,{m_nAxis_X},{m_fCMoveJerk})\r\n"); //設置空移JERK
m_Str3File.Append($"MOVE_PARA(SRAMP,{m_nAxis_X},{m_fCMoveSramp})\r\n"); //設置空移SRAMP
m_Str3File.Append($"FORCE_SPEED={m_fCMoveSpeed}\r\n"); //設置空移速度
double rDpos;
rDpos = MoveR(angle, CurRDpos);//計算起始角度
CurRDpos = (float)rDpos;
//空移到起點
m_Str3File.Append($"MOVEABSSP({Math.Round(Xpos, 4)},{Math.Round(Ypos, 4)},{m_fZSafePos},{rDpos})\r\n");
m_Str3File.Append($"WAIT IDLE\r\n");
Console.WriteLine($"G00 X{Math.Round(Xpos, 4)} Y{Math.Round(Ypos, 4)}");
//Z軸參數設置
m_Str3File.Append($"MOVE_PARA(SPEED,{m_nAxis_X},{m_fZSpeed})\r\n"); //設置空移速度
m_Str3File.Append($"MOVE_PARA(ACCEL,{m_nAxis_X},{m_fZAccel})\r\n"); //設置空移加速度
m_Str3File.Append($"MOVE_PARA(DECEL,{m_nAxis_X},{m_fZDecel})\r\n"); //設置空移減速度
m_Str3File.Append($"MOVE_PARA(JERK,{m_nAxis_X},{m_fZJerk})\r\n"); //設置空移JERK
m_Str3File.Append($"MOVE_PARA(SRAMP,{m_nAxis_X},{m_fZSramp})\r\n"); //設置空移SRAMP
m_Str3File.Append($"FORCE_SPEED={m_fZSpeed}\r\n"); //設置空移速度
//落刀
m_Str3File.Append($"MOVEABSSP({Math.Round(Xpos, 4)},{Math.Round(Ypos, 4)},{m_fZDownPos},{rDpos})\r\n");
m_Str3File.Append($"WAIT IDLE\r\n");
//落刀延時
//切割軌跡起始參數設置
if (m_bZsmoothMode == true)
{
m_Str3File.Append($"ZSMOOTH_START()AXIS({m_nAxis_X})\r\n"); //設置多段合一的小線段模式
}
m_Str3File.Append($"MOVE_PARA(ACCEL,{m_nAxis_X},{m_fXYAccel})\r\n"); //設置切割加速度
m_Str3File.Append($"MOVE_PARA(DECEL,{m_nAxis_X},{m_fXYDecel})\r\n"); //設置切割減速度
m_Str3File.Append($"MOVE_PARA(JERK,{m_nAxis_X},{m_fXYJerk})\r\n"); //設置切割JERK
m_Str3File.Append($"MOVE_PARA(SRAMP,{m_nAxis_X},{m_fXYSramp})\r\n"); //設置切割SRAMP
m_Str3File.Append($"MOVE_PARA(SPEED,{m_nAxis_X},{m_fXYSpeed})\r\n"); //設置切割速度
m_Str3File.Append($"FORCE_SPEED={m_fXYSpeed}\r\n"); //設置切割速度
m_dSetSpeed = m_fXYSpeed;
Z3pFile_Down();}//生成切割段字符串
public void Z3pFile_LineString(double Xpos, double Ypos, double angle, float runspeed, float lmtspeed, uint num)
{
//移動命令
if (m_dSetSpeed != runspeed) //避免每條重復寫速度，減小3次文件內容
{
m_dSetSpeed = runspeed;
m_Str3File.Append($"MOVE_PARA(SPEED,{m_nAxis_X},{runspeed})\r\n"); //設置速度
m_Str3File.Append($"FORCE_SPEED={runspeed}\r\n"); //設置速度
}
m_Str3File.Append($"MOVEABSSP({Math.Round(Xpos, 4)},{Math.Round(Ypos, 4)},{m_fZDownPos},{angle})\r\n");
if (num > 0)
{
if (lmtspeed > 0)
{
if (lmtspeed < runspeed - 1) m_Str3File.Append($"MOVELIMIT({lmtspeed} - 1)\r\n");
}
}
Console.WriteLine($"G01 X{Math.Round(Xpos, 4)} Y{Math.Round(Ypos, 4)} S{runspeed} L{lmtspeed - 1}");
Z3pFile_Down();
}
//生成閉合曲線起點 抬刀
public void Z3pFile_EndString(double Xpos, double Ypos)
{
if (m_bZsmoothMode == true)
{
m_Str3File.Append($"ZSMOOTH_END()AXIS({m_nAxis_X})\r\n"); //設置多段合一的小線段模式
}
m_Str3File.Append($"WAIT IDLE\r\n");
//Z軸參數設置
m_Str3File.Append($"MOVE_PARA(SPEED,{m_nAxis_X},{m_fZSpeed})\r\n"); //設置Z速度
m_Str3File.Append($"MOVE_PARA(ACCEL,{m_nAxis_X},{m_fZAccel})\r\n"); //設置Z加速度
m_Str3File.Append($"MOVE_PARA(DECEL,{m_nAxis_X},{m_fZDecel})\r\n"); //設置Z減速度
m_Str3File.Append($"MOVE_PARA(JERK,{m_nAxis_X},{m_fZJerk})\r\n"); //設置ZJERK
m_Str3File.Append($"MOVE_PARA(SRAMP,{m_nAxis_X},{m_fZSramp})\r\n"); //設置ZSRAMP
m_Str3File.Append($"FORCE_SPEED={m_fZSpeed}\r\n"); //設置Z速度
//抬刀
m_Str3File.Append($"MOVEABSSP({Math.Round(Xpos, 4)},{Math.Round(Ypos, 4)},{m_fZSafePos})\r\n");
m_Str3File.Append($"WAIT IDLE\r\n");
Z3pFile_Down();
}
//加載3次文件
public void Z3pFile_Down()
{
//判斷命令長度是否發(fā)送
if (m_Str3File.Length > FILE3_MAX_CHAR - 1000)
{
//動態(tài)加載程序
m_Str3File.Append("MOVE_RESUME\r\n");
int iret = 0;
uint nRemain = 0;
//等待3次文件緩沖足夠下發(fā)
iret = zmcaux.ZAux_3FileRamGetRemainSpace(g_handle, Z3P_FILE_NUM, ref nRemain);
while (nRemain <= (uint)m_Str3File.Length)
{
iret = zmcaux.ZAux_3FileRamGetRemainSpace(g_handle, Z3P_FILE_NUM, ref nRemain);
}
//加載3次文件字符
iret = zmcaux.ZAux_3FileRamDownPart(g_handle, Z3P_FILE_NUM, m_Str3File, (uint)m_Str3File.Length, ref nRemain); CommandHandler("ZAux_3FileRamDownPart",iret);
//System.IO.File.AppendAllText(@"C:\Users\32367\Desktop\ZMC_3ZPJ.bas", m_3FileStr.ToString());
m_Str3File.Length = 0;
if (m_bStartFile3Flag == false)
{
m_bStartFile3Flag = true;
iret = zmcaux.ZAux_Run3FileRam(g_handle, Z3P_FILE_NUM, Z3P_FILE_TASK); CommandHandler("ZAux_Run3FileRam", iret); //運行3次文件
}
}
}
//ZPJ文件加載結束
public void Z3pFile_End()
{
//關刀
m_Str3File.Append($"WAIT IDLE\r\n");
m_Str3File.Append("MOVE_RESUME\r\n");
uint nRemain = 0;
int iret = 0;
//等待3次文件緩沖足夠下發(fā)
iret = zmcaux.ZAux_3FileRamGetRemainSpace(g_handle, Z3P_FILE_NUM, ref nRemain);
while (nRemain <= (uint)m_Str3File.Length)
{
iret = zmcaux.ZAux_3FileRamGetRemainSpace(g_handle, Z3P_FILE_NUM, ref nRemain);
}
iret = zmcaux.ZAux_3FileRamDownPart(g_handle, Z3P_FILE_NUM, m_Str3File, (uint)m_Str3File.Length, ref nRemain); CommandHandler("ZAux_3FileRamDownPart", iret);
//System.IO.File.AppendAllText(@"C:\Users\32367\Desktop\ZMC_3ZPJ.bas", m_3FileStr.ToString());
m_Str3File.Length = 0;
if (m_bStartFile3Flag == false)
{
m_bStartFile3Flag = true;
iret = zmcaux.ZAux_Run3FileRam(g_handle, Z3P_FILE_NUM, Z3P_FILE_TASK); CommandHandler("ZAux_Run3FileRam", iret);//運行3次文件
}
//全部下載完成
iret = zmcaux.ZAux_3FileRamDownEnd(g_handle, Z3P_FILE_NUM); CommandHandler("ZAux_3FileRamDownEnd", iret);
m_bStartFile3Flag = false;
}
#endregion
功能四：輔助功能
?步驟1：讀寫參數
對應軸參數和前瞻參數在設置后需要保存，以便下次使用，程序里面通過kernel32.dll的功能，實現txt文本內容讀寫，用來保存設置數據。
?步驟2:計算R軸轉向
在插補運動的過程中，要使R非插補軸隨著插補運動的合成位移的變化而變化，從而實現在加工過程中，R軸始終處于合適的加工方向和位置的工藝。
程序中通過小線段前后的點位計算與X方向的夾角來確定實際R軸每段需要轉過的角度，其中還涉及到一些角度歸一以及旋轉最短路徑的計算。
相關代碼:
#region
計算R軸轉角
public double CalculateAngleWithX(double x1, double y1, double x2, double y2)
{
// 計算向量的模長
double length = Math.Sqrt(Math.Pow(x2 - x1, 2) + Math.Pow(y2 - y1, 2));
// 計算角度（弧度）
double radians = Math.Atan2(y2 - y1, x2 - x1);
// 將弧度轉換為度數
double degrees = radians * (180.0 / Math.PI);
return degrees;
}
//相對轉絕對
public double MoveR(double Absangle,double CurRDpos)
{
double lastAngle = CurRDpos;
double newAngle = NormalizeAngle((float)Absangle);
lastAngle = NormalizeAngle(lastAngle);
return CurRDpos + MinAngleDifference(lastAngle, newAngle);
}
public static double MinAngleDifference(double angle1, double angle2)
{
double difference = Math.Abs(angle2 - angle1);
double mindis = Math.Min(difference, 360 - difference);
if (angle2 - angle1 >= 0)
{
if (difference 360 - difference) difference = -(360 - difference);
}
else if (angle2 - angle1 < 0)
{
if (difference 360 - difference) difference = 360 - difference;
}
return difference;
}
public static double NormalizeAngle(double angle)
{
angle = angle % 360;
if (angle < 0)
{
angle += 360;
}
return angle;
}
public double CalculateAngle(double Pos_x1, double Pos_y1, double Pos_x2, double Pos_y2, double Pos_x1_2, double Pos_y1_2, double Pos_x2_2, double Pos_y2_2)
{
// 計算向量AB和CD的模長和點積
double abLength = Math.Sqrt(Math.Pow(Pos_x2 - Pos_x1, 2) + Math.Pow(Pos_y2 - Pos_y1, 2));
double cdLength = Math.Sqrt(Math.Pow(Pos_x2_2 - Pos_x1_2, 2) + Math.Pow(Pos_y2_2 - Pos_y1_2, 2));
double dotProduct = (Pos_x2 - Pos_x1) * (Pos_x2_2 - Pos_x1_2) + (Pos_y2 - Pos_y1) * (Pos_y2_2 - Pos_y1_2);
// 計算cos(θ)和θ
double cosTheta = dotProduct / (abLength * cdLength);
double theta = Math.Acos(cosTheta); // θ以弧度為單位
//Console.WriteLine("Angle between the vectors is: " + theta * (180 / Math.PI) + " degrees"); // 將弧度轉換為度數
double angle = theta * (180 / Math.PI);
return angle;
}
#endregion
?步驟3：當前XYR坐標在圖形上顯示
因為需要知道當前XYR軸實際的狀態(tài)，程序中通過定時器實時讀取XYR軸的當前坐標，并將其繪制到picbox控件上，方便用戶監(jiān)控。
相關函數：
ZAux_Direct_GetDpos(IntPtr handle, int iaxis, ref float pfValue) //讀取軸的指令位置
定時器代碼:
private void 讀取位置_Tick(object sender, EventArgs e)
{
if (m_GloFn.m_ZmotionCard.m_bInited && m_GloFn.m_ZCad.m_bInited)
{
int iret = 0;
iret = zmcaux.ZAux_Direct_GetDpos(m_GloFn.m_ZmotionCard.g_handle,m_GloFn.m_ZmotionCard.m_nAxis_X,ref XCurDpos);
m_GloFn.m_ZmotionCard.CommandHandler("ZAux_Direct_GetDpos", iret);
iret = zmcaux.ZAux_Direct_GetDpos(m_GloFn.m_ZmotionCard.g_handle, m_GloFn.m_ZmotionCard.m_nAxis_Y, ref YCurDpos);
m_GloFn.m_ZmotionCard.CommandHandler("ZAux_Direct_GetDpos", iret);
iret = zmcaux.ZAux_Direct_GetDpos(m_GloFn.m_ZmotionCard.g_handle, m_GloFn.m_ZmotionCard.m_nAxis_Z, ref ZCurDpos);
m_GloFn.m_ZmotionCard.CommandHandler("ZAux_Direct_GetDpos", iret);
iret = zmcaux.ZAux_Direct_GetDpos(m_GloFn.m_ZmotionCard.g_handle, m_GloFn.m_ZmotionCard.m_nAxis_U, ref UCurDpos);
m_GloFn.m_ZmotionCard.CommandHandler("ZAux_Direct_GetDpos", iret);
txt_XDpos.Text = XCurDpos.ToString();
txt_YDpos.Text = YCurDpos.ToString();
txt_ZDpos.Text = ZCurDpos.ToString();
txt_UDpos.Text = UCurDpos.ToString();
txt_ChooseNum.Text = m_GloFn.m_ZCad.m_nChooseVectNum.ToString();
tb_RunStatus.Text = RunStatus;
if (RunStatus == "自動運行中")
{
float vpspeed = 0;
iret = zmcaux.ZAux_Direct_GetVpSpeed(m_GloFn.m_ZmotionCard.g_handle, m_GloFn.m_ZmotionCard.m_nAxis_X, ref vpspeed);
m_GloFn.m_ZmotionCard.CommandHandler("ZAux_Direct_GetVpSpeed", iret);
txt_RunSpeed.Text = vpspeed.ToString() + "mm/s";
TimeSpan diff = DateTime.Now - WorkStart;
tb_RunTime.Text = diff.TotalMilliseconds.ToString() + "ms";
}
else
{
txt_RunSpeed.Text = "未運行";
tb_RunTime.Text = "未運行";
}
//繪制一遍CAD
if (!m_GloFn.m_ZCad.ZcadShowPic(CadShow.Width, CadShow.Height, 坐標系ToolStripMenuItem.Checked, 順序ToolStripMenuItem.Checked, 空移ToolStripMenuItem.Checked, 起點ToolStripMenuItem.Checked, 方向ToolStripMenuItem.Checked, XCurDpos, YCurDpos, UCurDpos))
{
return;
}
CadShow.Invalidate();
}
}
?步驟3：當前XYR坐標在圖形上顯示
因為需要知道當前XYR軸實際的狀態(tài)，程序中通過定時器實時讀取XYR軸的當前坐標，并將其繪制到picbox控件上，方便用戶監(jiān)控。
相關函數：
ZAux_Direct_GetDpos(IntPtr handle, int iaxis, ref float pfValue) //讀取軸的指令位置
定時器代碼:
private void 讀取位置_Tick(object sender, EventArgs e)
{
if (m_GloFn.m_ZmotionCard.m_bInited && m_GloFn.m_ZCad.m_bInited)
{
int iret = 0;
iret = zmcaux.ZAux_Direct_GetDpos(m_GloFn.m_ZmotionCard.g_handle,m_GloFn.m_ZmotionCard.m_nAxis_X,ref XCurDpos);
m_GloFn.m_ZmotionCard.CommandHandler("ZAux_Direct_GetDpos", iret);
iret = zmcaux.ZAux_Direct_GetDpos(m_GloFn.m_ZmotionCard.g_handle, m_GloFn.m_ZmotionCard.m_nAxis_Y, ref YCurDpos);
m_GloFn.m_ZmotionCard.CommandHandler("ZAux_Direct_GetDpos", iret);
iret = zmcaux.ZAux_Direct_GetDpos(m_GloFn.m_ZmotionCard.g_handle, m_GloFn.m_ZmotionCard.m_nAxis_Z, ref ZCurDpos);
m_GloFn.m_ZmotionCard.CommandHandler("ZAux_Direct_GetDpos", iret);
iret = zmcaux.ZAux_Direct_GetDpos(m_GloFn.m_ZmotionCard.g_handle, m_GloFn.m_ZmotionCard.m_nAxis_U, ref UCurDpos);
m_GloFn.m_ZmotionCard.CommandHandler("ZAux_Direct_GetDpos", iret);
txt_XDpos.Text = XCurDpos.ToString();
txt_YDpos.Text = YCurDpos.ToString();
txt_ZDpos.Text = ZCurDpos.ToString();
txt_UDpos.Text = UCurDpos.ToString();
txt_ChooseNum.Text = m_GloFn.m_ZCad.m_nChooseVectNum.ToString();
tb_RunStatus.Text = RunStatus;
if (RunStatus == "自動運行中")
{
float vpspeed = 0;
iret = zmcaux.ZAux_Direct_GetVpSpeed(m_GloFn.m_ZmotionCard.g_handle, m_GloFn.m_ZmotionCard.m_nAxis_X, ref vpspeed);
m_GloFn.m_ZmotionCard.CommandHandler("ZAux_Direct_GetVpSpeed", iret);
txt_RunSpeed.Text = vpspeed.ToString() + "mm/s";
TimeSpan diff = DateTime.Now - WorkStart;
tb_RunTime.Text = diff.TotalMilliseconds.ToString() + "ms";
}
else
{
txt_RunSpeed.Text = "未運行";
tb_RunTime.Text = "未運行";
}
//繪制一遍CAD
if (!m_GloFn.m_ZCad.ZcadShowPic(CadShow.Width, CadShow.Height, 坐標系ToolStripMenuItem.Checked, 順序ToolStripMenuItem.Checked, 空移ToolStripMenuItem.Checked, 起點ToolStripMenuItem.Checked, 方向ToolStripMenuItem.Checked, XCurDpos, YCurDpos, UCurDpos))
{
return;
}
CadShow.Invalidate();
}
}
04 DEMO效果演示
1.連接控制器,并導入CAD圖紙。
2.點擊視圖→參數設置開始設置各軸機械參數。
3.設置好軸參數后可以各個單軸測試回原，正反轉等，確定軸有沒有問題，沒有問題后點擊加工參數和前瞻參數，進行自動流程的設置，設好參數后點擊參數保存以便下次使用。
4.回到主界面可以在右邊操作面板進行操作，這時主界面上會顯示當前運行位置和U軸狀態(tài)。
5.點擊啟動BASIC代碼和G代碼標簽頁可以顯示實際生成的軌跡代碼。
6.上述步驟完成后點擊啟動就可以進行自動加工了。
教學視頻請點擊→C#運動控制開源(二): CAD導圖和小線段速度前瞻優(yōu)化
正運動技術專注于運動控制技術研究和通用運動控制軟硬件產品的研發(fā)，是國家級高新技術企業(yè)。正運動技術匯集了來自華為、中興等公司的優(yōu)秀人才，在堅持自主創(chuàng)新的同時，積極聯合各大高校協同運動控制基礎技術的研究。主要業(yè)務有：運動控制卡_運動控制器_EtherCAT運動控制卡_EtherCAT控制器_運動控制系統_視覺控制器__運動控制PLC_運動控制_機器人控制器_視覺定位_XPCIe/XPCI系列運動控制卡等等。
|
