i.c

nubers曲线的计算方法,在DSP6711DSK板上调试成功.

            double *Ddy)
           {
             double dt;
             double dxup;
             double dyup;
             double ddown;
             double ddxup;
             double ddyup;
             double dddown;
             dt=1/(U[i+4]-U[i+3]);
             dxup=3*B[i][3]*t*t*dt+2*B[i][2]*t*dt+B[i][1]*dt;
             dyup=3*B[i][7]*t*t*dt+2*B[i][6]*t*dt+B[i][5]*dt;
             ddown=3*B[i][11]*t*t*dt+2*B[i][10]*t*dt+B[i][9]*dt;
             ddxup=6*B[i][3]*t*dt*dt+2*B[i][2]*dt*dt;
             ddyup=6*B[i][7]*t*dt*dt+2*B[i][6]*dt*dt;
             dddown=6*B[i][11]*t*dt*dt+2*B[i][10]*dt*dt;
             *Dx=(dxup*down1-xup1*ddown)/(down1*down1);
             *Dy=(dyup*down1-yup1*ddown)/(down1*down1);
             *Ddx=((ddxup*down1-xup1*dddown)*down1-2*(dxup*down1-xup1*ddown)*ddown)/(down1*down1*down1);
             *Ddy=((ddyup*down1-yup1*dddown)*down1-2*(dyup*down1-yup1*ddown)*ddown)/(down1*down1*down1);
            }

/*计算插补点曲率*/
void curvature(double *kc,
               double dx,
               double dy,
               double ddx,
               double ddy)
            { 
             *kc=sqrt((dx*ddy-dy*ddx)*(dx*ddy-dy*ddx))/sqrt((dx*dx+dy*dy)*(dx*dx+dy*dy)*(dx*dx+dy*dy));
             }
   
   
/*计算插补点曲率半径*/
void curvature_radius(double *rc,
                      double kc)
                    {
                      *rc=1/kc;
                     }

/*计算插补点自适应Feedrate*/
void Feedrate(double rc,
              double *V)
            { 
             double Vad;
             Vad=2*sqrt(rc*rc-(rc-hm)*(rc-hm))/T;
             if(Vad>F)
                Vad=F;
             else
                Vad=Vad;
                *V=Vad;
              }

/*弓高误差*/
void chord_error(double *hr,
                 double rc,
                 double Vad)
               {
                *hr=rc-sqrt(rc*rc-(Vad*T/2)*(Vad*T/2)); 
                }
    
/*计算下一插补点参数ui*/
void para_ui(double *u,
             double u1,
             double Vad,
             double dx1,
             double dy1,
             double ddx1,
             double ddy1)
           {
             *u=u1+Vad*T/sqrt((dx1*dx1)+(dy1*dy1))-(Vad*T)*(Vad*T)*(dx1*ddx1+dy1*ddy1)/(2*((dx1*dx1)+(dy1*dy1))*((dx1*dx1)+(dy1*dy1)));
            }
   
   
/*判断最后一段加减速情况并得到最终分段数目及参数*/   
void Last_seg_judge(int seg_num,
                    int *seg_j)
  {
   int idec;
   double Ldec;
   for (idec=0;idec<seg_num;idec++)
       {
         Ldec=(seg[seg_num-1].feedratestr+seg[seg_num-1].feedrateend)*sqrt(abs(seg[seg_num-1].feedratestr+seg[seg_num-1].feedrateend)/J);
         if (Ldec<=seg[seg_num-1].seg_L)
             break;
         else
            { 
              seg[seg_num-2].feedrateend=seg[seg_num-1].feedrateend;
              seg[seg_num-2].seg_L=seg[seg_num-1].seg_L+seg[seg_num-2].seg_L;
              seg[seg_num-2].seg_ui=seg[seg_num-1].seg_ui;
              seg_num=seg_num-1;
             }
         }
       *seg_j=seg_num;
   }

/*根据参数值判断每一段曲线加减速度类型的极限值*/     
void seg_limit_value(int i)
  {
    seg_lim.Lr1=2*seg[i].feedratestr*T;
    seg_lim.Lr2=(seg[i].feedratestr+seg[i].feedrateend)*sqrt(abs(seg[i].feedratestr-seg[i].feedrateend)/J);
    seg_lim.Lr3=(seg[i].feedratestr+seg[i].feedratemax)*sqrt((seg[i].feedratemax-seg[i].feedratestr)/J)+(seg[i].feedrateend+seg[i].feedratemax)*sqrt((seg[i].feedratemax-seg[i].feedrateend)/J);
    seg_lim.Lr4=(seg[i].feedratestr+seg[i].feedratemax)*sqrt((seg[i].feedratemax-seg[i].feedratestr)/J);
    seg_lim.Lr5=(seg[i].feedrateend+seg[i].feedratemax)*sqrt((seg[i].feedratemax-seg[i].feedrateend)/J);
    seg_lim.Lr6=2*seg[i].feedrateend*T;
   }
   
