补偿算法

lingyufeng

补偿算法 [复制链接]

/*直线接直线*/ 

#include <stdio.h> 

#include <math.h> 

main() 

{ 

float r,x0,y0,x1,y1,x2,y2,xl1,yl1,xl2,yl2,xs1,ys1,xs2,ys2,xs3,ys3; 

int sng; 

char g1[3];/*字符数组存储半径补偿命令*/ 

printf("input r:"); 

scanf("%f",&r); 

printf("input G41 or G42:"); 

scanf("%s",g1); 

printf("input x0,y0,x1,y1,x2,y2 Separated by commas:"/*用逗号隔开*/); 

scanf("%f,%f,%f,%f,%f,%f",&x0,&y0,&x1,&y1,&x2,&y2); 

if(g1[2]=='2')/*判断刀补方向*/ 

r=-r; 

if(r>0) sng=1; 

else sng=-1; 

xl1=(x1-x0)/(sqrt(pow(x1-x0,2)+pow(y1-y0,2))); 

yl1=(y1-y0)/(sqrt(pow(x1-x0,2)+pow(y1-y0,2))); 

xl2=(x2-x1)/(sqrt(pow(x2-x1,2)+pow(y2-y1,2))); 

yl2=(y2-y1)/(sqrt(pow(x2-x1,2)+pow(y2-y1,2))); 

 

if((sng*(xl1*yl2-xl2*yl1))>=0)/*判断为缩短型*/ 

  { 

   printf("type of Reduction!\n");/*提示缩短型*/ 

                         /*刀补建立*/ 

                         /*xs1=x1-ryl2;*/ 

                         /*ys1=y1+rxl2;*/ 

                         /*刀补撤销*/ 

                         /*xs1=x1-ryl1;*/ 

                         /*ys1=y1+rxl1;*/ 

   /*刀补的进行*/ 

   if((xl1*yl2-xl2*yl1)==0)/*特殊情况两直线共线转接角为180°*/ 

    { 

     xs1=x1-r*yl1;  

     ys1=y1+r*xl1; 

     printf("xs1=%.2f\nys1=%.2f",xs1,ys1);/*输出补偿转接点坐标*/ 

    } 

   else 

    { 

     xs1=x1+((xl2-xl1)*r)/(xl1*yl2-xl2*yl1); 

     ys1=y1+((yl2-yl1)*r)/(xl1*yl2-xl2*yl1); 

     printf("xs1=%.2f\nys1=%.2f\n",xs1,ys1);/*输出补偿转接点坐标*/ 

    } 

  } 

else if(sng*((yl2*xl1-xl2*yl1))<0&&(yl2*yl1+xl2*xl1)>=0)/*判断为伸长型*/ 

  { 

   printf("type of Elongation!\n");/*提示伸长型*/ 

                         /*刀补建立*/ 

                         /*xs1=x1-r*yl1;*/ 

                         /*ys1=y1+r*xl1;*/ 

                         /*xs2=x1+((xl2-xl1)*r)/(xl1*yl2-xl2*yl1);*/ 

                         /*ys2=y1+((yl2-yl1)*r)/(xl1*yl2-xl2*yl1);*/ 

                         /*刀补撤销*/ 

                         /*xs1=x1+((xl2-xl1)*r)/(xl1*yl2-xl2*yl1);*/ 

                         /*ys1=y1+((yl2-yl1)*r)/(xl1*yl2-xl2*yl1);*/ 

                         /*xs2=x1-r*yl2;*/ 

                         /*ys2=y1+r*xl2;*/ 

   /*刀补进行*/ 

   xs1=x1+((xl2-xl1)*r)/(xl1*yl2-xl2*yl1); 

   ys1=y1+((yl2-yl1)*r)/(xl1*yl2-xl2*yl1); 

   printf("xs1=%.2f\nys1=%.2f\n",xs1,ys1);/*输出补偿转接点坐标*/ 

  } 

else if((sng*(yl2*xl1-xl2*yl1))<0&&(yl2*yl1+xl2*xl1)<0)/*判断为插入型*/ 

  { 

    

   printf("type of Insertion!\n");/*提示插入型*/ 

                          /*刀补建立*/ 

                          /*xs1=x1-r*yl1;*/ 

                          /*ys1=y1+r*xl1;*/ 

                          /*xs2=x1-r*yl1+abs(r)*xl1;*/ 

                          /*ys2=y1+r*xl1+abs(r)*yl1;*/ 

                          /*xs3=x1-r*yl2-abs(r)*xl2;*/ 

                          /*ys3=y1+r*xl2-abs(r)*yl2;*/ 

                          /*刀补撤销*/ 

                          /*xs1=x1-r*yl1+abs(r)*xl1;*/ 

                          /*ys1=y1+r*xl1+abs(r)*yl1;*/ 

                          /*xs2=x1-r*yl2-abs(r)*xl2;*/ 

                          /*ys2=y1+r*xl2-abs(r)*yl2;*/ 

                          /*xs3=x1-r*yl2;*/ 

                          /*ys3=y1+r*xl2;*/ 

   /*刀补进行*/ 

   xs1=x1-r*yl1+(abs(r)*xl1); 

   ys1=y1+r*xl1+(abs(r)*yl1); 

   xs2=x1-r*yl2-(abs(r)*xl2); 

   ys2=y1+r*xl2-(abs(r)*yl2); 

   printf("xs1=%.2f\nys1=%.2f\nxs2=%.2f\nys2=%.2f\n",xs1,ys1,xs2,ys2);/*输出补偿转接点坐标*/ 

  } 

}

eeleader

楼主，该补偿算法应用那个地方的？