TwinCAT 3 轴控制程序

已于 2024-12-05 14:25:13 修改
阅读量1.5k 收藏 20
点赞数 12
分类专栏： TwinCAT 3 文章标签： TwinCAT 3
于 2024-01-05 14:02:31 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/weixin_58619062/article/details/135407925
版权

ST

TYPE ENUM_index_axis :
(
	indexAxisStart:=0,

	//伺服索引编号
	indexAxisExample1,
	indexAxisExample2,
	indexAxisExample3,
	
	indexAxisEnd
);
END_TYPE
 
TYPE ENUM_index_axis_error :
(
	indexAxisErrorStart:=0,

	indexAxisErrorInterferenceError,
	indexAxisErrorTorqueLimitError,
	indexAxisErrorPositiveLimitError,
	indexAxisErrorNegativeLimitError,
	indexAxisErrorAxisError,
	indexAxisErrorJogError,
	indexAxisErrorHomeError,
	indexAxisErrorMoveRelativeError,
	indexAxisErrorMoveAbsoluteError,
	indexAxisErrorMoveVelocityError
);
END_TYPE
 
TYPE ST_axis :
STRUCT
	sComment:WSTRING;
	nControlMode:INT;	

	REF:AXIS_REF;
	com:ST_axis_com;
	control:ST_axis_control;
	parameter:ST_axis_parameter;
	signal:ST_axis_signal;
	other:ST_axis_other;
	status:ST_axis_status;
	FB:FB_axis;
END_STRUCT
END_TYPE
 
TYPE ST_axis_com :
STRUCT
	//关联硬件
	nDigitalInputs AT %I* :UDINT;//0.0负限位，0.1正限位，0.2原点
	nErrorCode AT %I* :UINT;//驱动器报错代码
	nState AT %I* :UINT;//通讯状态
	nPort AT %I* :WORD;//伺服通讯端口号
	nTorque AT %I* :INT;//扭矩

	nControlWord AT %Q* :UINT;//控制字
	nModesOfOperation AT %Q* :SINT;//运行模式

	//关联NC轴
	nCtrl1 AT %I* :USINT;//控制字
	nCtrl2 AT %I* :USINT;//控制字
	nCtrl5 AT %I* :USINT;//运行模式
END_STRUCT
END_TYPE
 
TYPE ST_axis_control :
STRUCT
	manual:ST_axis_control_manu_auto;
	automatic:ST_axis_control_manu_auto;
END_STRUCT
END_TYPE
 
TYPE ST_axis_control_manu_auto :
STRUCT
	bPower:BOOL:=TRUE;
	bStop:BOOL;
	bHalt:BOOL;
	bReset:BOOL;

	bHome:BOOL;
	bJogForward:BOOL;
	bJogBackward:BOOL;
	bMoveRelative:BOOL;
	bMoveAbsolute:BOOL;
	bMoveVelocity:BOOL;
END_STRUCT
END_TYPE
 
TYPE ST_axis_other :
STRUCT
	sAmsNetId:T_AmsNetID;
	nPort:WORD;

	bTorqueLimit:BOOL;
	nTorqueLimit:LREAL;
	
	bPositionLimit:BOOL;
	nPositiveLimit:LREAL;
	nNegativeLimit:LREAL;

	nPositionCompareMax:LREAL:=0.05;
END_STRUCT
END_TYPE
 
TYPE ST_axis_parameter :
STRUCT
	manual:ST_axis_parameter_manu_auto;
	automatic:ST_axis_parameter_manu_auto;
END_STRUCT
END_TYPE
 
TYPE ST_axis_parameter_manu_auto :
STRUCT
	nOverride:LREAL:=100;//速度百分比

	nAccelerationTime:LREAL:=0.2; //加速时间
	nDecelerationTime:LREAL:=0.2; //减速时间

	nVelocity:LREAL:=100;
	nAcceleration:LREAL:=1000;
	nDeceleration:LREAL:=1000;
	nJerk:LREAL:=10000;//加加速度

	nJogVelocity:LREAL:=1;
	nJogAcceleration:LREAL:=1000;
	nJogDeceleration:LREAL:=1000;
	nJogJerk:LREAL:=10000;//加加速度

	nDistance:LREAL;
	nPosition:LREAL;
	nDirection:MC_Direction;
	nHomeMethod:UDINT:=23;
END_STRUCT
END_TYPE
(*
nAcceleration:=nVelocity*2/nAccelerationTime;
nDeceleration:=nVelocity*2/nDecelerationTime;
nJerk:=nAcceleration*2/nAccelerationTime;
*)
 
TYPE ST_axis_signal :
STRUCT
	bPositiveSensor:BOOL;
	bNegativeSensor:BOOL;
	bHomeSensor:BOOL;
	
	bInterference:ARRAY[1..4] OF BOOL;
END_STRUCT
END_TYPE
 
TYPE ST_axis_status :
STRUCT
	nAxisErrorID:DWORD;
	nActualPosition:LREAL;
	nActualVelocity:LREAL;
	nActualTorque:LREAL;
	
	bInterferenceError:BOOL;
	bTorqueLimitError:BOOL;
	bPositiveLimitError:BOOL;
	bNegativeLimitError:BOOL;

	bPowerState:BOOL;

	bJogDone:BOOL;
	bJogBusy:BOOL;
	bJogError:BOOL;

	bHomeDone:BOOL;
	bHomeBusy:BOOL;
	bHomeError:BOOL;

	bMoveRelativeDone:BOOL;
	bMoveRelativeBusy:BOOL;
	bMoveRelativeError:BOOL;

	bMoveAbsoluteDone:BOOL;
	bMoveAbsoluteBusy:BOOL;
	bMoveAbsoluteError:BOOL;

	bMoveVelocityState:BOOL;
	bMoveVelocityBusy:BOOL;
	bMoveVelocityError:BOOL;

	bPositionCompare:BOOL;
	
	bError:BOOL;
	nErrorCode:INT;
	sErrorComment:WSTRING;

	nControlWord:UINT;//控制字
	nModesOfOperation:SINT;//运行模式
