//说明此程序仅供参考，有许多EXTERN函数及变量支持
double vmax,t,tm;
double vi,a,J,v1,v2;
double L=1;///
double Sup,G01_vs;
double xi,yi,zi;
double Ip_Data[3],vc;//三轴要插补的距离
double inc_l;
if(Ip_Data[0]!=0||Ip_Data[1]!=0||Ip_Data[2]!=0)
{
tm=T;//插补周期
// first S_curve
if((go_xy1[0]&0x07)==0)
{
vi=G01_vs+J*t*t/2; //up_velocity,a(mm/s),t(s),vi(mm/s)
t=t+T;//acc_velocity
time
inc_l=sqrt(inc[0]*inc[0]+inc[1]*inc[1]+inc[2]*inc[2]);
down_s1(vi,J);
//first rising distance is added,max_axis
value is subed with inc
if((vi>=v1)||(go_xy2[2]>Sup)||(vi>=vc))
{go_xy1[4]=go_xy1[4]&0xfffc;//normal
if(vi>=vc) //arrive at velocity gived
{
go_xy1[0]=(go_xy1[0]|0x03)&0xffe3;//go
to forth ,set first
vi=vc;
t=tm-sqrt(2.0*vi/J);
inc_l=sqrt(inc[0]*inc[0]+inc[1]*inc[1]+inc[2]*inc[2]);
down_s11(tm,vi,G01_vs,J);
}
else if(vi>=v1) //arrive at first velocity
{
go_xy1[0]=(go_xy1[0]|0x01)&0xffe1;
v1=vi;
tm=sqrt(2.0*vi/J);
t=0;
// down_s1(vi,J);//?
}
if(go_xy2[2]>=Sup)
{
go_xy1[0]=(go_xy1[0]|0x06)&0xffe6;
v1=vi;
tm=sqrt(2.0*vi/J);
t=0;
// down_s1(vi,J);
}
if(vc>(J*tm*tm/2.0))
v2=vc-J*tm*tm/2.0;
else
v2=vc;
}
// go_xy2[1]=go_xy2[1]+inc_l;//count rising
distance
go_xy2[0]=go_xy2[0]-inc_l;
} //up
// second S_curve
else if((go_xy1[0]&0x07)==0x01)
{
t=t+T;//line acc_velocity time
vi=v1+J*tm*t; //up_velocity,a(mm/s),t(s),vi(mm/s)
inc_l=sqrt(inc[0]*inc[0]+inc[1]*inc[1]+inc[2]*inc[2]);
down_s1(v1,J);
down_s2(tm,vi,v1, J);
//second rising distance is added,max_axis
value is subed with inc
if((vi>=v2)||((go_xy2[2]+inc_l)>Sup)||(vi>=vc))
{
go_xy1[4]=go_xy1[4]&0xfffc;//normal
if(vi>=vc) //arrive at velocity gived
{go_xy1[0]=(go_xy1[0]|0x0b)&0xffeb;//go
to forth,set second curve
vi=vc;
v2=vc;//?
inc_l=sqrt(inc[0]*inc[0]+inc[1]*inc[1]+inc[2]*inc[2]);
t=0;
down_s1(v1,J);
down_s2(tm,v2,v1, J);
}
else if(vi>=v2) //arrive at first velocity
{go_xy1[0]=(go_xy1[0]&0xfffa)|0x0a;//continue
,go to third
//? v2=vi;
vi=v2;
if((vc-v2)<0)
v2=vc;
t=tm-sqrt(2*(vc-v2)/J);
inc_l=sqrt(inc[0]*inc[0]+inc[1]*inc[1]+inc[2]*inc[2]);
down_s1(v1,J);
down_s2(tm,v2,v1, J);
}
if((go_xy2[2]+inc_l)>Sup)
{go_xy1[0]=(go_xy1[0]|0x05)&D1_0;//go
to sixth
vi=v1+J*tm*(t-T);//backward 1 step
v2=vi;
down_s1(v1,J);
down_s2(tm,v2,v1, J);
t=0;
inc_l=sqrt(inc[0]*inc[0]+inc[1]*inc[1]+inc[2]*inc[2]);
}
}
// go_xy2[1]=go_xy2[1]+inc_l;//count rising
distance
go_xy2[0]=go_xy2[0]-inc_l;
}
// third S_curve
else if((go_xy1[0]&0x07)==0x02)
{
t=t+T;//line acc_velocity time
if(t>=tm)
vi=vc;
else
vi=vc-J*(tm-t)*(tm-t)/2; //up_velocity,a(mm/s),t(s),vi(mm/s)
inc_l=sqrt(inc[0]*inc[0]+inc[1]*inc[1]+inc[2]*inc[2]);
down_s1(v1,J);
down_s2(tm,v2,v1, J);
down_s3(vi,v2,J);
//third rising distance is added,max_axis
value is subed with inc
if(((go_xy2[2]+inc_l)>Sup)||(vi>=vc))
{
if(vi>=vc) //arrive at velocity gived
{go_xy1[0]=(go_xy1[0]|0x13)&0xfff3;//go
to forth ,set third
vi=vc;
vmax=vc;
t=0;
inc_l=sqrt(inc[0]*inc[0]+inc[1]*inc[1]+inc[2]*inc[2]);
down_s1(v1,J);
down_s2(tm,v2,v1, J);
down_s3(vmax,v2,J);
}
if((go_xy2[2]+inc_l)>Sup)
{go_xy1[0]=(go_xy1[0]|0x13)&0xfff3;//go
to forth(011)
vmax=vi;
t=0;
}
go_xy1[4]=go_xy1[4]&0xfffc;//normal
}
// go_xy2[1]=go_xy2[1]+inc_l;//count rising
distance
go_xy2[0]=go_xy2[0]-inc_l;
}
// forth S_curve
else if((go_xy1[0]&0x07)==0x03)
{
inc_l=sqrt(inc[0]*inc[0]+inc[1]*inc[1]+inc[2]*inc[2]);
//high velocity machining
if((pause_flag&0x01)==0)
{
if((*(IO_data+IO_out_led+4)&D1_1)==D1_1)
{go_xy2[0]=go_xy2[2];
pause_flag=pause_flag|0x01;
t=0;
}
}
go_xy2[0]=go_xy2[0]-inc_l;
if((go_xy2[0]-go_xy2[2])<inc_l) //decelerate
distance is larger than Sup
{gotodown();
vmax=vi;
go_xy1[4]=go_xy1[4]&0xfffc;
}
if((go_xy1[4]&0x03)!=0)
{
if((go_xy1[4]&0x03)==0x01)//up speed
{if(go_xy2[0]>(go_xy2[2]+vc*8*T))
{Sup=(go_xy2[0]-go_xy2[2])/2.0;
go_xy2[1]=0;//clear up distance
if((go_xy1[0]&0x18)==0) //first
{if((vi-G01_vs)<0)
vi=G01_vs;
t=sqrt(2.0*(vi-G01_vs)/J);
go_xy1[0]=go_xy1[0]&0xffe0;//go to first
}
else if((go_xy1[0]&0x18)==0x08)//second
{go_xy1[0]=(go_xy1[0]|0x09)&0xffe9;//go
to second
tm=T;
t=(vi-v1)/J/tm;
v2=vc-J*tm*tm/2;
}
else
{go_xy1[0]=(go_xy1[0]|0x12)&0xfff2;//go
to third
if((vc-vi)<0)
vc=vi;
t=tm-sqrt((vc-vi)*2.0/J);
}
}
}//up speed
else //down speed
gotodown();
}
}
//fifth(go down)
else if((go_xy1[0]&0x07)==0x04)
{
t=t+T;//dec_velocity time
vi=vmax-J*t*t/2;
//down_velocity,a(mm/s),t(s),vi(mm/s)
if((vc<=v2||((go_xy1[4]&0x03)==0))&&vi<=v2)
{go_xy1[0]=(go_xy1[0]&0xffed)|0x0d;//go
to sixth
vi=v2;t=0;
}
else if(vc>v2&&vi<=vc&&((go_xy1[4]&0x03)!=0))//vc>v2
{vi=vc;
vmax=vc;
go_xy1[0]=(go_xy1[0]|0x13)&0xfff3;//go
to forth
go_xy1[4]=go_xy1[4]&0xfffc;//normal
t=0;
down_s1(v1,J);
down_s2(tm,v2,v1, J);
down_s3(vmax,v2,J);
}
inc_l=sqrt(inc[0]*inc[0]+inc[1]*inc[1]+inc[2]*inc[2]);
go_xy2[0]=go_xy2[0]-inc_l;
}
//sixth go down
else if((go_xy1[0]&0x07)==0x05)
{
t=t+T;//dec_velocity time
vi=v2-J*tm*t; //down_velocity,a(mm/s),t(s),vi(mm/s)
if((vc<=v1||((go_xy1[4]&0x03)==0))&&(vi<=v1))
{vi=v1;
t=tm-sqrt(2.0*v1/J);
go_xy1[0]=(go_xy1[0]&0xfff6)|0x16;//go
to seventh
}
else if(vc>v1&&vi<=vc&&((go_xy1[4]&0x03)!=0))
//vc>v1
{ vi=vc;
down_s1(v1,J);
down_s2(tm,vi,v1,
J);
go_xy1[0]=(go_xy1[0]|0x0b)&0xffeb;//go
to forth
go_xy1[4]=go_xy1[4]&0xfffc;//normal
}
// dis_sub_inc();
inc_l=sqrt(inc[0]*inc[0]+inc[1]*inc[1]+inc[2]*inc[2]);
go_xy2[0]=go_xy2[0]-inc_l;
//judge go_xy2[0]=0 ?
if((go_xy2[0]-inc_l)<=0)
{
t=0;
}
}
//seventh go down
else if((go_xy1[0]&0x07)==0x06)
{
t=t+T;//dec_velocity time
vi=J*(tm-t)*(tm-t)/2;
//down_velocity,a(mm/s),t(s),vi(mm/s)
inc_l=sqrt(inc[0]*inc[0]+inc[1]*inc[1]+inc[2]*inc[2]);
//declining distance is subed with inc
if((go_xy2[0]-inc_l)<=0)
{
go_xy1[1]=go_xy1[1]&0xfcff;//s=0,set
flag(0),stop interpolation
flag(0)
t=0;
}
else if(((vc<=G01_vs)||((go_xy1[4]&0x03)==0))&&(vi<=G01_vs))
//
{vi=G01_vs;
vc=vi;
go_xy2[1]=0;
t=0;
inc_l=sqrt(inc[0]*inc[0]+inc[1]*inc[1]+inc[2]*inc[2]);
if((go_xy2[0]-inc_l)<=0)
{go_xy1[1]=go_xy1[1]&0xfcff;//s=0,set
flag(0),stop interpolation
flag(0)
StartStop=StartStop&0xfe;
}
else //s>inc
{//proccess remain distance
if((go_xy2[0]-inc_l)<=inc_l) //s-inc<=inc
go_xy1[1]=go_xy1[1]|0x200;//s<inc,set
flag(1)
else
go_xy1[1]=go_xy1[1]&0xfdff;//s-inc>inc
go_xy1[0]=go_xy1[0]|0x07;//low velocity
}
t=0;
}//s>inc
else if((vc>G01_vs)&&(vi<=vc)&&((go_xy1[4]&0x03)!=0))
//
{vi=vc;
go_xy1[0]=(go_xy1[0]|0x03)&0xffe3;//go
to forth,first step
go_xy1[4]=go_xy1[4]&0xfffc;//normal
down_s1(vi,J);
}
inc_l=sqrt(inc[0]*inc[0]+inc[1]*inc[1]+inc[2]*inc[2]);
go_xy2[0]=go_xy2[0]-inc_l;
}
//low speed
else if((go_xy1[0]&0x07)==0x07)
{
inc_l=sqrt(inc[0]*inc[0]+inc[1]*inc[1]+inc[2]*inc[2]);
if((go_xy1[1]&0x200)==0x200)//s<inc
{if((pause_flag&0x01)==0)//
go_xy1[1]=go_xy1[1]&0xfcff;//s=0,set
flag(0),stop interpolation flag(0)
StartStop=StartStop&0xfe;
}
else //s>inc
{
go_xy2[0]=go_xy2[0]-inc_l;
if(go_xy2[0]<=inc_l)
go_xy1[1]=go_xy1[1]|0x200;//s<inc,set
flag(1)
else
go_xy1[1]=go_xy1[1]&0xfdff;//s>inc
}
if((pause_flag&0x01)==0)
{
if((*(IO_data+IO_out_led+4)&D1_1)==D1_1)
{go_xy1[1]=go_xy1[1]|0x200;//s<inc,set
flag(1)
pause_flag=pause_flag|0x01;
t=0;
}
}
if(((go_xy1[4]&0x03)!=0x0)&&(vc>G01_vs))//up
speed
{if(go_xy2[0]>(vc*8*T))
{Sup=go_xy2[0]/2.0;
t=0;
go_xy1[0]=go_xy1[0]&0xffe0;//go to first
}
else
vi=G01_vs;
go_xy1[4]=go_xy1[4]&0xfffc;//normal
}//up speed
else if(vc<=G01_vs)
{vi=vc;
go_xy1[4]=go_xy1[4]&0xfffc;//normal
}
}//low velocity
}
else
{inc[0]=0;inc[1]=0;inc[2]=0;
go_xy1[1]=go_xy1[1]&D8_0;
StartStop=StartStop&0xfe;
}
if((go_xy1[1]&D8_1)!=D8_1)
{if((pause_flag&0x01)==1)
{residue_XYZ(xi,yi,zi);
go_xy1[5]=go_xy1[5]&D10_0;//Data ready
}
else if((*(IO_data+IO_out_led+4)&D1_1)==D1_1)
{pause_flag=pause_flag|0x02;
go_xy1[5]=go_xy1[5]&D10_0;//Data ready
}
} |
|
