马上注册,享受更多特权
您需要 登录 才可以下载或查看，没有帐号？立即注册 
x
FUNCTION_BLOCK FB_CNC_REF3
VAR_IN_OUT
AxisX: AXIS_REF_SM3;
AxisY: AXIS_REF_SM3;
AxisZ: AXIS_REF_SM3;
END_VAR
VAR_INPUT
dwIpoTime:INT; //插补周期，与Ethercat周期一致；
bExecute: BOOL;
bAbort:BOOL;
ncprogIn: POINTER TO SMC_CNC_REF;
END_VAR
VAR_OUTPUT
Done:BOOL;
Busy:BOOL;
dwSwitches WORD;
Error:BOOL;
ErrorID:SMC_ERROR;
END_VAR
VAR
ncd: SMC_NCDecoder;
cv: SMC_CheckVelocities;
SmoothPath: SMC_SmoothPath;
RoundPath:SMC_RoundPath;
SMC_Interpolator1:SMC_Interpolator;
SMC_TRAFO_Gantry3D:SMC_TRAFO_Gantry3;
SMC_TRAFOF_Gantry3D:SMC_TRAFOF_Gantry3D;
SMC_ControlAxisByPosX,SMC_ControlAxisByPosY,SMC_ControlAxisByPosZ:SMC_ControlAxisByPos;
xStartIpo: BOOL;
iState: INT;
agiBufDecoder: ARRAY[0..199] OF SMC_GeoInfo;
agiBufSmooth: ARRAY[0..199] OF SMC_GEOINFO;
i:INT:=0;
bExecuteR: R_TRIG;
xStart: BOOL;
END_VAR
\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\
bExecuteR(CLK:=bExecute, Q=> );
IF bExecuteR.Q THEN
iState:=0;
xStart:=TRUE;
END_IF
CASE iState OF
0: // idle
IF xStart THEN
iState:=10;
xStartIpo := FALSE;
ncd(bExecute:=FALSE, ncprog:=ncprogIn^);
SmoothPath(bExecute:=FALSE, poqDataIn:=ncd.poqDataOut);
cv(bExecute:=FALSE, poqDataIn:=SmoothPath.poqDataOut);
i:=i+1;
xStart:=FALSE;
END_IF
10: // start reading and processing
ncd.piStartPosition.dX:=AxisX.fSetPosition;
ncd.piStartPosition.dY:=AxisY.fSetPosition;
ncd(
bExecute:=TRUE ,
bAppend:= ,
bStepSuppress:= ,
piStartPosition:= ,
nSizeOutQueue:=SIZEOF(agiBufDecoder) ,
pbyBufferOutQueue:=ADR(agiBufDecoder) ,
ncprog:=ncprogIn^,
bDone=> ,
bBusy=> ,
bError=> ,
wErrorID=> ,
poqDataOut=> ,
iStatus=> ,
iLineNumberDecoded=> ,
GCodeText=> );
SmoothPath(
bExecute:=ncd.bExecute,
bAbort:= ,
bAppend:= ,
poqDataIn:= ,
dEdgeDistance:= ,
dAngleTol:= ,
nSizeOutQueue:=SIZEOF(agiBufSmooth) ,
pbyBufferOutQueue:=ADR(agiBufSmooth) ,
eMode:= ,
bSymmetricalDistances:= ,
eAddAxMode:= ,
bDone=> ,
bBusy=> ,
bError=> ,
wErrorID=> ,
poqDataOut=> );
cv(
bExecute:=ncd.bExecute,
poqDataIn:= ,
dAngleTol:= ,
bBusy=> ,
bError=> ,
wErrorID=> ,
poqDataOut=> );
xStartIpo := ncd.bExecute;
IF NOT cv.bBusy THEN
iState:=0;
END_IF
IF SmoothPath.bDone AND cv.poqDataOut<>16#00000000 THEN
iState:=20;
END_IF
20:
ncd.bExecute:=FALSE;
SmoothPath.bExecute:=FALSE;
SMC_Interpolator1(
bExecute:=bExecute,
poqDataIn:=cv.poqDataOut,
bSlow_Stop:= ,
bEmergency_Stop:=SMC_ControlAxisByPosX.bError OR SMC_ControlAxisByPosX.bStopIpo OR SMC_ControlAxisByPosY.bError OR SMC_ControlAxisByPosY.bStopIpo
OR SMC_ControlAxisByPosZ.bError OR SMC_ControlAxisByPosZ.bStopIpo,
bWaitAtNextStop:= ,
dOverride:= ,
iVelMode:= ,
dwIpoTime:=INT_TO_DWORD(dwIpoTime),
dLastWayPos:= ,
bAbort:=bAbort,
bSingleStep:= ,
bAcknM:= ,
bQuick_Stop:= ,
dQuickDeceleration:= ,
dJerkMax:= ,
dQuickStopJerk:= ,
bDone=>Done,
bBusy=>Busy,
bError=>Error,
wErrorID=>ErrorID,
piSetPosition=> ,
iStatus=> ,
bWorking=> ,
iActObjectSourceNo=> ,
dVel=> ,
vecActTangent=> ,
iLastSwitch=> ,
dwSwitches=>dwSwitches ,
dWayPos=> ,
wM=> );
SMC_TRAFO_Gantry3D(
pi:=SMC_Interpolator1.piSetPosition,
dOffsetX:= ,
dOffsetY:= ,
dOffsetZ:= ,
dx=> ,
dy=> ,
dz=> );
SMC_ControlAxisByPosX(
Axis:=AxisX,
iStatus:=SMC_Interpolator1.iStatus,
bEnable:=SMC_Interpolator1.bWorking,
bAvoidGaps:=TRUE,
fSetPosition:=SMC_TRAFO_Gantry3D.dx,
fGapVelocity:=10,
fGapAcceleration:=10,
fGapDeceleration:=10,
fGapJerk:= ,
bBusy=> ,
bCommandAborted=> ,
bError=> ,
iErrorID=> ,
bStopIpo=> );
SMC_ControlAxisByPosY(
Axis:=AxisY,
iStatus:=SMC_Interpolator1.iStatus,
bEnable:=SMC_Interpolator1.bWorking,
bAvoidGaps:=TRUE,
fSetPosition:=SMC_TRAFO_Gantry3D.dy,
fGapVelocity:=10,
fGapAcceleration:=10,
fGapDeceleration:=10,
fGapJerk:= ,
bBusy=> ,
bCommandAborted=> ,
bError=> ,
iErrorID=> ,
bStopIpo=> );
SMC_ControlAxisByPosZ(
Axis:=AxisZ,
iStatus:=SMC_Interpolator1.iStatus,
bEnable:=SMC_Interpolator1.bWorking,
bAvoidGaps:=TRUE,
fSetPosition:=SMC_TRAFO_Gantry3D.dz,
fGapVelocity:=10,
fGapAcceleration:=10,
fGapDeceleration:=10,
fGapJerk:= ,
bBusy=> ,
bCommandAborted=> ,
bError=> ,
iErrorID=> ,
bStopIpo=> );
SMC_TRAFOF_Gantry3D(
dOffsetX:= ,
dOffsetY:= ,
dOffsetZ:= ,
minX:=0 ,
maxX:=100 ,
minY:=0 ,
maxY:=100 ,
minZ:=0 ,
maxZ:=100 ,
DriveX:=AxisX ,
DriveY:=AxisY ,
DriveZ:=AxisZ ,
dx=> ,
dy=> ,
dz=> ,
dnx=> ,
dny=> ,
dnz=> ,
dm=> );
END_CASE
|
