Zemengzhou Space
Zemengzhou Space

Post

LOM 纸叠层三维打印机五轴控制程序技术文档

针对 LOM_GTS800_5Axis_NewWiring.cpp 的完整技术说明，涵盖五轴轴号分配、限位回零设计、I/O 定义、关键参数、软件结构、状态机流程及调试顺序，基于固高 GTS-800 系列运动控制卡。

LOM纸叠层三维打印机五轴控制程序技术文档

1. 文档说明

本文档针对 LOM_GTS800_5Axis_NewWiring.cpp 控制程序进行说明。程序用于 LOM 纸叠层三维打印机的五轴运动控制，控制对象包括 X 轴、Y 轴、Z 轴、送料轴和热压滚筒轴，并配合安全输入、驱动继电器、加热控制和 CO₂ 激光 TTL 控制完成纸张送料、热压、激光切割和逐层下降等加工流程。

程序基于固高 GTS-800 系列运动控制卡编写，使用 gts.h 中的固高运动控制 API。当前代码中 USE_MOCK_IO 默认为 1，表示程序处于仿真 IO 模式；实机运行前需要改为 0，并根据现场端子和 SDK 补全 IO 读写函数。

2. 系统功能概述

本程序实现的主要功能包括：

  1. 固高 GTS-800 控制卡初始化；
  2. 五轴轴号分配和运动控制；
  3. 仅使用限位开关的回零流程；
  4. 安全输入检测和运行保护；
  5. 驱动继电器、热压滚筒加热、激光输出控制；
  6. 送料、热压滚筒滚压、激光轮廓切割、Z 轴逐层下降；
  7. 采用状态机组织整机加工流程；
  8. 在故障或安全异常时关闭激光、加热和驱动输出，并停止五轴运动。

3. 硬件与轴号分配

轴号程序宏定义设备对象驱动类型
Axis1AXIS_XX轴松下 A6 伺服驱动器
Axis2AXIS_YY轴松下 A6 伺服驱动器
Axis3AXIS_ZZ轴步进驱动器
Axis4AXIS_FEED送料轴步进驱动器
Axis5AXIS_ROLLER热压滚筒轴步进驱动器

程序中使用 AXIS_MASK_5 = 0x1F 表示同时控制 1~5 号轴，常用于急停或整体停止。

4. 限位与回零设计

系统没有单独设置原点开关，因此程序采用限位开关作为回零参考。限位分配如下：

限位端子对应轴含义
LIMIT0+X轴X轴正限位
LIMIT0-X轴X轴负限位
LIMIT1+Y轴Y轴正限位
LIMIT1-Y轴Y轴负限位
LIMIT2+Z轴Z轴上限位
LIMIT2-Z轴Z轴下限位
LIMIT3+送料轴送料轴正限位
LIMIT3-送料轴送料轴负限位
LIMIT4+热压滚筒轴热压滚筒轴正限位
LIMIT4-热压滚筒轴热压滚筒轴负限位

回零方式为：

轴回零参考程序函数
X轴LIMIT0-HomeAxisByNegLimit(AXIS_X)
Y轴LIMIT1-HomeAxisByNegLimit(AXIS_Y)
Z轴LIMIT2+HomeZByUpLimit()
送料轴LIMIT3-HomeAxisByNegLimit(AXIS_FEED)
热压滚筒轴LIMIT4-HomeAxisByNegLimit(AXIS_ROLLER)

限位回零流程采用“两次找限位”的方式：先快速寻找限位，退出限位后再慢速寻找限位，最后离开限位一小段距离并清零当前位置。这样做可以减少单次触发带来的误差，提高回零重复性。

5. 输入输出定义

类型端子/宏定义功能
输入EXI0 / EXI_SAFE_OK安全输入
输出EXO0 / EXO_DRIVE_RELAY驱动继电器输出
输出EXO1 / EXO_HEATER_EN加热控制输出
输出LASER+ / LASER-CO₂ 激光电源 TTL 控制端

程序中将安全输入、驱动继电器、加热输出和激光输出都做了函数封装：

函数作用
ReadRawEXI()读取 EXI 输入原始状态
WriteEXO()输出 EXO 信号
IsSafeOK()判断安全输入是否正常
DriveRelayOn() / DriveRelayOff()控制驱动继电器
HeaterOn() / HeaterOff()控制热压滚筒加热允许信号
LaserOn() / LaserOff()控制激光开关

实机运行前需要根据固高控制卡 SDK 和实际接线补全 ReadRawEXI()、WriteEXO()、ReadLimitTerminal()、LaserOn()、LaserOff()。

6. 关键参数说明

6.1 脉冲当量

参数含义当前值
X_PULSE_PER_MMX轴每毫米脉冲数80.0
Y_PULSE_PER_MMY轴每毫米脉冲数80.0
Z_PULSE_PER_MMZ轴每毫米脉冲数640.0
FEED_PULSE_PER_MM送料轴每毫米脉冲数33.9
R_PULSE_PER_MM热压滚筒轴每毫米脉冲数80.0

这些参数直接影响运动距离和速度换算。实机调试时应根据丝杆导程、同步带传动比、驱动器细分、电子齿轮比等参数重新校核。

6.2 工件坐标偏置

程序中定义：

#define X_WORK_OFFSET       230.0
#define Y_WORK_OFFSET       230.0

工件坐标通过以下关系转换为机床坐标：

机床X坐标 = 工件X坐标 + X_WORK_OFFSET
机床Y坐标 = 工件Y坐标 + Y_WORK_OFFSET

例如工件坐标 (-100, -100)，转换后机床坐标为 (130, 130)。

6.3 加工参数

参数含义当前值
LAYER_THICKNESS单层层厚0.20 mm
FEED_LENGTH每层送料长度520.0 mm
R_WAIT_POS热压滚筒等待位置0.0 mm
R_START_POS热压滚筒滚压起点30.0 mm
R_END_POS热压滚筒滚压终点630.0 mm
PREHEAT_TIME_MS预热等待时间30000 ms

这些参数需要结合实际纸张尺寸、滚筒行程、热压区域和层厚要求调整。

6.4 回零参数

参数含义当前值
HOME_SEARCH_VEL第一次找限位速度10.0 mm/s
HOME_CREEP_VEL慢速找限位速度3.0 mm/s
HOME_BACK_DIST退出限位距离5.0 mm
HOME_OFFSET_DIST回零完成后离开限位距离2.0 mm

调试初期建议适当降低回零速度，确认方向正确后再提高速度。

7. 软件结构说明

程序按功能分为以下模块：

模块主要内容
调试开关USE_MOCK_IO 控制仿真/实机 IO 模式
轴号与限位定义定义五轴编号、限位编号、输入输出编号
机械参数定义脉冲当量、层厚、送料长度、滚筒位置等
单位换算完成 mm、pulse、速度单位之间的换算
IO 封装封装 EXI、EXO、限位、激光、加热等控制
安全保护安全输入检测、限位保护、急停处理
运动控制单轴绝对/相对运动、Jog、XY 联动运动
限位回零X/Y/送料/滚筒负限位回零，Z 上限位回零
工艺动作预热、送料、热压、激光切割、Z 轴下降
控制卡初始化打开控制卡、加载配置、轴使能、输出初始化
主状态机按加工顺序组织整机运行流程

8. 主要函数说明

8.1 单位换算函数

函数功能
mm_to_pulse(short axis, double mm)将毫米换算为对应轴脉冲数
pulse_to_mm(short axis, double pulse)将脉冲数换算为毫米
vel_to_sdk(short axis, double vel_mm_s)将 mm/s 换算为 SDK 速度单位
WorkToMachineX(double x_work)工件 X 坐标转机床 X 坐标
WorkToMachineY(double y_work)工件 Y 坐标转机床 Y 坐标

8.2 安全保护函数

函数功能
SafetyCheck()判断安全输入是否正常，异常时关闭激光和加热
LimitProtectCheck()加工运行阶段检查各轴限位是否触发
EmergencyStop()关闭激光、加热、驱动继电器，并停止五轴

LimitProtectCheck() 不在回零阶段调用，避免回零找限位时被误判为故障。

8.3 运动控制函数

函数功能
StopOneAxis(short axis)停止单轴并清除状态
IsAxisMoving(short axis)判断单轴是否处于规划运动状态
WaitAxisStop(short axis)等待单轴停止，期间执行安全和限位保护
WaitAxesStop(long mask)等待多个轴停止，期间执行安全和限位保护
MoveAbsMM(short axis, double pos_mm, double vel_mm_s)单轴绝对位置运动
MoveRelMM(short axis, double distance_mm, double vel_mm_s)单轴相对位移运动
JogAxis(short axis, int dir, double vel_mm_s)单轴连续 Jog 运动
MoveXYMachineMM(double x_mm, double y_mm, double vel_mm_s)X/Y 机床坐标同步移动
MoveXYWorkMM(double x_work, double y_work, double vel_mm_s)X/Y 工件坐标移动

8.4 回零函数

函数功能
HomeAxisByNegLimit(short axis)使用负限位对指定轴回零
HomeZByUpLimit()使用 Z 轴上限位回零
HomeAllAxisByLimit()按 Z、X、Y、送料、滚筒顺序完成五轴回零

HomeAllAxisByLimit() 中先执行 Z 轴上限位回零，使 Z 轴先回到上方安全位置，再执行其他轴回零，降低平台或工具与机构发生干涉的风险。

8.5 工艺动作函数

函数功能
PreheatRoller()开启加热输出并等待预热时间
PaperFeed()执行送料轴送料动作
HotPress()移动到安全位置后执行热压滚筒滚压
EnsureRollerSafe()确保热压滚筒回到等待位置
LaserCutLine()执行一条直线切割动作
LaserCutCurrentLayer()执行当前层矩形轮廓切割示例
ZDownOneLayer()Z 轴下降一个层厚

当前激光切割路径为矩形轮廓示例，实际加工时应由切片文件或 G 代码生成加工路径。

9. 状态机流程

主程序使用 MachineState 枚举定义整机运行状态：

状态含义
S_POWER_ON系统上电初始化
S_SELF_CHECK安全自检
S_LIMIT_HOME五轴限位回零
S_PREHEAT热压滚筒预热
S_LAYER_PREPARE当前层加工准备
S_PAPER_FEED送料
S_HOT_PRESS热压滚筒滚压
S_LASER_CUT激光切割
S_Z_DOWNZ 轴下降一个层厚
S_NEXT_LAYER层数累加并进入下一层
S_FINISH加工完成
S_FAULT系统故障
S_EMERGENCY安全保护触发

程序主流程如下：

系统上电初始化
→ 安全自检
→ 五轴限位回零
→ 热压滚筒预热
→ 层加工准备
→ 送料
→ 热压滚筒滚压
→ 激光切割
→ Z轴下降一个层厚
→ 下一层
→ 达到总层数后加工完成

其中 currentLayer 表示当前层数，totalLayer 表示总层数。当前代码中总层数为 10。

10. 安全逻辑说明

程序中的安全保护主要包括以下几类：

  1. 安全输入异常时，关闭激光和加热，并进入保护状态；
  2. 加工运行阶段检测各轴正负限位，触发后立即停止；
  3. 激光切割前确认安全输入正常；
  4. 激光切割前确认热压滚筒已经退回等待位置；
  5. 空移阶段激光关闭，只有切割运动时才开启激光；
  6. 故障或急停时执行 EmergencyStop()，同时关闭激光、加热、驱动继电器并停止五轴。

需要注意的是，程序目前只提供了安全保护逻辑框架，实机运行时必须确保安全输入、限位信号、激光 TTL 信号和驱动继电器输出已经在硬件层面正确接线并完成测试。

11. 编译与运行说明

11.1 编译环境

程序需要在 Windows 环境下编译，并正确配置固高 GTS-800 系列控制卡 SDK。工程中应包含：

  1. gts.h 头文件；
  2. 固高 SDK 对应的 .lib 静态库或导入库；
  3. 运行时需要的 DLL 文件；
  4. 控制卡配置文件 GTS800.cfg。

11.2 实机运行前修改

当前代码中：

#define USE_MOCK_IO 1

表示程序处于仿真 IO 模式。实机运行前应改为：

#define USE_MOCK_IO 0

并补全以下函数中的实际 IO 读写代码：

ReadRawEXI()
WriteEXO()
ReadLimitTerminal()
LaserOn()
LaserOff()

11.3 推荐调试顺序

建议按以下顺序调试：

  1. 不接激光电源，只测试运动轴；
  2. 测试 EXI0 安全输入有效电平；
  3. 测试所有正负限位输入；
  4. 单独测试 Z 轴上限位回零；
  5. 单独测试 X 轴负限位回零；
  6. 单独测试 Y 轴负限位回零；
  7. 单独测试送料轴负限位回零；
  8. 单独测试热压滚筒轴负限位回零；
  9. 测试 EXO0 驱动继电器输出；
  10. 测试 EXO1 加热允许输出；
  11. 测试送料动作；
  12. 测试热压滚筒动作；
  13. 测试 X/Y 空移；
  14. 确认激光 TTL 输出默认关闭；
  15. 最后接入激光电源并测试完整流程。

12. 需要现场确认的内容

实机使用前，应重点确认以下内容：

  1. Axis1~Axis5 是否与实际电机接线一致；
  2. 各轴正方向是否与程序逻辑一致；
  3. Z 轴正方向是否为向上，若相反需要修改 HomeZByUpLimit() 和 ZDownOneLayer() 中的方向；
  4. 各限位触发电平是否与 LIMIT_TRIGGER_LEVEL 一致；
  5. 安全输入正常电平是否与 EXI_OK_LEVEL 一致；
  6. GTS800.cfg 是否为现场调试软件生成的正确配置文件；
  7. 每个轴的脉冲当量是否正确；
  8. 送料长度、层厚、滚筒起点和终点是否符合实际机械尺寸；
  9. 激光 TTL 输出是否为低电平安全关闭状态；
  10. 温控器、固态继电器、热压滚筒加热回路是否能独立安全工作。

13. 代码全文

/*
 * LOM纸叠层三维打印机五轴控制程序
 * 控制卡：固高 GTS-800 系列运动控制卡
 *
 * 轴定义：
 * Axis1：X轴，松下A6伺服驱动器
 * Axis2：Y轴，松下A6伺服驱动器
 * Axis3：Z轴，步进驱动器
 * Axis4：送料轴，步进驱动器
 * Axis5：热压滚筒轴，步进驱动器
 *
 * 限位定义：
 * LIMIT0+ / LIMIT0-：X轴正限位 / 负限位
 * LIMIT1+ / LIMIT1-：Y轴正限位 / 负限位
 * LIMIT2+ / LIMIT2-：Z轴上限位 / 下限位
 * LIMIT3+ / LIMIT3-：送料轴正限位 / 负限位
 * LIMIT4+ / LIMIT4-：热压滚筒轴正限位 / 负限位
 *
 * 输入输出定义：
 * EXI0：安全输入
 * EXO0：驱动继电器输出
 * EXO1：加热控制输出
 * LASER+ / LASER-：CO2激光电源TTL控制端
 *
 * 回零方式：
 * X轴、Y轴、送料轴、热压滚筒轴采用负限位回零。
 * Z轴采用上限位回零。
 */

#include <stdio.h>
#include <windows.h>
#include "gts.h"

/******************** 0. 调试开关 ********************/
/*
 * 1：仿真IO，用于查看程序流程。
 * 0：实机IO，需要补全 ReadRawEXI、WriteEXO、ReadLimitTerminal、LaserOn、LaserOff。
 */
#define USE_MOCK_IO 1

/******************** 1. 轴号定义 ********************/

#define AXIS_X       1
#define AXIS_Y       2
#define AXIS_Z       3
#define AXIS_FEED    4
#define AXIS_ROLLER  5

#define AXIS_MASK_5  0x1F

/******************** 2. 限位编号 ********************/

#define LIMIT_X       0
#define LIMIT_Y       1
#define LIMIT_Z       2
#define LIMIT_FEED    3
#define LIMIT_ROLLER  4

#define LIMIT_NEG     0
#define LIMIT_POS     1

/*
 * 光电限位触发电平。
 * 在调试软件中观察光电开关动作状态：
 * 遮挡时显示为1，则保持1；
 * 遮挡时显示为0，则改为0。
 */
#define LIMIT_TRIGGER_LEVEL  1

/******************** 3. 输入输出编号 ********************/

#define EXI_SAFE_OK       0

#define EXO_DRIVE_RELAY   0
#define EXO_HEATER_EN     1

/*
 * EXI有效电平。
 * 安全输入正常时，若调试软件显示为1，则保持1；
 * 若显示为0，则改为0。
 */
#define EXI_OK_LEVEL      1

/******************** 4. 机械参数 ********************/

#define X_PULSE_PER_MM      80.0
#define Y_PULSE_PER_MM      80.0
#define Z_PULSE_PER_MM      640.0
#define FEED_PULSE_PER_MM   33.9
#define R_PULSE_PER_MM      80.0

#define X_WORK_OFFSET       230.0
#define Y_WORK_OFFSET       230.0

#define LAYER_THICKNESS     0.20
#define FEED_LENGTH         520.0

#define R_WAIT_POS          0.0
#define R_START_POS         30.0
#define R_END_POS           630.0

#define XY_SAFE_WORK_X      0.0
#define XY_SAFE_WORK_Y      0.0

/*
 * 加热预热时间。
 * 如果温控器已经独立完成恒温控制，该时间用于等待热压滚筒达到工艺温度。
 * 实际调试时按温度上升速度修改。
 */
#define PREHEAT_TIME_MS     30000

/******************** 5. 回零参数 ********************/

#define HOME_SEARCH_VEL     10.0
#define HOME_CREEP_VEL      3.0
#define HOME_BACK_DIST      5.0
#define HOME_OFFSET_DIST    2.0

/******************** 6. 运动状态位 ********************/
/*
 * 常见固高示例中，0x400表示规划运动中。
 * 实际值以所用SDK和编程手册为准。
 */
#define STS_PROFILE_MOVING  0x400

/******************** 7. 状态机 ********************/

typedef enum
{
    S_POWER_ON = 0,
    S_SELF_CHECK,
    S_LIMIT_HOME,
    S_PREHEAT,
    S_LAYER_PREPARE,
    S_PAPER_FEED,
    S_HOT_PRESS,
    S_LASER_CUT,
    S_Z_DOWN,
    S_NEXT_LAYER,
    S_FINISH,
    S_FAULT,
    S_EMERGENCY
} MachineState;

/******************** 8. 单位换算 ********************/

long mm_to_pulse(short axis, double mm)
{
    switch(axis)
    {
    case AXIS_X:
        return (long)(mm * X_PULSE_PER_MM);
    case AXIS_Y:
        return (long)(mm * Y_PULSE_PER_MM);
    case AXIS_Z:
        return (long)(mm * Z_PULSE_PER_MM);
    case AXIS_FEED:
        return (long)(mm * FEED_PULSE_PER_MM);
    case AXIS_ROLLER:
        return (long)(mm * R_PULSE_PER_MM);
    default:
        return 0;
    }
}

double pulse_to_mm(short axis, double pulse)
{
    switch(axis)
    {
    case AXIS_X:
        return pulse / X_PULSE_PER_MM;
    case AXIS_Y:
        return pulse / Y_PULSE_PER_MM;
    case AXIS_Z:
        return pulse / Z_PULSE_PER_MM;
    case AXIS_FEED:
        return pulse / FEED_PULSE_PER_MM;
    case AXIS_ROLLER:
        return pulse / R_PULSE_PER_MM;
    default:
        return 0.0;
    }
}

double vel_to_sdk(short axis, double vel_mm_s)
{
    /*
     * 这里按 pulse/ms 换算。
     * 若所用SDK速度单位不同，在此统一换算。
     */
    switch(axis)
    {
    case AXIS_X:
        return vel_mm_s * X_PULSE_PER_MM / 1000.0;
    case AXIS_Y:
        return vel_mm_s * Y_PULSE_PER_MM / 1000.0;
    case AXIS_Z:
        return vel_mm_s * Z_PULSE_PER_MM / 1000.0;
    case AXIS_FEED:
        return vel_mm_s * FEED_PULSE_PER_MM / 1000.0;
    case AXIS_ROLLER:
        return vel_mm_s * R_PULSE_PER_MM / 1000.0;
    default:
        return 0.0;
    }
}

double WorkToMachineX(double x_work)
{
    return x_work + X_WORK_OFFSET;
}

double WorkToMachineY(double y_work)
{
    return y_work + Y_WORK_OFFSET;
}

/******************** 9. IO接口封装 ********************/

int ReadRawEXI(int exi_no)
{
#if USE_MOCK_IO
    if(exi_no == EXI_SAFE_OK) return EXI_OK_LEVEL;
    return 0;
#else
    /*
     * 按实际固高SDK读取EXI输入。
     * 返回值要求：0或1。
     */
    long value = 0;

    /*
     * 示例：
     * GT_GetDi(MC_GPI, &value);
     */

    return (value & (1 << exi_no)) ? 1 : 0;
#endif
}

void WriteEXO(int exo_no, int value)
{
#if USE_MOCK_IO
    printf("[EXO] EXO%d = %d\n", exo_no, value);
#else
    /*
     * 按实际固高SDK输出EXO信号。
     */
    /*
     * 示例：
     * GT_SetDoBit(MC_GPO, exo_no, value);
     */

    (void)exo_no;
    (void)value;
#endif
}

int ReadLimitTerminal(int limit_no, int pos_or_neg)
{
#if USE_MOCK_IO
    /*
     * 仿真状态下，默认所有限位未触发。
     */
    return 0;
#else
    /*
     * 按实际固高SDK读取LIMIT输入。
     *
     * limit_no:
     * 0 = LIMIT0
     * 1 = LIMIT1
     * 2 = LIMIT2
     * 3 = LIMIT3
     * 4 = LIMIT4
     *
     * pos_or_neg:
     * LIMIT_POS = 正限位
     * LIMIT_NEG = 负限位
     *
     * 返回值要求：
     * 1 = 限位触发
     * 0 = 限位未触发
     */
    int raw_level = 0;

    (void)limit_no;
    (void)pos_or_neg;

    return raw_level == LIMIT_TRIGGER_LEVEL ? 1 : 0;
#endif
}

int IsSafeOK()
{
    return ReadRawEXI(EXI_SAFE_OK) == EXI_OK_LEVEL ? 1 : 0;
}

void DriveRelayOn()
{
    WriteEXO(EXO_DRIVE_RELAY, 1);
}

void DriveRelayOff()
{
    WriteEXO(EXO_DRIVE_RELAY, 0);
}

void HeaterOn()
{
    WriteEXO(EXO_HEATER_EN, 1);
}

void HeaterOff()
{
    WriteEXO(EXO_HEATER_EN, 0);
}

void LaserOn()
{
#if USE_MOCK_IO
    printf("[LASER] ON\n");
#else
    /*
     * 按实际固高激光端子或数字输出端子打开激光。
     */
#endif
}

void LaserOff()
{
#if USE_MOCK_IO
    printf("[LASER] OFF\n");
#else
    /*
     * 按实际固高激光端子或数字输出端子关闭激光。
     */
#endif
}

/******************** 10. 限位判断 ********************/

int AxisToLimitNo(short axis)
{
    switch(axis)
    {
    case AXIS_X:
        return LIMIT_X;
    case AXIS_Y:
        return LIMIT_Y;
    case AXIS_Z:
        return LIMIT_Z;
    case AXIS_FEED:
        return LIMIT_FEED;
    case AXIS_ROLLER:
        return LIMIT_ROLLER;
    default:
        return -1;
    }
}

int IsPosLimitOn(short axis)
{
    return ReadLimitTerminal(AxisToLimitNo(axis), LIMIT_POS);
}

int IsNegLimitOn(short axis)
{
    return ReadLimitTerminal(AxisToLimitNo(axis), LIMIT_NEG);
}

int IsZUpLimitOn()
{
    return ReadLimitTerminal(LIMIT_Z, LIMIT_POS);
}

int IsZDownLimitOn()
{
    return ReadLimitTerminal(LIMIT_Z, LIMIT_NEG);
}

/******************** 11. 安全保护 ********************/

int SafetyCheck()
{
    if(!IsSafeOK())
    {
        printf("安全输入异常，停止运行。\n");
        LaserOff();
        HeaterOff();
        return 0;
    }

    return 1;
}

/*
 * 加工运行阶段调用。
 * 回零阶段不调用该函数，避免将找限位动作误判为故障。
 */
int LimitProtectCheck()
{
    if(IsNegLimitOn(AXIS_X) || IsPosLimitOn(AXIS_X))
    {
        printf("X轴限位触发。\n");
        return 0;
    }

    if(IsNegLimitOn(AXIS_Y) || IsPosLimitOn(AXIS_Y))
    {
        printf("Y轴限位触发。\n");
        return 0;
    }

    if(IsZUpLimitOn() || IsZDownLimitOn())
    {
        printf("Z轴限位触发。\n");
        return 0;
    }

    if(IsNegLimitOn(AXIS_FEED) || IsPosLimitOn(AXIS_FEED))
    {
        printf("送料轴限位触发。\n");
        return 0;
    }

    if(IsNegLimitOn(AXIS_ROLLER) || IsPosLimitOn(AXIS_ROLLER))
    {
        printf("热压滚筒轴限位触发。\n");
        return 0;
    }

    return 1;
}

void EmergencyStop()
{
    LaserOff();
    HeaterOff();
    DriveRelayOff();

    GT_Stop(AXIS_MASK_5, 0);
}

/******************** 12. 运动控制函数 ********************/

void StopOneAxis(short axis)
{
    GT_Stop(1 << (axis - 1), 0);
    Sleep(100);
    GT_ClrSts(axis, 1);
}

int IsAxisMoving(short axis)
{
    long sts = 0;
    unsigned long clk = 0;

    GT_GetSts(axis, &sts, 1, &clk);

    return (sts & STS_PROFILE_MOVING) ? 1 : 0;
}

int WaitAxisStop(short axis)
{
    while(IsAxisMoving(axis))
    {
        if(!SafetyCheck() || !LimitProtectCheck())
        {
            EmergencyStop();
            return 0;
        }

        Sleep(10);
    }

    return 1;
}

int WaitAxesStop(long mask)
{
    int moving = 1;

    while(moving)
    {
        moving = 0;

        for(short axis = 1; axis <= 5; axis++)
        {
            if(mask & (1 << (axis - 1)))
            {
                if(IsAxisMoving(axis))
                {
                    moving = 1;
                }
            }
        }

        if(!SafetyCheck() || !LimitProtectCheck())
        {
            EmergencyStop();
            return 0;
        }

        Sleep(10);
    }

    return 1;
}

int MoveAbsMM(short axis, double pos_mm, double vel_mm_s)
{
    TTrapPrm trap;
    long pos_pulse = mm_to_pulse(axis, pos_mm);
    double vel = vel_to_sdk(axis, vel_mm_s);

    GT_PrfTrap(axis);

    GT_GetTrapPrm(axis, &trap);
    trap.acc = 0.25;
    trap.dec = 0.25;
    trap.smoothTime = 20;
    GT_SetTrapPrm(axis, &trap);

    GT_SetVel(axis, vel);
    GT_SetPos(axis, pos_pulse);

    GT_Update(1 << (axis - 1));

    return 1;
}

int MoveRelMM(short axis, double distance_mm, double vel_mm_s)
{
    double current_pos = 0.0;
    unsigned long clk = 0;
    double current_mm = 0.0;
    double target_mm = 0.0;

    GT_GetPrfPos(axis, &current_pos, 1, &clk);

    current_mm = pulse_to_mm(axis, current_pos);
    target_mm = current_mm + distance_mm;

    return MoveAbsMM(axis, target_mm, vel_mm_s);
}

void JogAxis(short axis, int dir, double vel_mm_s)
{
    TJogPrm jog;
    double vel = vel_to_sdk(axis, vel_mm_s);

    GT_PrfJog(axis);

    GT_GetJogPrm(axis, &jog);
    jog.acc = 0.1;
    jog.dec = 0.1;
    GT_SetJogPrm(axis, &jog);

    GT_SetVel(axis, dir < 0 ? -vel : vel);

    GT_Update(1 << (axis - 1));
}

void MoveXYMachineMM(double x_mm, double y_mm, double vel_mm_s)
{
    TTrapPrm trapX, trapY;
    long x_pulse = mm_to_pulse(AXIS_X, x_mm);
    long y_pulse = mm_to_pulse(AXIS_Y, y_mm);

    GT_PrfTrap(AXIS_X);
    GT_PrfTrap(AXIS_Y);

    GT_GetTrapPrm(AXIS_X, &trapX);
    trapX.acc = 0.25;
    trapX.dec = 0.25;
    trapX.smoothTime = 20;
    GT_SetTrapPrm(AXIS_X, &trapX);

    GT_GetTrapPrm(AXIS_Y, &trapY);
    trapY.acc = 0.25;
    trapY.dec = 0.25;
    trapY.smoothTime = 20;
    GT_SetTrapPrm(AXIS_Y, &trapY);

    GT_SetVel(AXIS_X, vel_to_sdk(AXIS_X, vel_mm_s));
    GT_SetVel(AXIS_Y, vel_to_sdk(AXIS_Y, vel_mm_s));

    GT_SetPos(AXIS_X, x_pulse);
    GT_SetPos(AXIS_Y, y_pulse);

    GT_Update((1 << (AXIS_X - 1)) | (1 << (AXIS_Y - 1)));
}

void MoveXYWorkMM(double x_work, double y_work, double vel_mm_s)
{
    MoveXYMachineMM(WorkToMachineX(x_work), WorkToMachineY(y_work), vel_mm_s);
}

/******************** 13. 限位回零 ********************/

int HomeAxisByNegLimit(short axis)
{
    printf("Axis %d 负限位回零。\n", axis);
    LaserOff();

    if(!SafetyCheck())
    {
        return 0;
    }

    /*
     * 若轴已压在负限位上，先向正方向退出限位。
     */
    if(IsNegLimitOn(axis))
    {
        JogAxis(axis, 1, HOME_CREEP_VEL);

        while(IsNegLimitOn(axis))
        {
            if(!SafetyCheck())
            {
                StopOneAxis(axis);
                return 0;
            }

            Sleep(10);
        }

        StopOneAxis(axis);
    }

    /*
     * 第一次寻找负限位。
     */
    JogAxis(axis, -1, HOME_SEARCH_VEL);

    while(!IsNegLimitOn(axis))
    {
        if(!SafetyCheck())
        {
            StopOneAxis(axis);
            return 0;
        }

        Sleep(10);
    }

    StopOneAxis(axis);

    /*
     * 退出限位。
     */
    MoveRelMM(axis, HOME_BACK_DIST, HOME_CREEP_VEL);
    while(IsAxisMoving(axis))
    {
        if(!SafetyCheck())
        {
            EmergencyStop();
            return 0;
        }

        Sleep(10);
    }

    /*
     * 第二次慢速寻找负限位。
     */
    JogAxis(axis, -1, HOME_CREEP_VEL);

    while(!IsNegLimitOn(axis))
    {
        if(!SafetyCheck())
        {
            StopOneAxis(axis);
            return 0;
        }

        Sleep(10);
    }

    StopOneAxis(axis);

    /*
     * 离开限位一小段距离，并将当前位置清零。
     */
    MoveRelMM(axis, HOME_OFFSET_DIST, HOME_CREEP_VEL);
    while(IsAxisMoving(axis))
    {
        if(!SafetyCheck())
        {
            EmergencyStop();
            return 0;
        }

        Sleep(10);
    }

    GT_ZeroPos(axis, 1);

    printf("Axis %d 回零完成。\n", axis);

    return 1;
}

int HomeZByUpLimit()
{
    printf("Z轴上限位回零。\n");
    LaserOff();

    if(!SafetyCheck())
    {
        return 0;
    }

    /*
     * 默认Z轴正方向为向上。
     * 若实际方向相反，需要调整本函数中的运动方向。
     */
    if(IsZUpLimitOn())
    {
        JogAxis(AXIS_Z, -1, HOME_CREEP_VEL);

        while(IsZUpLimitOn())
        {
            if(!SafetyCheck())
            {
                StopOneAxis(AXIS_Z);
                return 0;
            }

            Sleep(10);
        }

        StopOneAxis(AXIS_Z);
    }

    /*
     * 第一次寻找上限位。
     */
    JogAxis(AXIS_Z, 1, HOME_SEARCH_VEL);

    while(!IsZUpLimitOn())
    {
        if(!SafetyCheck())
        {
            StopOneAxis(AXIS_Z);
            return 0;
        }

        Sleep(10);
    }

    StopOneAxis(AXIS_Z);

    /*
     * 向下退出上限位。
     */
    MoveRelMM(AXIS_Z, -HOME_BACK_DIST, HOME_CREEP_VEL);
    while(IsAxisMoving(AXIS_Z))
    {
        if(!SafetyCheck())
        {
            EmergencyStop();
            return 0;
        }

        Sleep(10);
    }

    /*
     * 第二次慢速寻找上限位。
     */
    JogAxis(AXIS_Z, 1, HOME_CREEP_VEL);

    while(!IsZUpLimitOn())
    {
        if(!SafetyCheck())
        {
            StopOneAxis(AXIS_Z);
            return 0;
        }

        Sleep(10);
    }

    StopOneAxis(AXIS_Z);

    /*
     * 离开上限位一小段距离，并将当前位置清零。
     */
    MoveRelMM(AXIS_Z, -HOME_OFFSET_DIST, HOME_CREEP_VEL);
    while(IsAxisMoving(AXIS_Z))
    {
        if(!SafetyCheck())
        {
            EmergencyStop();
            return 0;
        }

        Sleep(10);
    }

    GT_ZeroPos(AXIS_Z, 1);

    printf("Z轴回零完成。\n");

    return 1;
}

int HomeAllAxisByLimit()
{
    /*
     * 先让Z轴回到上方安全位置，再进行其他轴回零。
     */
    if(!HomeZByUpLimit())
    {
        return 0;
    }

    if(!HomeAxisByNegLimit(AXIS_X))
    {
        return 0;
    }

    if(!HomeAxisByNegLimit(AXIS_Y))
    {
        return 0;
    }

    if(!HomeAxisByNegLimit(AXIS_FEED))
    {
        return 0;
    }

    if(!HomeAxisByNegLimit(AXIS_ROLLER))
    {
        return 0;
    }

    return 1;
}

/******************** 14. 工艺动作 ********************/

int PreheatRoller()
{
    unsigned long start = GetTickCount();

    HeaterOn();

    while(GetTickCount() - start < PREHEAT_TIME_MS)
    {
        if(!SafetyCheck())
        {
            return 0;
        }

        Sleep(100);
    }

    return 1;
}

int PaperFeed()
{
    LaserOff();

    MoveRelMM(AXIS_FEED, FEED_LENGTH, 40.0);

    if(!WaitAxisStop(AXIS_FEED))
    {
        return 0;
    }

    return 1;
}

int HotPress()
{
    LaserOff();

    MoveXYWorkMM(XY_SAFE_WORK_X, XY_SAFE_WORK_Y, 100.0);
    if(!WaitAxesStop((1 << (AXIS_X - 1)) | (1 << (AXIS_Y - 1))))
    {
        return 0;
    }

    MoveAbsMM(AXIS_ROLLER, R_START_POS, 40.0);
    if(!WaitAxisStop(AXIS_ROLLER))
    {
        return 0;
    }

    MoveAbsMM(AXIS_ROLLER, R_END_POS, 30.0);
    if(!WaitAxisStop(AXIS_ROLLER))
    {
        return 0;
    }

    MoveAbsMM(AXIS_ROLLER, R_WAIT_POS, 50.0);
    if(!WaitAxisStop(AXIS_ROLLER))
    {
        return 0;
    }

    return 1;
}

int EnsureRollerSafe()
{
    MoveAbsMM(AXIS_ROLLER, R_WAIT_POS, 50.0);

    if(!WaitAxisStop(AXIS_ROLLER))
    {
        return 0;
    }

    return 1;
}

int LaserCutLine(double x1, double y1, double x2, double y2)
{
    LaserOff();

    MoveXYWorkMM(x1, y1, 120.0);
    if(!WaitAxesStop((1 << (AXIS_X - 1)) | (1 << (AXIS_Y - 1))))
    {
        LaserOff();
        return 0;
    }

    LaserOn();

    /*
     * 此处为直线切割示例。
     * 实机轮廓切割建议使用固高二维直线插补函数。
     */
    MoveXYWorkMM(x2, y2, 30.0);
    if(!WaitAxesStop((1 << (AXIS_X - 1)) | (1 << (AXIS_Y - 1))))
    {
        LaserOff();
        return 0;
    }

    LaserOff();

    return 1;
}

