i.c

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

#include<stdio.h>
#include<math.h>
#include "c6211dsk.h"
#include <c6x.h>
void muan();
void parati(double *t,double u,int *iut);
void XYvalue(double t,int i,double *xup1,double *yup1,double *down1,double *x1,double *y1);
void distance( double *Lp,double x1,double x2,double y1,double y2);
void distentire(double *L,double L1,double Lp);
void XYdiff(int i,double t,double xup1,double yup1,double down1,double *Dx,double *Dy,double *Ddx,double *Ddy);                
void curvature(double *kc,double dx1,double dy1,double ddx1,double ddy1);
void curvature_radius(double *rc,double kc);
void Feedrate(double rc,double *V);
void chord_error(double *hr,double rc,double Vad);
void para_ui(double *u,double u1,double Vad,double dx1,double dy1,double ddx1,double ddy1);
void Last_seg_judge(int seg_num,int *seg_j);
void seg_limit_value(int i);
void S_th7style(int i);
void S_th6style(int i);
void S_th5style(int i);
void S_th4style(int i);
void S_th3style(int i);
void S_th2style(int i);
void S_th1style(int i);
void simpletime();

double W[] = {1,2,2,1,1,1,1,1,1,2,2,1};
double U[] = {0,0,0,0,0.0629,0.1801,0.2998,0.4398,0.5602,0.7002,0.8199,0.9371,1.0000,1.0000,1.0000,1.0000};
double X[] = {0 ,0,6,4,1,5,11,15,12,10,16,16};
double Y[] = {1,0,0,2,11,16,16,11,2,0,0,1};
double T=0.001,hm=0.0005,J=30000,Am=2450,F=12*1000/60;/*插补参数*/
double V[3][3],M[4][4],B[20][12];

int K=3;/*curve degree*/ 
int W_num = (sizeof(W)/sizeof(double));       /*曲线权重个数=控制顶点数目*/

struct points
 {
   double time;
   double curvature;
   double para_ui;   
   double acc;
   double jerk;
   double x;
   double y;
  }point[2];

struct DIFF
   {
    double xup;
    double yup;
    double down; 
    double dx;
    double dy;
    double ddx;
    double ddy;
    }DIFF;
    
struct adaptivevalue
 {

   double para_ti;
   int i_ut;
   double chord_error;
   double curvature_radius;
   double dist_step;
   double dist_entire;    
   double feedrate;
   double Fmax;
   double seg_entire;
  }adap;

struct segment
 {
   double feedratemax;
   double feedrateend; 
   double feedratestr;
   double seg_ui;    
   double seg_L;
  }seg[10];
    
struct seg_limit
  {
    double Lr1;
    double Lr2;
    double Lr3;
    double Lr4;
    double Lr5;
    double Lr6;
   }seg_lim;

struct accdectime
  {
   double Tstr;
   double Tend;
   double Tc;
  }adtime;

struct simple_num
  {
   int n1;
   int n2;
   int n3;
   int n;
   }simp;


