SM3_CNC,轴组，G代码解析，CNC运动控制

核心： AXIS_REF_SM3

主要是解析G代码到空间坐标【笛卡尔坐标系】。

SM3_CNC库，主要做了空间坐标到电机位置的关系相互转换。

因为机械如果是三脚架，那3个电机插补运动才能走出直线。  SMC_TRAFO_Tripod(FB)

还有带刀片旋向的【裁布机】    SMC_TRAFO_GantryCutter3(FB)

以龙门架为例：
            龙门架系统Gantrysystems

【电机位置到坐标】SMC_TRAFOF_5Axes(FB)
//以伺服电机的位置正向计算【正】出参考点在空间中位姿【XYZ AB】。

【空间坐标到电机速度位置】SMC_TRAFOV_Gantry2(FB)
// 以空间中的坐标点，计算出电机此刻应该旋转的【速度】【位置】

【空间坐标到电机刀具位置】SMC_TRAFO_5Axes(FB)
// 以空间中的坐标点，计算出电机此刻应该旋转的【位置】用于移动刀具进行加工

//===============================================================
    软运动变换    SoftMotionTransformations
    龙门架系统    Gantrysystems

【正计算，电机位置到坐标位姿】SMC_TRAFOF_5Axes(FB)// 五轴雕刻机【正计算可绑定可视化控件】
                    SMC_TRAFOF_Gantry2(FB)// 二轴龙门
                    SMC_TRAFOF_Gantry2Tool1(FB)//    带刀具偏移的龙门系统
                    SMC_TRAFOF_Gantry2Tool2(FB)//    带刀具偏移的龙门系统
                    SMC_TRAFOF_Gantry3(FB)// 【标准3轴龙门】
                    SMC_TRAFOF_Gantry3D(FB)// 三维龙门架
                    SMC_TRAFOF_GantryCutter2(FB)//  带旋转方向【刀片】
                    SMC_TRAFOF_GantryCutter3(FB)
                    SMC_TRAFOF_GantryH2(FB) // H型架子，2个同步带串联移动出XY坐标
                    SMC_TRAFOF_GantryT2(FB) // T型同步带架子。
                    SMC_TRAFOF_GantryT2_O(FB)
【位姿到电机方向速度】SMC_TRAFOV_Gantry2(FB)// 【vf控制电机，和方向】
                    SMC_TRAFOV_Gantry3(FB)
                    SMC_TRAFOV_GantryCutter2(FB)
                    SMC_TRAFOV_GantryCutter3(FB)
                    SMC_TRAFOV_GantryH2(FB)
                    SMC_TRAFOV_GantryT2(FB)
                    SMC_TRAFOV_GantryT2_O(FB)

// 根据空间位置计算轴位置（逆变换）
【位姿到电机位置】   SMC_TRAFO_5Axes(FB)// 五轴雕刻机【计算结果，分发给5轴】
                    SMC_TRAFO_Gantry2(FB)// 标准二轴
                    SMC_TRAFO_Gantry2Tool1(FB)// 带刀具偏移
                    SMC_TRAFO_Gantry2Tool2(FB)
                    SMC_TRAFO_Gantry3(FB)// 标准龙门三轴
                    SMC_TRAFO_GantryCutter2(FB)//  二维带刀具偏行【刀片方向】
                    SMC_TRAFO_GantryCutter3(FB)// 三维带刀旋向【刀片方向】
                    SMC_TRAFO_GantryH2(FB)// 同步带串联 H架子
                    SMC_TRAFO_GantryT2(FB)
                    SMC_TRAFO_GantryT2_O(FB)
并行系统ParallelSystems
双脚架臂Bipod_Arm
        SMC_TRAFOF_Bipod_Arm(FB)
        SMC_TRAFO_Bipod_Arm(FB)
三脚架Tripod
        SMC_TRAFOF_Tripod(FB)
        SMC_TRAFO_Tripod(FB)
