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)<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

//功能块

//轴组使能

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 );

相关推荐
WX_LW1 年前
codesys 6轴机器人正解程序
st