TwinCAT 3 轴控制程序
ST
TYPE ST_axis :
STRUCT
sComment:WSTRING;
com:ST_axis_com;
REF:AXIS_REF;
automaticControl:ST_axis_control;
automaticParameter:ST_axis_parameter;
manualControl:ST_axis_control;
manualParameter:ST_axis_parameter;
other:ST_axis_other;
signal:ST_axis_signal;
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
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;
bPositiveEnable:BOOL:=TRUE;
bNegativeEnable:BOOL:=TRUE;
nTorque:LREAL;
bTorqueLimit:BOOL;
nTorqueLimit:LREAL;
bPositionLimit:BOOL;
nPositiveLimit:LREAL;
nNegativeLimit:LREAL;
nPositionCompareMax:LREAL:=0.05;
END_STRUCT
END_TYPE
TYPE ST_axis_parameter :
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;
nControlWord:UINT;//控制字
nModesOfOperation:SINT;//运行模式
END_STRUCT
END_TYPEFB_axis 功能块
FUNCTION_BLOCK FB_axis
VAR_IN_OUT
AxisRef:AXIS_REF;
END_VAR
VAR_INPUT
bSystemStop:BOOL;
bSystemEmergency:BOOL;
bSystemReset:BOOL;
bControlMode:BOOL;
AxisAutomaticControl:ST_axis_control;
AxisAutomaticParameter:ST_axis_parameter;
AxisManualControl:ST_axis_control;
AxisManualParameter:ST_axis_parameter;
AxisOther:ST_axis_other;
AxisSignal:ST_axis_signal;
END_VAR
VAR_OUTPUT
AxisStatus:ST_axis_status;
END_VAR
VAR
rTrigControlMode:R_TRIG;
fTrigControlMode:F_TRIG;
AxisControl:ST_axis_control;
AxisParameter:ST_axis_parameter;
i:INT;
bMoveAllow:BOOL;
bResetAllow:BOOL;
bPowerAllow:BOOL;
bHomeAllow:BOOL;
bJogForwardAllow:BOOL;
bJogBackwardAllow:BOOL;
bMoveRelativeAllow:BOOL;
bMoveAbsoluteAllow:BOOL;
bMoveVelocityAllow:BOOL;
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//控制模式上升沿、下降沿
rTrigControlMode(CLK:=bControlMode , Q=> );
fTrigControlMode(CLK:=bControlMode , Q=> );
//手模式控制、自动模式控制
IF bControlMode THEN
AxisControl:=AxisAutomaticControl;
AxisParameter:=AxisAutomaticParameter;
ELSE
AxisControl:=AxisManualControl;
AxisParameter:=AxisManualParameter;
END_IF
//状态切换
IF rTrigControlMode.Q OR fTrigControlMode.Q THEN
AxisControl.bJogBackward:=FALSE;
AxisControl.bJogForward:=FALSE;
AxisControl.bHome:=FALSE;
AxisControl.bMoveRelative:=FALSE;
AxisControl.bMoveAbsolute:=FALSE;
AxisControl.bMoveVelocity:=FALSE;
AxisControl.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 AxisControl.bJogForward)
OR (AxisParameter.nPosition>=AxisOther.nPositiveLimit AND AxisControl.bMoveAbsolute)
OR ((AxisParameter.nDistance+AxisStatus.nActualPosition)>=AxisOther.nPositiveLimit
AND (AxisControl.bMoveRelative AND NOT AxisStatus.bMoveRelativeBusy)) THEN
AxisStatus.bPositiveLimitError:=TRUE;
END_IF
IF (AxisStatus.nActualPosition<=AxisOther.nNegativeLimit AND AxisControl.bJogBackward)
OR (AxisParameter.nPosition<=AxisOther.nNegativeLimit AND AxisControl.bMoveAbsolute)
OR ((AxisParameter.nDistance+AxisStatus.nActualPosition)<=AxisOther.nNegativeLimit
AND (AxisControl.bMoveRelative AND NOT AxisStatus.bMoveRelativeBusy)) THEN
AxisStatus.bNegativeLimitError:=TRUE;
END_IF
END_IF
//扭矩软限位
AxisStatus.bTorqueLimitError:=FALSE;
IF AxisOther.bTorqueLimit THEN
IF AxisOther.nTorque>=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 AxisControl.bHalt OR AxisControl.bStop OR bSystemStop OR bSystemEmergency THEN
bMoveAllow:=FALSE;
END_IF
//复位
bResetAllow:=FALSE;
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 THEN
IF AxisControl.bReset OR bSystemReset THEN
bResetAllow:=TRUE;
END_IF
END_IF
//使能
IF AxisStatus.nAxisErrorID>0 OR NOT AxisControl.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 AxisControl.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 AxisControl.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 AxisControl.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 AxisControl.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 AxisControl.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 AxisControl.bMoveVelocity THEN
bMoveVelocityAllow:=TRUE;
END_IF
IF NOT bMoveAllow THEN
bMoveVelocityAllow:=FALSE;
END_IF
//加速度
AxisParameter.nAcceleration:=AxisParameter.nVelocity*2/AxisParameter.nAccelerationTime;
AxisParameter.nJogAcceleration:=AxisParameter.nJogVelocity*2/AxisParameter.nAccelerationTime;
//减速度
AxisParameter.nDeceleration:=AxisParameter.nVelocity*2/AxisParameter.nDecelerationTime;
AxisParameter.nJogDeceleration:=AxisParameter.nJogVelocity*2/AxisParameter.nDecelerationTime;
//加加速度
AxisParameter.nJerk:=AxisParameter.nAcceleration*2/AxisParameter.nAccelerationTime;
AxisParameter.nJogJerk:=AxisParameter.nJogAcceleration*2/AxisParameter.nAccelerationTime;
//位置比较
IF ABS(AxisStatus.nActualPosition-AxisParameter.nPosition) ,
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:=AxisOther.bPositiveEnable ,
Enable_Negative:=AxisOther.bNegativeEnable ,
Override:=AxisParameter.nOverride ,
BufferMode:=MC_Aborting ,
Options:= ,
Status=>AxisStatus.bPowerState ,
Busy=> ,
Active=> ,
Error=> ,
ErrorID=> );
Stop(
Axis:=AxisRef ,
Execute:=NOT bMoveAllow AND NOT AxisControl.bHalt ,
Deceleration:= ,
Jerk:= ,
Options:= ,
Done=> ,
Busy=> ,
Active=> ,
CommandAborted=> ,
Error=> ,
ErrorID=> );
Halt(
Axis:=AxisRef ,
Execute:=AxisControl.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:=AxisParameter.nJogVelocity ,
Acceleration:=AxisParameter.nJogAcceleration ,
Deceleration:=AxisParameter.nJogDeceleration ,
Jerk:=AxisParameter.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:=AxisParameter.nDistance ,
Velocity:=AxisParameter.nVelocity ,
Acceleration:=AxisParameter.nAcceleration ,
Deceleration:=AxisParameter.nDeceleration ,
Jerk:=AxisParameter.nJerk ,
BufferMode:=MC_Aborting ,
Options:= ,
Done=>AxisStatus.bMoveRelativeDone ,
Busy=>AxisStatus.bMoveRelativeBusy ,
Active=> ,
CommandAborted=> ,
Error=>AxisStatus.bMoveRelativeError ,
ErrorID=> );
MoveAbsolute(
Axis:=AxisRef ,
Execute:=bMoveAbsoluteAllow ,
Position:=AxisParameter.nPosition ,
Velocity:=AxisParameter.nVelocity ,
Acceleration:=AxisParameter.nAcceleration ,
Deceleration:=AxisParameter.nDeceleration ,
Jerk:=AxisParameter.nJerk ,
BufferMode:=MC_Aborting ,
Options:= ,
Done=>AxisStatus.bMoveAbsoluteDone ,
Busy=>AxisStatus.bMoveAbsoluteBusy ,
Active=> ,
CommandAborted=> ,
Error=>AxisStatus.bMoveAbsoluteError ,
ErrorID=> );
MoveVelocity(
Axis:=AxisRef ,
Execute:=bMoveVelocityAllow ,
Velocity:=AxisParameter.nVelocity ,
Acceleration:=AxisParameter.nAcceleration ,
Deceleration:=AxisParameter.nDeceleration ,
Jerk:=AxisParameter.nJerk ,
Direction:=AxisParameter.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:=AxisParameter.nHomeMethod ,
bHome:=bHomeAllow ,
bReset:=bResetAllow ,
bBusy=>AxisStatus.bHomeBusy ,
bDone=>AxisStatus.bHomeDone ,
bError=>AxisStatus.bHomeError ,
nErrorId=> ,
nWriteModesOfOperation=>AxisStatus.nModesOfOperation ,
nWriteControlWord=>AxisStatus.nControlWord ); 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 CONSTANT
//伺服最大编号
nAxisMaxNum:INT:=3;
//伺服索引编号
indexAxisExample:INT:=1;
indexAxisExample2:INT:=2;
indexAxisExample3:INT:=3;
END_VAR
VAR_GLOBAL
arAmsNetId AT %I* :AMSNETID;//通讯状态
sAmsNetId:T_AmsNetID;
axis:ARRAY[0..nAxisMaxNum] OF ST_axis;
axisManualId:INT;
END_VARP_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 nAxisMaxNum BY 1 DO
axis[i].other.sAmsNetId:=sAmsNetId;
axis[i].other.nPort:=axis[i].com.nPort;
axis[i].other.nTorque:=INT_TO_LREAL(axis[i].com.nTorque);
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].FB(
AxisRef:=axis[i].REF ,
bSystemStop:=system.control.bStop ,
bSystemEmergency:=system.control.bEmergency ,
bSystemReset:=system.control.bReset ,
bControlMode:= ,
AxisAutomaticControl:=axis[i].automaticControl ,
AxisAutomaticParameter:=axis[i].automaticParameter ,
AxisManualControl:=axis[i].manualControl ,
AxisManualParameter:=axis[i].manualParameter ,
AxisOther:=axis[i].other ,
AxisSignal:=axis[i].signal ,
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[indexAxisExample].sComment:=WCONCAT(INT_TO_WSTRING(indexAxisExample),"：示例轴");
axis[indexAxisExample2].sComment:=WCONCAT(INT_TO_WSTRING(indexAxisExample2),"：示例2轴");
axis[indexAxisExample3].sComment:=WCONCAT(INT_TO_WSTRING(indexAxisExample3),"：示例3轴");