linetoline.c

数控机床刀具半径补偿 直线接直线 C程序源码

/*直线接直线*/
#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);/*输出补偿转接点坐标*/
  }
}