?? processg72.c
字號:
#include "base.h"
FP32 G72_Pos_U;
FP32 G72_Pos_W;
FP32 G72_Pos_X;
FP32 G72_Pos_Z;
FP32 G72_Pos_I;
FP32 G72_Pos_K;
FP32 G72_Pos_R;
FP32 G72_Abs_PosX;
FP32 G72_Abs_PosZ;
FP32 G72_Abs_PosI;
FP32 G72_Abs_PosK;
FP32 G72_Abs_PosR;
BOOLEAN G72_bEnd;
BOOLEAN ProcessG72(INT32U StartHang,INT32U EndHang,FP32 ZaddPerTime,FP32 Zremain,FP32 Xremain,FP32 BackValue)
{
INT32U i;
INT8U ret;
FP32 LoopStartXpos;
FP32 LoopStartZpos;
FP32 LoopEndXpos;
FP32 LoopEndZpos;
INT16U LoopCount;
FP32 Start_X;
FP32 Start_Z;
FP32 TempX,TempZ;
for (i=0;i<MAXNUMBER;i++){
LineValue[i].Pos=0;
CircleValue[i].Pos=0;
LineValue[i].StartX=0;
CircleValue[i].StartZ=0;
LineValue[i].EndX=0;
CircleValue[i].EndZ=0;
LineValue[i].K=0;
CircleValue[i].K=0;
CircleValue[i].I=0;
}
G72_bEnd=0;
G72ResetPara();
CircleShapNum=0;
AllNum=0;
LineShapNum=0;
Xdec=0;Xinc=0;Zdec=0;Zinc=0;
G72.CurrentNum=G72FindStartHang(StartHang);
if (G72.CurrentNum==0xfffff){
DispErrCode(19);
return FALSE;
}
if (G72ProcessAllCode(EndHang)!=6){
DispErrCode(21);
return FALSE;
}
if(AllNum<=1){
DispErrCode(40);
return FALSE;
}
if(Xdec && Zinc){
DispErrCode(39);return FALSE;
}
if(Xdec && Zdec){
DispErrCode(39);return FALSE;
}
if(Xinc && Zinc){
ZaddPerTime=fabs(ZaddPerTime);BackValue=fabs(BackValue);bit=FALSE;
}
if(Xinc && Zdec){
ZaddPerTime=fabs(ZaddPerTime);BackValue=fabs(BackValue);bit=TRUE;
}
Start_X=Get_AbsPos(1);
Start_Z=Get_AbsPos(3);
i=0;
while(1){
if (CircleValue[i].Pos==1){
LoopStartZpos=CircleValue[i].StartZ;
break;
}
if (LineValue[i].Pos==1){
LoopStartZpos=LineValue[i].StartZ;
break;
}
i++;
if(g_Scram)return FALSE;
}
i=0;
while(1){
if (CircleValue[i].Pos==AllNum){
LoopEndZpos=CircleValue[i].EndZ;
LoopEndXpos=CircleValue[i].EndX;
LoopStartXpos=CircleValue[i].EndX;
break;
}
if (LineValue[i].Pos==AllNum){
LoopEndZpos=LineValue[i].EndZ;
LoopEndXpos=LineValue[i].EndX;
LoopStartXpos=LineValue[i].EndX;
break;
}
i++;
if(g_Scram)FALSE;
}
// LoopCount=(INT8U)(fabs((LoopStartZpos-LoopEndZpos)/ZaddPerTime));
// if (LoopCount>200){DispErrCode(26);return FALSE;}
LoopCount=(INT16U)(fabs((LoopStartZpos-LoopEndZpos)/ZaddPerTime));
if (LoopCount>LOOPTIMES){DispErrCode(26);return FALSE;}
if (LoopCount==0){DispErrCode(27);return FALSE;}
Set_InpSpeed(g_Sysparam.Max_FastSpeed);
Target_PosX=LoopStartXpos;
Target_PosZ=LoopStartZpos;
bRunInp=FALSE;
while(1)
{
G01_AbsPos(LoopStartXpos,LoopStartZpos);
g_MoveMode=1;
ret=WaitMotionEnd();
if(ret==255)return FALSE;
if(ret==0)break;
}
i=0;
while(1)
{
g_Current_ASpeed=g_Current_PSpeed*g_Current_Inp_Rate;
if(g_Current_ASpeed>g_Sysparam.Max_FastSpeed)
g_Current_ASpeed=g_Sysparam.Max_FastSpeed;
Set_InpSpeed(g_Current_ASpeed);
if(bit)
{
TempX=G72ConstructionFunc(LoopStartZpos-i*ZaddPerTime)+Xremain;
}
else
{
TempX=G72ConstructionFunc(LoopStartZpos+i*ZaddPerTime)+Xremain;
}
Target_PosX=TempX;
Target_PosZ=Get_AbsPos(3);
bRunInp=TRUE;
while(1){
G01_AbsPos(TempX,Get_AbsPos(3));
g_MoveMode=1;
ret=WaitMotionEnd();
if(ret==255)return FALSE;
if(ret==0)break;
}
g_Current_ASpeed=g_Current_PSpeed*g_Current_Inp_Rate;
if(g_Current_ASpeed>g_Sysparam.Max_FastSpeed)
g_Current_ASpeed=g_Sysparam.Max_FastSpeed;
Set_InpSpeed(g_Current_ASpeed);
if(bit){
TempX=Get_AbsPos(1)+BackValue;TempZ=Get_AbsPos(3)+BackValue;
}else{
TempX=Get_AbsPos(1)+BackValue;TempZ=Get_AbsPos(3)-BackValue;
}
Target_PosX=TempX;
Target_PosZ=TempZ;
bRunInp=TRUE;
while(1){
G01_AbsPos(TempX,TempZ);
g_MoveMode=1;
ret=WaitMotionEnd();
if(ret==255)return FALSE;
if(ret==0)break;
}
Set_SipSpeed(1,g_Current_Fast_Rate*g_Sysparam.X_FastSpeed);
Set_SipSpeed(3,g_Current_Fast_Rate*g_Sysparam.Z_FastSpeed);
Target_PosX=LoopStartXpos;
bRunInp=FALSE;
while(1){
Move_AbsPos(1,LoopStartXpos);
g_MoveMode=1;
ret=WaitMotionEnd();
if(ret==255)return FALSE;
if(ret==0)break;
}
Set_SipSpeed(1,g_Current_Fast_Rate*g_Sysparam.X_FastSpeed);
Set_SipSpeed(3,g_Current_Fast_Rate*g_Sysparam.Z_FastSpeed);
if(bit){
TempZ=Get_AbsPos(3)-ZaddPerTime-BackValue;
}else{
TempZ=Get_AbsPos(3)+ZaddPerTime+BackValue;
}
Target_PosZ=TempZ;
bRunInp=FALSE;
while(1){
Move_AbsPos(3,TempZ);
g_MoveMode=1;
ret=WaitMotionEnd();
if(ret==255)return FALSE;
if(ret==0)break;
}
i++;
if (i==LoopCount)break;
}
Set_InpSpeed(g_Sysparam.Max_FastSpeed);
Target_PosX=Start_X;
Target_PosZ=Start_Z;
bRunInp=FALSE;
while(1){
G01_AbsPos(Start_X,Start_Z);
g_MoveMode=1;
ret=WaitMotionEnd();
if(ret==255)return FALSE;
if(ret==0)break;
}
return TRUE;
}
INT8U G72GetNextSeg(INT8U *First,INT8U *buf,INT8U *End)
{
INT8U s;
INT16U i;
for(;;)
{
s=g_DataBuf.DataBuf[G72.CurrentNum];
switch(s)
{
case '%':
*First=s;
buf[0]=0x0;
Uart_Printf("%");
Uart_Printf("%d",G72.CurrentNum);
return 4;//
break;
case '(':
case '/'://注釋
*First=s;
i=0;
while(i<=255){
G72.CurrentNum++;
if(g_iCurrentNum>=g_DataBuf.DataLength)
{
buf[0]=0x0;
*End=FALSE;
return 255;
}
s=g_DataBuf.DataBuf[G72.CurrentNum];
if(s!=0x0a && s!=0x0d){
buf[i]=s;
}
if(s==0x0a){
buf[i]=0x00;
*End=FALSE;
G72.CurrentNum++;
if(G72.CurrentNum>=g_DataBuf.DataLength)return 255;
return 0;//一行代碼結束
}
i++;
}
buf[0]=0x0;
*End=TRUE;
return 2;//注釋超長
case 0x0d:
G72.CurrentNum++;
break;
case 0x0a:
*First=s;
G72.CurrentNum++;
if(G72.CurrentNum>=g_DataBuf.DataLength)
{
buf[0]=0x0;
*End=FALSE;
return 255;
}
return 0;//一行代碼結束
break;
case 'A':case 'B':case 'C':case 'D':case 'E':
case 'F':case 'G':case 'H':case 'I':case 'J':
case 'K':case 'L':case 'M':case 'N':case 'O':
case 'P':case 'Q':case 'R':case 'S':case 'T':
case 'U':case 'V':case 'W':case 'X':case 'Y':case 'Z':
*First=s;
i=0;
while(i<=255){
G72.CurrentNum++;
if(G72.CurrentNum>=g_DataBuf.DataLength)
{
buf[0]=0x0;
*End=TRUE;
return 255;
}
s=g_DataBuf.DataBuf[G72.CurrentNum];
if((s>='0' && s<='9') || s=='.' || s=='-' || s=='+'){
buf[i]=s;
i++;
}
else if(s==0x0d)
{
;
}
else if(s==0x0a)
{
buf[i]=0x00;
*End=FALSE;
G72.CurrentNum++;
if(G72.CurrentNum>=g_DataBuf.DataLength)
return 255;
return 0;//一行代碼結束
}
else
{
buf[i]=0x00;
*End=FALSE;
return 5;//字代碼結束
}
}
return 2; //數據超長
default:
*First=s;
buf[0]=0x00;
*End=TRUE;
return 3;//其它的字
}
}
}
INT8U G72ProcessAllCode(INT32U EndHang)
{
INT8U Error;
INT8U End;
INT8U num,len;
INT8U First,buf[255];
INT8U k,j;
INT32U Hang;
while(1){
Error=G72GetNextSeg(&First,buf,&End);
if(Error != 0 && Error !=5)break;
if(End)break;
switch(First)
{
case '%':
case '/':
case '(':
case 0x0d:
case 0x0a:
break;
case 'N':
if(strlen((char *)buf)==0){Error=72;break;}
if(bN_Code==1) {Error=72;break;}
k=0;Hang=0;j=strlen((char *)buf);
while(j){
Hang=(buf[k]-'0')+Hang*10;
k++;
j--;
}
if(EndHang==Hang)
{
G72_bEnd=TRUE;
G72ResetPara();
}
bN_Code=TRUE;
break;
case 'G':
len=strlen((char *)buf);
if(len !=1 && len !=2 ){Error=72;break;}
if(len==1) num=buf[0]-'0';
else num=(buf[0]-'0')*10+buf[1]-'0';
switch(num){
case 0:
case 1:
case 2:
case 3:
case 4:
bG_Code=1;
break;
default:
Error=72;
break;
}
iG_Code=num;
break;
case 'M':
len=strlen((char *)buf);
if(len !=1 && len !=2 ){Error=72;break;}
if(len==1) num=buf[0]-'0';
else num=(buf[0]-'0')*10+buf[1]-'0';
switch(num){
case 3:
case 4:
case 5:
case 8:
case 9:
case 10:
case 11:
case 32:
case 33:
iM_Code=num;
bM_Code=1;
break;
case 0:
g_IsRun=2;
break;
default:
Error=72;
break;
}
break;
case 'X':
if(strlen((char *)buf)==0){Error=72;break;}
if(bU_Code||bX_Code){Error=72;break;}
G72_Pos_X=BfFromStr(buf);
if(g_Sysparam.DiaOrRadius)G72_Pos_X=G72_Pos_X/2;
bX_Code=TRUE;
break;
case 'Z':
if(strlen((char *)buf)==0){Error=72;break;}
if(bW_Code||bZ_Code){Error=72;break;}
G72_Pos_Z=BfFromStr(buf);
bZ_Code=TRUE;
break;
case 'U':
if(strlen((char *)buf)==0){Error=72;break;}
if(bX_Code||bU_Code){Error=72;break;}
G72_Pos_U=BfFromStr(buf);
if(g_Sysparam.DiaOrRadius)G72_Pos_U=G72_Pos_U/2;
bU_Code =TRUE;
break;
case 'W':
if(strlen((char *)buf)==0){Error=72;break;}
if(bZ_Code||bW_Code){Error=72;break;}
G72_Pos_W=BfFromStr(buf);
bW_Code = 1;
break;
case 'I':
if(strlen((char *)buf)==0){Error=72;break;}
if(bI_Code){Error=72;break;}
G72_Pos_I=BfFromStr(buf);
bI_Code = TRUE;
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -