|
01 ZMC432-V2運(yùn)動(dòng)控制器介紹
ZMC432-V2高性能多軸運(yùn)動(dòng)控制器是一款兼容EtherCAT總線(xiàn)和脈沖型的獨(dú)立式運(yùn)動(dòng)控制器，自帶6軸本地差分脈沖軸，最多可擴(kuò)展至32軸，能實(shí)現(xiàn)總線(xiàn)軸+脈沖軸混合插補(bǔ)的多軸運(yùn)動(dòng)控制場(chǎng)合。同時(shí)支持正運(yùn)動(dòng)遠(yuǎn)程顯示功能，能提供網(wǎng)絡(luò)組態(tài)顯示，可實(shí)時(shí)監(jiān)控和調(diào)整參數(shù)配置。
ZMC432-V2硬件功能特性：
（1）支持32軸運(yùn)動(dòng)控制（脈沖+EtherCAT總線(xiàn)），EtherCAT最小通訊周期可達(dá)125us；
（2）24路通用輸入、12路通用輸出，2路模擬量輸出(DA)，其中包括2路高速輸入和2路高速輸出；
（3）6路差分脈沖軸輸出，總線(xiàn)軸、脈沖軸可混合插補(bǔ)；
（4）內(nèi)置多項(xiàng)實(shí)時(shí)性運(yùn)動(dòng)控制功能，例如視覺(jué)飛拍、多維PSO、高速位置鎖存，多軸同步運(yùn)行等；
（5）可通過(guò)EtherCAT擴(kuò)展模塊進(jìn)行IO硬件資源擴(kuò)展，可擴(kuò)展至4096個(gè)隔離輸入口和4096個(gè)隔離輸出口；
（6）具備豐富的運(yùn)動(dòng)控制功能，如點(diǎn)位運(yùn)動(dòng)、電子凸輪、直線(xiàn)插補(bǔ)、圓弧插補(bǔ)、連續(xù)軌跡加工；
（7）支持掉電檢測(cè)、掉電存儲(chǔ)，多種程序加密方式，能夠有效防止系統(tǒng)故障，保護(hù)項(xiàng)目工程文件數(shù)據(jù)，并提高系統(tǒng)的可靠性；
（8）通過(guò)純國(guó)產(chǎn)IDE開(kāi)發(fā)環(huán)境RTSys進(jìn)行項(xiàng)目開(kāi)發(fā)，可實(shí)時(shí)仿真、在線(xiàn)跟蹤以及診斷與調(diào)試，簡(jiǎn)便易用，支持多種高級(jí)上位機(jī)語(yǔ)言聯(lián)合編程進(jìn)行二次開(kāi)發(fā)。
02 內(nèi)容回顧
回顧上期推文，我們對(duì)CAD導(dǎo)圖和小線(xiàn)段速度前瞻的C#Demo中的CAD導(dǎo)圖模塊進(jìn)行了大致的介紹（詳情點(diǎn)擊→C#運(yùn)動(dòng)控制開(kāi)源(一):CAD導(dǎo)圖和小線(xiàn)段速度前瞻的優(yōu)化之CAD導(dǎo)圖），本期推文我們將對(duì)參數(shù)設(shè)置和前瞻優(yōu)化以及運(yùn)動(dòng)指令下發(fā)這三個(gè)功能進(jìn)行介紹。
參數(shù)設(shè)置主要是設(shè)置對(duì)應(yīng)軸參數(shù)和加工參數(shù)。
前瞻優(yōu)化的作用尤為關(guān)鍵，若CAD導(dǎo)圖軌跡為復(fù)雜異形軌跡，導(dǎo)出后往往會(huì)生成大量小線(xiàn)段。這時(shí)候想要保證機(jī)臺(tái)加工高效平穩(wěn)，則需要通過(guò)運(yùn)動(dòng)前瞻算法對(duì)軌跡進(jìn)行一定程度的平滑，并且在拐彎點(diǎn)合理降速，曲線(xiàn)段整體也要合理降速，保證各分軸速度連續(xù)不出現(xiàn)速度突變。
在前瞻優(yōu)化過(guò)后則需要用對(duì)應(yīng)函數(shù)把軌跡下發(fā)到控制器中運(yùn)行。
03 C#使用運(yùn)動(dòng)函數(shù)庫(kù)和軌跡平滑庫(kù)進(jìn)行參數(shù)設(shè)置和前瞻優(yōu)化
正運(yùn)動(dòng)技術(shù)提供開(kāi)放的Zmotion，ZAuxdll運(yùn)動(dòng)函數(shù)庫(kù)和專(zhuān)門(mén)的ZTrackSmooth前瞻平滑庫(kù)，可以對(duì)CAD等獲得的軌跡進(jìn)行前瞻優(yōu)化，通過(guò)運(yùn)動(dòng)函數(shù)庫(kù)下發(fā)到控制器中執(zhí)行，保證運(yùn)行的高效平穩(wěn)。
1.在VS2019菜單“文件”→“新建”→“項(xiàng)目”，啟動(dòng)創(chuàng)建項(xiàng)目向?qū)А?/p>
2.選擇開(kāi)發(fā)語(yǔ)言為“C#”和Windows窗體應(yīng)用程序，點(diǎn)擊下一步。
3.配置好項(xiàng)目名稱(chēng)和位置，以及相應(yīng)框架，點(diǎn)擊創(chuàng)建。
4.找到廠(chǎng)家提供的光盤(pán)資料里面的C#函數(shù)庫(kù)，路徑如下（64位庫(kù)為例）。
進(jìn)入廠(chǎng)商提供的光盤(pán)資料，找到zauxdll.dll，zmotion.dll和Zmcaux.cs這三個(gè)庫(kù)文件。庫(kù)文件路徑：【00光盤(pán)資料】→【04PC函數(shù)】→【01PC函數(shù)庫(kù)V2.1】→【W(wǎng)indows平臺(tái)】→【C#】→【64位】→【庫(kù)文件】。
5.將廠(chǎng)商提供的C#的庫(kù)文件以及相關(guān)文件復(fù)制到新建的項(xiàng)目中。
（1）將zmcaux.cs文件復(fù)制到新建的項(xiàng)目里面中。
（2）將zauxdll.dll和zmotion.dll文件放入bin\debug文件夾中。
（3）將Zmcaux.cs文件添加進(jìn)項(xiàng)目中。右鍵項(xiàng)目名稱(chēng)，選擇添加，再選擇現(xiàn)有項(xiàng)，選擇Zmcaux.cs文件。
6.雙擊Form1.cs里面的Form1，出現(xiàn)代碼編輯界面，在文件開(kāi)頭寫(xiě)入using cszmcaux。
7.至此，項(xiàng)目新建完成，可進(jìn)行C#項(xiàng)目開(kāi)發(fā)。
例程界面如下：
前瞻優(yōu)化下發(fā)運(yùn)動(dòng)流程：
這次推文介紹到的參數(shù)設(shè)置，前瞻優(yōu)化，運(yùn)動(dòng)指令下發(fā)的內(nèi)容基本都放在了ZmotionCard.cs中，方便大家學(xué)習(xí)。
功能一：軸參數(shù)配置以及單軸測(cè)試
步驟1：連接控制器并初始化參數(shù)
ZAux_OpenEth(string ipaddr, out IntPtr phandle) //網(wǎng)口連接控制器
ZAux_Direct_SetUnits(IntPtr handle, int iaxis, float fValue) //設(shè)置脈沖當(dāng)量
ZAux_Direct_SetSpeed(IntPtr handle, int iaxis, float fValue) //設(shè)置速度
ZAux_Direct_SetDecel(IntPtr handle, int iaxis, float fValue) //設(shè)置加速度
ZAux_Direct_SetSramp(IntPtr handle, int iaxis, float fValue) //設(shè)置加減速時(shí)間ZAux_Direct_SetFastDec(IntPtr handle, int iaxis, float iValue) //設(shè)置快減減速度
正運(yùn)動(dòng)的所有函數(shù)都可以接受函數(shù)返回值，非0即是錯(cuò)誤碼，可以用錯(cuò)誤碼判斷函數(shù)下發(fā)情況
通過(guò)ZAux_OpenEth連接控制器，然后讀取對(duì)應(yīng)參數(shù)設(shè)置到控制器中，具體代碼實(shí)現(xiàn):
#region
初始化運(yùn)動(dòng)控制控卡
public bool ZmcCardIni(string strIpAddress)
{
//讀取運(yùn)動(dòng)控制參數(shù)
ReadPara();
int iret = 0;
//打開(kāi)運(yùn)動(dòng)控制器(ip"192.168.0.11" 正運(yùn)動(dòng)控制卡 ; ip"127.0.0.1" 正運(yùn)動(dòng)仿真器)
iret += zmcaux.ZAux_OpenEth(strIpAddress, out g_handle); CommandHandler("ZAux_OpenEth", iret);
//設(shè)置初始軸參數(shù)
iret += AxisParaSet();
//返回結(jié)果：如果運(yùn)動(dòng)控制指令返回值為0，初始化成功
return iret == 0 ? m_bInited = true : m_bInited = false;
}
#endregion
#region
設(shè)置軸參數(shù)
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) }; //脈沖當(dāng)量,單位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 }; //加減速時(shí)間
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 }; //負(fù)軟限位
int[] datumin = { m_nAxis_X_DatumIn, m_nAxis_Y_DatumIn, m_nAxis_Z_DatumIn, m_nAxis_U_DatumIn }; //原點(diǎn)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 }; //負(fù)限位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說(shuō)明鏈接控制卡失敗,返回值sRtn為非0錯(cuò)誤碼。
if (g_handle != (IntPtr)0)
{
for (int i = 0; i < m_AxisNum; i++)
{
//設(shè)置各軸的運(yùn)動(dòng)參數(shù)
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);
//設(shè)置各軸的正限位、負(fù)限位、原點(diǎn)、報(bào)警信號(hào) 所對(duì)應(yīng)的輸入口
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：?jiǎn)屋S測(cè)試
ZAux_Direct_Single_Vmove(IntPtr handle, int iaxis, int idir) //單軸持續(xù)運(yùn)動(dòng)ZAux_Direct_Single_Move(IntPtr handle, int iaxis, float fdistance) //單軸相對(duì)運(yùn)動(dòng)ZAux_Direct_Single_MoveAbs(IntPtr handle, int iaxis, float fdistance)//絕對(duì)運(yùn)動(dòng)ZAux_Direct_Single_Cancel(IntPtr handle, int iaxis, int imode)//單軸停止
ZAux_Direct_SetFsLimit(IntPtr handle, int iaxis, float fValue) //設(shè)置正向軟限位
ZAux_Direct_SetRsLimit(IntPtr handle, int iaxis, float fValue) //設(shè)置負(fù)向軟限位
ZAux_Direct_SetFwdIn(IntPtr handle, int iaxis, int iValue) //設(shè)置正硬限位映射
ZAux_Direct_SetRevIn(IntPtr handle, int iaxis, int iValue) //設(shè)置負(fù)硬限位映射
ZAux_Direct_SetDatumIn(IntPtr handle, int iaxis, int iValue) //設(shè)置原點(diǎn)映射
ZAux_Direct_Single_Datum(IntPtr handle, int iaxis, int imode) //控制器回零
ZAux_BusCmd_Datum(IntPtr handle, UInt32 iaxis, UInt32 homemode) //驅(qū)動(dòng)器回零
設(shè)置好參數(shù)后可以通過(guò)按鈕調(diào)用對(duì)應(yīng)單軸軸控功能進(jìn)行測(cè)試，具體代碼如下:
#region
軸持續(xù)運(yùn)動(dòng)(不阻塞)
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 //負(fù)向
{
iret += zmcaux.ZAux_Direct_Single_Vmove(g_handle, nAxis, -1); CommandHandler("ZAux_Direct_Single_Vmove", iret);
}
return iret;
}
#endregion
#region
軸相對(duì)運(yùn)動(dòng)(不阻塞)
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
軸絕對(duì)運(yùn)動(dòng)(不阻塞)
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檢測(cè)軸是否正常運(yùn)動(dòng)完成
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軸單獨(dú)回原(下發(fā)進(jìn)緩沖,程序不阻塞)
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
{
//驅(qū)動(dòng)器回零
iret += zmcaux.ZAux_BusCmd_Datum(g_handle, (uint)nAxis, (uint)homemode); CommandHandler("ZAux_BusCmd_Datum", iret);
}
return iret;
}
#endregion
功能二：前瞻參數(shù)設(shè)置
?步驟1：設(shè)置控制器前瞻參數(shù)
之前推文也有介紹過(guò)，控制器的運(yùn)動(dòng)前瞻主要通過(guò)Corner_mode進(jìn)行設(shè)置。控制器的前瞻主要有以下幾個(gè)功能:
（一）Corner_Mode設(shè)置
1.拐角減速
拐角減速功能解決的問(wèn)題是：當(dāng)指令間夾角過(guò)大時(shí)，如果仍以較大速度運(yùn)行，會(huì)在夾角處產(chǎn)生較大的機(jī)械沖擊，軌跡偏離。
控制器會(huì)對(duì)指令間軌跡變化的夾角進(jìn)行提前識(shí)別，比較其與減速/停止角的大小關(guān)系，提前決定是否進(jìn)行減速，保證在指令連接處平穩(wěn)過(guò)渡。
如圖，OA過(guò)渡AB段位置時(shí)角度小于減速角度則，S1-S2段不進(jìn)行減速，AB過(guò)渡BC段時(shí)角度大于減速角度則進(jìn)行減速處理過(guò)渡過(guò)程如S2-S3段，BC過(guò)渡CD段角度大于停止角度速度需要降到零如S3-S4段位置處理。執(zhí)行效果如下：
（1）未開(kāi)啟拐角減速
（2）開(kāi)啟拐角減速
→達(dá)到減速角度，未達(dá)到停止角度，部分減速。
→達(dá)到停止角度，完全減速。
2.小圓限速
小圓限速功能用于處理在運(yùn)行軌跡中可能運(yùn)行圓弧軌跡擬合成的小圓，由于角度偏轉(zhuǎn)較大導(dǎo)致出現(xiàn)軌跡偏轉(zhuǎn)，因此在這種位置需要進(jìn)行速度限制的處理。開(kāi)啟小圓限速，小圓半徑超過(guò)限速半徑的時(shí)候不會(huì)對(duì)速度限制，小圓半徑小于限速半徑的時(shí)候則會(huì)開(kāi)始對(duì)速度進(jìn)行限制。
3.自動(dòng)倒角
自動(dòng)倒角功能一般是用于拐角處按照一定的倒角半徑進(jìn)行軌跡的弧度化處理，使速度變化更平滑。如圖所示：
（1）未開(kāi)啟倒角
（2）開(kāi)啟倒角
（二）適用于小線(xiàn)段應(yīng)用的新平滑指令(高系列控制器支持)
針對(duì)連續(xù)小線(xiàn)段應(yīng)用，可以開(kāi)啟zsmooth_mode平滑速度曲線(xiàn)模式，效果顯著。
1.啟用平滑速度曲線(xiàn)模式
①zsmooth_start()
②zsmooth_end()
上述兩個(gè)basic指令是用于開(kāi)啟和關(guān)閉新的平滑模式，這兩個(gè)指令都是進(jìn)入緩沖的，記得一定要調(diào)用zsmooth_end指令，否則可能導(dǎo)致最后幾個(gè)小運(yùn)動(dòng)段位于等待狀態(tài)。可以提前先調(diào)用一次zsmooth_end，以防上次沒(méi)有正常關(guān)閉。
在PC程序里調(diào)用的話(huà)可以用Execute函數(shù)發(fā)送字符串，注意：zsmooth_start()是作用于某個(gè)軸的，如果前面沒(méi)有base指令的話(huà)，就要用axis指令指定軸號(hào)，用法應(yīng)該是zsmooth_start() axis(xxx)，而不是zsmooth_start(xxx)。下圖execute示例主軸為軸0。
開(kāi)啟新的平滑模式后，zsmooth的效果就非常強(qiáng)。數(shù)值越大越平滑，但如果小線(xiàn)段點(diǎn)間隔過(guò)大，并且zsmooth也很大，可能會(huì)造成運(yùn)動(dòng)軌跡變形。如果出現(xiàn)變形，要么減小zsmooth，或者減少小線(xiàn)段的間隔。
2.設(shè)置CORNER_ACCEL
CORNER_ACCEL類(lèi)似之前的小圓限速，默認(rèn)值0不生效，設(shè)置數(shù)值后替換FULL_SP_RADIUS。
用于在曲率較大的地方去合理降速。這個(gè)拐彎加速度的大致理解為v=sqrt(a * r)，這個(gè)a就是拐彎加速度。
3.設(shè)置JERK
JERK用來(lái)控制加加速度大小，可以讓合成速度的不平滑處更平滑，也會(huì)約束空跑階段的加速度大小。默認(rèn)值0不生效，設(shè)置值后替換SRAMP。
相關(guān)函數(shù)：
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)
在運(yùn)動(dòng)開(kāi)始前設(shè)置好控制器運(yùn)動(dòng)前瞻參數(shù)，具體代碼如下
#region
控制器前瞻參數(shù)初始化
public bool RunParaIni()
{
//判斷控制卡是否初始化成功
if (!m_bInited)
return false; //-1代表初始化未成功
int iret = 0;
//控制器前瞻參數(shù)設(shè)置到主軸上
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); //打開(kāi)連續(xù)插補(bǔ)
iret += zmcaux.ZAux_Direct_SetFullSpRadius(g_handle, m_nAxis_X, m_fLimitR); CommandHandler("ZAux_Direct_SetFullSpRadius", iret); //設(shè)置小圓限速半徑
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); //起始速度設(shè)置成加工速度
iret += zmcaux.ZAux_Direct_SetEndMoveSpeed(g_handle, m_nAxis_X, m_fXYSpeed); CommandHandler("ZAux_Direct_SetEndMoveSpeed", iret); //結(jié)束速度設(shè)置成加工速度
if (iret != 0) return false;
return true;
}
#endregion
?步驟2：設(shè)置ZTrackSmooth平滑參數(shù)
在cad導(dǎo)出小線(xiàn)段數(shù)組后，可以通過(guò)ZTrackSmooth.dll庫(kù)中提供的ZTS_ContinueSmoothAndSpeed函數(shù)進(jìn)行運(yùn)動(dòng)前瞻規(guī)劃，它的作用主要是通過(guò)給定參數(shù)用樣條方式去連續(xù)平滑軌跡，并且在拐彎點(diǎn)和曲線(xiàn)段合理降速。
主要函數(shù)：
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導(dǎo)出的小線(xiàn)段數(shù)組。
● pSmoothPoints：平滑過(guò)后的坐標(biāo)點(diǎn)集。
● parryLmtSp：輸出每個(gè)點(diǎn)限速(mm/s)，對(duì)應(yīng)每條線(xiàn)段，用movelimit運(yùn)行，<=0就不用設(shè)置//。
● parryForceSp：輸出的每段force_speed運(yùn)行速度。
● disErr：參考誤差系數(shù)（設(shè)置越大，拐點(diǎn)處軌跡誤差越大，默認(rèn)可輸入1）。用來(lái)控制高級(jí)平滑算法平滑后的軌跡與原軌跡之間最大參考誤差，當(dāng)此參數(shù)設(shè)置越大，平滑后的軌跡距離原軌跡的最大誤差也越大，同時(shí)平滑效果越好，運(yùn)動(dòng)時(shí)的速度可以達(dá)到更快，曲率大的地方過(guò)彎速度可以更快，反之則是相反的效果。
同一段軌跡：
此參數(shù)是設(shè)置為1.0
此參數(shù)設(shè)3.0
● splineDisPrecision：樣條最小線(xiàn)段的參考長(zhǎng)度（設(shè)置的越小，樣條拆分越細(xì)）默認(rèn)可輸入0.1。用來(lái)平滑高級(jí)平滑后的軌跡是原來(lái)離散點(diǎn)的參考倍數(shù)，默認(rèn)0.1即可，代表10倍。此參數(shù)設(shè)置的倍數(shù)，平滑后的軌跡越密集，最小設(shè)置為4倍(0.25)，最大建議不要設(shè)置超過(guò)20(0.05)。
設(shè)置為4倍：
設(shè)置為10倍：
● plimitR：限速半徑數(shù)組，不同的限速半徑對(duì)應(yīng)不同的限速值。
● plimitRSp：限速半徑速度數(shù)組，和限速半徑數(shù)組對(duì)應(yīng)。
● limitRCnt：限速半徑的個(gè)數(shù)。用來(lái)根據(jù)曲率半徑對(duì)應(yīng)的限制速度進(jìn)行曲線(xiàn)段限速。和控制器底層的小圓限速區(qū)別在于：
①小圓限速只需要設(shè)置最大限速半徑，限制速度和最小速度，然后根據(jù)實(shí)際圓速度=限制速度*實(shí)際半徑/限速最大半徑進(jìn)行線(xiàn)性限速；
②該函數(shù)可以通過(guò)設(shè)置分段限速，應(yīng)對(duì)上面小圓限速效果不好的時(shí)候，更可以根據(jù)實(shí)際機(jī)臺(tái)效果進(jìn)行設(shè)置，比如設(shè)置限速半徑和對(duì)應(yīng)限速值為:
獲得CAD軌跡數(shù)據(jù)后可以通過(guò)ZTS_ContinueSmoothAndSpeed2進(jìn)行前瞻優(yōu)化，然后按照一定格式下發(fā)進(jìn)控制器，具體代碼如下:
#region
ZTrackSmooth函數(shù)平滑下發(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);
//使用函數(shù)平滑
if (m_dLimitRArray.Length > 0) //有設(shè)置非線(xiàn)性小圓限速
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];
//計(jì)算該點(diǎn)需要轉(zhuǎn)的角度值
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
功能三：運(yùn)動(dòng)軌跡下發(fā)
?步驟1：函數(shù)形式批量下發(fā)
如果用傳統(tǒng)的PC調(diào)用API指令方式下發(fā)運(yùn)動(dòng)方式執(zhí)行，指令調(diào)用速度可能遠(yuǎn)長(zhǎng)于運(yùn)動(dòng)執(zhí)行時(shí)間，造成運(yùn)動(dòng)不連續(xù)，控制效果不理想的情況。
正運(yùn)動(dòng)的Zaux_DirectCommand函數(shù)可以自定義封裝下發(fā)批量運(yùn)動(dòng)指令，實(shí)現(xiàn)效率的提升。
相關(guān)函數(shù)：
ZAux_DirectCommand(IntPtr handle, string pszCommand, StringBuilder psResponse, UInt32 uiResponseLength)
ZAux_Execute(IntPtr handle, string pszCommand, StringBuilder psResponse, UInt32 uiResponseLength)
對(duì)應(yīng)下發(fā)代碼：
#region
自定義封裝函數(shù)批量下發(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);
}
//生成閉合曲線(xiàn)起點(diǎn) 運(yùn)動(dòng)加落刀
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;
//空移參數(shù)設(shè)置
cmdstr.Append($"MOVE_PARA(SPEED,{m_nAxis_X},{m_fCMoveSpeed})\r\n"); //設(shè)置空移速度
cmdstr.Append($"MOVE_PARA(ACCEL,{m_nAxis_X},{m_fCMoveAccel})\r\n"); //設(shè)置空移加速度
cmdstr.Append($"MOVE_PARA(DECEL,{m_nAxis_X},{m_fCMoveDecel})\r\n"); //設(shè)置空移減速度
cmdstr.Append($"MOVE_PARA(JERK,{m_nAxis_X},{m_fCMoveJerk})\r\n"); //設(shè)置空移JERK
cmdstr.Append($"MOVE_PARA(SRAMP,{m_nAxis_X},{m_fCMoveSramp})\r\n"); //設(shè)置空移SRAMP
cmdstr.Append($"FORCE_SPEED={m_fCMoveSpeed}\r\n"); //設(shè)置空移速度
double rDpos;
rDpos = MoveR(angle, CurRDpos); //計(jì)算起始角度
CurRDpos = (float)rDpos;
//空移到起點(diǎn)
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軸參數(shù)設(shè)置
cmdstr.Append($"MOVE_PARA(SPEED,{m_nAxis_X},{m_fZSpeed})\r\n"); //設(shè)置空移速度
cmdstr.Append($"MOVE_PARA(ACCEL,{m_nAxis_X},{m_fZAccel})\r\n"); //設(shè)置空移加速度
cmdstr.Append($"MOVE_PARA(DECEL,{m_nAxis_X},{m_fZDecel})\r\n"); //設(shè)置空移減速度
cmdstr.Append($"MOVE_PARA(JERK,{m_nAxis_X},{m_fZJerk})\r\n"); //設(shè)置空移JERK
cmdstr.Append($"MOVE_PARA(SRAMP,{m_nAxis_X},{m_fZSramp})\r\n"); //設(shè)置空移SRAMP
cmdstr.Append($"FORCE_SPEED={m_fZSpeed}\r\n"); //設(shè)置空移速度
//落刀
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;
}
//落刀延時(shí)
//切割軌跡起始參數(shù)設(shè)置
if (m_bZsmoothMode == true)
{
//cmdstr.Append($"ZSMOOTH_START()AXIS({m_nAxis_X})\r\n"); //設(shè)置多段合一的小線(xià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"); //設(shè)置切割加速度
cmdstr.Append($"MOVE_PARA(DECEL,{m_nAxis_X},{m_fXYDecel})\r\n"); //設(shè)置切割減速度
cmdstr.Append($"MOVE_PARA(JERK,{m_nAxis_X},{m_fXYJerk})\r\n"); //設(shè)置切割JERK
cmdstr.Append($"MOVE_PARA(SRAMP,{m_nAxis_X},{m_fXYSramp})\r\n"); //設(shè)置切割SRAMP
cmdstr.Append($"MOVE_PARA(SPEED,{m_nAxis_X},{m_fXYSpeed})\r\n"); //設(shè)置切割速度
cmdstr.Append($"FORCE_SPEED={m_fXYSpeed}\r\n"); //設(shè)置切割速度
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);
//移動(dòng)命令
if (m_dSetSpeed != runspeed) //避免每條重復(fù)寫(xiě)速度，減小3次文件內(nèi)容
{
m_dSetSpeed = runspeed;
cmdstr.Append($"MOVE_PARA(SPEED,{m_nAxis_X},{runspeed})\r\n"); //設(shè)置速度
cmdstr.Append($"FORCE_SPEED={runspeed}\r\n"); //設(shè)置速度
}
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);
}
}
}
}
//生成閉合曲線(xiàn)起點(diǎn) 抬刀
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"); //設(shè)置多段合一的小線(xià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軸參數(shù)設(shè)置
cmdstr.Append($"MOVE_PARA(SPEED,{m_nAxis_X},{m_fZSpeed})\r\n"); //設(shè)置Z速度
cmdstr.Append($"MOVE_PARA(ACCEL,{m_nAxis_X},{m_fZAccel})\r\n"); //設(shè)置Z加速度
cmdstr.Append($"MOVE_PARA(DECEL,{m_nAxis_X},{m_fZDecel})\r\n"); //設(shè)置Z減速度
cmdstr.Append($"MOVE_PARA(JERK,{m_nAxis_X},{m_fZJerk})\r\n"); //設(shè)置ZJERK
cmdstr.Append($"MOVE_PARA(SRAMP,{m_nAxis_X},{m_fZSramp})\r\n"); //設(shè)置ZSRAMP
cmdstr.Append($"FORCE_SPEED={m_fZSpeed}\r\n"); //設(shè)置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：通過(guò)三次文件下發(fā)(高系列控制器)
另外,正運(yùn)動(dòng)高系列控制器也可以通過(guò)支持3次文件加載的方式來(lái)減低PC交互，軌跡轉(zhuǎn)換生成3次文件，批量的加載到控制內(nèi)獨(dú)立運(yùn)行即可。
相關(guān)函數(shù)：
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次文件運(yùn)行初始化
public void Z3pFile_Init()
{
int iret;
uint iRemainSpace = 0;
iret = zmcaux.
ZAux_3FileRamDownBegin(g_handle, Z3P_FILE_NUM, ref iRemainSpace, "ZmcScan.z3p"); //開(kāi)啟3次文件動(dòng)態(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"); //設(shè)置運(yùn)動(dòng)
}
//生成閉合曲線(xiàn)起點(diǎn) 運(yùn)動(dòng)加落刀public void Z3pFile_StartString(double Xpos, double Ypos, double angle,ref float CurRDpos)
{
StringBuilder cmdbuffack = new StringBuilder(2048);
//空移參數(shù)設(shè)置
m_Str3File.Append($"MOVE_PARA(SPEED,{m_nAxis_X},{m_fCMoveSpeed})\r\n"); //設(shè)置空移速度
m_Str3File.Append($"MOVE_PARA(ACCEL,{m_nAxis_X},{m_fCMoveAccel})\r\n"); //設(shè)置空移加速度
m_Str3File.Append($"MOVE_PARA(DECEL,{m_nAxis_X},{m_fCMoveDecel})\r\n"); //設(shè)置空移減速度
m_Str3File.Append($"MOVE_PARA(JERK,{m_nAxis_X},{m_fCMoveJerk})\r\n"); //設(shè)置空移JERK
m_Str3File.Append($"MOVE_PARA(SRAMP,{m_nAxis_X},{m_fCMoveSramp})\r\n"); //設(shè)置空移SRAMP
m_Str3File.Append($"FORCE_SPEED={m_fCMoveSpeed}\r\n"); //設(shè)置空移速度
double rDpos;
rDpos = MoveR(angle, CurRDpos);//計(jì)算起始角度
CurRDpos = (float)rDpos;
//空移到起點(diǎn)
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軸參數(shù)設(shè)置
m_Str3File.Append($"MOVE_PARA(SPEED,{m_nAxis_X},{m_fZSpeed})\r\n"); //設(shè)置空移速度
m_Str3File.Append($"MOVE_PARA(ACCEL,{m_nAxis_X},{m_fZAccel})\r\n"); //設(shè)置空移加速度
m_Str3File.Append($"MOVE_PARA(DECEL,{m_nAxis_X},{m_fZDecel})\r\n"); //設(shè)置空移減速度
m_Str3File.Append($"MOVE_PARA(JERK,{m_nAxis_X},{m_fZJerk})\r\n"); //設(shè)置空移JERK
m_Str3File.Append($"MOVE_PARA(SRAMP,{m_nAxis_X},{m_fZSramp})\r\n"); //設(shè)置空移SRAMP
m_Str3File.Append($"FORCE_SPEED={m_fZSpeed}\r\n"); //設(shè)置空移速度
//落刀
m_Str3File.Append($"MOVEABSSP({Math.Round(Xpos, 4)},{Math.Round(Ypos, 4)},{m_fZDownPos},{rDpos})\r\n");
m_Str3File.Append($"WAIT IDLE\r\n");
//落刀延時(shí)
//切割軌跡起始參數(shù)設(shè)置
if (m_bZsmoothMode == true)
{
m_Str3File.Append($"ZSMOOTH_START()AXIS({m_nAxis_X})\r\n"); //設(shè)置多段合一的小線(xiàn)段模式
}
m_Str3File.Append($"MOVE_PARA(ACCEL,{m_nAxis_X},{m_fXYAccel})\r\n"); //設(shè)置切割加速度
m_Str3File.Append($"MOVE_PARA(DECEL,{m_nAxis_X},{m_fXYDecel})\r\n"); //設(shè)置切割減速度
m_Str3File.Append($"MOVE_PARA(JERK,{m_nAxis_X},{m_fXYJerk})\r\n"); //設(shè)置切割JERK
m_Str3File.Append($"MOVE_PARA(SRAMP,{m_nAxis_X},{m_fXYSramp})\r\n"); //設(shè)置切割SRAMP
m_Str3File.Append($"MOVE_PARA(SPEED,{m_nAxis_X},{m_fXYSpeed})\r\n"); //設(shè)置切割速度
m_Str3File.Append($"FORCE_SPEED={m_fXYSpeed}\r\n"); //設(shè)置切割速度
m_dSetSpeed = m_fXYSpeed;
Z3pFile_Down();}//生成切割段字符串
public void Z3pFile_LineString(double Xpos, double Ypos, double angle, float runspeed, float lmtspeed, uint num)
{
//移動(dòng)命令
if (m_dSetSpeed != runspeed) //避免每條重復(fù)寫(xiě)速度，減小3次文件內(nèi)容
{
m_dSetSpeed = runspeed;
m_Str3File.Append($"MOVE_PARA(SPEED,{m_nAxis_X},{runspeed})\r\n"); //設(shè)置速度
m_Str3File.Append($"FORCE_SPEED={runspeed}\r\n"); //設(shè)置速度
}
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();
}
//生成閉合曲線(xiàn)起點(diǎn) 抬刀
public void Z3pFile_EndString(double Xpos, double Ypos)
{
if (m_bZsmoothMode == true)
{
m_Str3File.Append($"ZSMOOTH_END()AXIS({m_nAxis_X})\r\n"); //設(shè)置多段合一的小線(xiàn)段模式
}
m_Str3File.Append($"WAIT IDLE\r\n");
//Z軸參數(shù)設(shè)置
m_Str3File.Append($"MOVE_PARA(SPEED,{m_nAxis_X},{m_fZSpeed})\r\n"); //設(shè)置Z速度
m_Str3File.Append($"MOVE_PARA(ACCEL,{m_nAxis_X},{m_fZAccel})\r\n"); //設(shè)置Z加速度
m_Str3File.Append($"MOVE_PARA(DECEL,{m_nAxis_X},{m_fZDecel})\r\n"); //設(shè)置Z減速度
m_Str3File.Append($"MOVE_PARA(JERK,{m_nAxis_X},{m_fZJerk})\r\n"); //設(shè)置ZJERK
m_Str3File.Append($"MOVE_PARA(SRAMP,{m_nAxis_X},{m_fZSramp})\r\n"); //設(shè)置ZSRAMP
m_Str3File.Append($"FORCE_SPEED={m_fZSpeed}\r\n"); //設(shè)置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()
{
//判斷命令長(zhǎng)度是否發(fā)送
if (m_Str3File.Length > FILE3_MAX_CHAR - 1000)
{
//動(dòng)態(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); //運(yùn)行3次文件
}
}
}
//ZPJ文件加載結(jié)束
public void Z3pFile_End()
{
//關(guān)刀
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);//運(yùn)行3次文件
}
//全部下載完成
iret = zmcaux.ZAux_3FileRamDownEnd(g_handle, Z3P_FILE_NUM); CommandHandler("ZAux_3FileRamDownEnd", iret);
m_bStartFile3Flag = false;
}
#endregion
功能四：輔助功能
?步驟1：讀寫(xiě)參數(shù)
對(duì)應(yīng)軸參數(shù)和前瞻參數(shù)在設(shè)置后需要保存，以便下次使用，程序里面通過(guò)kernel32.dll的功能，實(shí)現(xiàn)txt文本內(nèi)容讀寫(xiě)，用來(lái)保存設(shè)置數(shù)據(jù)。
?步驟2:計(jì)算R軸轉(zhuǎn)向
在插補(bǔ)運(yùn)動(dòng)的過(guò)程中，要使R非插補(bǔ)軸隨著插補(bǔ)運(yùn)動(dòng)的合成位移的變化而變化，從而實(shí)現(xiàn)在加工過(guò)程中，R軸始終處于合適的加工方向和位置的工藝。
程序中通過(guò)小線(xiàn)段前后的點(diǎn)位計(jì)算與X方向的夾角來(lái)確定實(shí)際R軸每段需要轉(zhuǎn)過(guò)的角度，其中還涉及到一些角度歸一以及旋轉(zhuǎn)最短路徑的計(jì)算。
相關(guān)代碼:
#region
計(jì)算R軸轉(zhuǎn)角
public double CalculateAngleWithX(double x1, double y1, double x2, double y2)
{
// 計(jì)算向量的模長(zhǎng)
double length = Math.Sqrt(Math.Pow(x2 - x1, 2) + Math.Pow(y2 - y1, 2));
// 計(jì)算角度（弧度）
double radians = Math.Atan2(y2 - y1, x2 - x1);
// 將弧度轉(zhuǎn)換為度數(shù)
double degrees = radians * (180.0 / Math.PI);
return degrees;
}
//相對(duì)轉(zhuǎn)絕對(duì)
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)
{
// 計(jì)算向量AB和CD的模長(zhǎng)和點(diǎn)積
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);
// 計(jì)算cos(θ)和θ
double cosTheta = dotProduct / (abLength * cdLength);
double theta = Math.Acos(cosTheta); // θ以弧度為單位
//Console.WriteLine("Angle between the vectors is: " + theta * (180 / Math.PI) + " degrees"); // 將弧度轉(zhuǎn)換為度數(shù)
double angle = theta * (180 / Math.PI);
return angle;
}
#endregion
?步驟3：當(dāng)前XYR坐標(biāo)在圖形上顯示
因?yàn)樾枰喇?dāng)前XYR軸實(shí)際的狀態(tài)，程序中通過(guò)定時(shí)器實(shí)時(shí)讀取XYR軸的當(dāng)前坐標(biāo)，并將其繪制到picbox控件上，方便用戶(hù)監(jiān)控。
相關(guān)函數(shù)：
ZAux_Direct_GetDpos(IntPtr handle, int iaxis, ref float pfValue) //讀取軸的指令位置
定時(shí)器代碼:
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 == "自動(dòng)運(yùn)行中")
{
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 = "未運(yùn)行";
tb_RunTime.Text = "未運(yùn)行";
}
//繪制一遍CAD
if (!m_GloFn.m_ZCad.ZcadShowPic(CadShow.Width, CadShow.Height, 坐標(biāo)系ToolStripMenuItem.Checked, 順序ToolStripMenuItem.Checked, 空移ToolStripMenuItem.Checked, 起點(diǎn)ToolStripMenuItem.Checked, 方向ToolStripMenuItem.Checked, XCurDpos, YCurDpos, UCurDpos))
{
return;
}
CadShow.Invalidate();
}
}
?步驟3：當(dāng)前XYR坐標(biāo)在圖形上顯示
因?yàn)樾枰喇?dāng)前XYR軸實(shí)際的狀態(tài)，程序中通過(guò)定時(shí)器實(shí)時(shí)讀取XYR軸的當(dāng)前坐標(biāo)，并將其繪制到picbox控件上，方便用戶(hù)監(jiān)控。
相關(guān)函數(shù)：
ZAux_Direct_GetDpos(IntPtr handle, int iaxis, ref float pfValue) //讀取軸的指令位置
定時(shí)器代碼:
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 == "自動(dòng)運(yùn)行中")
{
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 = "未運(yùn)行";
tb_RunTime.Text = "未運(yùn)行";
}
//繪制一遍CAD
if (!m_GloFn.m_ZCad.ZcadShowPic(CadShow.Width, CadShow.Height, 坐標(biāo)系ToolStripMenuItem.Checked, 順序ToolStripMenuItem.Checked, 空移ToolStripMenuItem.Checked, 起點(diǎn)ToolStripMenuItem.Checked, 方向ToolStripMenuItem.Checked, XCurDpos, YCurDpos, UCurDpos))
{
return;
}
CadShow.Invalidate();
}
}
04 DEMO效果演示
1.連接控制器,并導(dǎo)入CAD圖紙。
2.點(diǎn)擊視圖→參數(shù)設(shè)置開(kāi)始設(shè)置各軸機(jī)械參數(shù)。
3.設(shè)置好軸參數(shù)后可以各個(gè)單軸測(cè)試回原，正反轉(zhuǎn)等，確定軸有沒(méi)有問(wèn)題，沒(méi)有問(wèn)題后點(diǎn)擊加工參數(shù)和前瞻參數(shù)，進(jìn)行自動(dòng)流程的設(shè)置，設(shè)好參數(shù)后點(diǎn)擊參數(shù)保存以便下次使用。
4.回到主界面可以在右邊操作面板進(jìn)行操作，這時(shí)主界面上會(huì)顯示當(dāng)前運(yùn)行位置和U軸狀態(tài)。
5.點(diǎn)擊啟動(dòng)BASIC代碼和G代碼標(biāo)簽頁(yè)可以顯示實(shí)際生成的軌跡代碼。
6.上述步驟完成后點(diǎn)擊啟動(dòng)就可以進(jìn)行自動(dòng)加工了。
教學(xué)視頻請(qǐng)點(diǎn)擊→C#運(yùn)動(dòng)控制開(kāi)源(二): CAD導(dǎo)圖和小線(xiàn)段速度前瞻優(yōu)化
正運(yùn)動(dòng)技術(shù)專(zhuān)注于運(yùn)動(dòng)控制技術(shù)研究和通用運(yùn)動(dòng)控制軟硬件產(chǎn)品的研發(fā)，是國(guó)家級(jí)高新技術(shù)企業(yè)。正運(yùn)動(dòng)技術(shù)匯集了來(lái)自華為、中興等公司的優(yōu)秀人才，在堅(jiān)持自主創(chuàng)新的同時(shí)，積極聯(lián)合各大高校協(xié)同運(yùn)動(dòng)控制基礎(chǔ)技術(shù)的研究。主要業(yè)務(wù)有：運(yùn)動(dòng)控制卡_運(yùn)動(dòng)控制器_EtherCAT運(yùn)動(dòng)控制卡_EtherCAT控制器_運(yùn)動(dòng)控制系統(tǒng)_視覺(jué)控制器__運(yùn)動(dòng)控制PLC_運(yùn)動(dòng)控制_機(jī)器人控制器_視覺(jué)定位_XPCIe/XPCI系列運(yùn)動(dòng)控制卡等等。
|
