?? i.c
字號:
#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,
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -