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)
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:= ,
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 );