3轴桁架机器人

硬件配置

• 主站: Windows 10 × 1 + Intel i5 × 1
• 耦合器: EK1100 耦合器 × 1 + EL2008 数字量输出 × 1 + EL1008 数字量输入 × 1
• 运动控制: 松下 MDDLT54BF 伺服驱动器 × 3 + 伺服电机 × 3
• 末端执行器: SMC MHZ2-16D 气动平行夹爪 × 1

成本分析

类别	型号/规格	参考单价 (¥)	数量	小计 (¥)
耦合器	EK1100	800	1	800
数字量输出	EL2008	400	1	400
数字量输入	EL1008	350	1	350
伺服驱动器	松下 MDDLT54BF	2,000	3	6,000
伺服电机	400W 伺服电机	800	3	2,400
气动夹爪	SMC MHZ2-16D	500	1	500
工控机	Intel i5 级别	3,000	1	3,000
整线参考总计				≈ ¥13,450

以上为核心部件参考价 (RMB)，不含机械结构、线缆、气源等。

性能指标

• 定位精度: ±0.05mm
• 最大速度: X/Y轴 2m/s, Z轴 1m/s
• 重复定位精度: ±0.02mm

应用场景

注塑机取件桁架。
松下 MDDLT54BF 伺服驱动 XYZ 三轴，气动夹爪抓取注塑件/加工件放置到托盘，梯形速度曲线插补保证运动平稳。

数据采集服务

本方案往往搭配数据采集系统, 同时运行于单一控制设备。
CNC / 注塑机数据采集等 本案例不提供，我们提供专业的设备数据采集解决方案，如有需求请联系我们。

代码示例

PDO 结构体定义

using System.Runtime.InteropServices;

/// <summary>
/// 松下 MDDLT54BF 伺服驱动器输入 PDO (TxPDO 0x1A00)
/// </summary>
[StructLayout(LayoutKind.Sequential, Pack = 1)]
public struct MDDLT54BF_Input
{
    public ushort ErrorCode;              // 0x603F 错误代码
    public ushort StatusWord;             // 0x6041 状态字
    public sbyte ModesOfOperationDisplay; // 0x6061 操作模式显示
    public int ActualPosition;            // 0x6064 实际位置
    public ushort TouchProbeStatus;       // 0x60B9 探针状态
    public int TouchProbePos1;            // 0x60BA 探针位置1
    public int FollowingError;            // 0x60F4 跟随误差
    public uint DigitalInputs;            // 0x60FD 数字输入
}

/// <summary>
/// 松下 MDDLT54BF 伺服驱动器输出 PDO (RxPDO 0x1600)
/// </summary>
[StructLayout(LayoutKind.Sequential, Pack = 1)]
public struct MDDLT54BF_Output
{
    public ushort ControlWord;            // 0x6040 控制字
    public sbyte ModesOfOperation;        // 0x6060 操作模式
    public int TargetPosition;            // 0x607A 目标位置
    public ushort TouchProbeFunction;     // 0x60B8 探针功能
}

/// <summary>
/// EL1008 数字量输入 PDO
/// </summary>
[StructLayout(LayoutKind.Sequential, Pack = 1)]
public struct EL1008_Input
{
    public byte DigitalInputs;    // 8位输入: bit0=原点X, bit1=原点Y, bit2=原点Z,
                                  //         bit3=夹爪检测, bit4=注塑机开模完成信号
}

/// <summary>
/// EL2008 数字量输出 PDO
/// </summary>
[StructLayout(LayoutKind.Sequential, Pack = 1)]
public struct EL2008_Output
{
    public byte DigitalOutputs;   // 8位输出: bit0=夹爪开, bit1=夹爪关, bit2=取件完成信号
}

/// <summary>
/// 三维坐标点
/// </summary>
public struct Point3D
{
    public double X { get; set; }
    public double Y { get; set; }
    public double Z { get; set; }

    public Point3D(double x, double y, double z)
    {
        X = x; Y = y; Z = z;
    }
}

控制代码

using System;
using System.Threading;
using System.Collections.Generic;
using DarraEtherCAT_Master;

class GantryRobotController
{
    // 从站索引
    const int SLAVE_EK1100 = 0;
    const int SLAVE_SERVO_X = 1;  // 松下 MDDLT54BF
    const int SLAVE_SERVO_Y = 2;  // 松下 MDDLT54BF
    const int SLAVE_SERVO_Z = 3;  // 松下 MDDLT54BF
    const int SLAVE_EL2008 = 4;
    const int SLAVE_EL1008 = 5;

