倍福PLC通过EtherCAT总线控制伺服电机---（三）FB控制篇

前言

当遇到多个轴的项目时，如果使用简单程序控制，不仅工作量大且容易出错。常用的方法是根据自己的控制和工艺要求去进行功能块二次封装。在不同项目的积累下，可以形成较为完善的功能块。本文将结合一个简单的伺服定位控制案例，介绍如何使用自定义功能块去控制伺服电机，后面功能块叫FB。

一、编写轴控制功能块

1,在POU新建功能块FB；

2，新建功能块的变量
（1）案例中把轴的变量建立在同一个FB中，参考建立如下变量：

FUNCTION_BLOCK PUBLIC Axis_Ctl
VAR_INPUT
	iReset:BOOL;					//轴复位
	iStop:BOOL;						//轴停止		
	iEMG:BOOL;						//EMS急停
	AxisPowerIN:BOOL;				//轴上电
	JogF:BOOL;						//轴正向JOG启动
	JogR:BOOL;						//轴反向JOG启动
	AutoR:BOOL;						//轴反向定位启动
	AutoF:BOOL;						//轴正向定位启动
	MoveABSPosR:LREAL;				//轴反向的目标位置
	MoveABSPosF:LREAL;				//轴正向的目标位置
	MoveVelF:LREAL;					//轴正向的速度
	MoveVelR:LREAL;					//轴反向的速度
	MoveACC:LREAL;					//轴运动加速度
	MoveDec:LREAL;					//轴运行减速度
	MoveJerk:LREAL;					//轴运动加加速度
	MaxPos:INT;						//轴正向极限位置
	MinPos:INT;						//轴反向极限位置
	JogVel:LREAL;					//轴JOG速度
	UpLimitSensor:BOOL;				//轴正极限传感器
	DownLimitSensor:BOOL;			//轴负极限传感器
	
END_VAR
VAR_OUTPUT
	AxisError:BOOL;					//轴状态-出错
	AxisMoving:BOOL;				//轴状态-运动中
	AxisPowerOK:BOOL;				//轴状态-上电完成
	oArriveFPos:BOOL;				//轴状态-到达前进位置
	oArriveRPos:BOOL;				//轴状态-到达后退位置
	oArriveMPos:BOOL;				//轴状态-到达中间位置
	AxisActPos:LREAL;				//轴实际位置
	AxisActVel:LREAL;				//轴实际速度
	
END_VAR
VAR_IN_OUT
	AxisRef:AXIS_REF;				//轴引用对象
END_VAR
VAR
	fbMCreadStatus:MC_READSTATUS;	//轴状态读取功能块
	fbMCPowerON:MC_Power;			//轴上电功能块
	fbMCJog:MC_JOG;					//轴JOG功能块
	fbMCMoveABS:MC_Moveabsolute;	//轴绝对定位功能块
	fbMCStop:MC_Stop;				//轴停止功能块
	fbMCReset:MC_Reset;				//轴复位功能块
	TOF1:tof;
END_VAR