END_STRUCT
END_TYPE

 FB_axis 功能块

FUNCTION_BLOCK FB_axis
VAR_IN_OUT
	AxisRef:AXIS_REF;
END_VAR
VAR_INPUT
	bSystemStop:BOOL;
	bSystemEmergency:BOOL;
	bSystemReset:BOOL;
	sAxisComment:WSTRING;
	nAxisControlMode:INT;
	AxisControl:ST_axis_control;
	AxisParameter:ST_axis_parameter;
	AxisSignal:ST_axis_signal;
	AxisOther:ST_axis_other;
END_VAR
VAR_OUTPUT
	AxisStatus:ST_axis_status;
END_VAR
VAR
	control:ST_axis_control_manu_auto;
	parameter:ST_axis_parameter_manu_auto;
	
	nLastMode:INT;
	bModeChange:BOOL;

	i:INT;
	bMoveAllow:BOOL;
	bResetAllow:BOOL;
	bPowerAllow:BOOL;
	bHomeAllow:BOOL;
	bJogForwardAllow:BOOL;
	bJogBackwardAllow:BOOL;
	bMoveRelativeAllow:BOOL;
	bMoveAbsoluteAllow:BOOL;
	bMoveVelocityAllow:BOOL;
	sComment:WSTRING:="";

	Power:MC_Power;
	Stop:MC_Stop;
	Halt:MC_Halt;
	Reset:MC_Reset;
	Home:MC_Home;
	Jog:MC_Jog;
	MoveRelative:MC_MoveRelative;
	MoveAbsolute:MC_MoveAbsolute;
	MoveVelocity:MC_MoveVelocity;
	ReadActualPosition:MC_ReadActualPosition;
	ReadActualVelocity:MC_ReadActualVelocity;
	ReadAxisError:MC_ReadAxisError;

	AxisHome:FB_axis_home;
END_VAR
//模式改变
bModeChange:=FALSE;
IF nAxisControlMode<>nLastMode THEN
	nLastMode:=nAxisControlMode;
	bModeChange:=TRUE;
END_IF

//手模式控制、自动模式控制
IF nAxisControlMode>ENUM_mode.nInManual THEN
	control:=AxisControl.automatic;
	parameter:=AxisParameter.automatic;
ELSIF nAxisControlMode=ENUM_mode.nInManual THEN
	control:=AxisControl.manual;
	parameter:=AxisParameter.manual;
END_IF

//状态切换
IF bModeChange THEN
	control.bJogBackward:=FALSE;
	control.bJogForward:=FALSE;
	control.bHome:=FALSE;
	control.bMoveRelative:=FALSE;
	control.bMoveAbsolute:=FALSE;
	control.bMoveVelocity:=FALSE;
	control.bHalt:=TRUE;
END_IF

//干涉
AxisStatus.bInterferenceError:=FALSE;
FOR i:=1 TO 4 BY 1 DO
	IF AxisSignal.bInterference[i] THEN
		AxisStatus.bInterferenceError:=TRUE;
	END_IF
END_FOR

//正负软限位
AxisStatus.bPositiveLimitError:=FALSE;
AxisStatus.bNegativeLimitError:=FALSE;
IF AxisOther.bPositionLimit THEN
	IF (AxisStatus.nActualPosition>=AxisOther.nPositiveLimit AND control.bJogForward)
	OR (parameter.nPosition>=AxisOther.nPositiveLimit AND control.bMoveAbsolute)
	OR ((parameter.nDistance+AxisStatus.nActualPosition)>=AxisOther.nPositiveLimit
	AND (control.bMoveRelative AND NOT AxisStatus.bMoveRelativeBusy)) THEN
		AxisStatus.bPositiveLimitError:=TRUE;
	END_IF

	IF (AxisStatus.nActualPosition<=AxisOther.nNegativeLimit AND control.bJogBackward)
	OR (parameter.nPosition<=AxisOther.nNegativeLimit AND control.bMoveAbsolute)
	OR ((parameter.nDistance+AxisStatus.nActualPosition)<=AxisOther.nNegativeLimit
	AND (control.bMoveRelative AND NOT AxisStatus.bMoveRelativeBusy)) THEN
		AxisStatus.bNegativeLimitError:=TRUE;
	END_IF
END_IF

//扭矩软限位
AxisStatus.bTorqueLimitError:=FALSE;
IF AxisOther.bTorqueLimit THEN
	IF AxisStatus.nActualTorque>=AxisOther.nTorqueLimit THEN
		AxisStatus.bTorqueLimitError:=TRUE;
	END_IF
END_IF

//控制允许
bMoveAllow:=TRUE;
IF AxisStatus.bInterferenceError OR AxisStatus.bTorqueLimitError
OR AxisStatus.bPositiveLimitError OR AxisStatus.bNegativeLimitError
OR AxisStatus.nAxisErrorID>0 OR AxisStatus.bHomeError OR AxisStatus.bJogError
OR AxisStatus.bMoveRelativeError OR AxisStatus.bMoveAbsoluteError OR AxisStatus.bMoveVelocityError
OR control.bHalt OR control.bStop OR bSystemStop OR bSystemEmergency THEN
	bMoveAllow:=FALSE;
END_IF

//复位
bResetAllow:=control.bReset OR bSystemReset;

//使能
IF AxisStatus.nAxisErrorID>0 OR NOT control.bPower OR AxisStatus.bHomeBusy OR bSystemEmergency THEN
	bPowerAllow:=FALSE;
ELSE
	bPowerAllow:=TRUE;
END_IF

//回原点
IF bMoveAllow AND NOT AxisStatus.bJogBusy 
AND NOT AxisStatus.bMoveRelativeBusy AND NOT AxisStatus.bMoveAbsoluteBusy AND NOT AxisStatus.bMoveVelocityBusy
AND control.bHome THEN
	bHomeAllow:=TRUE;
