倍福TwinCAT3伺服控制常用功能块的实现

2、伺服PTP功能块定义
2.1定义轴变量
_axis1: AXIS_REF; //轴变量
2.2添加轴控制指令
定义功能块变量：

MC_POWER_0:							MC_POWER;				//轴使能										
MC_Reset_0:							MC_Reset;				//轴复位
MC_Stop_0:							MC_Stop;				//轴停止			
MC_Jog_0:							MC_Jog;
MC_MoveAdditive_0:					MC_MoveAdditive;
MC_MoveRelative_0:					MC_MoveRelative;
MC_MoveAbsolute_0:					MC_MoveAbsolute;
MC_MoveVelocity_0:					MC_MoveVelocity;
MC_SetOverride_0:					MC_SetOverride;			//设置轴速度缩放系数
MC_SetPosition_0:					MC_SetPosition;			//设置当前位置
MC_ReadActualPosition_0:			MC_ReadActualPosition;
MC_ReadActualVelocity_0:			MC_ReadActualVelocity;	
MC_ReadStatus_0:					MC_ReadStatus;

2.3添加全局变量
定义控制的全局变量：

//轴控制指令	
bi_Power:								BOOL;			//使能
bi_Reset:								BOOL;			//复位
bi_Stop:								BOOL;			//停止	
bi_JogForward:							BOOL;			//正向点动
bi_JogBackwards:						BOOL;			//反向点动		
bi_MoveAdditive:						BOOL;			//增量位置运动
bi_MoveRelative:						BOOL;			//相对位置运动
bi_MoveAbsolute:						BOOL;			//绝对位置运动	
bi_MoveVelocity:						BOOL;			//速度方式运动
bi_SetOverride:							BOOL;			//轴速度缩放启用
bi_SetPosition:							BOOL;			//增量式伺服，设置当前位置值
bi_AxisRead:							BOOL;			//读取轴的状态

定义输入的控制参数变量：

//轴运动参数设置
di_Stop_Deceleration:					LREAL:=4000;	//停止速度(MC_Stop)
di_SetPosition:							LREAL:=0;		//

di_Jog_Velocity:						LREAL;
di_Jog_Acceleration:					LREAL:=3000;
di_Jog_Deceleration:					LREAL:=3000;
di_Jog_Jerk:							LREAL:=0;		//加加速度。	

di_MoveAdditive_Distance:				LREAL:=0;
di_MoveAdditive_Velocity:				LREAL:=100;
di_MoveAdditive_Acceleration:			LREAL:=500;	
di_MoveAdditive_Deceleration:			LREAL:=500;
di_MoveAdditive_Jerk:					LREAL:=0;

di_MoveRelative_Distance:				LREAL:=0;
di_MoveRelative_Velocity:				LREAL:=100;
di_MoveRelative_Acceleration:			LREAL:=500;
di_MoveRelative_Deceleration:			LREAL:=500;
di_MoveRelative_Jerk:					LREAL:=0;

di_MoveAbsolute_Position:				LREAL:=0;
di_MoveAbsolute_Velocity:				LREAL:=100;
di_MoveAbsolute_Acceleration:			LREAL:=500;
di_MoveAbsolute_Deceleration:			LREAL:=500;
di_MoveAbsolute_Jerk:					LREAL:=0;

di_MoveVelocity_Velocity:				LREAL:=100;
di_MoveVelocity_Acceleration:			LREAL:=500;
di_MoveVelocity_Deceleration:			LREAL:=500;
di_MoveVelocity_Jerk:					LREAL:=0;
di_MoveVelocity_Direction:				MC_Direction:=1;			//1:Positive	3:Negative
di_VelFactor:							LREAL:=1.0;					//1.0=100%		范围：0.01-1.0

定义存储反馈得轴状态变量

//轴状态反馈
bo_AxisError:							BOOL;
bo_ErrorStop:							BOOL;			//故障未复位的情况下再次转动电机输出故障停车信号
bo_Homing:								BOOL;			//轴回原点进行中
bo_Homed:								BOOL;			//轴已经回原点		
bo_Moving:								BOOL;			//轴运动中
bo_Disabled:							BOOL;
bo_Stopping:							BOOL;			//停止信号输出一次
bo_StandStill:							BOOL;			//待机中
bo_DiscreteMotion:						BOOL;			//不连续运动
bo_ContinuousMotion:					BOOL;			//连续运动
bo_SynchronizedMotion:					BOOL;			//同步运动
bo_ConstantVelocity:					BOOL;			//恒速运行中
bo_Accelerating:						BOOL;			//加速中
bo_Decelerating:						BOOL;			//减速中
		
//
do_ActualPosition:						LREAL;			//轴当前位置
do_ActualVelocity:						LREAL;			//轴当前速度
do_AxisErroID:							DWORD;			//故障代码
	
dg_ActualTorque			AT%I*:			INT;			// Axises's Actual Torque

2.4轴控功能块ST语言实现

//*轴使能
MC_POWER_0(
Axis:= _axis1,
Enable:= bi_Power, //TRUE使能，FALSE去使能
Enable_Positive:= TRUE,
Enable_Negative:= TRUE,
Override:= ,
BufferMode:= ,
Options:= ,
Status=> ,
Busy=> ,
Active=> ,
Error=> ,
ErrorID=> );

//*轴复位
MC_Reset_0(
Axis:= _axis1,
Execute:= bi_Reset, //上升沿信号触发
Done=> ,
Busy=> ,
Error=> ,
ErrorID=> );

//*轴停机
MC_Stop_0(
Axis:= _axis1,
Execute:= bi_Stop, //上升沿信号触发
Deceleration:= di_Stop_Deceleration,
Jerk:= ,
Options:= ,
Done=> ,
Busy=> ,
Active=> ,
CommandAborted=> ,
Error=> ,
ErrorID=> );

//Jog点动
MC_Jog_0(
Axis:= _axis1,
JogForward:= bi_JogForward,
JogBackwards:= bi_JogBackwards,
Mode:= MC_JOGMODE_CONTINOUS, //点动模式
Position:= ,
Velocity:= di_Jog_Velocity,
Acceleration:=di_Jog_Acceleration ,
Deceleration:= di_Jog_Deceleration,
Jerk:= di_Jog_Jerk,
Done=> ,
Busy=> ,
Active=> ,
CommandAborted=> ,
Error=> ,
ErrorID=> );

//*轴相对定位-MoveAdd //
MC_MoveAdditive_0(
Axis:= _axis1,
Execute:= bi_MoveAdditive, //上升沿信号触发
Distance:= di_MoveAdditive_Distance,
Velocity:= di_MoveAdditive_Velocity,
Acceleration:= di_MoveAdditive_Acceleration,
Deceleration:= di_MoveAdditive_Deceleration,
Jerk:= di_MoveAdditive_Jerk,
BufferMode:= ,
Options:= ,
Done=> ,
Busy=> ,
Active=> ,
CommandAborted=> ,
Error=> ,
ErrorID=> );

//*轴相对定位-MoveRela //
MC_MoveRelative_0(
Axis:= _axis1,
Execute:= bi_MoveRelative, //上升沿信号触发
Distance:= di_MoveRelative_Distance,
Velocity:= di_MoveRelative_Velocity,
Acceleration:= di_MoveRelative_Acceleration,
Deceleration:= di_MoveRelative_Deceleration,
Jerk:= di_MoveRelative_Jerk,
BufferMode:= ,
Options:= ,
Done=> ,
Busy=> ,
Active=> ,
CommandAborted=> ,
Error=> ,
ErrorID=> );

//**轴绝对定位启动
MC_MoveAbsolute_0(
Axis:= _axis1,
Execute:= bi_MoveAbsolute, //上升沿信号触发
Position:= di_MoveAbsolute_Position,
Velocity:= di_MoveAbsolute_Velocity,
Acceleration:= di_MoveAbsolute_Acceleration,
Deceleration:= di_MoveAbsolute_Deceleration,
Jerk:= di_MoveAbsolute_Jerk,
BufferMode:= ,
Options:= ,
Done=> ,
Busy=> ,
Active=> ,
CommandAborted=> ,
Error=> ,
ErrorID=> );

//**以速度方式运行
MC_MoveVelocity_0(
Axis:= _axis1,
Execute:= bi_MoveVelocity, //上升沿信号触发
Velocity:=di_MoveVelocity_Velocity ,
Acceleration:= di_MoveVelocity_Acceleration,
Deceleration:= di_MoveVelocity_Deceleration,
Jerk:= di_MoveVelocity_Jerk,
Direction:= di_MoveVelocity_Direction, //MC_Negative_Direction -> 反向 MC_Positive_Direction -> 正向
BufferMode:= ,
Options:= ,
InVelocity=> ,
Busy=> ,
Active=> ,
CommandAborted=> ,
Error=> ,
ErrorID=> );