三脚架臂Tripod_Arm
        SMC_TRAFOF_Tripod_Arm(FB)
        SMC_TRAFO_Tripod_Arm(FB)
Tripod_Lin
        SMC_TrafoF_Tripod_Lin(FB)
        SMC_Trafo_Tripod_Lin(FB)
运动学机器人RobotKinematiks
        SMC_TrafoF_4AxisPalletizer(FB)// 4轴机器人
        SMC_TrafoF_ArticulatedRobot_6DOF(FB)// 6轴机器人【电机位置转位姿】
        SMC_Trafo_4AxisPalletizer(FB)
        SMC_Trafo_ArticulatedRobot_6DOF(FB)// 6轴机器人【位姿转电机位置】
水平摇臂ScaraSystem
        SMC_TRAFOF_Polar(FB)
        SMC_TRAFOF_Scara2(FB)
        SMC_TRAFOF_Scara3(FB) // 水平摇臂， 死盖啦【3臂】
        SMC_TRAFO_Polar(FB)
        SMC_TRAFO_Scara2(FB)
        SMC_TRAFO_Scara3(FB)
附加功能additionalFBs
        SMC_CalcDirectionFromVector(FB)

根据位置，实现电机定位：    SMC_ControlAxisByPos (FB)

Direct Axis Control    //直接轴控制
轴定位         SMC_ControlAxisByPos (FB)
轴定速度位置    SMC_ControlAxisByPosVel (FB)
轴定速度        SMC_ControlAxisByVel (FB)
//================================================
配置轴位置1：SMC_ControlAxisByPos(
	Axis:= x1, //目标轴
	iStatus:= ,// SMC_Interpolator实例的状态
	bEnable:= , //TRUE：开始执行
	bAvoidGaps:= , //TRUE：开始监测位置
	fSetPosition:= SM_Drive_Virtual_1, //在[u]中设置轴的位置。
	fGapVelocity:= , //跳过的速度，单位为[u/s]
	fGapAcceleration:= , //跳过的加速度[u/s2]
	fGapDeceleration:= , //跳跃绕过的减速[u/s2]
	fGapJerk:= , //在[u/s3]中跳过跳跃的跳跃
	bBusy=> , //TRUE：功能块运行
	bCommandAborted=> , //TRUE：执行被另一个在轴上操作的功能块实例中断。
	bError=> , //TRUE：出现错误
	iErrorID=> , //错误码
	bStopIpo=> );//TRUE：速度或位置发生跳跃，并且正在运行对新位置的适应。
————————————————
版权声明：本文为CSDN博主「cfqq1989」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/cfqq1989/article/details/131160892

SMC_INT_STATUS (ENUM)
0：SMC_Interpolator完全通过后可能不会出现的内部状态。
1：初始化状态；运动尚未开始
2：插值正在进行；当前正在加速
3：以恒定速度进行插值
4：插值正在进行；当前正在减速
5：插值完成
6：插值正在进行；当前正在等待，例如通过停止输入或M命令
7：插值正在进行；目前正在加速
8：插值正在进行；当前正在降低加速度
9：插值正在进行；目前正在加速减速
10：插值正在进行；目前正在减速

从轴跟随：   SMC_TrackAxis (FB)

因为涉及到双X轴的龙门，所以我加了这一节：
轴组模型内，》绑定虚轴。》然后实轴跟随虚轴。

SMC_FollowPosition (FB)
SMC_FollowPositionVelocity (FB)
SMC_FollowSetValues (FB)
SMC_FollowVelocity (FB)
SMC_SetTorque (FB)
SMC_TrackAxis (FB)
SMC_TrackSetValues (FB)

这里主要使用：
直接轴【跟随】    SMC_TrackAxis (FB)// 从轴要使能状态下
                        bExecute上升沿：开始执行功能块。
                        Velocity 最高速度[u/s]
                        Acceleration 最大加速度，单位为[u/s²]。
                        Deceleration 最大减速度，单位为【u/s²】。
                        Jerk    起跳速度，单位为【u/s³】。

————————————————
版权声明：本文为CSDN博主「cfqq1989」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/cfqq1989/article/details/131799133

实轴使能后，再激活这个功能块，电机就会快速跟随。如果飞车【降低最大加速度和最高速度】

高阶的从轴跟随：
    从轴配置成【力矩定位模式】》从轴跟随实轴。

另一部分是解析G代码，分析出轨迹：

这样轨迹再给【机械坐标转换】再给伺服电机。

 

这三个，主要区别是：
File》SMC_CNC_REF》SMC_OutQueue

File:这个是G代码文件，CAD软件导出的dxf文件再转G代码。【上层】最终要解析到底层
     SMC_CNC_REF：这个是G代码的半成品，带工具刀补等。【中层】
       SMC_OutQueue：这个是最终的轨迹图，所见即所得。【底层】

项目可以加载这3种CNC，但程序最终结果是处理：SMC_OutQueue
//================================
File：
        // 老版本，读取G代码文件
        SMC_ReadNCFile (FB)    》SMC_NCDecoder (FB)
        SMC_ReadNCQueue (FB)   》SMC_NCDecoder, SMC_SmoothPath, SMC_RoundPath, 
                                 SMC_AvoidLoop and SMC_ToolCorr

        // 最新的
        SMC_ReadNCFile2 (FB)    》SMC_NCInterpreter (FB)
        SMC_NCLookupFile (FB)
//=================================
SMC_CNC_REF：
        //这个半成品后面跟》刀具补偿》圆弧插补》速度平滑》最后解析到 SMC_OutQueue
        SMC_NCDecoder (FB)
//==================================
SMC_OutQueue：
        SMC_Interpolator (FB)//解析出空间轨迹 //输入是指针【adr(CNC)】
        SMC_Interpolator2Dir (FB)// 带【加工回退】，可反向插补运动

//===========================================
SMC_Interpolator (FB)// 解析空间轨迹
    bExecute 执行从上升边缘开始。
    poqDataIn 通常它指向SMC_CheckVelocities的输出poqDataOut。
    bSlow_Stop        true为停止。
    bEmergency_Stop   True为急停
    bWaitAtNextStop   只要此变量为FALSE（默认值），路径就会不间断地传递。
    dOverride         加工速度，【速度倍率】
    iVelMode          该输入定义了SMC_INT_VELMODE中定义的速度剖面。
        dwIpoTime【间隔时间】  必须为每次调用设置此变量。它表示循环时间，单位为μ秒。
    dLastWayPos    该输入允许用户测量由插值器绘制的路径的拉伸。
    bAbort  此输入设置为TRUE将中止功能块并重置输出。需要bExecute的上升沿才能再次启动插值器。
    bSingleStep    此输入会使插值器在一个循环的持续时间内停止在两个路径对象之间的过渡处
    bAcknM         TRUE为确认M功能，并继续。
    bQuick_Stop    TRUE，插值器将把速度降低到零，直到bQuick_Stop重置为FALSE
    dQuickDeceleration    用于bQuick_Stop的减速值
    dJerkMax        起跳速度。它必须是正的，并且在插值器运行时不能更改。
    dQuickStopJerk    如果选择了二次速度模式之一，则急动的幅度由快速停止用于降低加速度。
    bSuppressSystemMFunctions	如果设置了此选项，则不会为G75或G4命令创建的内部M函数设置输出wM。

SMC_Interpolator2Dir (FB)解析出空间轨迹》

SMC_TRAFO_5Axes (FB)以空间坐标计算出电机位置》

SMC_ControlAxisByPos (FB)配置电机运动到位置》

1： 在 codesys里，添加CNC程序

文件File转到 》 SMC_CNC_REF  用 【SMC_ReadNCFile (FB)】
文件File转到 》 SMC_OutQueue 用 【SMC_ReadNCQueue (FB)】

  

选中对象》 按F1   //获取帮助

说明：  核心函数  SMC_NCInterpreter     // File2的插补解析

                             SMC_Interpolator  旧版本插补解析 

SM3_CNC库文档
公司：3S-智能软件解决方案有限公司
标题：SM3_CNC
版本：4.13.0.0
分类：实习生| SoftMotion
作者：
占位符：SM3_CNC
说明[1]
该库的功能块允许读取、解释和插值g代码文件（根据DIN 66025）。
解释器（SMC_NCInterpreter）将路径转换为可以修改、转换和插值的结构，以便为驱动器创建循环设置位置。（例如
SMC_ReadNCFile2、//读取G代码文件
SMC_NCInterpreter、// G代码解析器，用与File2
SMC_ToolRadiusCorr、//  刀具补偿
SMC_AvoidLoop、// 轨迹
SMC_SmoothPath、// 平滑路径
SMC_RoundPath、// 圆形路径
SMC_Interpolator）。// 插补算法 它的上层是 SMC_CheckVelocities检查速度
//=========================
SMC_TRAFO_5Axes //获取5轴电机坐标  SMC_TRAFO_GantryCutter3 (FB)解析，XYZR轨迹
SMC_ControlAxisByPos   // X axis  轴定位 XYZ R ABC

功能块的功能分解允许特定组件被为特殊需求设计的功能块所取代。此外，进一步的功能块——比如单独的路径预处理器——可以与现有组件组合，而不会出现任何问题。
库的数据结构（例如SMC_POSINFO、SMC_GEOINFO和SMC_VECTOR3D）描述位置，

SM_CNC_POUs  普通轴控制
SM_Trafo_POUs  龙门轴组控制
               Forward and inverse transformation  正逆变换
               SMC_TRAFOV_  // 解析坐标位置
// OF：(正计算)以实轴位置计算参考中心点在空间内坐标姿态
SMC_TRAFOF_5Axes (FB)            以5个实轴位置，计算参考点在笛卡尔内的坐标
SMC_TRAFOF_Gantry2 (FB)          龙门2轴，以轴位置计算出点坐标在笛卡尔内的位置
SMC_TRAFOF_Gantry2Tool1 (FB)     XY+方位A轴
SMC_TRAFOF_Gantry2Tool2 (FB)     XY+AB轴（刀具方向和角度）
SMC_TRAFOF_Gantry3 (FB)          实例，3轴龙门坐标位置
SMC_TRAFOF_Gantry3D (FB)         实例，空间实际坐标位置
SMC_TRAFOF_GantryCutter2 (FB)    龙门滚刀，XYR旋转轴（核桃机？）
SMC_TRAFOF_GantryCutter3 (FB)    龙门柱，  XYZR
SMC_TRAFOF_GantryH2 (FB)         实例，龙门H形，XY
SMC_TRAFOF_GantryT2 (FB)         实例，龙门T形，XY
SMC_TRAFOF_GantryT2_O (FB)           与T2相比，驱动器配置有所不同（Y轴反向）
// OV：以坐标姿态计算轴速度和位置
SMC_TRAFOV_Gantry2 (FB)          解析，XY轨迹（用于图像比例显示）
SMC_TRAFOV_Gantry3 (FB)          解析，XYZ轨迹
SMC_TRAFOV_GantryCutter2 (FB)    解析，XYR切割轨迹（R旋转）
SMC_TRAFOV_GantryCutter3 (FB)    解析，XYZR切割轨迹
SMC_TRAFOV_GantryH2 (FB)         解析H龙门，XY轨迹 （参阅SM_Trafo_POUs概述。）
SMC_TRAFOV_GantryT2 (FB)         解析T龙门，XY速度
SMC_TRAFOV_GantryT2_O (FB)
// O：以坐标姿态反向计算实轴位置
SMC_TRAFO_5Axes (FB)             解析，XYZ，+倾斜+方向（用于硬件轴运动定位加工）5轴
SMC_TRAFO_Gantry2 (FB)           解析，XY轨迹
SMC_TRAFO_Gantry2Tool1 (FB)      解析，XY+刀具距离
SMC_TRAFO_Gantry2Tool2 (FB)      解析，带刀补的XY轨迹
SMC_TRAFO_Gantry3 (FB)           解析，XYZ轨迹
SMC_TRAFO_GantryCutter2 (FB)     解析，XYR轨迹（R刀片方向）// 圆刀片裁布机
SMC_TRAFO_GantryCutter3 (FB)     解析，XYZR轨迹 //雕核桃机，雕刻鸡蛋
SMC_TRAFO_GantryH2 (FB)          解析H，由XY计算出辅助AB轴位置
SMC_TRAFO_GantryT2 (FB)          解析T，由XY计算出辅助AB轴位置
SMC_TRAFO_GantryT2_O (FB)        具有固定驱动器的T型龙门架系统的反向转换（替代驱动器配置）。

使用请注明出处：维修电工 ,LSP电工，qq750273008

硬件要求：

中型PLC汇川AM600，禾川HCQ0-1100-d，且带 SM3_CNC.library 库（3.5.6支持离线仿真）

G代码标准： DIN66025 

DIN66025-1标准

G0    运动定位
G1    线性插补
G2    顺圆插补
G3    逆圆插补
G4    等待
G6    抛物线插补
G8    椭圆插补

M指令块：SMC_Interpolator  插补算法（按时间节拍，截取位置）

SMC_Interpolator    //插补器（遇到M指令会暂停）

bAcknM置位（继续插补）
wM输出M指令值

H点胶指令：开关量

SMC_Interpolator    //插补器（遇到H指令不停顿）
N90    G1 X20
N100   G1 X100 H2 L20  H-2 L-10  //起点加20位置点胶，终点-10位置取消
                                // H指令不会影响运动速度

辅助轴：

插补轴：X，Y，Z
辅助轴：A，B，C，P，Q，U，V，W    // 8个辅助轴
轴排序：ALL := 16#FFFF;
X := 2#1 ;
Y := 2#10 ;
Z := 2#100 ;
A,B,C,P,Q,U,V,W,A6,ALL,

 

库的用法：解析G代码文件

SMC_Interpolator  插补算法（按时间节拍，截取位置）

SMC_OutQueue，SMC_CNC_REF，File    //位置流，带参位置流，文件流

SMC_ReadNCFile        // G代码文件读取//仿真路径
                      // C:\ProgramData\CODESYS\Simulation\PlcLogic\

SMC_NCDecoder         // G代码文件解码》输出poqDataOut（位置坐标流/轨迹坐标）
SMC_ToolCorr          // 工具参数
SMC_SmoothPath  // 轨迹平滑
SMC_CheckVelocities   // 轨迹速度检查》输出的轨迹数据要给插补器 poqDataOut

SMC_Interpolator      // 插补器，运算： 》输入：轨迹数据poqDataOut
                      //               》输出：piSetPosition
SMC_TRAFO_Gantry3     // 解析出3轴坐标  》输入：pi
                      //               》输出：dx,dy,dz分发给轴定位控制

SMC_ControlAxisByPos   // X axis  轴定位 XYZABC

1读取G代码文件解析

G代码文件》保存到 _cnc文件夹内

NC文件路径: STRING := '_cnc/gcode.cnc'; //PLC参考路径在本体主根目录下，G代码放在_cnc文件夹内
                                   // （后续以这个路径参考点再添加文件深度）

放该文件夹内 ，文件名例如： SouJiKe.txt     // 雕刻手机壳的G代码文件

2插补运算：SMC_Interpolator插补器，  poqDataOut轨迹