int LaserCutCurrentLayer()
{
    if(!IsSafeOK())
    {
        printf("安全输入异常，禁止激光切割。\n");
        return 0;
    }

    if(!EnsureRollerSafe())
    {
        printf("热压滚筒未退回等待位置，禁止激光切割。\n");
        return 0;
    }

    /*
     * 示例路径：矩形轮廓。
     * 实际路径可由切片文件或G代码生成。
     */
    if(!LaserCutLine(-100, -100,  100, -100)) return 0;
    if(!LaserCutLine( 100, -100,  100,  100)) return 0;
    if(!LaserCutLine( 100,  100, -100,  100)) return 0;
    if(!LaserCutLine(-100,  100, -100, -100)) return 0;

    LaserOff();

    return 1;
}

int ZDownOneLayer()
{
    LaserOff();

    /*
     * Z轴上限位回零后，默认向下为负方向。
     */
    MoveRelMM(AXIS_Z, -LAYER_THICKNESS, 5.0);

    if(!WaitAxisStop(AXIS_Z))
    {
        return 0;
    }

    return 1;
}

/******************** 15. 控制卡初始化 ********************/

int ControllerInit()
{
    short rtn;

    rtn = GT_Open();

    if(rtn != 0)
    {
        printf("固高控制卡打开失败，错误码：%d\n", rtn);
        return 0;
    }

    GT_Reset();
    Sleep(200);

    /*
     * 配置文件由固高调试软件生成，文件名按现场配置填写。
     */
    GT_LoadConfig("GTS800.cfg");

    GT_ClrSts(1, 5);

    GT_AxisOn(AXIS_X);
    GT_AxisOn(AXIS_Y);
    GT_AxisOn(AXIS_Z);
    GT_AxisOn(AXIS_FEED);
    GT_AxisOn(AXIS_ROLLER);

    LaserOff();
    HeaterOff();
    DriveRelayOn();

    return 1;
}

/******************** 16. 主程序 ********************/

int main()
{
    MachineState state = S_POWER_ON;

    int currentLayer = 0;
    int totalLayer = 10;

    while(1)
    {
        switch(state)
        {
        case S_POWER_ON:
            printf("S0：系统上电初始化。\n");

            if(ControllerInit())
            {
                state = S_SELF_CHECK;
            }
            else
            {
                state = S_FAULT;
            }
            break;

        case S_SELF_CHECK:
            printf("S1：安全自检。\n");

            if(!SafetyCheck())
            {
                state = S_EMERGENCY;
                break;
            }

            state = S_LIMIT_HOME;
            break;

        case S_LIMIT_HOME:
            printf("S2：五轴限位回零。\n");

            if(!HomeAllAxisByLimit())
            {
                state = S_FAULT;
                break;
            }

            MoveAbsMM(AXIS_ROLLER, R_WAIT_POS, 50.0);
            if(!WaitAxisStop(AXIS_ROLLER))
            {
                state = S_FAULT;
                break;
            }

            MoveXYWorkMM(XY_SAFE_WORK_X, XY_SAFE_WORK_Y, 100.0);
            if(!WaitAxesStop((1 << (AXIS_X - 1)) | (1 << (AXIS_Y - 1))))
            {
                state = S_FAULT;
                break;
            }

            state = S_PREHEAT;
            break;

        case S_PREHEAT:
            printf("S3：热压滚筒预热。\n");

            if(PreheatRoller())
            {
                state = S_LAYER_PREPARE;
            }
            else
            {
                state = S_EMERGENCY;
            }
            break;

        case S_LAYER_PREPARE:
            printf("S4：第 %d 层准备。\n", currentLayer + 1);

            if(currentLayer >= totalLayer)
            {
                state = S_FINISH;
            }
            else
            {
                state = S_PAPER_FEED;
            }
            break;

        case S_PAPER_FEED:
            printf("S5：送料。\n");

            if(PaperFeed())
            {
                state = S_HOT_PRESS;
            }
            else
            {
                state = S_FAULT;
            }
            break;

        case S_HOT_PRESS:
            printf("S6：热压滚筒滚压。\n");

            if(HotPress())
            {
                state = S_LASER_CUT;
            }
            else
            {
                state = S_FAULT;
            }
            break;

        case S_LASER_CUT:
            printf("S7：激光切割。\n");

            if(LaserCutCurrentLayer())
            {
                state = S_Z_DOWN;
            }
            else
            {
                state = S_FAULT;
            }
            break;

        case S_Z_DOWN:
            printf("S8：Z轴下降一个层厚。\n");

            if(ZDownOneLayer())
            {
                state = S_NEXT_LAYER;
            }
            else
            {
                state = S_FAULT;
            }
            break;

        case S_NEXT_LAYER:
            printf("S9：进入下一层。\n");

            currentLayer++;
            state = S_LAYER_PREPARE;
            break;

        case S_FINISH:
            printf("S10：加工完成。\n");

            LaserOff();
            HeaterOff();
            DriveRelayOff();

            GT_Stop(AXIS_MASK_5, 0);
            GT_Close();

            return 0;

        case S_FAULT:
            printf("SF：系统故障。\n");

            EmergencyStop();
            GT_Close();

            return -1;

        case S_EMERGENCY:
            printf("SE：安全保护触发。\n");

            EmergencyStop();
            GT_Close();

            return -1;

        default:
            state = S_FAULT;
            break;
        }

        Sleep(50);
    }
}
Back to archive

Discussion

Comments

Post

Share questions, corrections, or extra notes about this post.