    // 操作模式
    const sbyte MODE_CSP = 8;  // 周期同步位置模式

    // IO 信号位定义
    const byte DI_MOLD_OPEN = 0x10;      // bit4: 注塑机开模完成信号
    const byte DI_GRIPPER_CHECK = 0x08;  // bit3: 夹爪夹紧检测
    const byte DO_GRIPPER_OPEN = 0x01;   // bit0: 夹爪开
    const byte DO_GRIPPER_CLOSE = 0x02;  // bit1: 夹爪关
    const byte DO_TAKEOUT_DONE = 0x04;   // bit2: 取件完成信号 (反馈给注塑机)

    // ============ 可配置参数 (外部设置) ============

    /// <summary> 机械参数: X轴脉冲/mm </summary>
    public static int PulsesPerMmX { get; set; } = 10000;
    /// <summary> 机械参数: Y轴脉冲/mm </summary>
    public static int PulsesPerMmY { get; set; } = 10000;
    /// <summary> 机械参数: Z轴脉冲/mm </summary>
    public static int PulsesPerMmZ { get; set; } = 20000;

    /// <summary> 运动参数: XY轴最大速度 (mm/s) </summary>
    public static double MaxSpeedXY { get; set; } = 2000;
    /// <summary> 运动参数: Z轴最大速度 (mm/s) </summary>
    public static double MaxSpeedZ { get; set; } = 1000;

    /// <summary> 取件位置 (mm) </summary>
    public static Point3D PickPosition { get; set; } = new Point3D(100, 200, 50);
    /// <summary> 放件位置 (mm) </summary>
    public static Point3D PlacePosition { get; set; } = new Point3D(300, 200, 50);
    /// <summary> 安全高度 (mm) </summary>
    public static double SafeZ { get; set; } = 150.0;

    // 当前位置
    static Point3D currentPosition = new Point3D(0, 0, 0);
    static bool isHomed = false;

    static void Main(string[] args)
    {
        // 1. 使用 ENI 文件导入配置
        var master = new DarraEtherCAT()
            .LoadENI("config/gantry_robot.xml")
            .Build();

        if (master == null)
        {
            Console.WriteLine("主站初始化失败!");
            return;
        }

        try
        {
            // 2. 切换到 OP 状态
            var (success, msg) = master.SetState(EcState.OP);
            if (!success)
            {
                Console.WriteLine($"无法进入 OP 状态: {msg}");
                return;
            }

            // 3. 初始化松下 MDDLT54BF 伺服
            InitializeServos(master);

            Console.WriteLine("3轴桁架机器人启动!");
            Console.WriteLine("按 Ctrl+C 退出...\n");

            // 4. 执行回原点
            Console.WriteLine("开始回原点...");
            HomeAllAxes(master);

            // 5. 一次性映射 IO PDO
            ref var dio = ref master.Slaves[SLAVE_EL1008].InputsMapping<EL1008_Input>();
            ref var dout = ref master.Slaves[SLAVE_EL2008].OutputsMapping<EL2008_Output>();

            // 6. 取放料循环 — 由注塑机开模信号触发
            var waitPosition = new Point3D(PickPosition.X, PickPosition.Y, SafeZ);

            int cycleCount = 0;
            bool lastMoldSignal = false;

            // 初始动作: 打开夹爪, 移动到等待位
            GripperOpen(ref dout);
            MoveToPosition(master, waitPosition);
            Console.WriteLine("就绪, 等待注塑机开模信号...\n");

            while (true)
            {
                // 读取注塑机开模完成信号 (上升沿触发)
                bool moldOpen = (dio.DigitalInputs & DI_MOLD_OPEN) != 0;

                if (moldOpen && !lastMoldSignal)
                {
                    // ── 收到开模信号, 开始取件 ──

                    // 清除取件完成信号
                    dout.DigitalOutputs &= unchecked((byte)~DO_TAKEOUT_DONE);

                    // 1. 从等待位快速下降到取件位
                    MoveToPosition(master, PickPosition);

                    // 2. 夹爪关闭, 等待夹紧确认
                    GripperClose(ref dout);
                    WaitGripperConfirm(ref dio);

                    // 3. 抬起到安全高度
                    MoveToPosition(master, new Point3D(PickPosition.X, PickPosition.Y, SafeZ));

                    // 4. 输出取件完成信号 → 注塑机可以合模开始下一周期
                    dout.DigitalOutputs |= DO_TAKEOUT_DONE;
                    Console.WriteLine("取件完成, 通知注塑机合模");

                    // 5. 移动到放件位上方
                    MoveToPosition(master, new Point3D(PlacePosition.X, PlacePosition.Y, SafeZ));

                    // 6. 下降到放件位, 松开夹爪
                    MoveToPosition(master, PlacePosition);
                    GripperOpen(ref dout);
                    Thread.Sleep(200);

                    // 7. 抬起并回到等待位, 准备下一次触发
                    MoveToPosition(master, new Point3D(PlacePosition.X, PlacePosition.Y, SafeZ));
                    MoveToPosition(master, waitPosition);

                    cycleCount++;
                    Console.WriteLine($"第 {cycleCount} 次取放循环完成, 等待下一次开模信号...\n");
                }

                lastMoldSignal = moldOpen;
                Thread.Sleep(1);
            }
        }
        finally
        {
            master.Dispose();
        }
    }