ELSE
	bHomeAllow:=FALSE;
END_IF

//正向点动
IF bMoveAllow AND NOT AxisStatus.bHomeBusy  
AND NOT AxisStatus.bMoveRelativeBusy AND NOT AxisStatus.bMoveAbsoluteBusy AND NOT AxisStatus.bMoveVelocityBusy
AND NOT AxisSignal.bPositiveSensor AND control.bJogForward THEN
	bJogForwardAllow:=TRUE;
ELSE
	bJogForwardAllow:=FALSE;
END_IF

//负向点动
IF bMoveAllow AND NOT AxisStatus.bHomeBusy  
AND NOT AxisStatus.bMoveRelativeBusy AND NOT AxisStatus.bMoveAbsoluteBusy AND NOT AxisStatus.bMoveVelocityBusy
AND NOT AxisSignal.bNegativeSensor AND control.bJogBackward THEN
	bJogBackwardAllow:=TRUE;
ELSE
	bJogBackwardAllow:=FALSE;	
END_IF

//相对运动
IF bMoveAllow AND NOT AxisStatus.bHomeBusy AND NOT AxisStatus.bJogBusy
AND NOT AxisStatus.bMoveAbsoluteBusy AND NOT AxisStatus.bMoveVelocityBusy
AND NOT AxisSignal.bPositiveSensor AND NOT AxisSignal.bNegativeSensor 
AND control.bMoveRelative THEN
	bMoveRelativeAllow:=TRUE;	
ELSE
	bMoveRelativeAllow:=FALSE;
END_IF

//绝对运动
IF bMoveAllow AND NOT AxisStatus.bHomeBusy AND NOT AxisStatus.bJogBusy
AND NOT AxisStatus.bMoveRelativeBusy AND NOT AxisStatus.bMoveVelocityBusy
AND NOT AxisSignal.bPositiveSensor AND NOT AxisSignal.bNegativeSensor
AND control.bMoveAbsolute THEN
	bMoveAbsoluteAllow:=TRUE;
ELSE
	bMoveAbsoluteAllow:=FALSE;
END_IF

//速度运动
IF bMoveAllow AND NOT AxisStatus.bHomeBusy AND NOT AxisStatus.bJogBusy
AND NOT AxisStatus.bMoveRelativeBusy AND NOT AxisStatus.bMoveAbsoluteBusy 
AND NOT AxisSignal.bPositiveSensor AND NOT AxisSignal.bNegativeSensor
AND control.bMoveVelocity THEN
	bMoveVelocityAllow:=TRUE;
END_IF
IF NOT bMoveAllow THEN
	bMoveVelocityAllow:=FALSE;
END_IF

//加速度//加加速度
IF parameter.nAccelerationTime<>0 THEN
	IF parameter.nVelocity<>0 THEN 
		parameter.nAcceleration:=parameter.nVelocity*2/parameter.nAccelerationTime;
		parameter.nJerk:=parameter.nAcceleration*2/parameter.nAccelerationTime;
	END_IF
	
	IF parameter.nJogVelocity<>0 THEN
		parameter.nJogAcceleration:=parameter.nJogVelocity*2/parameter.nAccelerationTime;
		parameter.nJogJerk:=parameter.nJogAcceleration*2/parameter.nAccelerationTime;
	END_IF
END_IF

//减速度
IF parameter.nDecelerationTime<>0 THEN
	IF parameter.nVelocity<>0 THEN 
		parameter.nDeceleration:=parameter.nVelocity*2/parameter.nDecelerationTime;
	END_IF
	
	IF parameter.nJogVelocity<>0 THEN
		parameter.nJogDeceleration:=parameter.nJogVelocity*2/parameter.nDecelerationTime;
	END_IF
END_IF

//位置比较
IF ABS(AxisStatus.nActualPosition-parameter.nPosition)<AxisOther.nPositionCompareMax THEN 
	AxisStatus.bPositionCompare:=TRUE;
ELSE
   	AxisStatus.bPositionCompare:=FALSE;
END_IF

//轴功能块
ReadActualPosition(
	Axis:=AxisRef , 
	Enable:=TRUE , 
	Valid=> , 
	Busy=> , 
	Error=> , 
	ErrorID=> , 
	Position=>AxisStatus.nActualPosition );
	
ReadActualVelocity(
	Axis:=AxisRef , 
	Enable:=TRUE , 
	Valid=> , 
	Busy=> , 
	Error=> , 
	ErrorID=> , 
	ActualVelocity=>AxisStatus.nActualVelocity );	

ReadAxisError(
	Axis:=AxisRef , 
	Enable:=TRUE , 
	Valid=> , 
	Busy=> , 
	Error=> , 
	ErrorID=> , 
	AxisErrorID=>AxisStatus.nAxisErrorID );
	
Power(
	Axis:=AxisRef , 
	Enable:=bPowerAllow , 
	Enable_Positive:=TRUE , 
	Enable_Negative:=TRUE , 
	Override:=parameter.nOverride , 
	BufferMode:=MC_Aborting , 
	Options:= , 
	Status=>AxisStatus.bPowerState , 
	Busy=> , 
	Active=> , 
	Error=> , 
	ErrorID=> );
	
Stop(
	Axis:=AxisRef , 
	Execute:=NOT bMoveAllow AND NOT control.bHalt , 
	Deceleration:= , 
	Jerk:= , 
	Options:= , 
	Done=> , 
	Busy=> , 
	Active=> , 
	CommandAborted=> , 
	Error=> , 
	ErrorID=> );

Halt(
	Axis:=AxisRef , 
	Execute:=control.bHalt , 
	Deceleration:= , 
	Jerk:= , 
	BufferMode:= , 
	Options:= , 
	Done=> , 
	Busy=> , 
	Active=> , 
	CommandAborted=> , 
	Error=> , 
	ErrorID=> );
	
Reset(
	Axis:=AxisRef , 
	Execute:=bResetAllow , 
	Done=> , 
	Busy=> , 
	Error=> , 
	ErrorID=> );
	
Jog(
	Axis:=AxisRef , 
	JogForward:=bJogForwardAllow , 
	JogBackwards:=bJogBackwardAllow , 
	Mode:=MC_JOGMODE_CONTINOUS , 
	Position:= , 
	Velocity:=parameter.nJogVelocity , 
	Acceleration:=parameter.nJogAcceleration , 
	Deceleration:=parameter.nJogDeceleration , 
	Jerk:=parameter.nJogJerk , 
	Done=>AxisStatus.bJogDone , 
	Busy=>AxisStatus.bJogBusy , 
	Active=> , 
	CommandAborted=> , 
	Error=>AxisStatus.bJogError , 
	ErrorID=> );
(*
Home(
	Axis:=AxisRef , 
	Execute:=bHomeAllow , 
	Position:=0 , 
	HomingMode:= , 
	BufferMode:= , 
	Options:= , 
	bCalibrationCam:=AxisSignal.bHomeSensor , 
	Done=>AxisStatus.bHomeDone , 
	Busy=>AxisStatus.bHomeBusy , 
	Active=> , 
	CommandAborted=> , 
	Error=>AxisStatus.bHomeError , 
	ErrorID=> );
*)	
MoveRelative(
	Axis:=AxisRef , 
	Execute:=bMoveRelativeAllow , 
	Distance:=parameter.nDistance , 
	Velocity:=parameter.nVelocity , 
	Acceleration:=parameter.nAcceleration , 
	Deceleration:=parameter.nDeceleration , 
	Jerk:=parameter.nJerk , 
	BufferMode:=MC_Aborting , 
	Options:= , 
	Done=>AxisStatus.bMoveRelativeDone , 
	Busy=>AxisStatus.bMoveRelativeBusy , 
	Active=> , 
	CommandAborted=> , 
	Error=>AxisStatus.bMoveRelativeError , 
	ErrorID=> );
	
MoveAbsolute(
	Axis:=AxisRef , 
	Execute:=bMoveAbsoluteAllow , 
	Position:=parameter.nPosition , 
	Velocity:=parameter.nVelocity , 
	Acceleration:=parameter.nAcceleration , 
	Deceleration:=parameter.nDeceleration , 
	Jerk:=parameter.nJerk , 
	BufferMode:=MC_Aborting , 
	Options:= , 
	Done=>AxisStatus.bMoveAbsoluteDone , 
	Busy=>AxisStatus.bMoveAbsoluteBusy , 
	Active=> , 
	CommandAborted=> , 
	Error=>AxisStatus.bMoveAbsoluteError , 
	ErrorID=> );

MoveVelocity(
	Axis:=AxisRef , 
	Execute:=bMoveVelocityAllow , 
	Velocity:=parameter.nVelocity , 
	Acceleration:=parameter.nAcceleration , 
	Deceleration:=parameter.nDeceleration , 
	Jerk:=parameter.nJerk , 
	Direction:=parameter.nDirection , 
	BufferMode:=MC_Aborting , 
	Options:= , 
	InVelocity=>AxisStatus.bMoveVelocityState , 
	Busy=>AxisStatus.bMoveVelocityBusy , 
	Active=> , 
	CommandAborted=> , 
	Error=>AxisStatus.bMoveVelocityError , 
	ErrorID=> );

AxisHome(
	Axis:=AxisRef , 
	sAmsNetId:=AxisOther.sAmsNetId ,
	nPort:=AxisOther.nPort ,
	nHomeMethod:=parameter.nHomeMethod ,
	bHome:=bHomeAllow , 
	bReset:=bResetAllow ,
	bBusy=>AxisStatus.bHomeBusy , 
	bDone=>AxisStatus.bHomeDone , 
	bError=>AxisStatus.bHomeError ,
	nErrorId=> ,
	nWriteModesOfOperation=>AxisStatus.nModesOfOperation ,
	nWriteControlWord=>AxisStatus.nControlWord );
	
//错误码
IF sComment="" THEN
	sComment:=WCONCAT("轴:",sAxisComment);
END_IF

IF AxisStatus.bInterferenceError THEN
	AxisStatus.bError:=TRUE;
	AxisStatus.nErrorCode:=indexAxisErrorInterferenceError;
	AxisStatus.sErrorComment:=WCONCAT(sComment,"--干涉报警");
ELSIF AxisStatus.bPositiveLimitError THEN
	AxisStatus.bError:=TRUE;
	AxisStatus.nErrorCode:=indexAxisErrorPositiveLimitError;
	AxisStatus.sErrorComment:=WCONCAT(sComment,"--正极限值报警");
ELSIF AxisStatus.bNegativeLimitError THEN
	AxisStatus.bError:=TRUE;
	AxisStatus.nErrorCode:=indexAxisErrorNegativeLimitError;
	AxisStatus.sErrorComment:=WCONCAT(sComment,"--负极限值报警");
ELSIF AxisStatus.bTorqueLimitError THEN
	AxisStatus.bError:=TRUE;
	AxisStatus.nErrorCode:=indexAxisErrorTorqueLimitError;
	AxisStatus.sErrorComment:=WCONCAT(sComment,"--扭矩限值报警");
ELSIF AxisStatus.nAxisErrorID>0 THEN
	AxisStatus.bError:=TRUE;
	AxisStatus.nErrorCode:=indexAxisErrorAxisError;
	AxisStatus.sErrorComment:=WCONCAT(sComment,"--NC轴报警");
ELSIF AxisStatus.bHomeError THEN
	AxisStatus.bError:=TRUE;
	AxisStatus.nErrorCode:=indexAxisErrorHomeError;
	AxisStatus.sErrorComment:=WCONCAT(sComment,"--回原点报警");
ELSIF AxisStatus.bJogError THEN
	AxisStatus.bError:=TRUE;
	AxisStatus.nErrorCode:=indexAxisErrorJogError;
	AxisStatus.sErrorComment:=WCONCAT(sComment,"--点动报警");
ELSIF AxisStatus.bMoveRelativeError THEN
	AxisStatus.bError:=TRUE;
	AxisStatus.nErrorCode:=indexAxisErrorMoveRelativeError;
	AxisStatus.sErrorComment:=WCONCAT(sComment,"--相对运动报警");
ELSIF AxisStatus.bMoveAbsoluteError THEN
	AxisStatus.bError:=TRUE;
	AxisStatus.nErrorCode:=indexAxisErrorMoveAbsoluteError;
	AxisStatus.sErrorComment:=WCONCAT(sComment,"--绝对运动报警");
ELSIF AxisStatus.bMoveVelocityError THEN
	AxisStatus.bError:=TRUE;
	AxisStatus.nErrorCode:=indexAxisErrorMoveVelocityError;
	AxisStatus.sErrorComment:=WCONCAT(sComment,"--速度运动报警");
ELSE
	AxisStatus.bError:=FALSE;
END_IF

 FB_axis_home 功能块

FUNCTION_BLOCK FB_axis_home
VAR_IN_OUT
	Axis:AXIS_REF;
END_VAR
VAR_INPUT
	sAmsNetId:T_AmsNetID;
	nPort:WORD;
	nHomeMethod:UDINT;
	bHome:BOOL;
	bReset:BOOL;
END_VAR
VAR_OUTPUT
	bBusy:BOOL;
	bDone:BOOL;
	bError:BOOL;
	nErrorId:INT;
	nWriteModesOfOperation:SINT;
	nWriteControlWord:UINT;
END_VAR
VAR
	nHomStep:INT;
	nHomeLastStep:INT;

	i:INT;
	tonDelayHigh:TON;
	tonDelayLow:TON;
	tonDelayStep:TON;
	tonDelayAlarm:TON;

	nWriteHomeAcceleration:UDINT;
	nWriteHomeVelocityHigh:UDINT;
	nWriteHomeVelocityLow:UDINT;
	nWriteHomingMethod:UDINT;
	//nWriteModesOfOperation:SINT;
	//nWriteControlWord:UINT;

	nReadHomeAcceleration:UDINT;
	nReadHomeVelocityHigh:UDINT;
	nReadHomeVelocityLow:UDINT;
	nReadHomingMethod:UDINT;
	nReadModesOfOperation:SINT;
	nReadStatusWord:UINT;

	FB_EcCoESdoWrite_6040:FB_EcCoESdoWrite;
	FB_EcCoESdoRead_6041:FB_EcCoESdoRead;
	FB_EcCoESdoWrite_6060:FB_EcCoESdoWrite;	
	FB_EcCoESdoRead_6061:FB_EcCoESdoRead;
	FB_EcCoESdoWrite_609A:FB_EcCoESdoWrite;
	FB_EcCoESdoRead_609A:FB_EcCoESdoRead;
	FB_EcCoESdoWrite_6099_1:FB_EcCoESdoWrite;
	FB_EcCoESdoRead_6099_1:FB_EcCoESdoRead;
	FB_EcCoESdoWrite_6099_2:FB_EcCoESdoWrite;
	FB_EcCoESdoRead_6099_2:FB_EcCoESdoRead;
	FB_EcCoESdoWrite_6098:FB_EcCoESdoWrite;
	FB_EcCoESdoRead_6098:FB_EcCoESdoRead;

	Reset:MC_Reset;
END_VAR
//复位报警
IF bReset THEN
	bError:=FALSE;
	nErrorId:=0;
END_IF

