tjuzhen.h

《51系列单片机高级实例开发指南》一书的can总线的PC端的源代码

#include "matlab.hpp"
int tsuccess=1;//判断是否超范围,超范围为0
#define pi 3.14159
mwArray cross(mwArray PP, mwArray NN)
{
	double P1,P2,P3; 
    P1=PP(2,1)*NN(3,1)-PP(3,1)*NN(2,1);
    P2=PP(3,1)*NN(1,1)-PP(1,1)*NN(3,1);
    P3=PP(1,1)*NN(2,1)-PP(2,1)*NN(1,1);
    double PN1[]={P1,P2,P3};
    mwArray PN(3,1,PN1);
    return PN;

}
//计算角度，速度，角加速度的函数
mwArray Tjuzhen(double xx, double yy, double zz)
{   
	int i;
	char tbuf[80];
	double a2=940;
    double a3=1638;
    double a4=158;
	double PP []={2,0,0,0,0,0};//目标速度
    mwArray P(6,1,PP);
	double PA1 []={20,0,0,0,0,0};//目标加速度
    mwArray PA(6,1,PA1);
	double ww[]={0,0,0};
	mwArray w(3,1,ww);//初始化w,wa,v,a
	mwArray wa(3,1,ww);
	mwArray v(3,1,ww);
	mwArray a(3,1,ww);
	mwArray w1,wa1,v1,a1;
	double TB6 [] ={1,0,0,0,0,1,0,0,0,0,1,0,1796+xx,yy+1e-025,940+zz,1};
	double r1,r2,r3,r4,r5,r6,px,py,pz,p1,p2,c3,T11,T12,T13,T21,T22,T23,T32;
	r1=r2=r3=r4=r5=r6=0;
	mwArray T6(4,4,TB6);
    cout<<"T6="<<endl;
    cout<<T6<<endl;
    px=T6(1,4);
    py=T6(2,4);   
    pz=T6(3,4);
    for(i=1;i<16;i++)//迭代15次
	{
	//---------r1求解-------------//
      r1=atan2(py,px);
	//-------判断r1的范围---------//
	if ((r1<-pi/3)|(r1>pi/6))
	{  r1=atan2(-py,-px);
	   if ((r1<-pi/3)|(r1>pi/6))
	   { tsuccess=0;
	    sprintf(tbuf,"r1 is out of range!!");
		AfxMessageBox(tbuf);
	   break;
	   }
	}
	//---------r3的求解------------//
	p1=px*cos(r1)+py*sin(r1);
    p2=-pz;
	c3=(p1*p1+p2*p2-a3*a3-a2*a2)/(2*a2*a3);
    r3=atan2(sqrt(1-c3*c3),c3);
    //--------判断r3的范围---------//
    if ((r3<(5*pi/18))|(r3>(7*pi/9)))
	{  r3=atan2(-sqrt(1-c3*c3),-c3);
       if ((r3<(5*pi/18))|(r3>(7*pi/9)))
	   {tsuccess=0;
	    sprintf(tbuf,"r3 is out of range!!");
		AfxMessageBox(tbuf);
		break;
	   }
	}
    //---------r2的求解------------//
    r2=atan2(p2*(a3*cos(r3)+a2)-a3*sin(r3)*p1,p1*(a3*cos(r3)+a2)+a3*sin(r3)*p2);
    //--------判断r2的范围---------//
    if ((r2<(-13*pi/18))|(r2>(-2*pi/9)))
	{  r2=atan2(-p2*(a3*cos(r3)+a2)+a3*sin(r3)*p1,-p1*(a3*cos(r3)+a2)-a3*sin(r3)*p2);
       if ((r2<(-13*pi/18))|(r2>(-2*pi/9)))
	   { //tsuccess=0;
         sprintf(tbuf,"r2 is out of range!!");
		 AfxMessageBox(tbuf);
		 break;
	   }
	}
	//-----求A1,A2,A3矩阵----------//
    double AB1 [] ={cos(r1),sin(r1),0,0,0,0,-1,0,-sin(r1),cos(r1),0,0,0,0,0,1};
	mwArray A1(4,4,AB1);
    double AB2 [] ={cos(r2),sin(r2),0,0,-sin(r2),cos(r2),0,0,0,0,1,0,940*cos(r2),940*sin(r2),0,1};
    mwArray A2(4,4,AB2);
	double AB3 [] ={cos(r3),sin(r3),0,0,0,0,1,0,sin(r3),-cos(r3),0,0,1638*cos(r3),1638*sin(r3),0,1};
	mwArray A3(4,4,AB3);
	mwArray A=A1*A2*A3;
    mwArray T66=mldivide(A,T6);
    T11=T66(1,1);
    T12=T66(1,2);
    T13=T66(1,3);
    T21=T66(2,1);
    T22=T66(2,2);
    T23=T66(2,3);
    T32=T66(3,2);
	//---------r4求解-------------//
    r4=atan2(T23,T13);
	//--------判断r2的范围---------//
	if ((r4<(-7*pi/12))|(r4>(7*pi/12)))
	{  r4=atan2(-T23,-T13);
       if ((r4<(-7*pi/12))|(r4>(7*pi/12)))
	   {  tsuccess=0;
	      sprintf(tbuf,"r4 is out of range!!");
		  AfxMessageBox(tbuf);
		  break;
	   }
	}
    //---------r5求解-------------//
    r5=atan2(-T32,T12*cos(r4)+T22*sin(r4));
	//--------判断r5的范围---------//
    if ((r5<(-7*pi/12))|(r5>(7*pi/12)))
	{  r5=atan2(T32,-T12*cos(r4)-T22*sin(r4));
       if ((r5<(-7*pi/12))|(r5>(7*pi/12)))
	   {  tsuccess=0;
	      sprintf(tbuf,"r5 is out of range!!");
	      AfxMessageBox(tbuf);
		  break;
	   }
	}
    //---------r6求解-------------//
    r6=atan2(-T11*sin(r4)+T21*cos(r4),-T12*sin (r4)+T22*cos(r4));
    //--------判断r6的范围---------//
	if ((r6<(-7*pi/9))|(r6>(7*pi/9)))
	{  r6=atan2(T11*sin(r4)-T21*cos(r4),T12*sin(r4)-T22*cos(r4));
       if ((r6<(-7*pi/9))|(r6>(7*pi/9)))
	   {  tsuccess=0;
	      sprintf(tbuf,"r6 is out of range!!");
		  AfxMessageBox(tbuf);
		  break;
	   }
	}
    //--------求A4,A5,A6---------//
	double AB4 [] ={cos(r4),sin(r4),0,0,0,0,-1,0,-sin(r4),cos(r4),0,0,158*cos(r4),158*sin(r4),0,1};
	double AB5 [] ={cos(r5),sin(r5),0,0,0,0,1,0,sin(r5),-cos(r5),0,0,0,0,0,1};
	double AB6 [] ={cos(r6),sin(r6),0,0,-sin(r6),cos(r6),0,0,0,0,1,0,0,0,0,1};
	mwArray A4(4,4,AB4);
	mwArray A5(4,4,AB5);
	mwArray A6(4,4,AB6);
	mwArray B=A4*A5*A6;
	mwArray T62=A*B; 
	T66=T6/B;
    px=T66(1,4);
    py=T66(2,4);
    pz=T66(3,4);
	}//循环结束
	//------------- 求速度----------------------//
    if (tsuccess)
	{
   
    double RB[]={r1,r2+pi/2,r3-pi/2,r4,r5,r6};
	mwArray Rr(6,1,RB);	
	double AB1 [] ={cos(r1),sin(r1),0,0,0,0,-1,0,-sin(r1),cos(r1),0,0,0,0,0,1};
	mwArray A1(4,4,AB1);
    double AB2 [] ={cos(r2),sin(r2),0,0,-sin(r2),cos(r2),0,0,0,0,1,0,940*cos(r2),940*sin(r2),0,1};
    mwArray A2(4,4,AB2);
	double AB3 [] ={cos(r3),sin(r3),0,0,0,0,1,0,sin(r3),-cos(r3),0,0,1638*cos(r3),1638*sin(r3),0,1};
	mwArray A3(4,4,AB3);
	double AB4 [] ={cos(r4),sin(r4),0,0,0,0,-1,0,-sin(r4),cos(r4),0,0,158*cos(r4),158*sin(r4),0,1};
	double AB5 [] ={cos(r5),sin(r5),0,0,0,0,1,0,sin(r5),-cos(r5),0,0,0,0,0,1};
	double AB6 [] ={cos(r6),sin(r6),0,0,-sin(r6),cos(r6),0,0,0,0,1,0,0,0,0,1};
	mwArray A4(4,4,AB4);
	mwArray A5(4,4,AB5);
	mwArray A6(4,4,AB6);
    mwArray TT=A1*A2*A3*A4*A5*A6;
	mwArray T6_5=A6*A5;
    mwArray T6_4=A6*A5*A4;
    mwArray T6_3=A6*A5*A4*A3;
    mwArray T6_2=A6*A5*A4*A3*A2;
    mwArray T6_1=A6*A5*A4*A3*A2*A1;
	//---求JL1,JA1---------------------//
	mwArray PP1=T6_1(horzcat(1,2,3),4); 
	mwArray NN1=T6_1(horzcat(1,2,3),1);
	mwArray OO1=T6_1(horzcat(1,2,3),2);
	mwArray AA1=T6_1(horzcat(1,2,3),3);//PP1为P矢量
	mwArray PN1=cross(PP1,NN1);
	mwArray PO1=cross(PP1,OO1);
	mwArray PA1=cross(PP1,AA1);//PN1为叉积
    double JL11 []={PN1(3,1),PO1(3,1),PA1(3,1)};
	mwArray JL1(3,1,JL11);
    double JA11 []={T6_1(3,1),T6_1(3,2),T6_1(3,3)};
    mwArray JA1(3,1,JA11);
	//---求JL2,JA2---------------------//
	mwArray PP2=T6_2(horzcat(1,2,3),4);
	mwArray NN2=T6_2(horzcat(1,2,3),1);
	mwArray OO2=T6_2(horzcat(1,2,3),2);
    mwArray AA2=T6_2(horzcat(1,2,3),3);//PP1为P矢量
	mwArray PN2=cross(PP2,NN2);
	mwArray PO2=cross(PP2,OO2);
	mwArray PA2=cross(PP2,AA2);//PN1为叉积
	double JL22 []={PN2(3,1),PO2(3,1),PA2(3,1)};
    mwArray JL2(3,1,JL22);
    double JA22 []={T6_2(3,1),T6_2(3,2),T6_2(3,3)};
	mwArray JA2(3,1,JA22);
	//---求JL3,JA3---------------------//
	mwArray PP3=T6_3(horzcat(1,2,3),4);
	mwArray NN3=T6_3(horzcat(1,2,3),1);
	mwArray OO3=T6_3(horzcat(1,2,3),2);
	mwArray AA3=T6_3(horzcat(1,2,3),3);//PP3为P矢量
	mwArray PN3=cross(PP3,NN3);
	mwArray PO3=cross(PP3,OO3);
	mwArray PA3=cross(PP3,AA3);//PN3为叉积
	double JL33 []={PN3(3,1),PO3(3,1),PA3(3,1)};
    mwArray JL3(3,1,JL33);
    double JA33 []={T6_3(3,1),T6_3(3,2),T6_3(3,3)};
	mwArray JA3(3,1,JA33);
    //---求JL4,JA4---------------------//
	mwArray PP4=T6_4(horzcat(1,2,3),4);
	mwArray NN4=T6_4(horzcat(1,2,3),1);
	mwArray OO4=T6_4(horzcat(1,2,3),2);
	mwArray AA4=T6_4(horzcat(1,2,3),3);//PP4为P矢量
	mwArray PN4=cross(PP4,NN4);
	mwArray PO4=cross(PP4,OO4);
	mwArray PA4=cross(PP4,AA4);//PN4为叉积
	double JL44 []={PN4(3,1),PO4(3,1),PA4(3,1)};
    mwArray JL4(3,1,JL44);
    double JA44 []={T6_4(3,1),T6_4(3,2),T6_4(3,3)};
	mwArray JA4(3,1,JA44);
	//---求JL5,JA5---------------------//
	mwArray PP5=T6_5(horzcat(1,2,3),4);
	mwArray NN5=T6_5(horzcat(1,2,3),1);
	mwArray OO5=T6_5(horzcat(1,2,3),2);
	mwArray AA5=T6_5(horzcat(1,2,3),3);//PP5为P矢量
	mwArray PN5=cross(PP5,NN5);
	mwArray PO5=cross(PP5,OO5);
	mwArray PA5=cross(PP5,AA5);//PN5为叉积
    double JL55 []={PN5(3,1),PO5(3,1),PA5(3,1)};
    mwArray JL5(3,1,JL55);
    double JA55 []={T6_5(3,1),T6_5(3,2),T6_5(3,3)};
	mwArray JA5(3,1,JA55);
	//---求JL6,JA6---------------------//
	double JL66 []={0,0,0};
    mwArray JL6(3,1,JL66);
    double JA66 []={0,0,1};
	mwArray JA6(3,1,JA66);
    mwArray J=horzcat(vertcat(JL1,JA1),vertcat(JL2,JA2),vertcat(JL3,JA3),
		              vertcat(JL4,JA4),vertcat(JL5,JA5),vertcat(JL6,JA6));
	mwArray Q=mldivide(J,P);//速度矩阵
    //-----------------求加速度------------------//
	mwArray R1=A1(horzcat(1,2,3),horzcat(1,2,3));
	mwArray R2=A2(horzcat(1,2,3),horzcat(1,2,3));
	mwArray R3=A3(horzcat(1,2,3),horzcat(1,2,3));
	mwArray R4=A4(horzcat(1,2,3),horzcat(1,2,3));
	mwArray R5=A5(horzcat(1,2,3),horzcat(1,2,3));
	mwArray R6=A6(horzcat(1,2,3),horzcat(1,2,3));
	mwArray Z1=R1*JA6;//JA6为[0;0;1]
    mwArray Z2=R1*R2*JA6;
    mwArray Z3=R1*R2*R3*JA6;
    mwArray Z4=R1*R2*R3*R4*JA6;
    mwArray Z5=R1*R2*R3*R4*R5*JA6;
    mwArray Z6=R1*R2*R3*R4*R5*R6*JA6;
	static double DW1[]={0,0,0,1};
	mwArray DW(4,1,DW1);
	mwArray X1=A1*DW;
    mwArray X2=(A1*A2-A1)*DW;
    mwArray X3=(A1*A2*A3-A1*A2)*DW;
    mwArray X4=(A1*A2*A3*A4-A1*A2*A3)*DW;
    mwArray X5=(A1*A2*A3*A4*A5-A1*A2*A3*A4)*DW;
    mwArray X6=(A1*A2*A3*A4*A5*A6-A1*A2*A3*A4*A5)*DW;
	mwArray RE1=X1(vertcat(1,2,3));
	mwArray RE2=X2(vertcat(1,2,3));
	mwArray RE3=X3(vertcat(1,2,3));
	mwArray RE4=X4(vertcat(1,2,3));
	mwArray RE5=X5(vertcat(1,2,3));
	mwArray RE6=X6(vertcat(1,2,3));
	mwArray Z=horzcat(Z1,Z2,Z3,Z4,Z5,Z6);
	mwArray RE=horzcat(RE1,RE2,RE3,RE4,RE5,RE6);
	for(i=1;i<7;i++)
	{
		w1=w+Q(i,1)*Z(horzcat(1,2,3),i);
		if(i==1) wa1=wa;
		else wa1=wa+cross(w,Q(i-1,1)*Z(horzcat(1,2,3),i));//wa为角加速度 
        v1=v+cross(w1,RE(horzcat(1,2,3),i));
        a1=a+cross(wa1,RE(horzcat(1,2,3),i))+cross(w1,cross(w1,RE(horzcat(1,2,3),i)));//a1为加速度
        w=w1;wa=wa1;v=v1;a=a1;
	}
    double P2P[]={a1(1,1),a1(2,1),a1(3,1),wa1(1,1),wa1(2,1),wa1(3,1)};
	mwArray P2(6,1,P2P);
	mwArray QA=mldivide(J,(PA-P2));//加速度矩阵
	mwArray HUANHUI=horzcat(Rr,Q,QA);
	return HUANHUI;
    }
	else
	{   mwArray w=zeros(6,3);
		return w;
	}
}