codesys 控制轴组程序

WX_LW

已于 2024-04-30 13:54:10 修改

阅读量1.1k

点赞数

分类专栏： codesys 文章标签： st

于 2023-10-08 14:29:55 首次发布

本文链接：https://blog.csdn.net/weixin_58619062/article/details/133681162

版权

codesys 专栏收录该内容

12 篇文章 10 订阅

订阅专栏

FUNCTION_BLOCK FB_axisGroup
VAR_IN_OUT
	AxisGroup:AXIS_GROUP_REF_SM3;
END_VAR
VAR_INPUT
	AxisGroupControl:ST_axisGroup_control;
	AxisGroupParameter:ST_axisGroup_parameter;
	AxisGroupOther:ST_axisGroup_other;
END_VAR
VAR_OUTPUT
	AxisGroupStatus:ST_axisGroup_status;
END_VAR
VAR
	bPowerAllow:BOOL;
	bEnableAllow:BOOL;
	bResetAllow:BOOL;
	bMoveAllow:BOOL;
	bJogAllow:BOOL;
	bMoveDirectAbsoluteAllow:BOOL;
	bMoveLinearAbsoluteAllow:BOOL;
	bMoveCircularAbsoluteAllow:BOOL;
	
	CoordSystem:SMC_COORD_SYSTEM;

	SMC_GroupPower:SMC_GroupPower;
	MC_GroupEnable:MC_GroupEnable;
	MC_GroupDisable:MC_GroupDisable;
	MC_GroupReset:MC_GroupReset;
	MC_GroupStop:MC_GroupStop;
	MC_GroupHalt:MC_GroupHalt;
	SMC_GroupJog2:SMC_GroupJog2;
	MC_MoveDirectAbsolute:MC_MoveDirectAbsolute;
	MC_MoveLinearAbsolute:MC_MoveLinearAbsolute;
	MC_MoveCircularAbsolute:MC_MoveCircularAbsolute;
	
	MC_GroupReadStatus:MC_GroupReadStatus;
	MC_GroupReadActualPosition:MC_GroupReadActualPosition;
	MC_GroupReadActualVelocity:MC_GroupReadActualVelocity;
END_VAR

//轴组使能允许
IF AxisGroupControl.bPower
AND NOT AxisGroupStatus.bPowerError THEN
    bPowerAllow:=TRUE;
ELSE
    bPowerAllow:=FALSE;
END_IF

//轴组有效允许
IF AxisGroupControl.bEnable
AND NOT AxisGroupControl.bDisable THEN
    bEnableAllow:=TRUE;
ELSE
    bEnableAllow:=FALSE;
END_IF

//故障
IF AxisGroupControl.bReset THEN
    AxisGroupStatus.bError:=FALSE;
END_IF

IF AxisGroupStatus.bPowerError
OR AxisGroupStatus.bEnableError
OR AxisGroupStatus.bDisableError
OR AxisGroupStatus.bJogError
OR AxisGroupStatus.bMoveDirectAbsoluteError
OR AxisGroupStatus.bMoveLinearAbsoluteError
OR AxisGroupStatus.bMoveCircularAbsoluteError THEN
    AxisGroupStatus.bError:=TRUE;
END_IF

//运动允许
IF AxisGroupStatus.bEnable
AND AxisGroupStatus.bPowerStatus
AND NOT AxisGroupStatus.bError
AND NOT AxisGroupControl.bHalt
AND NOT AxisGroupControl.bStop THEN
    bMoveAllow:=TRUE;
ELSE
    bMoveAllow:=FALSE;
END_IF

//复位允许
IF AxisGroupControl.bReset
AND AxisGroupStatus.bError THEN
    bResetAllow:=TRUE;
ELSE
    bResetAllow:=FALSE;
END_IF

//点动允许
IF bMoveAllow
AND AxisGroupControl.bJog
AND NOT AxisGroupStatus.bMoveDirectAbsoluteBusy
AND NOT AxisGroupStatus.bMoveLinearAbsoluteBusy
AND NOT AxisGroupStatus.bMoveCircularAbsoluteBusy THEN
    bJogAllow:=TRUE;
ELSE
    bJogAllow:=FALSE;
END_IF

//关节绝对运动允许
IF bMoveAllow
AND AxisGroupControl.bMoveDirectAbsolute
AND NOT AxisGroupStatus.bMoveLinearAbsoluteBusy
AND NOT AxisGroupStatus.bMoveCircularAbsoluteBusy
AND NOT AxisGroupStatus.bJogBusy THEN
    bMoveDirectAbsoluteAllow:=TRUE;
ELSE
    bMoveDirectAbsoluteAllow:=FALSE;
END_IF

//直线绝对运动允许
IF bMoveAllow
AND AxisGroupControl.bMoveLinearAbsolute
AND NOT AxisGroupStatus.bMoveDirectAbsoluteBusy
AND NOT AxisGroupStatus.bMoveCircularAbsoluteBusy
AND NOT AxisGroupStatus.bJogBusy THEN
    bMoveLinearAbsoluteAllow:=TRUE;
ELSE
    bMoveLinearAbsoluteAllow:=FALSE;
END_IF

//圆弧绝对运动允许
IF bMoveAllow
AND AxisGroupControl.bMoveCircularAbsolute
AND NOT AxisGroupStatus.bMoveDirectAbsoluteBusy
AND NOT AxisGroupStatus.bMoveLinearAbsoluteBusy
AND NOT AxisGroupStatus.bJogBusy THEN
    bMoveCircularAbsoluteAllow:=TRUE;
ELSE
    bMoveCircularAbsoluteAllow:=FALSE;
END_IF

//加速度
AxisGroupParameter.nJogAcceleration:=AxisGroupParameter.nJogVelocity*2/AxisGroupParameter.nJogAccelerationTime;
AxisGroupParameter.nAcceleration:=AxisGroupParameter.nVelocity*2/AxisGroupParameter.nAccelerationTime;

//减速度
AxisGroupParameter.nJogDeceleration:=AxisGroupParameter.nJogVelocity*2/AxisGroupParameter.nJogDecelerationTime;
AxisGroupParameter.nDeceleration:=AxisGroupParameter.nVelocity*2/AxisGroupParameter.nDecelerationTime;

//加加速度
AxisGroupParameter.nJogJerk:=AxisGroupParameter.nJogAcceleration*2/AxisGroupParameter.nJogAccelerationTime;
AxisGroupParameter.nJerk:=AxisGroupParameter.nAcceleration*2/AxisGroupParameter.nAccelerationTime;

//位置比较
IF ABS(AxisGroupStatus.ActualPosition.c.X-AxisGroupParameter.Position.c.X)<AxisGroupOther.nPositionCompareMax
AND ABS(AxisGroupStatus.ActualPosition.c.Y-AxisGroupParameter.Position.c.Y)<AxisGroupOther.nPositionCompareMax
AND ABS(AxisGroupStatus.ActualPosition.c.Z-AxisGroupParameter.Position.c.Z)<AxisGroupOther.nPositionCompareMax
AND ABS(AxisGroupStatus.ActualPosition.c.A-AxisGroupParameter.Position.c.A)<AxisGroupOther.nPositionCompareMax
AND ABS(AxisGroupStatus.ActualPosition.c.B-AxisGroupParameter.Position.c.B)<AxisGroupOther.nPositionCompareMax
AND ABS(AxisGroupStatus.ActualPosition.c.Z-AxisGroupParameter.Position.c.Z)<AxisGroupOther.nPositionCompareMax THEN 
    AxisGroupStatus.bPositionCompare:=TRUE;
ELSE
       AxisGroupStatus.bPositionCompare:=FALSE;
END_IF

//轴组坐标系选择
CASE AxisGroupParameter.nCoordSystem OF
	0:
		CoordSystem:=SMC_COORD_SYSTEM.ACS ;
	1:
		CoordSystem:=SMC_COORD_SYSTEM.MCS ;
	2:
		CoordSystem:=SMC_COORD_SYSTEM.WCS ;
	3:
		CoordSystem:=SMC_COORD_SYSTEM.TCS ;
	ELSE
		CoordSystem:=SMC_COORD_SYSTEM.ACS ;
END_CASE

//功能块
//轴组使能
SMC_GroupPower(
    AxisGroup:=AxisGroup , 
    Enable:=bPowerAllow , 
    bRegulatorOn:=bPowerAllow , 
    bDriveStart:=bPowerAllow , 
    Status=> , 
    Busy=>AxisGroupStatus.bPowerStatus , 
    Error=> , 
    ErrorID=> );

//轴组有效
MC_GroupEnable(
    AxisGroup:=AxisGroup , 
    Execute:=bEnableAllow , 
    CompatibilityOptions:= , 
    Done=> , 
    Busy=> , 
    Error=>AxisGroupStatus.bEnableError , 
    ErrorID=> );

//轴组无效
MC_GroupDisable(
    AxisGroup:=AxisGroup , 
    Execute:=AxisGroupControl.bDisable , 
    Done=> , 
    Busy=> , 
    Error=>AxisGroupStatus.bDisableError , 
    ErrorID=> );

IF MC_GroupEnable.Done THEN
    AxisGroupStatus.bEnable:=TRUE;
ELSIF MC_GroupDisable.Done THEN
    AxisGroupStatus.bEnable:=FALSE;
END_IF

//轴组复位
MC_GroupReset(
    AxisGroup:=AxisGroup , 
    Execute:=bResetAllow , 
    Done=> , 
    Busy=> , 
    Error=> , 
    ErrorID=> );

//停止
MC_GroupStop(
    AxisGroup:=AxisGroup , 
    Execute:=AxisGroupControl.bStop , 
    Deceleration:= , 
    Jerk:= , 
    AccFactor:= , 
    JerkFactor:= , 
    Done=> , 
    Busy=> , 
    Active=> , 
    CommandAborted=> , 
    CommandAccepted=> , 
    Error=> , 
    ErrorID=> , 
    MovementId=> );

//暂停
MC_GroupHalt(
    AxisGroup:=AxisGroup , 
    Execute:=AxisGroupControl.bHalt , 
    Deceleration:= , 
    Jerk:= , 
    AccFactor:= , 
    JerkFactor:= , 
    Done=> , 
    Busy=> , 
    Active=> , 
    CommandAborted=> , 
    CommandAccepted=> , 
    Error=> , 
    ErrorID=> , 
    MovementId=> );
    
//点动
SMC_GroupJog2(
    AxisGroup:=AxisGroup , 
    Enable:=bJogAllow , 
    Forward:=AxisGroupControl.bJogForward , 
    Backward:=AxisGroupControl.bJogBackaward , 
    MaxLinearDistance:=1000 , 
    MaxAngularDistance:=1000 , 
    Velocity:=AxisGroupParameter.nJogVelocity , 
    Acceleration:=AxisGroupParameter.nJogAcceleration , 
    Deceleration:=AxisGroupParameter.nJogDeceleration , 
    Jerk:=AxisGroupParameter.nJogJerk , 
    VelFactor:=1 , 
    AccFactor:=1 , 
    JerkFactor:=1 , 
    TorqueFactor:=1 , 
    CoordSystem:=CoordSystem , 
    ABC_as_ACS:=TRUE , 
    Active=> , 
    Busy=>AxisGroupStatus.bJogBusy , 
    Error=>AxisGroupStatus.bJogError , 
    ErrorID=> , 
    CurrentPosition=> );

//关节插补
MC_MoveDirectAbsolute(
    AxisGroup:=AxisGroup , 
    Execute:=bMoveDirectAbsoluteAllow , 
    Position:=AxisGroupParameter.Position , 
    MovementType:= , 
    CoordSystem:=SM3_Robotics.SMC_COORD_SYSTEM.MCS , 
    BufferMode:=SM3_Common.MC_BUFFER_MODE.Buffered , 
    TransitionMode:= , 
    TransitionParameter:= , 
    VelFactor:= , 
    AccFactor:= , 
    JerkFactor:= , 
    Done=>AxisGroupStatus.bMoveDirectAbsoluteDone , 
    Busy=>AxisGroupStatus.bMoveDirectAbsoluteBusy , 
    Active=> , 
    CommandAborted=> , 
    CommandAccepted=> , 
    Error=>AxisGroupStatus.bMoveDirectAbsoluteError , 
    ErrorID=> , 
    MovementId=> );