    /// <summary>
    /// 初始化松下 MDDLT54BF 伺服驱动器
    /// </summary>
    static void InitializeServos(DarraEtherCAT master)
    {
        int[] servoSlaves = { SLAVE_SERVO_X, SLAVE_SERVO_Y, SLAVE_SERVO_Z };

        foreach (int slaveIdx in servoSlaves)
        {
            ref var output = ref master.Slaves[slaveIdx].OutputsMapping<MDDLT54BF_Output>();

            // 设置 CSP 模式
            output.ModesOfOperation = MODE_CSP;

            // CIA402 状态机切换
            output.ControlWord = 0x0006;  // Shutdown
            Thread.Sleep(5);
            output.ControlWord = 0x0007;  // Switch On
            Thread.Sleep(5);
            output.ControlWord = 0x000F;  // Enable Operation
            Thread.Sleep(5);
        }

        Console.WriteLine("所有松下 MDDLT54BF 伺服已使能");
    }

    /// <summary>
    /// 回原点
    /// </summary>
    static void HomeAllAxes(DarraEtherCAT master)
    {
        // 1. Z轴先回原点 (安全)
        HomeAxis(master, SLAVE_SERVO_Z, 2, "Z");

        // 2. X, Y 轴同时回原点
        var xThread = new Thread(() => HomeAxis(master, SLAVE_SERVO_X, 0, "X"));
        var yThread = new Thread(() => HomeAxis(master, SLAVE_SERVO_Y, 1, "Y"));

        xThread.Start();
        yThread.Start();

        xThread.Join();
        yThread.Join();

        currentPosition = new Point3D(0, 0, 0);
        isHomed = true;
        Console.WriteLine("回原点完成!");
    }

    /// <summary>
    /// 单轴回原点 (简化实现: 以固定速度向负方向移动直到触发原点传感器)
    /// 生产环境中应结合 Z 脉冲精确定位, 并增加低速二次寻原点流程
    /// </summary>
    static void HomeAxis(DarraEtherCAT master, int slaveIdx, int sensorBit, string axisName)
    {
        ref var output = ref master.Slaves[slaveIdx].OutputsMapping<MDDLT54BF_Output>();
        ref var input = ref master.Slaves[slaveIdx].InputsMapping<MDDLT54BF_Input>();
        ref var dio = ref master.Slaves[SLAVE_EL1008].InputsMapping<EL1008_Input>();

        int homeSpeed = -5000;  // 负方向寻找原点
        const int HOME_TIMEOUT_MS = 30000;  // 回原点超时时间 30 秒
        int elapsed = 0;

        while (true)
        {
            // 检查原点传感器
            bool atHome = (dio.DigitalInputs & (1 << sensorBit)) != 0;

            if (atHome)
            {
                output.TargetPosition = input.ActualPosition;  // 停止
                Console.WriteLine($"{axisName}轴到达原点");
                break;
            }

            // 超时保护: 传感器故障时避免无限循环
            if (elapsed >= HOME_TIMEOUT_MS)
            {
                output.TargetPosition = input.ActualPosition;  // 立即停止
                Console.WriteLine($"错误: {axisName}轴回原点超时! 请检查原点传感器");
                throw new TimeoutException($"{axisName}轴回原点超时 ({HOME_TIMEOUT_MS}ms)");
            }

            // 继续向原点方向移动
            output.TargetPosition = input.ActualPosition + homeSpeed;
            output.ControlWord = 0x001F;

            Thread.Sleep(1);
            elapsed++;
        }

        // 清零位置
        // 实际应用中需要通过SDO设置编码器清零
    }