//**轴速度缩放系数设置
MC_SetOverride_0(
Axis:= _axis1,
Enable:= bi_SetOverride, //TRUE生效，FALSE无效
VelFactor:= di_VelFactor, //1.0=100% 0.01-1.0
AccFactor:= ,
JerkFactor:= ,
Enabled=> ,
Busy=> ,
Error=> ,
ErrorID=> );

//*当前轴位置设置(增量式编码器电机)
MC_SetPosition_0(
Axis:= _axis1,
Execute:= bi_SetPosition, //TRUE生效，FALSE无效
Position:= 0,
Mode:= ,
Options:= ,
Done=> ,
Busy=> ,
Error=> ,
ErrorID=> );
IF MC_SetPosition_0.Done THEN
bi_SetPosition:=FALSE;
END_IF

//*读轴位置
MC_ReadActualPosition_0(
Axis:= _axis1,
Enable:= bi_AxisRead, //TRUE生效，FALSE无效
Valid=> ,
Busy=> ,
Error=> ,
ErrorID=> ,
Position=> do_ActualPosition);

//*读轴速度
MC_ReadActualVelocity_0(
Axis:= _axis1,
Enable:= bi_AxisRead, //TRUE生效，FALSE无效
Valid=> ,
Busy=> ,
Error=> ,
ErrorID=> ,
ActualVelocity=>do_ActualVelocity );

//*读轴状态
MC_ReadStatus_0(
Axis:= _axis1,
Enable:= bi_AxisRead, //TRUE生效，FALSE无效
Valid=> ,
Busy=> ,
Error=> bo_AxisError,
ErrorID=>do_AxisErroID ,
ErrorStop=> bo_ErrorStop,
Disabled=> bo_Disabled,
Stopping=> bo_Stopping,
StandStill=> bo_StandStill,
DiscreteMotion=> bo_DiscreteMotion,
ContinuousMotion=> bo_ContinuousMotion,
SynchronizedMotion=> bo_SynchronizedMotion,
Homing=> bo_Homing,
ConstantVelocity=> bo_ConstantVelocity,
Accelerating=> bo_Accelerating,
Decelerating=> bo_Decelerating,
Status=> );

2.5轴控功能块梯形图(LD)实现
1




2.6程序在线监控

ST程序在线

优点：注释和程序一目了然

梯形图程序在线

优点：逻辑功能清晰，功能块得输入和输出清清楚楚

3、第三方伺服其它功能
3.1扭矩读取
读取第三方伺服的扭矩，需要通过关联PDO Maping的输出来读取，有的厂家伺服在IO扫描出来PDO Mapping中就有Torque，有的伺服厂家扫描出来没有，PDO Maping中没有Torque的需要手动添加。以台达A2和汇川IS620N分别举例：

首先在PLC程序添加变量：

hmi_ActualTorque AT%I*: INT; // Axises’s Actual Torque
变量添加好以后，PLC程序工程重新编译。

汇川IS620N

3.2增量式伺服回零
第三方增量式伺服回零方式主要有两种

方式1：DS402协议内部PDO方式回零。

优点：伺服自己运行、定位精度高

缺点：程序逻辑复杂、不同厂家数据协议不一样

具体用法参见：

TwinCAT3中台达A2增量编码器伺服使用PDO方式回零_panjinliang066333的博客-CSDN博客

方式2：使用伺服功能块MC_Jog和MC_SetPosition

优点：不同的厂家都可以通用、程序逻辑简单

缺点：定位精度低

用法：回零指令起动，伺服以Jog运动方式缓慢向零运动，当零点传感器检测到伺服到达信号Jog运动停止，然后使用MC_SetPosition设置当前位置为0，即伺服回零完成。若伺服刚开始就在零点传感器上，则Jog不会运动，伺服位置直接设置为零。

具体用法参见：

（注意：因为伺服回零是往一个方向运动，因此不管用哪种方式回零，在第一次回零的时候务必确保零点传感器在Jog运动的方向范围之内）
————————————————
版权声明：本文为CSDN博主「Big_潘大师」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/panjinliang066333/article/details/123423911