?? processg72.c
字號:
break;
case 'K':
if(strlen((char *)buf)==0){Error=72;break;}
if(bK_Code){Error=72;break;}
G72_Pos_K=BfFromStr(buf);
bK_Code = TRUE;
break;
case 'R':
if(strlen((char *)buf)==0){Error=72;break;}
if(bR_Code){Error=72;break;}
G72_Pos_R=BfFromStr(buf);
bR_Code = TRUE;
break;
default:
break;
}
if (Error==0){
if(G72ProcessHangCode()==FALSE)Error=7;
if(G72_bEnd)Error=6;
G72ResetPara();
}
if (Error==72)break;
if (Error==6)break;
if (Error==7)break;
}
return Error;
}
BOOLEAN G72ProcessHangCode(void)
{
INT8U i;
if(iM_Code!=100 && iM_Code!=98&& iM_Code!=99)
{
switch(iM_Code)
{
case 3:case 4:case 5:case 8:case 9:case 10:case 11:
case 12:case 13:case 32:case 33:case 41:case 42:case 43:case 44:
DispErrCode(73);
return FALSE;
}
g_DelayTime=g_Sysparam.M_DelayTime/5;
if(WaitDelayEnd())return TRUE;
}
if(iM_Code==99 || iM_Code==98 || (iM_Code>=50 && iM_Code<=59))
{
DispErrCode(23);
return FALSE;
}
if(bX_Code||bZ_Code||bU_Code||bW_Code)
{
switch(iG_Code)
{
case 0:
LineShapNum++;
AllNum++;
if (AllNum!=1){
for (i=1;i<MAXNUMBER;i++){
if (CircleValue[i].Pos==(AllNum-1)){
LineValue[LineShapNum].StartX=CircleValue[i].EndX;
LineValue[LineShapNum].StartZ=CircleValue[i].EndZ;
break;
}
if (LineValue[i].Pos==(AllNum-1)){
LineValue[LineShapNum].StartX=LineValue[i].EndX;
LineValue[LineShapNum].StartZ=LineValue[i].EndZ;
break;
}
}
}
else{
if(bX_Code) G72_Abs_PosX=G72_Pos_X;
else if(bU_Code) G72_Abs_PosX=G72_Pos_U+Get_AbsPos(1);
else G72_Abs_PosX=Get_AbsPos(1);
if(bZ_Code) G72_Abs_PosZ=G72_Pos_Z;
else if(bW_Code) G72_Abs_PosZ=G72_Pos_W+Get_AbsPos(3);
else G72_Abs_PosZ=Get_AbsPos(3);
LineValue[LineShapNum].StartX=G72_Abs_PosX;
LineValue[LineShapNum].StartZ=G72_Abs_PosZ;
LineValue[LineShapNum].EndX=G72_Abs_PosX;
LineValue[LineShapNum].EndZ=G72_Abs_PosZ;
LineValue[LineShapNum].Pos=AllNum;
break;
}
if(bX_Code) G72_Abs_PosX=G72_Pos_X;
else if(bU_Code) G72_Abs_PosX=G72_Pos_U+LineValue[LineShapNum].StartX;
else G72_Abs_PosX=LineValue[LineShapNum].StartX;
if(bZ_Code) G72_Abs_PosZ=G72_Pos_Z;
else if(bW_Code) G72_Abs_PosZ=G72_Pos_W+LineValue[LineShapNum].StartZ;
else G72_Abs_PosZ=LineValue[LineShapNum].StartZ;
LineValue[LineShapNum].EndX=G72_Abs_PosX;
LineValue[LineShapNum].EndZ=G72_Abs_PosZ;
LineValue[LineShapNum].Pos=AllNum;
if (LineValue[LineShapNum].EndX!=LineValue[LineShapNum].StartX)
LineValue[LineShapNum].K=(LineValue[LineShapNum].EndZ-LineValue[LineShapNum].StartZ)/(LineValue[LineShapNum].EndX-LineValue[LineShapNum].StartX);
if (LineValue[LineShapNum].EndZ>LineValue[LineShapNum].StartZ)Zinc=1;
else if (LineValue[LineShapNum].EndZ<LineValue[LineShapNum].StartZ)Zdec=1;
if (Zdec==1&&Zinc==1){DispErrCode(24);return FALSE;}
if (LineValue[LineShapNum].EndX>LineValue[LineShapNum].StartX)Xinc=1;
else if (LineValue[LineShapNum].EndX<LineValue[LineShapNum].StartX)Xdec=1;
if (Xdec==1&&Xinc==1){DispErrCode(24);return FALSE;}
break;
case 1:
LineShapNum++;
AllNum++;
if (AllNum!=1){
for (i=1;i<MAXNUMBER;i++){
if (CircleValue[i].Pos==(AllNum-1)){
LineValue[LineShapNum].StartX=CircleValue[i].EndX;
LineValue[LineShapNum].StartZ=CircleValue[i].EndZ;
break;
}
if (LineValue[i].Pos==(AllNum-1)){
LineValue[LineShapNum].StartX=LineValue[i].EndX;
LineValue[LineShapNum].StartZ=LineValue[i].EndZ;
break;
}
}
}
else{
if(bX_Code) G72_Abs_PosX=G72_Pos_X;
else if(bU_Code) G72_Abs_PosX=G72_Pos_U+Get_AbsPos(1);
else G72_Abs_PosX=Get_AbsPos(1);
if(bZ_Code) G72_Abs_PosZ=G72_Pos_Z;
else if(bW_Code) G72_Abs_PosZ=G72_Pos_W+Get_AbsPos(3);
else G72_Abs_PosZ=Get_AbsPos(3);
LineValue[LineShapNum].StartX=G72_Abs_PosX;
LineValue[LineShapNum].StartZ=G72_Abs_PosZ;
LineValue[LineShapNum].EndX=G72_Abs_PosX;
LineValue[LineShapNum].EndZ=G72_Abs_PosZ;
LineValue[LineShapNum].Pos=AllNum;
break;
}
if(bX_Code) G72_Abs_PosX=G72_Pos_X;
else if(bU_Code) G72_Abs_PosX=G72_Pos_U+LineValue[LineShapNum].StartX;
else G72_Abs_PosX=LineValue[LineShapNum].StartX;
if(bZ_Code) G72_Abs_PosZ=G72_Pos_Z;
else if(bW_Code) G72_Abs_PosZ=G72_Pos_W+LineValue[LineShapNum].StartZ;
else G72_Abs_PosZ=LineValue[LineShapNum].StartZ;
LineValue[LineShapNum].EndX=G72_Abs_PosX;
LineValue[LineShapNum].EndZ=G72_Abs_PosZ;
LineValue[LineShapNum].Pos=AllNum;
if (LineValue[LineShapNum].EndX!=LineValue[LineShapNum].StartX)
LineValue[LineShapNum].K=(LineValue[LineShapNum].EndZ-LineValue[LineShapNum].StartZ)/(LineValue[LineShapNum].EndX-LineValue[LineShapNum].StartX);
if (LineValue[LineShapNum].EndZ>LineValue[LineShapNum].StartZ)Zinc=1;
else if (LineValue[LineShapNum].EndZ<LineValue[LineShapNum].StartZ)Zdec=1;
if (Zdec==1&&Zinc==1){DispErrCode(24);return FALSE;}
if (LineValue[LineShapNum].EndX>LineValue[LineShapNum].StartX)Xinc=1;
else if (LineValue[LineShapNum].EndX<LineValue[LineShapNum].StartX)Xdec=1;
if (Xdec==1&&Xinc==1){DispErrCode(24);return FALSE;}
break;
case 2:
case 3:
CircleShapNum++;
AllNum++;
if (AllNum!=1){
for (i=1;i<MAXNUMBER;i++){
if (CircleValue[i].Pos==(AllNum-1)){
CircleValue[CircleShapNum].StartX=CircleValue[i].EndX;
CircleValue[CircleShapNum].StartZ=CircleValue[i].EndZ;
break;
}
if (LineValue[i].Pos==(AllNum-1)){
CircleValue[CircleShapNum].StartX=LineValue[i].EndX;
CircleValue[CircleShapNum].StartZ=LineValue[i].EndZ;
break;
}
}
}
else{
DispErrCode(22);
return FALSE;
}
if(bX_Code) G72_Abs_PosX=G72_Pos_X;
else if(bU_Code) G72_Abs_PosX=G72_Pos_U+CircleValue[CircleShapNum].StartX;
else G72_Abs_PosX=CircleValue[CircleShapNum].StartX;
if(bZ_Code) G72_Abs_PosZ=G72_Pos_Z;
else if(bW_Code) G72_Abs_PosZ=G72_Pos_W+CircleValue[CircleShapNum].StartZ;
else G72_Abs_PosZ=CircleValue[CircleShapNum].StartZ;
if(bR_Code){
if(G72_Pos_R==0){DispErrCode(25);return FALSE;}
if(iG_Code==3)G72_Pos_R=-G72_Pos_R;
if(GetCircleCenter(G72_Abs_PosX-CircleValue[CircleShapNum].StartX,G72_Abs_PosZ-CircleValue[CircleShapNum].StartZ,G72_Pos_R,&G72_Abs_PosI,&G72_Abs_PosK)){DispErrCode(25);return FALSE;}
G72_Abs_PosI+=CircleValue[CircleShapNum].StartX;
G72_Abs_PosK+=CircleValue[CircleShapNum].StartZ;
}
else
{
if(bI_Code) G72_Abs_PosI=G72_Pos_I+CircleValue[CircleShapNum].StartX;
else G72_Abs_PosI=CircleValue[CircleShapNum].StartX;
if(bK_Code) G72_Abs_PosK=G72_Pos_K+CircleValue[CircleShapNum].StartZ;
else G72_Abs_PosK=CircleValue[CircleShapNum].StartZ;
}
CircleValue[CircleShapNum].EndX=G72_Abs_PosX;
CircleValue[CircleShapNum].EndZ=G72_Abs_PosZ;
CircleValue[CircleShapNum].I=G72_Abs_PosI;
CircleValue[CircleShapNum].K=G72_Abs_PosK;
CircleValue[CircleShapNum].R=G72_Pos_R;
CircleValue[CircleShapNum].Pos=AllNum;
CircleValue[CircleShapNum].Cw=iG_Code;
if (CircleValue[CircleShapNum].EndZ>CircleValue[CircleShapNum].StartZ)Zinc=1;
else if (CircleValue[CircleShapNum].EndZ<CircleValue[CircleShapNum].StartZ)Zdec=1;
if (Zdec==1&&Zinc==1){DispErrCode(24);return FALSE;}
if (CircleValue[CircleShapNum].EndX>CircleValue[CircleShapNum].StartX)Xinc=1;
else if (CircleValue[CircleShapNum].EndX<CircleValue[CircleShapNum].StartX)Xdec=1;
if (Xdec==1&&Xinc==1){DispErrCode(24);return FALSE;}
break;
default:
DispErrCode(37);return FALSE;
break;
}
}
return TRUE;
}
FP32 G72ConstructionFunc(FP32 Zpos){
INT8U i=0,Num;
BOOLEAN b_Line;
BOOLEAN b_Circle;
b_Circle=FALSE;b_Line=FALSE;
if(bit)
{
for (i=1;i<=AllNum;i++)
{
if(Zpos>=LineValue[i].EndZ && Zpos<=LineValue[i].StartZ)
{
Num=i;b_Line=TRUE;break;
}
if(Zpos>=CircleValue[i].EndZ && Zpos<=CircleValue[i].StartZ)
{
Num=i;b_Circle=TRUE;break;
}
}
}else
{
for (i=1;i<=AllNum;i++)
{
if(Zpos<=LineValue[i].EndZ && Zpos>=LineValue[i].StartZ)
{
Num=i;b_Line=TRUE;break;
}
if(Zpos<=CircleValue[i].EndZ && Zpos>=CircleValue[i].StartZ)
{
Num=i;b_Circle=TRUE;break;
}
}
}
if(b_Line)
{
if(LineValue[Num].K==0) return LineValue[Num].EndX;
return (Zpos-LineValue[Num].StartZ)/LineValue[Num].K+LineValue[Num].StartX;
}
if(b_Circle)
{
if(CircleValue[Num].Cw==3 && !bit)
{
return sqrt(CircleValue[Num].R*CircleValue[Num].R-(Zpos-CircleValue[Num].K)*(Zpos-CircleValue[Num].K))+CircleValue[Num].I;
}
if(CircleValue[Num].Cw==2 && !bit)
{
return -sqrt(CircleValue[Num].R*CircleValue[Num].R-(Zpos-CircleValue[Num].K)*(Zpos-CircleValue[Num].K))+CircleValue[Num].I;
}
if(CircleValue[Num].Cw==3 && bit)
{
return -sqrt(CircleValue[Num].R*CircleValue[Num].R-(Zpos-CircleValue[Num].K)*(Zpos-CircleValue[Num].K))+CircleValue[Num].I;
}
if(CircleValue[Num].Cw==2 && bit)
{
return sqrt(CircleValue[Num].R*CircleValue[Num].R-(Zpos-CircleValue[Num].K)*(Zpos-CircleValue[Num].K))+CircleValue[Num].I;
}
}
DispErrCode(38);
return 0; //add by zxj at 2006.10.23
}
INT32U G72FindStartHang(INT32U Hang){
INT32U i;
INT8U j,k,Err;
INT8U buf[10];
INT32U FindHang;
i=g_DataBuf.ProgAdd[g_Usrparam.CurrentFileNum];
while(1){
if (g_DataBuf.DataBuf[i]=='N'){
j=0;i++;Err=0;
while(g_DataBuf.DataBuf[i]>='0'&&g_DataBuf.DataBuf[i]<='9'){
buf[j]=g_DataBuf.DataBuf[i];
j++;i++;
if (i>=g_DataBuf.DataLength)return 0xfffff;
if (j>6){Err=1;break;}
}
if (Err==0){
k=0;FindHang=0;
while(j){
FindHang=(buf[k]-'0')+FindHang*10;
k++;
j--;
}
if (Hang==FindHang){
return i;
}
}
else{;}
}
else{
i++;
if (i>=g_DataBuf.DataLength)return 0xfffff;
if (g_DataBuf.DataBuf[i]=='%'){
return 0xfffff;
}
}
}
}
void G72ResetPara(void){
bX_Code=0;
bZ_Code=0;
bU_Code=0;
bW_Code=0;
bI_Code=0;
bK_Code=0;
bR_Code=0;
bM_Code=0;
bG_Code=0;
bN_Code=0;
iM_Code=0;
iG_Code=0;
G72_Pos_U=0;
G72_Pos_W=0;
G72_Pos_X=0;
G72_Pos_Z=0;
G72_Pos_I=0;
G72_Pos_K=0;
G72_Pos_R=0;
G72.ZaddPerTime=0;
G72.Zremain=0;
G72.Xremain=0;
G72.BackValue=0;
G72.StartHang=0;
G72.EndHang=0;
}
/*
BOOLEAN G72Motion(void){
INT8U j,k,ret;
BOOLEAN bCircle,bLine;
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);
while(1){
g_MoveMode=1;
G01_AbsPos(LineValue[1].StartX,LineValue[1].StartZ);
ret=WaitMotionEnd();
if(ret==255)return TRUE;
if(ret==0)break;
}
bCircle=0;bLine=0;
for (j=2;j<=AllNum;j++){
for (k=1;k<MAXNUMBER;k++){
if (CircleValue[k].Pos==j){
bCircle=1;
break;
}
if (LineValue[k].Pos==j){
bLine=1;
break;
}
}
if (bLine){
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);
while(1){
g_MoveMode=1;
G01_AbsPos(LineValue[k].EndX,LineValue[k].EndZ);
ret=WaitMotionEnd();
if(ret==255)return TRUE;
if(ret==0)break;
}
}
if (bCircle){
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);
while(1){
if(CircleValue[CircleShapNum].Cw==2)
G02_AbsPos(CircleValue[k].EndX,CircleValue[k].EndZ,CircleValue[k].I,CircleValue[k].K);
else
G03_AbsPos(CircleValue[k].EndX,CircleValue[k].EndZ,CircleValue[k].I,CircleValue[k].K);
g_MoveMode=1;
ret=WaitMotionEnd();
if(ret==255)return TRUE;//停止運行
if(ret==0)break;//運行結(jié)束
}
}
bCircle=0;bLine=0;
}
return TRUE;
}
*/?? 快捷鍵說明
復(fù)制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -