精准24码全年无错版-资料免费精选

Technical support

技术分享

C#之Delta并联机械手正逆解的建立和手动运动

ZMC406R-V2硬件介绍

ZMC406R-V2是正运动推出的一款多轴高性能 EtherCAT 总线运动控制器，具有EtherCAT、EtherNET、RS232、CAN和U盘等通讯接口，ZMC系列运动控制器可应用于各种需要脱机或联机运行的场合。

ZMC406R-V2支持6轴运动控制，最多可扩展至32轴，支持直线插补、任意圆弧插补、空间圆弧、螺旋插补、电子凸轮、电子齿轮、同步跟随等功能。

ZMC406R-V2可以采用脱机的方式将编辑好的程序下载到控制器上，可利用触摸屏示教的方式编辑想要运动的轨迹。也可以用PC API函数调用方式或者实时发送指令操作，在PC 上位机 C#，C++，Labview，Python等语言来开发Delta并联机械手的应用。

ZMC406R与多种上位机开发.png

ZMC406R-V2支持6轴运动控制，可采用脉冲轴（带编码器反馈）或EtherCAT总线轴，通用IO包含24路输入口和12路输出口，模拟量AD/DA各两路，EtherCAT最快125us的刷新周期。

硬件 接线参考

1.通用输入口接线参考图

1.通用输入口接线参考图.png

2.通用输出口接线参考图

C#语言进行Delta并联机械手的开发

1.在VS2010菜单“文件”→“新建”→“项目”，启动创建项目向导。

2.选择开发语言为“Visual C#”和.NET Framework 4以及Windows窗体应用程序。

3.找到厂家提供的光盘资料里面的C#函数库，路径如下（32位库为例）。

1）进入厂商提供的光盘资料找到“04PC函数”文件夹，并点击进入。

2）选择“函数库2.1”文件夹。

3）选择“Windows平台”文件夹。

4）根据需要选择对应的函数库，这里选择32位库。

5）解压C#压缩包,里面有C#对应的函数库。

6）函数库具体路径如下。

4.将厂商提供的C#的库文件以及相关文件复制到新建的项目中。

1）将zmcaux.cs文件复制到新建的项目里面中。

2）将zaux.dll和zmotion.dll文件放入bin\debug文件夹中。

5.用vs打开新建的项目文件，在右边的解决方案资源管理器中点击显示所有文件，然后鼠标右击zmcaux.cs文件，点击包括在项目中。

6. 双击Form1.cs里面的Form1，出现代码编辑界面，在文件开头写入using cszmcaux，并声明控制器句柄g_handle。

PC函数介绍

1.PC函数手册可在光盘资料查看，具体路径如下。

2.链接控制器，获取链接句柄
16.链接控制器，获取链接句柄.png

3.机械手模型正解的建立。
16.机械手模型正解的建立.png

4.机械手模型逆解的建立。
17.机械手模型逆解的建立.png

5.机械手坐标系旋转，用于得到用户坐标系。
18.机械手坐标系旋转，用于得到用户坐标系.png

C#例程建设之Delta正逆解的建立

1.轴方向要求

使用正运动Delta并联机械手模型，需要保证3个关节轴向下旋转时为正向，末端旋转轴逆时针旋转为正（俯视）。

2.结构参数 （结构参数由机械手本体厂家提供）

机械手的结构参数需要先写进控制器的TABLE寄存器后，才可以调接口建立机械手的正逆解。

3.原点定义

A、各关节轴的连杆L1都处于水平位置时，认为是关节零点位置。 (一般本体会提供定位销，没有提供的可以通过水平仪确保零点时L1处于水平位置)

B、此时轴0 和轴1 连线方向为直角坐标系X 方向，直角坐标系零点位于连杆L1水平面的中心。

4.关节轴与虚拟轴

关节轴是实际存在的电机轴，Delta有关节轴0电机、关节轴1电机、关节轴2电机和末端旋转轴电机，这些关机轴脉冲当量的设置都需要设置成对应关节轴转1度需要的脉冲数，如：（Units = 电机一圈脉冲数 * 减速比 / 360）。

虚拟轴是虚拟出来的直角坐标系的X轴、Y轴、Z轴和一个旋转轴RZ,其中X、Y、Z轴的脉冲当量推荐设置成1000，旋转轴RZ的脉冲当量设置成旋转轴转1度需要的脉冲数。

相关例程演示

1.链接控制器获取句柄，上位机操作通过获取到的句柄来操作控制器。

//连接控制器,控制器默认IP是192.168.0.11
ZauxErr = zmcaux.ZAux_OpenEth("192.168.0.11", out g_Handle);
if (0 != ZauxErr)
{
AlmInifFile.Write(DateTime.Now.ToString("F"), "ZAux_OpenEth执行错误,错误码:" + ZauxErr.ToString(), "错误码信息");
}

2.Delta并联机械手参数设置。

3.机械手正逆解的建立。

/************************************************************************************
'任务编号：无
'函数功能：机械手的正逆解的建立 
'Input：Mode=0断开机械手；1建立机械手的逆解；2建立机械手的正解
'Output：无 
'返回值：0:表示机械手建立成功; -1:表示机械手建立失败
'备注：1、控制器型号带R后缀的才支持该机械手模型
'              2、正解状态时，虚拟轴的MTYPE=34
'              3、逆解状态时，关节轴的MTYPE=33
'              4、机械手处于逆解状态时不要断使能，不然操作不当容易飞车。
**************************************************************************************/
public int ScaraEstab(int Mode)
{
    //更新Ui界面的Delta参数
    ScaraParaWindows.DeltaParaSave();
    //设置机械参数到控制器
    SetControPara();
    //保存Delta参数到Ini文件
    WriteIniFile();
    //清除控制器报警
    ZauxErr = zmcaux.ZAux_Direct_Single_Datum(g_Handle, 0, 0);
    //更新轴列表 建立正逆解的轴顺序是 J1、J2、Ju、Jz(关节轴)  Vx、Vy、Vr、Vz（虚拟轴）
    int[] mJScaraAxisList = new int[4]; //关节轴列表
    int[] mVScaraAxisList = new int[4]; //虚拟轴列表
    for (int i = 0; i < 4; i++)
    {
        mJScaraAxisList[i] = gDeltaAxisList[i];
        mVScaraAxisList[i] = gVAxisList[i];
    }
    //判读机械手参数是否设置正确
    if ((DeltaR <= 0) || (Deltar <= 0) || (DeltaL1 <= 0) || (DeltaL2 <= 0))
    {
        MessageBox.Show("Delta机械手参数有问题请确认！");
        return -1;
    }
    //机械手的结构参数要先写进控制器的TABLE寄存器后，才可以调接口建立机械手的正逆解
    //更新机械手参数到控制器的TABLE寄存器
    float[] ScaraParaList = new float[11] { DeltaR, Deltar, DeltaL1, DeltaL2, gMotorPulNum[0] * gReducRatio[0], gMotorPulNum[1] * gReducRatio[1], gMotorPulNum[2] * gReducRatio[2], 0, 0, 0, gMotorPulNum[3] * gReducRatio[3] };
    ZauxErr = zmcaux.ZAux_Direct_SetTable(g_Handle, TableStaraId, 11, ScaraParaList);
    if (0 != ZauxErr)
    {
        return -1;
    }
    if ((1 == Mode) && (0 == ZauxErr)) //建立机械手的逆解(机械手参数更新成功的情况下)
    {
        //建立Scara逆解
        ZauxErr = zmcaux.ZAux_Direct_Connframe(g_Handle, 4, mJScaraAxisList, 14, TableStaraId, 4, mVScaraAxisList);
        if (0 != ZauxErr)
        {
            ProceWindows.WriteLog("切换为逆解模式失败");
            return -1;
        }
        ProceWindows.WriteLog("切换为逆解模式");
    }
    else if ((2 == Mode) && (0 == ZauxErr)) //建立机械手的正解(机械手参数更新成功的情况下)
    {
        //建立Scara正解
        ZauxErr = zmcaux.ZAux_Direct_Connreframe(g_Handle, 4, mVScaraAxisList, 14, TableStaraId, 4, mJScaraAxisList);
        if (0 != ZauxErr)
        {
            ProceWindows.WriteLog("切换为正解模式失败");
            return -1;
        }
        ProceWindows.WriteLog("切换为正解模式");
        }
    }
    return 0;
}

4.手动运动界面

下图是例程手动运动的界面。

“关节坐标系手动控制” 的手动按钮是进行关节轴的手动运动的按下时会自动建立机械手的正解。

“直角坐标系手动控制” 的手动按钮是进行虚拟轴XYZ的手动运动的按下时会自动建立机械手的逆解。

当手动速度降低到一定值时会自动切换到寸动模式。

5.关节轴手动运动

/************************************************************************************
'任务编号：     无
'函数功能：     关节轴负向手动运动，负向点动按钮按下时调用
'Input：        无
'Output：       无 
'返回值：       无
'备注：         无       
**************************************************************************************/
private void JHandDirRev0_MouseDown(object sender, MouseEventArgs e)
{
    //判断是否上锁，上锁才可以运动
    int EnableState = 0;
    cszmcaux.zmcaux.ZAux_Direct_GetAxisEnable(MainWindows.g_Handle, MainWindows.gDeltaAxisList[0], ref EnableState);
    //如果使能上了
    if ((1 == EnableState) || (MainWindows.IsDebug)) 
    {
        int Vmtype = 0;//虚拟轴MTYPE
        cszmcaux.zmcaux.ZAux_Direct_GetMtype(MainWindows.g_Handle, MainWindows.gVAxisList[0], ref Vmtype); 
        //如果不是正解状态
        if (Vmtype != 34)
        {
            //建立机械手的正解
            if (0 != MainWindows.ScaraEstab(2))
            {
                //机械手没有成功建立
                return;
            }
        }
        //哪个按钮按下
        int Id = 0;
        for (int i = 0; i < 4; i++)
        {
            if (((Button)sender).Name == ("JHandDirRev" + i))
            {
                Id = i;
            }
        }
        int TempAxis = 0;
        TempAxis = MainWindows.gDeltaAxisList[Id];
        //手动速度小于5是寸动模式
        if (5 <= MainWindows.HnadSpeedPerc)
        {
            //设置点动速度
            cszmcaux.zmcaux.ZAux_Direct_SetSpeed(MainWindows.g_Handle, TempAxis, MainWindows.HnadSpeedPerc * MainWindows.gAxisHandSpeed[Id] / 100);
            //发送负向运动指令
            cszmcaux.zmcaux.ZAux_Direct_Single_Vmove(MainWindows.g_Handle, TempAxis, -1);
        }
        else
        {
            //设置寸动速度
            cszmcaux.zmcaux.ZAux_Direct_SetSpeed(MainWindows.g_Handle, TempAxis, MainWindows.gAxisHandSpeed[Id] / 10);
            //发送寸动运动指令
            cszmcaux.zmcaux.ZAux_Direct_Single_Move(MainWindows.g_Handle, TempAxis, -1 * MainWindows.HnadSpeedPerc);
        }
    }
}
/************************************************************************************
'任务编号：     无
'函数功能：     关节轴正向手动运动，正向点动按钮按下时调用
'Input：        无
'Output：       无 
'返回值：       无
'备注：         无       
**************************************************************************************/
private void JHandDirFwd0_MouseDown(object sender, MouseEventArgs e)
{
    //判断是否上锁，上锁才可以运动
    int EnableState = 0;
    cszmcaux.zmcaux.ZAux_Direct_GetAxisEnable(MainWindows.g_Handle, MainWindows.gDeltaAxisList[0], ref EnableState);
    //如果使能上了
    if ((1 == EnableState) || (MainWindows.IsDebug)) 
    {
        int Vmtype = 0;//虚拟轴MTYPE
        cszmcaux.zmcaux.ZAux_Direct_GetMtype(MainWindows.g_Handle, MainWindows.gVAxisList[0], ref Vmtype); 
        //如果不是正解状态
        if (Vmtype != 34)
        {
            //建立机械手的正解
            if (0 != MainWindows.ScaraEstab(2))
            {
                //机械手没有成功建立
                return;
            }
        }
        //哪个按钮按下
        int Id = 0;
        for (int i = 0; i < 4; i++)
        {
            if (((Button)sender).Name == ("JHandDirFwd" + i))
            {
                Id = i;
            }
        }
        int TempAxis = 0;
        TempAxis = MainWindows.gDeltaAxisList[Id];
        //手动速度小于5是寸动模式
        if (5 <= MainWindows.HnadSpeedPerc)
        {
            //设置点动速度
            cszmcaux.zmcaux.ZAux_Direct_SetSpeed(MainWindows.g_Handle, TempAxis, MainWindows.HnadSpeedPerc * MainWindows.gAxisHandSpeed[Id] / 100);
            //发送正向运动指令
            cszmcaux.zmcaux.ZAux_Direct_Single_Vmove(MainWindows.g_Handle, TempAxis, 1);
        }
        else
        {
            //设置寸动速度
            cszmcaux.zmcaux.ZAux_Direct_SetSpeed(MainWindows.g_Handle, TempAxis, MainWindows.gAxisHandSpeed[Id] / 10);
            //发送寸动运动指令
            cszmcaux.zmcaux.ZAux_Direct_Single_Move(MainWindows.g_Handle, TempAxis, 1 * MainWindows.HnadSpeedPerc);
        }
    }
}
/************************************************************************************
'任务编号：     无
'函数功能：     关节轴停止手动运动，点动按钮松开时调用。
'Input：        无
'Output：       无 
'返回值：       无
'备注：         无       
**************************************************************************************/
private void JHandDirRev0_MouseUp(object sender, MouseEventArgs e)
{
    //哪个按钮按下
    int Id = 0;
    for (int i = 0; i < 4; i++)
    {
        if (((Button)sender).Name == ("JHandDirRev" + i))
        {
            Id = i;
        }
    }
    int TempAxis = 0;
    TempAxis = MainWindows.gDeltaAxisList[Id];
    //手动速度小于5是寸动模式
    if (5 <= MainWindows.HnadSpeedPerc)
    {
        MainWindows.ZauxErr = cszmcaux.zmcaux.ZAux_Direct_Single_Cancel(MainWindows.g_Handle, TempAxis, 2);
    }
}

6.虚拟轴手动运动

/************************************************************************************
'任务编号：无
'函数功能：虚拟轴负向手动运动，负向点动按钮按下时调用
'Input：无
'Output：无 
'返回值：无
'备注：无       
**************************************************************************************/
private void VHandDirRev0_MouseDown(object sender, MouseEventArgs e)
{
    //判断是否上锁，上锁才可以运动
    int EnableState = 0;
    cszmcaux.zmcaux.ZAux_Direct_GetAxisEnable(MainWindows.g_Handle, MainWindows.gDeltaAxisList[0], ref EnableState);
    //如果使能上了
    if ((1 == EnableState) || (MainWindows.IsDebug)) 
    {
        int Jmtype = 0;//关节轴Mtype
        cszmcaux.zmcaux.ZAux_Direct_GetMtype(MainWindows.g_Handle, gDeltaAxisList[0], ref Jmtype);
        //如果不是逆解状态
        if (Jmtype != 33)
        {
            //建立机械手的逆解
            if (0 != MainWindows.ScaraEstab(1))
            {
                //机械手没有成功建立
                return;
            }
        }
        //哪个按钮按下
        int Id = 0;
        for (int i = 0; i < 5; i++)
        {
            if (((Button)sender).Name == ("VHandDirRev" + i))
            {
                Id = i;
            }
        }
        int TempAxis = 0;
        //更新当前机械手状态
        TempAxis = MainWindows.gVAxisList[Id];
        //手动速度小于5是寸动模式
        if (5 <= MainWindows.HnadSpeedPerc)
        {
            //设置点动速度
            cszmcaux.zmcaux.ZAux_Direct_SetSpeed(MainWindows.g_Handle, TempAxis, MainWindows.HnadSpeedPerc * MainWindows.gAxisHandSpeed[Id] / 100);
           //发送负向运动指令
           cszmcaux.zmcaux.ZAux_Direct_Single_Vmove(MainWindows.g_Handle, TempAxis, -1); 
        }
        else
        {
            //设置寸动速度
            cszmcaux.zmcaux.ZAux_Direct_SetSpeed(MainWindows.g_Handle, TempAxis, MainWindows.gAxisHandSpeed[Id] / 10);
            //发送寸动运动指令
           cszmcaux.zmcaux.ZAux_Direct_Single_Move(MainWindows.g_Handle, TempAxis, -1 * MainWindows.HnadSpeedPerc);
        }
    }
}
/************************************************************************************
'任务编号：无
'函数功能：虚拟轴负向手动运动，负向点动按钮按下时调用
'Input：无
'Output：无 
'返回值：无
'备注：无       
**************************************************************************************/
private void VHandDirFwd0_MouseDown(object sender, MouseEventArgs e)
{
    //判断是否上锁，上锁才可以运动
    int EnableState = 0;
    cszmcaux.zmcaux.ZAux_Direct_GetAxisEnable(MainWindows.g_Handle, MainWindows.gDeltaAxisList[0], ref EnableState);
    //如果使能上了
    if ((1 == EnableState) || (MainWindows.IsDebug)) 
    {
        int Jmtype = 0;//关节轴Mtype
        cszmcaux.zmcaux.ZAux_Direct_GetMtype(MainWindows.g_Handle, gDeltaAxisList[0], ref Jmtype);
        //如果不是逆解状态
        if (Jmtype != 33)
        {
            //建立机械手的逆解
            if (0 != MainWindows.ScaraEstab(1))
            {
                //机械手没有成功建立
                return;
            }
        }
        //哪个按钮按下
        int Id = 0;
        for (int i = 0; i < 5; i++)
        {
            if (((Button)sender).Name == ("VHandDirFwd" + i))
            {
                Id = i;
            }
        }
        int TempAxis = 0;
        //更新当前机械手状态
        TempAxis = MainWindows.gVAxisList[Id];
        //手动速度小于5是寸动模式
        if (5 <= MainWindows.HnadSpeedPerc)
        {
            //设置点动速度
            cszmcaux.zmcaux.ZAux_Direct_SetSpeed(MainWindows.g_Handle, TempAxis, MainWindows.HnadSpeedPerc * MainWindows.gAxisHandSpeed[Id] / 100);
           //发送正向运动指令
           cszmcaux.zmcaux.ZAux_Direct_Single_Vmove(MainWindows.g_Handle, TempAxis, 1); 
        }
        else
        {
            //设置寸动速度
            cszmcaux.zmcaux.ZAux_Direct_SetSpeed(MainWindows.g_Handle, TempAxis, MainWindows.gAxisHandSpeed[Id] / 10);
            //发送寸动运动指令
           cszmcaux.zmcaux.ZAux_Direct_Single_Move(MainWindows.g_Handle, TempAxis, 1 * MainWindows.HnadSpeedPerc);
        }
    }
}
/************************************************************************************
'任务编号：无
'函数功能：虚拟轴停止轴运动，点动按钮松开时调用
'Input：无
'Output：无 
'返回值：无
'备注：无       
**************************************************************************************/
//停止运动
private void VHandDirRev0_MouseUp(object sender, MouseEventArgs e)
{
    //哪个按钮按下
    int Id = 0;
    for (int i = 0; i < 5; i++)
    {
        if (((Button)sender).Name == ("VHandDirRev" + i))
        {
            Id = i;
        }
    }
    int TempAxis = 0;
    TempAxis = MainWindows.gVAxisList[Id];
    //手动速度小于5是寸动模式
    if (5 <= MainWindows.HnadSpeedPerc)
    {
        cszmcaux.zmcaux.ZAux_Direct_Single_Cancel(MainWindows.g_Handle, TempAxis, 2);
    }
}

7.机械手仿真工具的使用

打开【ZRobotView】软件，点击【连接】输入控制器的IP（默认IP：192.168.0.11），点击连接，待连接成功后点击切换即可 3D仿真机械手的运动情况。

本次，正运动技术C#之Delta并联机械手正逆解的建立和手动运动，就分享到这里。

更多精彩内容请关注“ 正运动小助手 ”公众号，需要相关开发环境与例程代码，请咨询正运动技术销售工程师：400-089-8936。

本文由正运动技术原创，欢迎大家转载，共同学习，一起提高中国智能制造水平。文章版权归正运动技术所有，如有转载请注明文章来源。

正运动技术专注于运动控制技术研究和通用运动控制软硬件产品的研发，是国家级高新技术企业。正运动技术汇集了来自华为、中兴等公司的优秀人才，在坚持自主创新的同时，积极联合各大高校协同运动控制基础技术的研究，是国内工控领域发展最快的企业之一，也是国内少有、完整掌握运动控制核心技术和实时工控软件平台技术的企业。主要业务有：运动控制卡_运动控制器_EtherCAT运动控制卡_EtherCAT控制器_运动控制系统_视觉控制器__运动控制PLC_运动控制_机器人控制器_视觉定位_XPCIe/XPCI系列运动控制卡等等。

协助伙伴成功，是我们的价值所在

Technical support

技术分享

C#之Delta并联机械手正逆解的建立和手动运动

邮箱留言