codesys 控制轴组程序

//轴组使能允许
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) AND ABS(AxisGroupStatus.ActualPosition.c.Y-AxisGroupParameter.Position.c.Y) AND ABS(AxisGroupStatus.ActualPosition.c.Z-AxisGroupParameter.Position.c.Z) AND ABS(AxisGroupStatus.ActualPosition.c.A-AxisGroupParameter.Position.c.A) AND ABS(AxisGroupStatus.ActualPosition.c.B-AxisGroupParameter.Position.c.B) AND ABS(AxisGroupStatus.ActualPosition.c.Z-AxisGroupParameter.Position.c.Z)     AxisGroupStatus.bPositionCompare:=TRUE;
ELSE
       AxisGroupStatus.bPositionCompare:=FALSE;
END_IF

//功能块
//轴组使能
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:=AxisGroupParameter.nCoordSystem , 
    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 );