马上注册,享受更多特权
您需要 登录 才可以下载或查看，没有帐号？立即注册 
x
//判断G 代码的有效区域
IF ProgramInit AND InitIndex<>3 THEN
CASE InitIndex OF
0:
//预处理只能地址
LINE_Interpolation1.pbyBuffer:=ADR(LINE_Interpolation1_0);
//初始化程序空间,新建200个缓存
SMC_SetQueueCapacity(poq:=ADR(LINE_Interpolation1) , nSizeInBytes:= SIZEOF(LINE_Interpolation1_0));
InitIndex:=1;
1:
CASE ProgramNumber OF
1:
Posval.wAuxData:=3;//轴选os
Proval.iObjNo:=1;//MASK;
Proval.iSourceLine_No:=0;//G代码行号
proval.diSentenceNo:=0;
Proval.iMoveType:=SMC_MOVTYP.LIN;//类型
posval.dX:=Data.StartPosX;
posval.dy:=Data.StartPosY;
Proval.piStartPos:=posval;
posval.dX:=Data.DestPosX;
posval.dy:=Data.DestPosY;
Proval.piDestPos:=posval;
proval.dvel:=Data.Dvel;//速度
Proval.dVelEnd:=Data.DEndvel;//结束速度
Proval.dAccel:=Data.dAccel;//加速度
proval.dDecel:=Data.dDecel;//减速度
SMC_CalcLengthGeo(pg:=ADR(Proval));
SMC_AppendObj(poq:=ADR(LINE_Interpolation1) , pgi:=ADR(Proval));
2:
Posval.wAuxData:=3;//轴选os
Proval.iObjNo:=1;//MASK;
Proval.iSourceLine_No:=0;//G代码行号
proval.diSentenceNo:=0;
Proval.iMoveType:=SMC_MOVTYP.CLW;//类型
posval.dX:=Data.StartPosX;
posval.dy:=Data.StartPosY;
Proval.piStartPos:=posval;
posval.dX:=Data.DestPosX;
posval.dy:=Data.DestPosY;
Proval.piDestPos:=posval;
CriclePointThree(
Enable:=TRUE ,
X1:=Data.StartPosX,
Y1:=Data.StartPosY ,
X2:=Data.ICentreCircle ,
Y2:=Data.JCentreCircle ,
X3:= Data.DestPosX ,
Y3:= Data.DestPosY,
Start:=ProgramNumber=2 ,
RX=> Proval.dP1 ,
RY=> Proval.dP2 ,
Radius=> Proval.dP3 ,
Done=> );
CW_CCW_Circle(
Seclect:= 1,
ICentreCircle:= Proval.dP1 ,
JCentreCircle:= Proval.dP2 ,
AxisX_fActPosition:= Data.StartPosX,
AxisY_fActPosition:= Data.StartPosY,
XEndPosition:=Data.DestPosX ,
YEndPosition:=Data.DestPosY ,
DT1=>Proval.dT1 ,
DT2=>Proval.dT2 );
proval.dvel:=Data.Dvel;//速度
Proval.dVelEnd:=Data.DEndvel;//结束速度
Proval.dAccel:=Data.dAccel;//加速度
proval.dDecel:=Data.dDecel;//减速度
//数据刷入
SMC_CalcLengthGeo(pg:=ADR(Proval));
SMC_AppendObj(poq:=ADR(LINE_Interpolation1) , pgi:=ADR(Proval));
3:
Posval.wAuxData:=3;//轴选os
Proval.iObjNo:=1;//MASK;
Proval.iSourceLine_No:=0;//G代码行号
proval.diSentenceNo:=0;
Proval.iMoveType:=SMC_MOVTYP.CCLW;//类型
posval.dX:=Data.StartPosX;
posval.dy:=Data.StartPosY;
Proval.piStartPos:=posval;
posval.dX:=Data.DestPosX;
posval.dy:=Data.DestPosY;
Proval.piDestPos:=posval;
CriclePointThree(
Enable:=TRUE ,
X1:=Data.StartPosX,
Y1:=Data.StartPosY ,
X2:=Data.ICentreCircle ,
Y2:=Data.JCentreCircle ,
X3:= Data.DestPosX ,
Y3:= Data.DestPosY,
Start:=ProgramNumber=3 ,
RX=> Proval.dP1 ,
RY=> Proval.dP2 ,
Radius=> Proval.dP3 ,
Done=> );
CW_CCW_Circle(
Seclect:= 2,
ICentreCircle:= Proval.dP1 ,
JCentreCircle:= Proval.dP2 ,
AxisX_fActPosition:= Data.StartPosX,
AxisY_fActPosition:= Data.StartPosY,
XEndPosition:=Data.DestPosX ,
YEndPosition:=Data.DestPosY ,
DT1=>Proval.dT1 ,
DT2=>Proval.dT2 );
proval.dvel:=Data.Dvel;//速度
Proval.dVelEnd:=Data.DEndvel;//结束速度
Proval.dAccel:=Data.dAccel;//加速度
proval.dDecel:=Data.dDecel;//减速度
//数据刷入
SMC_CalcLengthGeo(pg:=ADR(Proval));
SMC_AppendObj(poq:=ADR(LINE_Interpolation1) , pgi:=ADR(Proval));
// 4:
// Posval.wAuxData:=3;//轴选os
// Proval.iObjNo:=1;//MASK;
// Proval.iSourceLine_No:=0;//G代码行号
// proval.diSentenceNo:=0;
// Proval.iMoveType:=SMC_MOVTYP.MCOMMAND;//类型
// posval.dX:=Data.StartPosX;
// posval.dy:=Data.StartPosY;
// Proval.piStartPos:=posval;
// posval.dX:=Data.StartPosX;
// posval.dy:=Data.StartPosY;
// Proval.piDestPos:=posval;
// proval.iHelpID[0]:=Data.iHelpID;
// proval.dvel:=Data.Dvel;//速度
// Proval.dVelEnd:=Data.DEndvel;//结束速度
// Proval.dAccel:=Data.dAccel;//加速度
// proval.dDecel:=Data.dDecel;//减速度
// //数据刷入
// SMC_CalcLengthGeo(pg:=ADR(Proval));
// SMC_AppendObj(poq:=ADR(LINE_Interpolation1) , pgi:=ADR(Proval));
END_CASE ;
LINE_Interpolation1.bEndOfList:=TRUE;
InitIndex:=2;
2:
PathOptimize:=TRUE;
ProgramInit:=FALSE;
InitIndex:=3;
END_CASE
END_IF
SMC_SmoothPath_0(
bExecute:=PathOptimize ,
bAbort:= ,
bAppend:= ,
poqDataIn:= ADR(LINE_Interpolation1 ),
dEdgeDistance:=,
dAngleTol:= ,
nSizeOutQueue:=SIZEOF(LINE_Interpolation1_1) ,
pbyBufferOutQueue:=ADR(LINE_Interpolation1_1),
eMode:= ,
bSymmetricalDistances:= ,
bImprovedSymmetricCuts:= ,
eAddAxMode:= ,
dMinimumCurvatureRadius:= ,
bCheckCurvature:= ,
dRelativeCurvatureTol:= ,
bCheckAddAxVelJump:= ,
dMaxAddAxVelDifference:= ,
bDone=> ,
bBusy=> ,
bError=> ,
wErrorID=> ,
poqDataOut=> ,
udiStopsDueToCurvatureRadius=> );
SMC_CheckVelocities_0(
bExecute:= SMC_SmoothPath_0.bDone,
bAbort:= ,
poqDataIn:=SMC_SmoothPath_0.poqDataOut ,
dAngleTol:=,
bCheckAddAxVelJump:= ,
dMaxAddAxVelDifference:= ,
bBusy=> ,
bError=> ,
wErrorID=> ,
poqDataOut=> );
IF SMC_CheckVelocities_0.bBusy THEN
PathOptimize:=FALSE;
bExecute:=TRUE;
InitIndex:=0;
END_IF
SMC_Interpolator_0(
bExecute:=bExecute,
poqDataIn:=SMC_CheckVelocities_0.poqDataOut,
bSlow_Stop:= ,
bEmergency_Stop:=SMC_ControlAxisByPos_X.bError OR SMC_ControlAxisByPos_X.bStopIpo OR SMC_ControlAxisByPos_Y.bError OR SMC_ControlAxisByPos_Y.bStopIpo,
bWaitAtNextStop:= ,
dOverride:= ,
iVelMode:= ,
dwIpoTime:=4000,
dLastWayPos:= ,
bAbort:=,
bSingleStep:= ,
bAcknM:=,
bQuick_Stop:= ,
dQuickDeceleration:= ,
dJerkMax:= ,
dQuickStopJerk:= ,
bDone=>,
bBusy=>,
bError=>,
wErrorID=>,
piSetPosition=> ,
iStatus=> ,
bWorking=> ,
iActObjectSourceNo=> ,
dVel=> ,
vecActTangent=> ,
iLastSwitch=> ,
dwSwitches=> ,
dWayPos=> ,
wM=>M_OUT );
IF SMC_Interpolator_0.bBusy THEN
bExecute:=FALSE;
END_IF
SMC_TRAFO_Gantry2_0(
pi:= SMC_Interpolator_0.piSetPosition
);
SMC_ControlAxisByPos_X(
Axis:= AxisX,
iStatus:= SMC_Interpolator_0.iStatus,
bAvoidGaps:=TRUE,
bEnable:= SMC_Interpolator_0.bWorking,
fSetPosition:= SMC_TRAFO_Gantry2_0.dx,
fGapVelocity:= 10,
fGapAcceleration:= 10,
fGapDeceleration:= 10 );
SMC_ControlAxisByPos_Y(
Axis:= AxisY,
iStatus:= SMC_Interpolator_0.iStatus,
bAvoidGaps:=TRUE,
bEnable:= SMC_Interpolator_0.bWorking,
fSetPosition:= SMC_TRAFO_Gantry2_0.dY,
fGapVelocity:= 10,
fGapAcceleration:= 10,
fGapDeceleration:= 10 );
SMC_TRAFOF_Gantry2D(
DriveX:=AxisX,
DriveY:=AxisY,
dOffsetX:= ,
dOffsetY:= ,
minX:=0,
maxX:=100,
minY:=0,
maxY:=100,
dx=> ,
dy=> ,
dnx=> ,
dny=> ,
ratio=> ,
dnOffsetX=> ,
dnOffsetY=> );
|