利用 SMC_CheckVelocities（轨迹速度检查）的输出做输入
poqDataOut轨迹流

主从轴，位置跟随：  SMC_ControlAxisByPos（FB） 

SMC_Interpolator  插补算法（按时间节拍，截取位置）
                  piSetPosition   输出以时间为节拍的坐标流

SMC_TRAFO_Scara2   解析出节拍坐标位置》并发给对应的伺服电机

 库函数：POU 功能块

SM3_CNC  ， （ 3.5.8.0 ）
SM_CNC_POUs》SoftMotion CNC》SoftMotion Function Blocks

SM3_CNC, 3.5.8.0
Project Information        // get项目信息
SM_CNC_Data types          // SM_CNC_数据类型
SM_CNC_POUs                // CNC程序块  SMC_ReadNCFile文件  SMC_ReadNCQueue队列
SM_Trafo_Data type         // 轴组，龙门，机器人，数据类型
SM_Trafo_POUs              // 轴组转换功能块  Gantry systems龙门系统
Visu_Templates             // 模板
Visu_Templates_New         // 新模板

轴组：龙门轴组，6轴机械臂，码垛机，行架 等

添加轴组后，可以选轴组类型，龙门数控或者搬运机械臂等。。。

轴组的库是 robotics （机器人）

 

转换FB块为5轴_龙门。
5轴由：X，Y，Z，A，B 轴组成
4是A轴：是旋转轴。（垂直于Z轴旋转，比如刀片）
3是B轴：是倾斜轴。（与Z角度仰角调节，如铣刀倒角）

 

刀具长度（u） ：

一般绑定虚轴。然后实轴跟随虚轴运动。实现主从运动
从轴，位置跟随：  SMC_ControlAxisByPos（FB） 

双X轴龙门：

这里涉及到2个电机：x1轴和x2轴
x1轴为主，x2轴为从，x2轴一直去追赶x1轴的坐标位置。用 MC_GearIN 电子齿轮

在齿条型的X大梁，x2轴如果与x1轴位置超差过大，会导致X大梁卡死。所以x2轴伺服要配置成：（力矩加定位）的混合模式PRT。

函数使用：  MC_GearIN    电子齿轮功能  （注：注意从轴的追赶速度值大小）
脱开：      MC_GearOut   断开耦合

另一种方案是：
            新建一个虚轴。
            x1和x2去追赶虚轴的坐标。从轴，位置跟随：  SMC_ControlAxisByPos（FB） 
H型龙门：这种如3D打印机，喷头单一方向移动，需要2个电机配合旋转，XY的皮带是串联的。

圆弧插补：

SMC_RoundPath

轴组状态：

 

SM3_Robotics  4.13.0.0

SMC_GroupPower    电源
MC_GroupEnable    使能    MC_GroupDisable    关闭

MC_GroupReset    复位
MC_GroupStop     停止
SMC_GroupWait    等待
MC_GroupContinue    继续

SMC_GroupJog2    点动
MC_GroupSetPosition        设置轴位置 a，c，v，坐标

MC_MoveLinearAbsolute    MC_直线绝对移动
MC_MoveLinearRelative    MC_直线相对移动

MC_MoveCircularAbsolute    MC_循环绝对移动
MC_MoveCircularRelative    MC_循环相对值移动

MC_MoveDirectAbsolute    MC_直接绝对值移动
MC_MoveDirectRelative    MC_直接相对移动

MC_AddAxisToGroup        添加轴组
MC_RemoveAxisFromGroup   删除一个轴
MC_UngroupAllAxes        解散轴组
SMC_GroupTargetPosition     SMC_组目标位置

SMC_GroupEnableResumeAfterError    错误后组启用恢复
SMC_GroupUpdateContinueData    组更新连续数据
MC_SetCoordinateTransform    设置坐标转换（工件坐标，机械臂坐标，轴坐标）