（2）把系统封装的功能块进行二次封装，完成轴使能（MC_Power）,轴JOG(MC_JOG），轴绝对运动(MC_Moveabsolute)，轴停止(MC_Stop)，轴复位(MC_Reset)，案例中使用的是多圈绝对编码器，所以不需要回零，如果需要可以自行封装回零MC_Home，相对运动MC_MoveRelative等。本文常用的一些参数已进行了说明，如果自己有其他使用，建议参考TC3倍福的帮助文档。

参考代码如下：

IF NOT AxisError AND AxisPowerOK AND iEMG THEN
		fbMCMoveABS.Acceleration:=MoveACC;	//设置轴的加速度			
		fbMCMoveABS.Deceleration:=MoveDec;	//设置轴的减速度
		fbMCMoveABS.Jerk:=MoveJerk;			//设置轴的加加速度
	IF AutoF THEN
		fbMCMoveABS.Position:=MoveABSPosF;	//设置轴正向定位的绝对位置
		fbMCMoveABS.Velocity:=MoveVelF	;	//设置轴正向定位的速度
		fbMCMoveABS.Execute:=AutoF AND NOT oArriveFPos AND NOT fbMCMoveABS.Busy;	//绝对定位执行		
	END_IF
	IF AutoR THEN
		fbMCMoveABS.Position:=MoveABSPosR;	//设置轴反向定位的绝对位置
		fbMCMoveABS.Velocity:=MoveVelR	;	//设置轴反向定位的速度	
		fbMCMoveABS.Execute:=AutoR AND NOT oArriveRPos AND NOT fbMCMoveABS.Busy;	//绝对定位执行		
	END_IF
	IF NOT AutoF AND NOT AutoR THEN 
		fbMCMoveABS.Execute:=FALSE;
	END_IF			
ELSE
	fbMCMoveABS.Execute:=FALSE;
END_IF

fbMCreadStatus(Axis:=AxisRef);
TOF1(in:=AxisPowerIN AND NOT AxisError,pt:=T#2S);
//轴上电功能
fbMCPowerON(
	Axis:= AxisRef, 
	Enable:= TOF1.Q, 
	Enable_Positive:= UpLimitSensor, 				//正极限SensorOn时，运行正转
	Enable_Negative:= DownLimitSensor, 				//负极限SensorOn时，运行反转
	Override:= , 
	BufferMode:= , 
	Options:= , 
	Status=> AxisPowerOK, 
	Busy=> , 
	Active=> , 
	Error=> , 
	ErrorID=> );

//轴JOG功能
IF JogF THEN
	fbMCJog.Velocity:=JogVel;
ELSIF JogR THEN
	fbMCJog.Velocity:=JogVel;	
END_IF
fbMCJog(Axis:=Axisref,
		Mode:= Tc2_MC2.MC_JOGMODE_CONTINOUS,	//轴控制模式，根据需要选择INCH或者JOG
		Velocity:=JogVel,
		jogforward:=JogF AND NOT JogR AND NOT AxisError AND AxisPowerOK AND iEMG,
		jogbackwards:=JOGR AND NOT JogF AND NOT AxisError AND AxisPowerOK AND iEMG);
//轴绝对运动功能
fbMCMoveABS(Axis:=Axisref);
//轴复位功能
fbMCReset(Axis:=AxisRef,Execute:=iReset AND AxisError);
//轴停止功能
fbMCStop(Axis:=AxisRef,Execute:=iStop OR AxisError OR  (not iEMG) );
//轴状态数据监控
AxisError:=axisref.NcToPlc.ErrorCode<>0;		//轴状态-出错
AxisMoving:=axisref.NcToPlc.AxisState<>0;		//轴状态-运动中
AxisActPos:=axisref.NcToPlc.ActPos;				//轴状态-实际位置
AxisActVel:=axisref.NcToPlc.ActVelo;			//轴状态-实际速度
//轴正向运动到位
IF ABS(AxisActPos-MoveABSPosF)<0.5 THEN
	oArriveFPos:=TRUE;
ELSE
	oArriveFPos:=FALSE;
END_IF
//轴反向运动到位
IF ABS(AxisActPos-MoveABSPosR)<0.5 THEN
	oArriveRPos:=TRUE;
ELSE
	oArriveRPos:=FALSE;
END_IF

二、如何使用编写好的功能块

1，创建测试程序变量

VAR
	testAxis:Axis_Ctl;		//Axis_Ctl即是自定义封装的FB，这里建立它的实例轴
	axis1:Tc2_MC2.AXIS_REF;	//axis1即为NC轴对应的对象，让功能块知道控制的是哪根轴，轴绑定方法见第二篇
	
	//建立测试变量
	step:UINT;				//工艺步骤			
	init:BOOL;				//初始化启动
	inialOK:BOOL;			//初始化OK
	Enable:BOOL;			//启动允许
	StartRTrig:Tc2_Standard.R_TRIG;//上升沿
	moveDone:BOOL;			//工艺完成
END_VAR

2，规划控制程序的逻辑架构，这里的案例我们实现将轴正向定位3圈，反向定位1圈后结束。
那么程序应该包括:
（1）初始化：包括回零（使用增量编码器时），轴移动到准备位置，机构复位等；
（2）定义工艺步骤，细化每一步工艺应轴应该到的位置，移动速度等；
（3）实现轴功能块的调用；

参考代码如下：

//初始化
IF init THEN    
	step:=0;
	inialOK:=FALSE;
	testAxis.MoveVelF:=testAxis.MoveVelR:=500;			//设置定位速度	
	testAxis.MoveABSPosR:=testAxis.MinPos:=0;			//设置反向定位位置为0
	testAxis.AutoR:=TRUE;testAxis.AutoF:=FALSE;			//反向定位给启动
	IF	testAxis.oArriveRPos  THEN 						//判断是否到达位置
		testAxis.AutoR:=FALSE;testAxis.AutoF:=FALSE;
		inialOK:=TRUE;
		init:=FALSE;
	END_IF	
END_IF

//轴定位启动
StartRTrig(clk:=Enable AND inialOK);
IF StartRTrig.q THEN 
	Step:=10;	
END_IF
//轴工艺步骤
CASE step OF
	0:
	IF NOT init THEN 
		testAxis.AutoR:=testAxis.AutoF:=FALSE;	
	END_IF
	
	10:
	testAxis.MoveVelF:=500;								//设置定位速度	
	testAxis.MoveABSPosF:=1080;							//设置正向定位位置是300
	testAxis.AutoF:=TRUE;testAxis.AutoR:=FALSE;			//反向定位给启动
	IF	testAxis.oArriveFPos THEN 						//判断是否到达位置
		testAxis.AutoR:=FALSE;testAxis.AutoF:=FALSE;
		step:=20;
	END_IF		
	
	20:
	testAxis.MoveVelR:=250;								//设置定位速度	
	testAxis.MoveABSPosR:=360;							//设置正向定位位置是300
	testAxis.AutoF:=FALSE;testAxis.AutoR:=TRUE;			//反向定位给启动
	IF	testAxis.oArriveRPos THEN 						//判断是否到达位置
		testAxis.AutoR:=FALSE;testAxis.AutoF:=FALSE;
		step:=100;
	END_IF		
	
	100:
	
	moveDone:=TRUE;
	

END_CASE

//轴功能块调用
testAxis(
	iReset:= , 
	iStop:= , 
	iEMG:= , 
	AxisPowerIN:= , 
	JogF:= , 
	JogR:= , 
	AutoR:= , 
	AutoF:= , 
	MoveABSPosR:= , 
	MoveABSPosF:= , 
	MoveVelF:= , 
	MoveVelR:= , 
	MoveACC:= , 
	MoveDec:= , 
	MoveJerk:= , 
	MaxPos:= , 
	MinPos:= , 
	JogVel:= , 
	UpLimitSensor:= , 
	DownLimitSensor:= , 
	AxisError=> , 
	AxisMoving=> , 
	AxisPowerOK=> , 
	oArriveFPos=> , 
	oArriveRPos=> , 
	oArriveMPos=> , 
	AxisActPos=> , 
	AxisActVel=> , 
	AxisRef:= axis1);

说明：1，在初始化阶段，我们可以把轴放到一个待机位置，如果有其他执行机构需要控制，也一并放到初始化中，作为运动的起始位置；2，在轴工艺阶段，把相关的控制动作根据工艺目标进行分步骤编写，上述案例程序实现将轴定位到1080°(即3圈的位置)，再反向定位到360°(1圈)的位置，然后完成定位任务；3，在轴调用的功能块中，可以看到有些输入参数是在逻辑程序中进行使用，并未在功能块直接赋值，使用方法是功能块名称.输入参数。

总结

本文介绍了倍福PLC如何自定义功能块进行轴定位控制，同时说明了如何使用FB，进行轴控制程序的编写的基本方法。后续大家想了解倍福PLC哪些方面的功能，可以留言。