|
01 ZMC432-V2运动控制器介绍
ZMC432-V2高性能多轴运动控制器是一款兼容EtherCAT总线和脉冲型的独立式运动控制器，自带6轴本地差分脉冲轴，最多可扩展至32轴，能实现总线轴+脉冲轴混合插补的多轴运动控制场合。同时支持正运动远程显示功能，能提供网络组态显示，可实时监控和调整参数配置。
ZMC432-V2硬件功能特性：
（1）支持32轴运动控制（脉冲+EtherCAT总线），EtherCAT最小通讯周期可达125us；
（2）24路通用输入、12路通用输出，2路模拟量输出(DA)，其中包括2路高速输入和2路高速输出；
（3）6路差分脉冲轴输出，总线轴、脉冲轴可混合插补；
（4）内置多项实时性运动控制功能，例如视觉飞拍、多维PSO、高速位置锁存，多轴同步运行等；
（5）可通过EtherCAT扩展模块进行IO硬件资源扩展，可扩展至4096个隔离输入口和4096个隔离输出口；
（6）具备丰富的运动控制功能，如点位运动、电子凸轮、直线插补、圆弧插补、连续轨迹加工；
（7）支持掉电检测、掉电存储，多种程序加密方式，能够有效防止系统故障，保护项目工程文件数据，并提高系统的可靠性；
（8）通过纯国产IDE开发环境RTSys进行项目开发，可实时仿真、在线跟踪以及诊断与调试，简便易用，支持多种高级上位机语言联合编程进行二次开发。
02 内容回顾
回顾上期推文，我们对CAD导图和小线段速度前瞻的C#Demo中的CAD导图模块进行了大致的介绍（详情点击→C#运动控制开源(一):CAD导图和小线段速度前瞻的优化之CAD导图），本期推文我们将对参数设置和前瞻优化以及运动指令下发这三个功能进行介绍。
参数设置主要是设置对应轴参数和加工参数。
前瞻优化的作用尤为关键，若CAD导图轨迹为复杂异形轨迹，导出后往往会生成大量小线段。这时候想要保证机台加工高效平稳，则需要通过运动前瞻算法对轨迹进行一定程度的平滑，并且在拐弯点合理降速，曲线段整体也要合理降速，保证各分轴速度连续不出现速度突变。
在前瞻优化过后则需要用对应函数把轨迹下发到控制器中运行。
03 C#使用运动函数库和轨迹平滑库进行参数设置和前瞻优化
正运动技术提供开放的Zmotion，ZAuxdll运动函数库和专门的ZTrackSmooth前瞻平滑库，可以对CAD等获得的轨迹进行前瞻优化，通过运动函数库下发到控制器中执行，保证运行的高效平稳。
1.在VS2019菜单“文件”→“新建”→“项目”，启动创建项目向导。
2.选择开发语言为“C#”和Windows窗体应用程序，点击下一步。
3.配置好项目名称和位置，以及相应框架，点击创建。
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#项目开发。
例程界面如下：
前瞻优化下发运动流程：
这次推文介绍到的参数设置，前瞻优化，运动指令下发的内容基本都放在了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即是错误码，可以用错误码判断函数下发情况
通过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) //单轴持续运动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
轴持续运动(不阻塞)
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轴单独回原(下发进缓冲,程序不阻塞)
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.拐角减速
拐角减速功能解决的问题是：当指令间夹角过大时，如果仍以较大速度运行，会在夹角处产生较大的机械冲击，轨迹偏离。
控制器会对指令间轨迹变化的夹角进行提前识别，比较其与减速/停止角的大小关系，提前决定是否进行减速，保证在指令连接处平稳过渡。
如图，OA过渡AB段位置时角度小于减速角度则，S1-S2段不进行减速，AB过渡BC段时角度大于减速角度则进行减速处理过渡过程如S2-S3段，BC过渡CD段角度大于停止角度速度需要降到零如S3-S4段位置处理。执行效果如下：
（1）未开启拐角减速
（2）开启拐角减速
→达到减速角度，未达到停止角度，部分减速。
→达到停止角度，完全减速。
2.小圆限速
小圆限速功能用于处理在运行轨迹中可能运行圆弧轨迹拟合成的小圆，由于角度偏转较大导致出现轨迹偏转，因此在这种位置需要进行速度限制的处理。开启小圆限速，小圆半径超过限速半径的时候不会对速度限制，小圆半径小于限速半径的时候则会开始对速度进行限制。
3.自动倒角
自动倒角功能一般是用于拐角处按照一定的倒角半径进行轨迹的弧度化处理，使速度变化更平滑。如图所示：
（1）未开启倒角
（2）开启倒角
（二）适用于小线段应用的新平滑指令(高系列控制器支持)
针对连续小线段应用，可以开启zsmooth_mode平滑速度曲线模式，效果显著。
1.启用平滑速度曲线模式
①zsmooth_start()
②zsmooth_end()
上述两个basic指令是用于开启和关闭新的平滑模式，这两个指令都是进入缓冲的，记得一定要调用zsmooth_end指令，否则可能导致最后几个小运动段位于等待状态。可以提前先调用一次zsmooth_end，以防上次没有正常关闭。
在PC程序里调用的话可以用Execute函数发送字符串，注意：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); //打开连续插补
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函数进行运动前瞻规划，它的作用主要是通过给定参数用样条方式去连续平滑轨迹，并且在拐弯点和曲线段合理降速。
主要函数：
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：限速半径的个数。用来根据曲率半径对应的限制速度进行曲线段限速。和控制器底层的小圆限速区别在于：
①小圆限速只需要设置最大限速半径，限制速度和最小速度，然后根据实际圆速度=限制速度*实际半径/限速最大半径进行线性限速；
②该函数可以通过设置分段限速，应对上面小圆限速效果不好的时候，更可以根据实际机台效果进行设置，比如设置限速半径和对应限速值为:
获得CAD轨迹数据后可以通过ZTS_ContinueSmoothAndSpeed2进行前瞻优化，然后按照一定格式下发进控制器，具体代码如下:
#region
ZTrackSmooth函数平滑下发
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;
//下发平滑轨迹
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
功能三：运动轨迹下发
?步骤1：函数形式批量下发
如果用传统的PC调用API指令方式下发运动方式执行，指令调用速度可能远长于运动执行时间，造成运动不连续，控制效果不理想的情况。
正运动的Zaux_DirectCommand函数可以自定义封装下发批量运动指令，实现效率的提升。
相关函数：
ZAux_DirectCommand(IntPtr handle, string pszCommand, StringBuilder psResponse, UInt32 uiResponseLength)
ZAux_Execute(IntPtr handle, string pszCommand, StringBuilder psResponse, UInt32 uiResponseLength)
对应下发代码：
#region
自定义封装函数批量下发
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;
//判断缓冲区是否有剩余
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");
//判断缓冲区是否有剩余
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;
}
//判断缓冲区是否有剩余
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：通过三次文件下发(高系列控制器)
另外,正运动高系列控制器也可以通过支持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)
三次文件下发具体代码:
#region
三次文件下发
// ZPJ3次文件运行初始化
public void Z3pFile_Init()
{
int iret;
uint iRemainSpace = 0;
iret = zmcaux.
ZAux_3FileRamDownBegin(g_handle, Z3P_FILE_NUM, ref iRemainSpace, "ZmcScan.z3p"); //开启3次文件动态加载
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()
{
//判断命令长度是否发送
if (m_Str3File.Length > FILE3_MAX_CHAR - 1000)
{
//动态加载程序
m_Str3File.Append("MOVE_RESUME\r\n");
int iret = 0;
uint nRemain = 0;
//等待3次文件缓冲足够下发
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次文件缓冲足够下发
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轴实际的状态，程序中通过定时器实时读取XYR轴的当前坐标，并将其绘制到picbox控件上，方便用户监控。
相关函数：
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轴实际的状态，程序中通过定时器实时读取XYR轴的当前坐标，并将其绘制到picbox控件上，方便用户监控。
相关函数：
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轴状态。
5.点击启动BASIC代码和G代码标签页可以显示实际生成的轨迹代码。
6.上述步骤完成后点击启动就可以进行自动加工了。
教学视频请点击→C#运动控制开源(二): CAD导图和小线段速度前瞻优化
正运动技术专注于运动控制技术研究和通用运动控制软硬件产品的研发，是国家级高新技术企业。正运动技术汇集了来自华为、中兴等公司的优秀人才，在坚持自主创新的同时，积极联合各大高校协同运动控制基础技术的研究。主要业务有：运动控制卡_运动控制器_EtherCAT运动控制卡_EtherCAT控制器_运动控制系统_视觉控制器__运动控制PLC_运动控制_机器人控制器_视觉定位_XPCIe/XPCI系列运动控制卡等等。
|