void main(void)
  {
   

    double k_seg=2*hm/(hm*hm+F*F*(T*0.5)*(T*0.5));
    double temporary_curvature;
    int s_n_f;
    int i=0,j=0;
    point[1].para_ui=0.0;
    adap.dist_step=0.0;
    adap.Fmax=0.0;
    adap.seg_entire=0.0;
    seg[0].feedratestr=0.0;
    adap.dist_entire=0.0;
    point[0].curvature=0.0;

	muan();

  do
    {
     parati(&adap.para_ti,point[1].para_ui,&adap.i_ut);
     point[0].x=point[1].x;
     point[0].y=point[1].y;
     XYvalue(adap.para_ti,adap.i_ut,&DIFF.xup,&DIFF.yup,&DIFF.down,&point[1].x,&point[1].y);
     XYdiff(adap.i_ut,adap.para_ti,DIFF.xup,DIFF.yup,DIFF.down,&DIFF.dx,&DIFF.dy,&DIFF.ddx,&DIFF.ddy);
     temporary_curvature=point[0].curvature;
     point[0].curvature=point[1].curvature;
     curvature(&point[1].curvature,DIFF.dx,DIFF.dy,DIFF.ddx,DIFF.ddy);
     /*判断分段点以及记录各段需要的参数*/
     if ((point[0].curvature>k_seg)&&(temporary_curvature<point[0].curvature)&&(point[0].curvature>point[1].curvature))
        {
          seg[j].feedrateend=adap.feedrate;
          seg[j].seg_ui=point[0].para_ui;
          seg[j].seg_L=adap.dist_entire-adap.seg_entire;
          adap.seg_entire=adap.dist_entire;
          seg[j].feedratemax=adap.Fmax;
          adap.Fmax=0.0;
          /*printf("ui=%f;fend=%f ;fstr=%f ;s_L=%f ;fmax=%f\n",seg[j].seg_ui,seg[j].feedrateend,seg[j].feedratestr,seg[j].seg_L,seg[j].feedratemax); */    
          j++;
          seg[j].feedratestr=seg[j-1].feedrateend;
        }     
     curvature_radius(&adap.curvature_radius,point[1].curvature);
     Feedrate(adap.curvature_radius,&adap.feedrate);
     /*找出最大速度*/
     if (adap.Fmax<adap.feedrate)
       {
         adap.Fmax=adap.feedrate;
        }
     chord_error(&adap.chord_error,adap.curvature_radius,adap.feedrate);
     if(i>=1)
      {
        distance(&adap.dist_step,point[0].x,point[1].x,point[0].y,point[1].y);
        distentire(&adap.dist_entire, adap.dist_entire, adap.dist_step);
      }
     printf("ui=%f;\n",point[1].para_ui);
     printf("adap.dist_step=%f ;alldistance=%f ;ui=%f ;\n",adap.dist_step,adap.dist_entire,point[1].para_ui);  
     point[0].para_ui=point[1].para_ui;
     para_ui(&point[1].para_ui,point[0].para_ui,adap.feedrate,DIFF.dx,DIFF.dy,DIFF.ddx,DIFF.ddy);
    
      /*判断结束点并计算各相应参数*/
     if (point[1].para_ui>=1)
      {
       i++;
       point[1].para_ui=1.0;
       point[1].curvature=point[1].curvature;
       adap.curvature_radius=adap.curvature_radius;
       adap.chord_error=adap.chord_error;
       point[0].x=point[1].x;
       point[0].y=point[1].y;
       point[1].x=X[W_num-1];
       point[1].y=Y[W_num-1];
       distance(&adap.dist_step,point[0].x,point[1].x,point[0].y,point[1].y);
       distentire(&adap.dist_entire, adap.dist_entire, adap.dist_step); 
       seg[j].seg_L=adap.dist_entire-adap.seg_entire;
       seg[j].feedrateend=0;
       seg[j].feedratemax=adap.Fmax;
       seg[j].seg_ui=1.0;  
       printf("ui=%f;fend=%f ;fstr=%f ;s_L=%f ;fmax=%f\n",seg[j].seg_ui,seg[j].feedrateend,seg[j].feedratestr,seg[j].seg_L,seg[j].feedratemax);     
       j++;   
       printf("ui=%f;\n",point[1].para_ui);
       printf("adap.dist_step=%f ;alldistance=%f ;ui=%f ;\n",adap.dist_step,adap.dist_entire,point[1].para_ui);                  
       }
     i++;
	 }while(point[1].para_ui<1.0);
	 printf("j=%d; k_seg=%f\n",j,k_seg);
	 printf("x0=%f ;y0=%f ;x1=%f ;y1=%f ",point[0].x,point[0].y,point[1].x,point[1].y);
	 Last_seg_judge(j,&s_n_f);
	 printf("s_n_f=%d\n",s_n_f);
	 
	 for (j=0;j<s_n_f;j++)
	 {
	  printf("fend=%f ;fstr=%f ;s_L=%f ;fmax=%f\n",seg[j].feedrateend,seg[j].feedratestr,seg[j].seg_L,seg[j].feedratemax);    
      seg_limit_value(j);
      S_th7style(j);
      S_th6style(j);
      S_th5style(j);
      S_th4style(j);
      S_th3style(j);
      S_th2style(j);
      S_th1style(j);
      simpletime();
      printf("ui=%f;fend=%f ;fstr=%f ;s_L=%f ;fmax=%f\n",seg[j].seg_ui,seg[j].feedrateend,seg[j].feedratestr,seg[j].seg_L,seg[j].feedratemax); 
      printf("Tstr=%f ;Tc=%f ;Tend=%f;\n",adtime.Tstr,adtime.Tc,adtime.Tend);
      printf("n1=%d ;n2=%d ;n3=%d;n=%d\n",simp.n1,simp.n2,simp.n3,simp.n);
      }
  
	while(1);
  }
 

 
 
/*预处理中矩阵形式系数求解*/ 
void muan()
{
    int ii,jj,l1,aa;
    for( ii=0;ii<(W_num-K);ii++)
    {
     jj=ii+K;
     V[0][0]=U[jj+1]-U[jj];
     V[0][1]=U[jj+2]-U[jj];
     V[0][2]=U[jj+3]-U[jj];
     V[1][0]=U[jj]-U[jj-1];
     V[1][1]=U[jj+1]-U[jj-1];
     V[1][2]=U[jj+2]-U[jj-1];
     V[2][2]=U[jj+1]-U[jj-2];
     V[2][0]=0;
     V[2][1]=0;
     M[0][0]=-(V[0][0]*V[0][0])/(V[1][1]*V[2][2]);
     M[0][3]=V[0][0]*V[0][0]/(V[0][1]*V[0][2]);
     M[1][2]=3*V[0][0]*V[0][0]/(V[1][2]*V[1][1]);
     M[2][2]=3*V[0][0]*V[1][0]/(V[1][1]*V[1][2]);
     M[3][2]=V[1][0]*V[1][0]/(V[1][1]*V[1][2]);
     M[0][2]=-(M[1][2]/3+M[0][3]+V[0][0]*V[0][0]/(V[0][1]*V[1][2]));
     M[0][1]=-(M[0][0]+M[0][2]+M[0][3]);
     M[1][0]=-3*M[0][0];
     M[2][0]=3*M[0][0];
     M[3][0]=-M[0][0];
     M[1][1]=3*M[0][0]-M[1][2];
     M[2][1]=-3*M[0][0]-M[2][2];
     M[3][1]=1+M[0][0]-M[3][2];
     M[1][3]=0;
     M[2][3]=0;
     M[3][3]=0;
     for(l1=0;l1<4;l1++)
       {
        aa=K-l1;
        B[ii][l1]=M[aa][0]*W[jj-3]*X[jj-3]+M[aa][1]*W[jj-2]*X[jj-2]+M[aa][2]*W[jj-1]*X[jj-1]+M[aa][3]*W[jj]*X[jj];
        B[ii][l1+8]=M[aa][0]*W[jj-3]+M[aa][1]*W[jj-2]+M[aa][2]*W[jj-1]+M[aa][3]*W[jj];
        B[ii][l1+4]=M[aa][0]*W[jj-3]*Y[jj-3]+M[aa][1]*W[jj-2]*Y[jj-2]+M[aa][2]*W[jj-1]*Y[jj-1]+M[aa][3]*W[jj]*Y[jj];
       }	   	   
     }
     return;	 
 }
 
 
/*求u对应变量t,及U中的位置*/
void parati(double *t,
            double u,
            int *iut)
       {
         int i;
         for(i=0;i<(W_num-3);i++)
           {
             if(u>=U[i+3]&&u<U[i+4])
              {
                *t=(u-U[i+3])/(U[i+4]-U[i+3]);
                *iut=i;
                break;
               }
             }
         }

/*求曲线上点坐标*/
void XYvalue(double t,
             int i,
             double *xup1,
             double *yup1,
             double *down1,
             double *x1,
             double *y1)
            {
               *xup1=B[i][3]*t*t*t+B[i][2]*t*t+B[i][1]*t+B[i][0];
               *yup1=B[i][7]*t*t*t+B[i][6]*t*t+B[i][5]*t+B[i][4];
               *down1=B[i][11]*t*t*t+B[i][10]*t*t+B[i][9]*t+B[i][8];
               *x1 = (*xup1)/(*down1);
               *y1 = (*yup1)/(*down1);
             }


/*求每步插补步长*/
void distance( double *Lp,
               double x1,
               double x2,
               double y1,
               double y2)
            {
             *Lp=sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1));
             }

/*求插补总长*/
void distentire(double *L,
                double L1,
                double Lp)
              {
                *L=L1+Lp;
               }

/*计算X，Y导数*/
void XYdiff(int i,
            double t,
            double xup1,
            double yup1,
            double down1,
            double *Dx,
            double *Dy,
            double *Ddx,