//直线插补
MC_MoveLinearAbsolute(
    AxisGroup:=AxisGroup , 
    Execute:=bMoveLinearAbsoluteAllow , 
    Position:=AxisGroupParameter.Position , 
    Velocity:=AxisGroupParameter.nVelocity , 
    Acceleration:=AxisGroupParameter.nAcceleration , 
    Deceleration:=AxisGroupParameter.nDeceleration , 
    Jerk:=AxisGroupParameter.nJerk , 
    CoordSystem:=SM3_Robotics.SMC_COORD_SYSTEM.MCS , 
    BufferMode:=SM3_Common.MC_BUFFER_MODE.Buffered , 
    TransitionMode:= , 
    TransitionParameter:= , 
    OrientationMode:=SM3_Robotics.SMC_ORIENTATION_MODE.Axis , 
    VelFactor:= , 
    AccFactor:= , 
    JerkFactor:= , 
    Done=>AxisGroupStatus.bMoveLinearAbsoluteDone , 
    Busy=>AxisGroupStatus.bMoveLinearAbsoluteBusy , 
    Active=> , 
    CommandAborted=> , 
    CommandAccepted=> , 
    Error=>AxisGroupStatus.bMoveLinearAbsoluteError , 
    ErrorID=> , 
    MovementId=> );
    
//圆弧插补
MC_MoveCircularAbsolute(
    AxisGroup:=AxisGroup , 
    Execute:=bMoveCircularAbsoluteAllow , 
    CircMode:=AxisGroupParameter.CircMode , 
    AuxPoint:=AxisGroupParameter.PositionAux , 
    EndPoint:=AxisGroupParameter.Position , 
    PathChoice:= , 
    Velocity:=AxisGroupParameter.nVelocity , 
    Acceleration:=AxisGroupParameter.nAcceleration , 
    Deceleration:=AxisGroupParameter.nDeceleration , 
    Jerk:=AxisGroupParameter.nJerk ,
    CoordSystem:=SM3_Robotics.SMC_COORD_SYSTEM.MCS , 
    BufferMode:=SM3_Common.MC_BUFFER_MODE.Buffered , 
    TransitionMode:= , 
    TransitionParameter:= , 
    OrientationMode:=SM3_Robotics.SMC_ORIENTATION_MODE.Axis , 
    VelFactor:= , 
    AccFactor:= , 
    JerkFactor:= , 
    Done=>AxisGroupStatus.bMoveCircularAbsoluteDone , 
    Busy=>AxisGroupStatus.bMoveCircularAbsoluteBusy , 
    Active=> , 
    CommandAborted=> , 
    CommandAccepted=> , 
    Error=>AxisGroupStatus.bMoveCircularAbsoluteError , 
    ErrorID=> , 
    MovementId=> );
    
//读取状态
MC_GroupReadStatus(
    AxisGroup:=AxisGroup , 
    Enable:=TRUE , 
    Valid=> , 
    Busy=> , 
    Error=>AxisGroupStatus.bReadStatusError , 
    ErrorID=> , 
    GroupMoving=> , 
    GroupHoming=> , 
    GroupErrorStop=> , 
    GroupStandby=> , 
    GroupStopping=> , 
    GroupDisabled=> , 
    TrackingDynamicCS=> , 
    InSync=> , 
    ActiveMovementId=> , 
    LastAcceptedMovementId=> );

//读取当前位置
MC_GroupReadActualPosition(
    AxisGroup:=AxisGroup , 
    Enable:=TRUE , 
    CoordSystem:=SM3_Robotics.SMC_COORD_SYSTEM.MCS , 
    Valid=> , 
    Busy=> , 
    Error=>AxisGroupStatus.bReadActualPositionError , 
    ErrorID=> , 
    Position=>AxisGroupStatus.ActualPosition , 
    KinematicConfig=> );
    
//读取当前速度
MC_GroupReadActualVelocity(
    AxisGroup:=AxisGroup , 
    Enable:=TRUE , 
    CoordSystem:=SM3_Robotics.SMC_COORD_SYSTEM.MCS , 
    Valid=> , 
    Busy=> , 
    Error=>AxisGroupStatus.bReadActualVelocityError , 
    ErrorID=> , 
    Velocity=>AxisGroupStatus.ActualVelocity );