    /// <summary>
    /// 移动到目标位置 (直线插补)
    /// </summary>
    static void MoveToPosition(DarraEtherCAT master, Point3D target)
    {
        if (!isHomed)
        {
            Console.WriteLine("错误: 未回原点!");
            return;
        }

        // 计算位移
        double dx = target.X - currentPosition.X;
        double dy = target.Y - currentPosition.Y;
        double dz = target.Z - currentPosition.Z;
        double distance = Math.Sqrt(dx * dx + dy * dy + dz * dz);

        if (distance < 0.01) return;  // 已经在目标位置

        // 计算各轴行程，以最慢轴的运动时间为基准实现同步运动
        double timeX = Math.Abs(dx) > 0.001 ? Math.Abs(dx) / MaxSpeedXY : 0;
        double timeY = Math.Abs(dy) > 0.001 ? Math.Abs(dy) / MaxSpeedXY : 0;
        double timeZ = Math.Abs(dz) > 0.001 ? Math.Abs(dz) / MaxSpeedZ : 0;
        double totalTime = Math.Max(Math.Max(timeX, timeY), timeZ);
        if (totalTime < 0.001) return;

        int steps = (int)(totalTime * 1000);  // 1ms步进
        if (steps < 1) steps = 1;

        // 梯形速度曲线参数: 加速20% + 匀速60% + 减速20%
        int accelSteps = steps / 5;       // 加速阶段步数
        int decelStart = steps - accelSteps; // 减速阶段起始

        // 一次性映射 PDO (引用指向共享内存, 数据由主站循环自动更新)
        ref var outX = ref master.Slaves[SLAVE_SERVO_X].OutputsMapping<MDDLT54BF_Output>();
        ref var outY = ref master.Slaves[SLAVE_SERVO_Y].OutputsMapping<MDDLT54BF_Output>();
        ref var outZ = ref master.Slaves[SLAVE_SERVO_Z].OutputsMapping<MDDLT54BF_Output>();

        // 插补计算
        for (int i = 0; i <= steps; i++)
        {
            // 梯形速度曲线: 加速段/匀速段/减速段
            double ratio;
            if (i < accelSteps && accelSteps > 0)
            {
                // 加速阶段: 二次曲线平滑起步
                double t = (double)i / accelSteps;
                ratio = 0.5 * t * t * ((double)accelSteps / steps);
            }
            else if (i >= decelStart && accelSteps > 0)
            {
                // 减速阶段: 二次曲线平滑停止
                double t = (double)(steps - i) / accelSteps;
                ratio = 1.0 - 0.5 * t * t * ((double)accelSteps / steps);
            }
            else
            {
                // 匀速阶段: 线性插补
                ratio = (double)i / steps;
            }

            double x = currentPosition.X + dx * ratio;
            double y = currentPosition.Y + dy * ratio;
            double z = currentPosition.Z + dz * ratio;

            // 转换为脉冲并发送
            outX.TargetPosition = (int)(x * PulsesPerMmX);
            outY.TargetPosition = (int)(y * PulsesPerMmY);
            outZ.TargetPosition = (int)(z * PulsesPerMmZ);

            outX.ControlWord = 0x001F;
            outY.ControlWord = 0x001F;
            outZ.ControlWord = 0x001F;

            Thread.Sleep(1);
        }

        currentPosition = target;
        Console.WriteLine($"到达位置: X={target.X:F2}, Y={target.Y:F2}, Z={target.Z:F2}");
    }

    /// <summary>
    /// 夹爪打开
    /// </summary>
    static void GripperOpen(ref EL2008_Output dout)
    {
        dout.DigitalOutputs = (byte)((dout.DigitalOutputs & ~(DO_GRIPPER_OPEN | DO_GRIPPER_CLOSE))
            | DO_GRIPPER_OPEN);
        Console.WriteLine("夹爪打开");
    }

    /// <summary>
    /// 夹爪关闭
    /// </summary>
    static void GripperClose(ref EL2008_Output dout)
    {
        dout.DigitalOutputs = (byte)((dout.DigitalOutputs & ~(DO_GRIPPER_OPEN | DO_GRIPPER_CLOSE))
            | DO_GRIPPER_CLOSE);
        Console.WriteLine("夹爪关闭");
    }

    /// <summary>
    /// 等待夹爪夹紧确认 (超时500ms自动通过)
    /// </summary>
    static void WaitGripperConfirm(ref EL1008_Input dio)
    {
        for (int i = 0; i < 500; i++)
        {
            if ((dio.DigitalInputs & DI_GRIPPER_CHECK) != 0)
                return;
            Thread.Sleep(1);
        }
        Console.WriteLine("警告: 夹爪确认超时, 继续执行");
    }
}