//步
CASE nHomStep OF
	0://复位动作
		bBusy:=FALSE;
		bDone:=FALSE;
		Reset.Execute:=FALSE;
		IF bHome AND NOT bError THEN
			bBusy:=TRUE;
			nHomStep:=10;
		END_IF

	10://调用复位功能块
		Reset.Execute:=TRUE;
		IF Reset.Done THEN
			Reset.Execute:=FALSE;
			nHomStep:=20;
		ELSIF Reset.Error THEN
			Reset.Execute:=FALSE;
			nHomStep:=1000;
		END_IF

	20://写入伺服，回零加速度，回零高速，回零低速，回零方式
		nWriteHomeAcceleration:=3276800;
		nWriteHomeVelocityHigh:=32768;
		nWriteHomeVelocityLow:=5678;
		nWriteHomingMethod:=nHomeMethod;
		//读取伺服，判断参数是否写入
		IF nWriteHomeAcceleration=nReadHomeAcceleration
		AND nWriteHomeVelocityHigh=nReadHomeVelocityHigh AND nWriteHomeVelocityLow=nReadHomeVelocityLow
		AND nWriteHomingMethod=nReadHomingMethod AND tonDelayStep.Q THEN
			nHomStep:=30;
		END_IF

		//延时
		tonDelayStep(IN:=TRUE , PT:=T#1S , Q=> , ET=> );

		//延时报警
		tonDelayAlarm(IN:=TRUE , PT:=T#5S , Q=> , ET=> );
		IF tonDelayAlarm.Q THEN
			nHomStep:=1000;
		END_IF

	30://写入伺服，回零模式
		nWriteModesOfOperation:=6;
		//读取伺服，判断参数是否写入
		IF nWriteModesOfOperation=nReadModesOfOperation THEN
			nHomStep:=50;
		END_IF

		//延时报警
		tonDelayAlarm(IN:=TRUE , PT:=T#5S , Q=> , ET=> );
		IF tonDelayAlarm.Q THEN
			nHomStep:=1000;
		END_IF

	50://写入伺服，接通主回路电
		nWriteControlWord:=2#110;

		//延时
		tonDelayStep(IN:=TRUE , PT:=T#1S , Q=> , ET=> );
		IF tonDelayStep.Q THEN
			nHomStep:=60;
		END_IF

	60://写入伺服，伺服准备好
		nWriteControlWord:=2#111;

		//延时
		tonDelayStep(IN:=TRUE , PT:=T#1S , Q=> , ET=> );
		IF tonDelayStep.Q THEN
			nHomStep:=70;
		END_IF

	70://写入伺服，伺服运行
		nWriteControlWord:=2#1111;

		//延时
		tonDelayStep(IN:=TRUE , PT:=T#1S , Q=> , ET=> );
		IF tonDelayStep.Q THEN
			nHomStep:=80;
		END_IF

	80://写入伺服，启动回零
		nWriteControlWord:=2#11111;

		//延时
		tonDelayStep(IN:=TRUE , PT:=T#1S , Q=> , ET=> );
		IF tonDelayStep.Q THEN
			nHomStep:=90;
		END_IF

	90://读取伺服，判断回零是否完成
		IF nReadStatusWord.10 AND nReadStatusWord.12 AND NOT nReadStatusWord.13 THEN
			nHomStep:=100;
		END_IF

		//延时报警
		tonDelayAlarm(IN:=TRUE , PT:=T#120S , Q=> , ET=> );
		IF tonDelayAlarm.Q THEN
			nHomStep:=1000;
		END_IF

	100://调用复位功能块
		Reset.Execute:=TRUE;
		IF Reset.Done THEN
			Reset.Execute:=FALSE;
			nHomStep:=110;
		ELSIF Reset.Error THEN
			Reset.Execute:=FALSE;
			nHomStep:=1000;
		END_IF

	110://写入伺服，循环同步位置模式
		nWriteModesOfOperation:=8;
		//读取伺服，判断模式是否写入
		IF nWriteModesOfOperation=nReadModesOfOperation THEN
			bBusy:=FALSE;
			bDone:=TRUE;
			nHomStep:=0;
		END_IF


	1000://故障，写入伺服，循环同步位置模式
		nWriteModesOfOperation:=8;
		//读取伺服，判断模式是否写入
		IF nWriteModesOfOperation=nReadModesOfOperation THEN
			bBusy:=FALSE;
			bError:=TRUE;
			nHomStep:=0;
		END_IF
END_CASE

//复位定时器
IF nHomeLastStep<>nHomStep THEN
	nHomeLastStep:=nHomStep;
	tonDelayStep(IN:=FALSE , PT:= , Q=> , ET=> );
	tonDelayAlarm(IN:=FALSE , PT:= , Q=> , ET=> );
END_IF

//高低脉冲信号(脉冲触发读写参数)
tonDelayHigh(IN:=bBusy AND NOT tonDelayLow.Q , PT:=T#10MS , Q=> , ET=> );
tonDelayLow(IN:=tonDelayHigh.Q , PT:=T#10MS , Q=> , ET=> );

//写入回零加速度
FB_EcCoESdoWrite_609A(
	sNetId:=sAmsNetId , 
	nSlaveAddr:=nPort , 
	nSubIndex:=16#00 , 
	nIndex:=16#609A , 
	pSrcBuf:=ADR(nWriteHomeAcceleration) , 
	cbBufLen:=SIZEOF(nWriteHomeAcceleration) , 
	bExecute:=tonDelayHigh.Q AND (nWriteHomeAcceleration<>nReadHomeAcceleration) , 
	tTimeout:= , 
	bBusy=> , 
	bError=> , 
	nErrId=> );

//读取回零加速度
FB_EcCoESdoRead_609A(
	sNetId:=sAmsNetId , 
	nSlaveAddr:=nPort , 
	nSubIndex:=16#00 , 
	nIndex:=16#609A , 
	pDstBuf:=ADR(nReadHomeAcceleration) , 
	cbBufLen:=SIZEOF(nReadHomeAcceleration) , 
	bExecute:=tonDelayHigh.Q , 
	tTimeout:= , 
	bBusy=> , 
	bError=> , 
	nErrId=> );

//写入回零高速
FB_EcCoESdoWrite_6099_1(
	sNetId:=sAmsNetId , 
	nSlaveAddr:=nPort , 
	nSubIndex:=16#01 , 
	nIndex:=16#6099 , 
	pSrcBuf:=ADR(nWriteHomeVelocityHigh) , 
	cbBufLen:=SIZEOF(nWriteHomeVelocityHigh) , 
	bExecute:=tonDelayHigh.Q AND (nWriteHomeVelocityHigh<>nReadHomeVelocityHigh) , 
	tTimeout:= , 
	bBusy=> , 
	bError=> , 
	nErrId=> );

//读取回零高速
FB_EcCoESdoRead_6099_1(
	sNetId:=sAmsNetId , 
	nSlaveAddr:=nPort , 
	nSubIndex:=16#01 , 
	nIndex:=16#6099 , 
	pDstBuf:=ADR(nReadHomeVelocityHigh) , 
	cbBufLen:=SIZEOF(nReadHomeVelocityHigh) , 
	bExecute:=tonDelayHigh.Q , 
	tTimeout:= , 
	bBusy=> , 
	bError=> , 
	nErrId=> );

//写入回零低速
FB_EcCoESdoWrite_6099_2(
	sNetId:=sAmsNetId , 
	nSlaveAddr:=nPort , 
	nSubIndex:=16#02 , 
	nIndex:=16#6099 , 
	pSrcBuf:=ADR(nWriteHomeVelocityLow) , 
	cbBufLen:=SIZEOF(nWriteHomeVelocityLow) , 
	bExecute:=tonDelayHigh.Q AND (nWriteHomeVelocityLow<>nReadHomeVelocityLow) , 
	tTimeout:= , 
	bBusy=> , 
	bError=> , 
	nErrId=> );

//读取回零低速
FB_EcCoESdoRead_6099_2(
	sNetId:=sAmsNetId , 
	nSlaveAddr:=nPort , 
	nSubIndex:=16#02 , 
	nIndex:=16#6099 , 
	pDstBuf:=ADR(nReadHomeVelocityLow) , 
	cbBufLen:=SIZEOF(nReadHomeVelocityLow) , 
	bExecute:=tonDelayHigh.Q , 
	tTimeout:= , 
	bBusy=> , 
	bError=> , 
	nErrId=> );

//写入回零方式
FB_EcCoESdoWrite_6098(
	sNetId:=sAmsNetId , 
	nSlaveAddr:=nPort , 
	nSubIndex:=16#00 , 
	nIndex:=16#6098 , 
	pSrcBuf:=ADR(nWriteHomingMethod) , 
	cbBufLen:=1 , 
	bExecute:=tonDelayHigh.Q AND (nWriteHomingMethod<>nReadHomingMethod) , 
	tTimeout:= , 
	bBusy=> , 
	bError=> , 
	nErrId=> );

//读取回零方式
FB_EcCoESdoRead_6098(
	sNetId:=sAmsNetId , 
	nSlaveAddr:=nPort , 
	nSubIndex:=16#00 , 
	nIndex:=16#6098 , 
	pDstBuf:=ADR(nReadHomingMethod) , 
	cbBufLen:=1 , 
	bExecute:=tonDelayHigh.Q , 
	tTimeout:= , 
	bBusy=> , 
	bError=> , 
	nErrId=> );
(*
//写入控制字
FB_EcCoESdoWrite_6040(
	sNetId:=sAmsNetId , 
	nSlaveAddr:=nPort , 
	nSubIndex:=16#00 , 
	nIndex:=16#6040 , 
	pSrcBuf:=ADR(nWriteControlWord) , 
	cbBufLen:=SIZEOF(nWriteControlWord) , 
	bExecute:=tonDelayHigh.Q , 
	tTimeout:= , 
	bBusy=> , 
	bError=> , 
	nErrId=> );
*)
//读取状态字
FB_EcCoESdoRead_6041(
	sNetId:=sAmsNetId , 
	nSlaveAddr:=nPort , 
	nSubIndex:=16#00 , 
	nIndex:=16#6041 , 
	pDstBuf:=ADR(nReadStatusWord) , 
	cbBufLen:=SIZEOF(nReadStatusWord) , 
	bExecute:=tonDelayHigh.Q , 
	tTimeout:= , 
	bBusy=> , 
	bError=> , 
	nErrId=> );
(*
//写入运行模式
FB_EcCoESdoWrite_6060(
	sNetId:=sAmsNetId , 
	nSlaveAddr:=nPort , 
	nSubIndex:=16#00 , 
	nIndex:=16#6060 , 
	pSrcBuf:=ADR(nWriteModesOfOperation) , 
	cbBufLen:=SIZEOF(nWriteModesOfOperation) , 
	bExecute:=tonDelayHigh.Q AND (nWriteModesOfOperation<>nReadModesOfOperation) , 
	tTimeout:= , 
	bBusy=> , 
	bError=> , 
	nErrId=> );
*)
//读取运行模式
FB_EcCoESdoRead_6061(
	sNetId:=sAmsNetId , 
	nSlaveAddr:=nPort , 
	nSubIndex:=16#00 , 
	nIndex:=16#6061 , 
	pDstBuf:=ADR(nReadModesOfOperation) , 
	cbBufLen:=SIZEOF(nReadModesOfOperation) , 
	bExecute:=tonDelayHigh.Q , 
	tTimeout:= , 
	bBusy=> , 
	bError=> , 
	nErrId=> );

//复位
Reset(
	Axis:=Axis , 
	Execute:= , 
	Done=> , 
	Busy=> , 
	Error=> , 
	ErrorID=> );

 GVL_axis 

VAR_GLOBAL
	arAmsNetId AT %I* :AMSNETID;//通讯状态
	sAmsNetId:T_AmsNetID;

	axis:ARRAY[0..indexAxisEnd-1] OF ST_axis;
	
	axisManualId:INT;
END_VAR

 P_axis 

//arAmsNetId转换sAmsNetId
sAmsNetId:='';
sAmsNetId:=BYTE_TO_STRING(arAmsNetId[0]);
FOR i:=1 TO 5 BY 1 DO
	sAmsNetId:=CONCAT(sAmsNetId,'.');
	sAmsNetId:=CONCAT(sAmsNetId,BYTE_TO_STRING(arAmsNetId[i]));
END_FOR

//描述信息
ACT_comment();

//干涉（若有则添加）

//轴功能块控制
FOR i:=1 TO indexAxisEnd-1 BY 1 DO
	axis[i].other.sAmsNetId:=sAmsNetId;
	axis[i].other.nPort:=axis[i].com.nPort;
	axis[i].signal.bNegativeSensor:=axis[i].com.nDigitalInputs.0;
	axis[i].signal.bPositiveSensor:=axis[i].com.nDigitalInputs.1;
	axis[i].signal.bHomeSensor:=axis[i].com.nDigitalInputs.2;
	axis[i].status.nActualTorque:=INT_TO_LREAL(axis[i].com.nTorque);
	axis[i].FB(
		AxisRef:=axis[i].REF , 
		bSystemStop:=system.control.bStop ,
		bSystemEmergency:=system.control.bEmergency ,
		bSystemReset:=system.control.bReset ,
		sAxisComment:=axis[i].sComment , 
		nAxisControlMode:=axis[i].nControlMode , 
		AxisControl:=axis[i].control , 
		AxisParameter:=axis[i].parameter , 
		AxisSignal:=axis[i].signal , 
		AxisOther:=axis[i].other , 
		AxisStatus=>axis[i].status );
	IF axis[i].status.bHomeBusy THEN
		axis[i].com.nModesOfOperation:=axis[i].status.nModesOfOperation;
		axis[i].com.nControlWord:=axis[i].status.nControlWord;
	ELSE
		axis[i].com.nModesOfOperation:=USINT_TO_SINT(axis[i].com.nCtrl5);
		axis[i].com.nControlWord:=USINT_TO_UINT(axis[i].com.nCtrl1)
									+SHL(USINT_TO_UINT(axis[i].com.nCtrl2),8);
	END_IF
END_FOR 
axis[0].sComment:="0：未选中轴";

axis[indexAxisExample1].sComment:=WCONCAT(INT_TO_WSTRING(indexAxisExample1),"：示例轴");
axis[indexAxisExample2].sComment:=WCONCAT(INT_TO_WSTRING(indexAxisExample2),"：示例2轴");
axis[indexAxisExample3].sComment:=WCONCAT(INT_TO_WSTRING(indexAxisExample3),"：示例3轴");

TwinCAT3添加PLC轴，并建立PLC轴与NC轴的链接
qq_30150579的博客
10-20 704
TwinCAT3添加PLC轴，并建立PLC轴与NC轴的链接
TwinCAT3添加NC轴
qq_30150579的博客
10-19 586
TwinCAT3添加NC轴
Ethercat总线初次接触TWINCAT3主站抓包被坑记录
最新发布
河北正定欢迎您-少占鱼
02-10 967
第一次，我安装TWINCAT3软件，没有伺服，直接扫描，也抓不到包。关键群友发了俩版本我安装上老是卡，花屏。到晚上突然说到网卡安装，我才想起，在公司安装过。又重新扫描设备，后来就有了。3、打开TC3软件，忘了点显示网卡，安装协议。第一次安装TC3软件，网卡没有相关协议，点安装后，就会显示到上面。1、安装TWINCAT3软件，默认安装就行。安装Wireshark抓包软件，也是默认就行。4、扫描到从站了，就是成功了99%。3、扫描从站之前，记得把从站的xml文件复制到TC3软件的IO设备目录下。
TwinCAT3例子程序V3.2.zip
09-27
TwinCAT3使用的例子程序
TwinCAT3编写程序对轴进行控制
qq_30150579的博客
10-21 529
TwinCAT3编写程序对轴进行控制
TWINCAT3项目实例
05-15
BECKHOFF TWINCAT3项目实例，VS2013编写，带FESTO伺服驱动。
TwinCAT3入门测试程序2.rar
12-13
通过这个测试程序，初学者可以深入了解TwinCAT3在I/O管理和伺服控制方面的应用，掌握如何构建一个完整的自动化控制解决方案。同时，实践中的问题解决和调试经验将有助于提升在实际项目中的技能水平。
TwinCAT3_C _Simulink教程V3.1,twincat3仿真运行
09-11
TwinCAT3是由Beckhoff公司开发的一款先进的自动化软件平台，它集成了PLC（可编程逻辑控制器）、NC（数字控制）和IO（输入输出）功能，支持IEC 61131-3标准编程语言，如C++，为工业自动化提供了一站式解决方案。Twin...
GUI_上位机界面_上位机_TwinCAT3_双轴控制_PLC_
10-02
这是一个能够控制两个轴的PLC程序及其上位机界面，通过将它和NC轴关联并将NC轴与物理轴关联，即可实现对两个轴的包括始能，位置显示，单周点动，位置和速度设定，停止，设定原点，寻参和双轴耦合以及解耦的控制功能
TwinCAT3 教程，通过本教程可学习使用倍福软件
01-15
倍福TwinCAT3软件的安装使用教程。包含以下主要内容 一、 TwinCAT3安装 二、 TwinCAT3的授权激活 三、 TwinCAT3扫描IO变量连接 四、 TwinCAT3编程语言的IEC61131-3标准 五、 TwinCAT3 PLC简单程序编写与调试 六、 TwinCAT3 PLC HMI可视化编程 七、 运动控制介绍 八、 TwinCAT-3 Scope View的使用 九、 TwinCAT3库管理 十、 TwinCAT3源代码管理 十一、 TwinCAT2项目到TwinCAT3转换 十二、 TwinSAFE安全模块使用 十三、 OPC UA 使用 十四、 EtherCAT性能介绍及诊断
TWINCAT程序实例
09-06
TWINCAT程序实例 有助于用TWINCAT软件编写PLC程序
TwinCat3入门教程
03-01
本文是倍福控制器的开发软件TwinCat3的入门教材，对于初次使用倍福控制器的技术人员可以有很好的指导作用
TWINCAT3控制电机简单动作操作指南.pdf
02-04
TWINCAT3控制电机简单动作操作指南.pdf
TwinCAT3_运动控制教程V1.11_TWINCAT3入门_TwinCAT3_plc运动控制_
09-28
基于Twincat3平台的PLC编程教程，使用ST语言控制电机运动
TwinCAT3和第三方伺服电机——汇川电机使用
05-13
然后，通过编写TwinCAT3程序，利用TwinCAT3的运动控制功能，如轴管理、位置控制、速度控制和力矩控制，来设定汇川电机的工作模式和参数。 在位置控制模式下，TwinCAT3可以设定精确的脉冲序列，控制汇川电机完成精确...
【TwinCAT3教程】TwinCAT3 PLC 简单程序编写与调试
m0_56451176的博客
08-04 1412
（1）打开 TwinCAT 3，点击新建 TC3 项目。（2）选择，输入项目名称和项目保存路径，然后点击确定。
TWINCAT3
weixin_41011717的博客
01-12 5462
想不到 我这个研究僧在csdn上的第一篇文章…竟然是关于TWinCAT的，， 额，，，因为我们实验室周一要开例会 emmmm 老板这周玩了新花样 让研一的分为机械和电控两个组 我就被分到了电控组（中间经历一些波折就不说了） 但是这还没开始老板的表演呢 老板玩的最花的地方在于 让我们研一的向师兄们拜师 学习他们的经验 完成传承 说白了 就是培养继续供老板使用的工具…Orz 因为我们实验室做项目主要用...
倍福Twincat3运动控制模板详解
运动控制模板是Twincat3中的一个关键组成部分，它提供了一套完整的运动控制解决方案，允许工程师快速开发运动控制应用程序。模板包含了预设的配置、函数块和数据类型，可用来简化运动轴和伺服驱动器的配置与编程。 ...
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值