/*第七种情况*/
void S_th7style(int i)
  {
    if (seg[i].seg_L>=seg_lim.Lr3&&seg[i].feedrateend!=seg[i].feedratemax&&seg[i].feedratestr!=seg[i].feedratemax)
       {
        adtime.Tstr=sqrt((seg[i].feedratemax-seg[i].feedratestr)/J);
        adtime.Tend=sqrt((seg[i].feedratemax-seg[i].feedrateend)/J);
        adtime.Tc=(seg[i].seg_L-(seg[i].feedratestr+seg[i].feedratemax)*(adtime.Tstr)-(seg[i].feedrateend+seg[i].feedratemax)*(adtime.Tend))/seg[i].feedratemax;
        }
  }


 /*第6种情况*/
void S_th6style(int i)
    {
      if (seg[i].seg_L<seg_lim.Lr3&&seg[i].seg_L>=seg_lim.Lr2&&seg[i].feedratestr!=seg[i].feedratemax&&seg[i].feedrateend!=seg[i].feedratemax)
        {  
         int js,l,k;
         double xx,eps,y1[2],d,p,x0,x1;
         eps=0.00001;
         js=60;
         xx=0.01;
         if(seg[i].feedratestr==seg[i].feedrateend)
         {
           y1[0]=xx*(xx*xx+2*seg[i].feedratestr/J)-seg[i].seg_L/(2*J);
           y1[1]=3*xx*xx+2*seg[i].feedratestr/J;
           }
         if(seg[i].feedratestr!=seg[i].feedrateend)
         {
           y1[0]=xx*(xx*(xx*(xx+2*seg[i].seg_L/(seg[i].feedrateend-seg[i].feedratestr))-(seg[i].feedrateend-seg[i].feedratestr)/J)+4*seg[i].feedratestr*seg[i].seg_L/(J*(seg[i].feedrateend-seg[i].feedratestr)))+((seg[i].feedratestr+seg[i].feedrateend)*(seg[i].feedratestr+seg[i].feedrateend)*(seg[i].feedratestr-seg[i].feedrateend)/J-seg[i].seg_L*seg[i].seg_L)/(J*(seg[i].feedrateend-seg[i].feedratestr));
           y1[1]=4*xx*xx*xx+6*seg[i].seg_L/(seg[i].feedrateend-seg[i].feedratestr)*xx*xx-2*xx*(seg[i].feedrateend-seg[i].feedratestr)/J+4*seg[i].feedratestr*seg[i].seg_L/(J*(seg[i].feedrateend-seg[i].feedratestr));
           }
         l=js;
         x0=xx;
         d=eps+1.0;
         while((d>=eps)&&(1!=0))
           {
             if(fabs(y1[1])+1.0==1.0)
               {
                printf("err\n");
                }
             x1=x0-y1[0]/y1[1];
             xx=x1;
             if(seg[i].feedratestr==seg[i].feedrateend)
              {
               y1[0]=xx*(xx*xx+2*seg[i].feedratestr/J)-seg[i].seg_L/(2*J);
               y1[1]=3*xx*xx+2*seg[i].feedratestr/J;
               }
             if(seg[i].feedratestr!=seg[i].feedrateend)
               {
                y1[0]=xx*(xx*(xx*(xx+2*seg[i].seg_L/(seg[i].feedrateend-seg[i].feedratestr))-(seg[i].feedrateend-seg[i].feedratestr)/J)+4*seg[i].feedratestr*seg[i].seg_L/(J*(seg[i].feedrateend-seg[i].feedratestr)))+((seg[i].feedratestr+seg[i].feedrateend)*(seg[i].feedratestr+seg[i].feedrateend)*(seg[i].feedratestr-seg[i].feedrateend)/J-seg[i].seg_L*seg[i].seg_L)/(J*(seg[i].feedrateend-seg[i].feedratestr));
                y1[1]=4*xx*xx*xx+6*seg[i].seg_L/(seg[i].feedrateend-seg[i].feedratestr)*xx*xx-2*xx*(seg[i].feedrateend-seg[i].feedratestr)/J+4*seg[i].feedratestr*seg[i].seg_L/(J*(seg[i].feedrateend-seg[i].feedratestr));
                }
             d=fabs(x1-x0);
             p=fabs(y1[0]);
             if(p>d)
                d=p; 
             x0=x1;
             l=l-1;
            }
          k=js-l;
          if(k>=0)
          adtime.Tstr=xx;
          adtime.Tend=sqrt(adtime.Tstr*adtime.Tstr+(seg[i].feedratestr-seg[i].feedrateend)/J);
          adtime.Tc=0.0;
        }
      }
      
/*第五种情况*/ 
void S_th5style(int i)
    {
     if (seg[i].seg_L>=seg_lim.Lr5&&seg[i].feedrateend!=seg[i].feedratemax&&seg[i].feedratestr==seg[i].feedratemax)
       {
         adtime.Tend=sqrt((seg[i].feedratestr-seg[i].feedrateend)/J);
         adtime.Tc=(seg[i].seg_L-seg_lim.Lr5)/seg[i].feedratemax;
         adtime.Tstr=0.0;
        }
     }
     
/*第四种情况 */
void S_th4style(int i)
     {
     if (seg[i].seg_L>=seg_lim.Lr4&&seg[i].feedratestr!=seg[i].feedratemax&&seg[i].feedrateend==seg[i].feedratemax)
        { 
         adtime.Tstr=sqrt((seg[i].feedrateend-seg[i].feedratestr)/J);
         adtime.Tc=(seg[i].seg_L-seg_lim.Lr4)/seg[i].feedratemax;
         adtime.Tend=0.0;
         }
       }  
       
       
/*第3种情况*/
void S_th3style(int i)
    {

       	int js,l,k;
         double xx,eps,y1[2],d,p,x0,x1;
     if (seg[i].feedratestr>seg[i].feedrateend&&seg[i].seg_L<seg_lim.Lr2&&seg[i].seg_L<seg_lim.Lr5&&seg[i].seg_L>=seg_lim.Lr6)
        {  
         seg[i].feedratemax=seg[i].feedratestr;
         
         eps=0.00001;
         js=60;
         xx=0.01;
         y1[0]=xx*(xx*xx+2*seg[i].feedrateend/J)-seg[i].seg_L/J;
         y1[1]=3*xx*xx+2*seg[i].feedrateend/J;
         l=js;
         x0=xx;
         d=eps+1.0;
         while((d>=eps)&&(1!=0))
           {
             if(fabs(y1[1])+1.0==1.0)
               {
                printf("err\n");
                }
             x1=x0-y1[0]/y1[1];
             xx=x1;
             y1[0]=xx*(xx*xx+2*seg[i].feedrateend/J)-seg[i].seg_L/J;
             y1[1]=3*xx*xx+2*seg[i].feedrateend/J;
             d=fabs(x1-x0);
             p=fabs(y1[0]);
             if(p>d)
                d=p; 
             x0=x1;
             l=l-1;
            }
          k=js-l;
          if(k>=0)
          adtime.Tend=xx;
          adtime.Tstr=0.0;
          adtime.Tc=0.0;
        }
     }
     
/*第2种情况*/
void S_th2style(int i)
    {

	     int js,l,k;
         double xx,eps,y1[2],d,p,x0,x1;
     if (seg[i].feedrateend>seg[i].feedratestr&&seg[i].seg_L<seg_lim.Lr2&&seg[i].seg_L<seg_lim.Lr4&&seg[i].seg_L>=seg_lim.Lr1)
        {  
         seg[i].feedratemax=seg[i].feedrateend;
         
         eps=0.00001;
         js=60;
         xx=0.01;
         y1[0]=xx*(xx*xx+2*seg[i].feedratestr/J)-seg[i].seg_L/J;
         y1[1]=3*xx*xx+2*seg[i].feedratestr/J;
         l=js;
         x0=xx;
         d=eps+1.0;
         while((d>=eps)&&(1!=0))
           {
             if(fabs(y1[1])+1.0==1.0)
               {
                printf("err\n");
                }
             x1=x0-y1[0]/y1[1];
             xx=x1;
             y1[0]=xx*(xx*xx+2*seg[i].feedratestr/J)-seg[i].seg_L/J;
             y1[1]=3*xx*xx+2*seg[i].feedratestr/J;
             d=fabs(x1-x0);
             p=fabs(y1[0]);
             if(p>d)
                d=p; 
             x0=x1;
             l=l-1;
            }
          k=js-l;
          if(k>=0)
          adtime.Tstr=xx;
          adtime.Tend=0.0;
          adtime.Tc=0.0;
        }
     }


/*第1种情况*/
void S_th1style(int i)     
    { 
     if (seg[i].feedrateend==seg[i].feedratemax&&seg[i].feedratestr==seg[i].feedratemax)
        {
         adtime.Tc=seg[i].seg_L/seg[i].feedratestr;
         adtime.Tstr=0.0;
         adtime.Tend=0.0;
        }
     }
     
/*插补采样时间优化,并计算每段所需采样次数*/
void simpletime()
  {
   if (adtime.Tstr>0)
     simp.n1=((int)(adtime.Tstr/T)+1);
   else
     simp.n1=0;
   if (adtime.Tc>0)
     simp.n2=((int)(adtime.Tc/T)+1);
   else
     simp.n2=0;
   if (adtime.Tend>0)
     simp.n3=((int)(adtime.Tend/T)+1);
   else
     simp.n3=0;
   simp.n=2*simp.n1+simp.n2+2*simp.n3;
   }