HIL 14
Dependencies: mbed MatrixMath Matrix
Diff: main.cpp
- Revision:
- 0:19aa346c5a6a
diff -r 000000000000 -r 19aa346c5a6a main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sun Apr 11 10:32:29 2021 +0000 @@ -0,0 +1,709 @@ +#include "mbed.h" +#include "Matrix.h" +#include "MatrixMath.h" + +CAN can1(PD_0, PD_1); // rd, td Transmitter + +//Ticker tick1; +//Ticker tick2; +//Ticker tick3; + + +const double Mn=9.8; //Nominalni moment +const double mi_x=1.2l; //Koeficijent trenja gume po x osi +const double mi_y=1.2l; //Koeficijent trenja gume po y osi +const double g=9.81; +const double m_sprung=1100; //sprung masa vozila +const double m_unsprung=110; //unsprung masa vozila +const double L=2.64; //Medjuosovinsko rastojanje +const double lf=1.35; //Rastojanje centra mase do prednje osovine +const double lr=L-lf; //Rastojanje centra mase do zadnje osovine +const double Wfl=1.458/2; //Rastojanje levog tocka od centra prednje osovine +const double Wfr=1.458/2; //Rastojanje desnog tocka od centra prednje osovine +const double Wrl=1.455/2; //Rastojanje levog tocka od centra zadnje osovine +const double Wrr=1.455/2; //Rastojanje desnog tocka od centra zadnje osovine +const double h=0.507; //Visina centra mase +const double Ixx=441; //Moment inercije oko x ose +const double Iyy=1748; //Moment inercije oko y ose +const double Izz=1945; //Moment inercije oko z ose +const double Ixy=0; +const double Iyz=0; +const double Ixz=0; +const double It=2; //Moment inercije tocka oko ose rotacije(uracunati i motor i planetarac) +const double rt=0.3; //poluprecnik pneumatika +const double Cv=1.14; //koeficijent otpora vazduha +const double Ap=1.23; //ref. ceona povrs +const double fo=0.011; //koef. kotrljanja staticki +const double ro=1.2; //gustina vazduha +const double kp=0.9; //Koeficijent efikasnosti planetarca PROVERITI +const double sw_ratio=12.0; //Prenosni odnos volana +const double N=12.3; //PROVERITI +const double Ts=0.002; + + +const double m[3]={m_sprung+m_unsprung, m_sprung+m_unsprung, m_sprung};// matrica mase + +const double I[3][3]={ + {Ixx, Ixy, Ixz}, + {Ixy, Iyy, Iyz}, + {Ixz, Iyz, Izz} + };// tenzor inercije + +double I_inv[3][3];// inverzija tenzora inercije + +const double Iw[4]={It, It, It, It};//momenti inercije tockova + +const double mi[2][4]={ + {1.2, 1.2, 1.3, 1.3}, // flx frx rlx rrx + {1.2, 1.2, 1.3, 1.3} //fly fry rly rry + };//koeficijenti trenja gume po x i y osi za sve tockove + +const double in[2][4]={ + {0.8, 0.8, 0.9, 0.9}, + {0.8, 0.8, 0.9, 0.9} + };// Coefficient of friction with infinite slip [no unit] + +const double C[2][4]={ + {70000, 70000, 100000, 100000}, + {70000, 70000, 100000, 100000} + };//Cornering stiffness [x - N; y - N/rad] + +const double r[4]={rt, rt, rt, rt};// Matrica poluprecnika tockova [rfl rfr rrl rrr] + +const double cs[4]={26125, 26125, 26125, 26125};//Spring effective stiffness [N/m] + +const double ca[2]={12000, 15000};// Antiroll bar stiffnes [N/m] + +const double cd[4]={2000, 2000, 2000, 2000};//Damper effective dampening [N*s/m] + +const double p_init[3]={0, 0, h};// inital position of CM in global fixed coord sys [m] + +const double v_init[3]={0, 0, 0};// inital velocity in body fixed coord sys [m/s] + +const double pa_init[3]={0, 0, 0};// Initial roll pitch and yaw [rad] + +const double va_init[3]={0, 0, 0};// initial roll pitch and yaw velocities [rad/s] + +const double pw_init[3][4]={ + {lf, lf, -lr, -lr}, //xfl xfr xrl xrr + {Wfl, -Wfr, Wrl, -Wrr}, //yfl yfr yrl yrr + {h, h, h, h} //zfl zfr zrl zrr + };// initial wheel positions in body fixed coord sys + +const double w_init[4]={v_init[0]/rt, v_init[0]/rt, v_init[0]/rt, v_init[0]/rt} ;// Initial wheel angular speed around spin axis [rad/s] + +const double mg_unsprung[4]={lr/(lr+lf)*Wfl/(Wfl+Wfr)*m_unsprung*g, lr/(lr+lf)*Wfr/(Wfl+Wfr)*m_unsprung*g, lf/(lr+lf)*Wrl/(Wrl+Wrr)*m_unsprung*g, lf/(lr+lf)*Wrr/(Wrl+Wrr)*m_unsprung*g};//Static weight distribution force [N] [Ffl Ffr Frl Frr] + +const double mg_sprung[4]={m_sprung/m_unsprung*mg_unsprung[0], m_sprung/m_unsprung*mg_unsprung[1], m_sprung/m_unsprung*mg_unsprung[2], m_sprung/m_unsprung*mg_unsprung[3]}; //Static weight distribution force on acting on springs [N] + +const double h_spring[4]={mg_sprung[0]/cs[0]+h, mg_sprung[1]/cs[1]+h, mg_sprung[2]/cs[2]+h, mg_sprung[3]/cs[3]+h}; + + +double p[3]; //position in global coordinate system +double v[3]; //velocity in CM fixed coordinate system +double pa[3]; //angular position - x and y are in vehicle local and z is in global coordinate system +double va[3]; //angular velocity +double pw[3][4]; //wheel contact patch position in CM fixed coordinate system +double vw[3][4]; //wheel contact patch speed in CM fixed coordinate system +double w[4]; //wheel angular speed +double Fs[4]; //spring force from each wheel +double Fa[4]; //antiroll bar force from each wheel +double Fd[4]; //damper force from each wheel +double Fi[3][4]; //force of each wheel in CM fixed coordinate system + +double FL[3]; + + + +double d=0; + + +double T_1=0, T_2=0, T_3=0, T_4=0; +double T_1p=0, T_2p=0, T_3p=0, T_4p=0; + +int cnt=0; +volatile bool flag=0; + + +int brojac=0; +const unsigned int RX_ID = 0x0184; +CANMessage messageOutVel1; +CANMessage messageOutVel2; +CANMessage messageOutVel3; +CANMessage messageOutVel4; + + + +Serial pc(USBTX, USBRX); + +void initialize(){ + Matrix I_mat(3,3); + I_mat<<Ixx<<Ixy<<Ixz + <<Ixy<<Iyy<<Iyz + <<Ixz<<Iyz<<Izz; + + Matrix I_mat_inv(3,3); + I_mat_inv=MatrixMath::Inv(I_mat); + + I_inv[0][0]=I_mat_inv(1,1); + I_inv[0][1]=I_mat_inv(1,2); + I_inv[0][2]=I_mat_inv(1,3); + + I_inv[1][0]=I_mat_inv(2,1); + I_inv[1][1]=I_mat_inv(2,2); + I_inv[1][2]=I_mat_inv(2,3); + + I_inv[2][0]=I_mat_inv(3,1); + I_inv[2][1]=I_mat_inv(3,2); + I_inv[2][2]=I_mat_inv(3,3); + + p[0]=p_init[0]; + p[1]=p_init[1]; + p[2]=p_init[2]; + + v[0]=v_init[0]; + v[1]=v_init[1]; + v[2]=v_init[2]; + + pa[0]=pa_init[0]; + pa[1]=pa_init[1]; + pa[2]=pa_init[2]; + + va[0]=va_init[0]; + va[1]=va_init[1]; + va[2]=va_init[2]; + + pw[0][0]=pw_init[0][0]; + pw[0][1]=pw_init[0][1]; + pw[0][2]=pw_init[0][2]; + pw[0][3]=pw_init[0][3]; + + pw[1][0]=pw_init[1][0]; + pw[1][1]=pw_init[1][1]; + pw[1][2]=pw_init[1][2]; + pw[1][3]=pw_init[1][3]; + + pw[2][0]=pw_init[2][0]; + pw[2][1]=pw_init[2][1]; + pw[2][2]=pw_init[2][2]; + pw[2][3]=pw_init[2][3]; + + w[0]=w_init[0]; + w[1]=w_init[1]; + w[2]=w_init[2]; + w[3]=w_init[3]; + + +} + + +void model(double Tfl, double Tfr, double Trl, double Trr, double Steering_angle){ + double d=Steering_angle/sw_ratio; + ////////////// + pw[2][0]=-(p[2]+pw[1][0]*sin(pa[0])-pw[0][0]*sin(pa[1])); + pw[2][1]=-(p[2]+pw[1][1]*sin(pa[0])-pw[0][1]*sin(pa[1])); + pw[2][2]=-(p[2]+pw[1][2]*sin(pa[0])-pw[0][2]*sin(pa[1])); + pw[2][3]=-(p[2]+pw[1][3]*sin(pa[0])-pw[0][3]*sin(pa[1])); + + //pc.printf("pw \n"); + //pc.printf("%f %f %f %f\n",pw[0][0],pw[0][1],pw[0][2],pw[0][3]); + //pc.printf("%f %f %f %f\n",pw[1][0],pw[1][1],pw[1][2],pw[1][3]); + //pc.printf("%f %f %f %f\n",pw[2][0],pw[2][1],pw[2][2],pw[2][3]); + //pc.printf("aaaaaaaa \n"); + ///////////// + vw[0][0]=v[0]-va[2]*pw[1][0]; + vw[0][1]=v[0]-va[2]*pw[1][1]; + vw[0][2]=v[0]-va[2]*pw[1][2]; + vw[0][3]=v[0]-va[2]*pw[1][3]; + + vw[1][0]=v[1]+va[2]*pw[0][0]; + vw[1][1]=v[1]+va[2]*pw[0][1]; + vw[1][2]=v[1]+va[2]*pw[0][2]; + vw[1][3]=v[1]+va[2]*pw[0][3]; + + vw[2][0]=v[2]+va[0]*pw[1][0]-va[1]*pw[0][0]; + vw[2][1]=v[2]+va[0]*pw[1][1]-va[1]*pw[0][1]; + vw[2][2]=v[2]+va[0]*pw[1][2]-va[1]*pw[0][2]; + vw[2][3]=v[2]+va[0]*pw[1][3]-va[1]*pw[0][3]; + + //pc.printf("vw \n"); + //pc.printf("%f %f %f %f\n",vw[0][0],vw[0][1],vw[0][2],vw[0][3]); + //pc.printf("%f %f %f %f\n",vw[1][0],vw[1][1],vw[1][2],vw[1][3]); + //pc.printf("%f %f %f %f\n",vw[2][0],vw[2][1],vw[2][2],vw[2][3]); + + //////////// + double cosin=cos(d); + double sinus=sin(d); + double p1 = cosin*vw[0][0]+sinus*vw[1][0]; + double p2 = cosin*vw[0][1]+sinus*vw[1][1]; + double p3 = -sinus*vw[0][0]+cosin*vw[1][0]; + double p4 = -sinus*vw[0][1]+cosin*vw[1][1]; + + vw[0][0]=p1; + vw[0][1]=p2; + vw[1][0]=p3; + vw[1][1]=p4; + //pc.printf("vw 2 put \n"); + //pc.printf("%f %f %f %f\n",vw[0][0],vw[0][1],vw[0][2],vw[0][3]); + //pc.printf("%f %f %f %f\n",vw[1][0],vw[1][1],vw[1][2],vw[1][3]); + //pc.printf("%f %f %f %f\n",vw[2][0],vw[2][1],vw[2][2],vw[2][3]); + + //////////// + double pw_dif[2]={0.5*(pw[2][0]-pw[2][1]),0.5*(pw[2][2]-pw[2][3])}; + + //pc.printf("pw_dif \n"); + //pc.printf("%f %f\n",pw_dif[0],pw_dif[1]); + /////////// + Fs[0]=cs[0]*(h_spring[0]+pw[2][0]); + Fs[1]=cs[1]*(h_spring[1]+pw[2][1]); + Fs[2]=cs[2]*(h_spring[2]+pw[2][2]); + Fs[3]=cs[3]*(h_spring[3]+pw[2][3]); + //pc.printf("Fs \n"); + //pc.printf("%.20f %.20f %.20f %.20f\n",Fs[0],Fs[1],Fs[2],Fs[3]); + /////////// + Fa[0]=ca[0]*pw_dif[0]*1+ca[1]*pw_dif[1]*0; + Fa[1]=ca[0]*pw_dif[0]*(-1)+ca[1]*pw_dif[1]*0; + Fa[2]=ca[0]*pw_dif[0]*0+ca[1]*pw_dif[1]*1; + Fa[3]=ca[0]*pw_dif[0]*0+ca[1]*pw_dif[1]*(-1); + //pc.printf("Fa \n"); + //pc.printf("%.20f %.20f %.20f %.20f\n",Fa[0],Fa[1],Fa[2],Fa[3]); + ////////// + Fd[0]=-cd[0]*vw[2][0]; + Fd[1]=-cd[1]*vw[2][1]; + Fd[2]=-cd[2]*vw[2][2]; + Fd[3]=-cd[3]*vw[2][3]; + + //pc.printf("Fd \n"); + //pc.printf("%.20f %.20f %.20f %.20f\n",Fd[0],Fd[1],Fd[2],Fd[3]); + ////////// + Fi[2][0]=Fs[0]+Fd[0]+Fa[0]-mg_sprung[0]; + Fi[2][1]=Fs[1]+Fd[1]+Fa[1]-mg_sprung[1]; + Fi[2][2]=Fs[2]+Fd[2]+Fa[2]-mg_sprung[2]; + Fi[2][3]=Fs[3]+Fd[3]+Fa[3]-mg_sprung[3]; + //pc.printf("mg_sprung \n"); + //pc.printf("%.20f %.20f %.20f %.20f\n",mg_sprung[0],mg_sprung[1],mg_sprung[2],mg_sprung[3]); + //pc.printf("Fi \n"); + //pc.printf("%f %f %f %f\n",Fi[0][0],Fi[0][1],Fi[0][2],Fi[0][3]); + //pc.printf("%f %f %f %f\n",Fi[1][0],Fi[1][1],Fi[1][2],Fi[1][3]); + //pc.printf("%.20f %.20f %.20f %.20f\n",Fi[2][0],Fi[2][1],Fi[2][2],Fi[2][3]); + ////////// + // if dealing with vx = 0, vy = 0 state, use these two lines + double sx[4]; + sx[0]=(w[0]*r[0]-vw[0][0])/(abs(vw[0][0])+0.5); + sx[1]=(w[1]*r[1]-vw[0][1])/(abs(vw[0][1])+0.5); + sx[2]=(w[2]*r[2]-vw[0][2])/(abs(vw[0][2])+0.5); + sx[3]=(w[3]*r[3]-vw[0][3])/(abs(vw[0][3])+0.5); + //pc.printf("sx \n"); + //pc.printf("%f %f %f %f\n",sx[0],sx[1],sx[2],sx[3]); + ////////// + double sy[4]; + sy[0]=atan2(vw[1][0],(abs(vw[0][0])+0.5)); + sy[1]=atan2(vw[1][1],(abs(vw[0][1])+0.5)); + sy[2]=atan2(vw[1][2],(abs(vw[0][2])+0.5)); + sy[3]=atan2(vw[1][3],(abs(vw[0][3])+0.5)); + //pc.printf("sy \n"); + //pc.printf("%f %f %f %f\n",sy[0],sy[1],sy[2],sy[3]); + ////////// + // if vx > e and e is larger than 0 use these + // sx = w.*r./vw(1, :) - 1; + // sy = atan2(vw(2, :), abs(vw(1, :))); + ////////// + double tempF[2][4]; + tempF[0][0]=Fs[0]+Fd[0]+Fa[0]+mg_unsprung[0]; + tempF[0][1]=Fs[1]+Fd[1]+Fa[1]+mg_unsprung[1]; + tempF[0][2]=Fs[2]+Fd[2]+Fa[2]+mg_unsprung[2]; + tempF[0][3]=Fs[3]+Fd[3]+Fa[3]+mg_unsprung[3]; + + tempF[1][0]=tempF[0][0]; + tempF[1][1]=tempF[0][1]; + tempF[1][2]=tempF[0][2]; + tempF[1][3]=tempF[0][3]; + + for (int i = 0; i < 2; i++){ + for (int j = 0; j < 4; j++){ + if(tempF[i][j]<1){ + tempF[i][j]=1; + } + } + } + + //pc.printf("tempF \n"); + //pc.printf("%f %f %f %f\n",tempF[0][0],tempF[0][1],tempF[0][2],tempF[0][3]); + //pc.printf("%f %f %f %f\n",tempF[1][0],tempF[1][1],tempF[1][2],tempF[1][3]); + //pc.printf("%f %f %f %f\n",tempF[2][0],tempF[2][1],tempF[2][2],tempF[2][3]); + + ////////// TM_easy_combined + double sy_pom[4]; + sy_pom[0]=sy[0]; + sy_pom[1]=-sy[1]; + sy_pom[2]=sy[2]; + sy_pom[3]=-sy[3]; + + double K[2][4]; //Isto sto i F_max + K[0][0]=mi[0][0]*tempF[0][0]; + K[0][1]=mi[0][1]*tempF[0][1]; + K[0][2]=mi[0][2]*tempF[0][2]; + K[0][3]=mi[0][3]*tempF[0][3]; + + K[1][0]=mi[1][0]*tempF[1][0]; + K[1][1]=mi[1][1]*tempF[1][1]; + K[1][2]=mi[1][2]*tempF[1][2]; + K[1][3]=mi[1][3]*tempF[1][3]; + + //pc.printf("K \n"); + //pc.printf("%f %f %f %f\n",K[0][0],K[0][1],K[0][2],K[0][3]); + //pc.printf("%f %f %f %f\n",K[1][0],K[1][1],K[1][2],K[1][3]); + //pc.printf("%f %f %f %f\n",K[2][0],K[2][1],K[2][2],K[2][3]); + + double F_inf[2][4]; + F_inf[0][0]=in[0][0]*tempF[0][0]; + F_inf[0][1]=in[0][1]*tempF[0][1]; + F_inf[0][2]=in[0][2]*tempF[0][2]; + F_inf[0][3]=in[0][3]*tempF[0][3]; + + F_inf[1][0]=in[1][0]*tempF[1][0]; + F_inf[1][1]=in[1][1]*tempF[1][1]; + F_inf[1][2]=in[1][2]*tempF[1][2]; + F_inf[1][3]=in[1][3]*tempF[1][3]; + + //pc.printf("F_inf \n"); + //pc.printf("%f %f %f %f\n",F_inf[0][0],F_inf[0][1],F_inf[0][2],F_inf[0][3]); + //pc.printf("%f %f %f %f\n",F_inf[1][0],F_inf[1][1],F_inf[1][2],F_inf[1][3]); + //pc.printf("%f %f %f %f\n",F_inf[2][0],F_inf[2][1],F_inf[2][2],F_inf[2][3]); + + double B[2][4]; + B[0][0]=3.14159265359-asin(F_inf[0][0]/K[0][0]); + B[0][1]=3.14159265359-asin(F_inf[0][1]/K[0][1]); + B[0][2]=3.14159265359-asin(F_inf[0][2]/K[0][2]); + B[0][3]=3.14159265359-asin(F_inf[0][3]/K[0][3]); + + B[1][0]=3.14159265359-asin(F_inf[1][0]/K[1][0]); + B[1][1]=3.14159265359-asin(F_inf[1][1]/K[1][1]); + B[1][2]=3.14159265359-asin(F_inf[1][2]/K[1][2]); + B[1][3]=3.14159265359-asin(F_inf[1][3]/K[1][3]); + + + //pc.printf("B \n"); + //pc.printf("%f %f %f %f\n",B[0][0],B[0][1],B[0][2],B[0][3]); + //pc.printf("%f %f %f %f\n",B[1][0],B[1][1],B[1][2],B[1][3]); + //pc.printf("%f %f %f %f\n",B[2][0],B[2][1],B[2][2],B[2][3]); + + double A[2][4]; + A[0][0]=K[0][0]*B[0][0]/C[0][0]; + A[0][1]=K[0][1]*B[0][1]/C[0][1]; + A[0][2]=K[0][2]*B[0][2]/C[0][2]; + A[0][3]=K[0][3]*B[0][3]/C[0][3]; + + A[1][0]=K[1][0]*B[1][0]/C[1][0]; + A[1][1]=K[1][1]*B[1][1]/C[1][1]; + A[1][2]=K[1][2]*B[1][2]/C[1][2]; + A[1][3]=K[1][3]*B[1][3]/C[1][3]; + + + //pc.printf("A \n"); + //pc.printf("%f %f %f %f\n",A[0][0],A[0][1],A[0][2],A[0][3]); + //pc.printf("%f %f %f %f\n",A[1][0],A[1][1],A[1][2],A[1][3]); + //pc.printf("%f %f %f %f\n",A[2][0],A[2][1],A[2][2],A[2][3]); + + double G[4]; + G[0]=(A[1][0]*K[0][0]*B[0][0])/(A[0][0]*K[1][0]*B[1][0]); + G[1]=(A[1][1]*K[0][1]*B[0][1])/(A[0][1]*K[1][1]*B[1][1]); + G[2]=(A[1][2]*K[0][2]*B[0][2])/(A[0][2]*K[1][2]*B[1][2]); + G[3]=(A[1][3]*K[0][3]*B[0][3])/(A[0][3]*K[1][3]*B[1][3]); + + //p.printf("G \n"); + //pc.printf("%f %f %f %f\n",G[0],G[1],G[2],G[3]); + double s[2][4]; + s[0][0]=sx[0]; + s[0][1]=sx[1]; + s[0][2]=sx[2]; + s[0][3]=sx[3]; + + s[1][0]=-sy_pom[0]/G[0]; + s[1][1]=-sy_pom[1]/G[1]; + s[1][2]=-sy_pom[2]/G[2]; + s[1][3]=-sy_pom[3]/G[3]; + + //%s_norm = sqrt(s(1, :).^2 + s(2, :).^2); + + double angle[4]; + angle[0]=atan2(s[1][0],s[0][0]); + angle[1]=atan2(s[1][1],s[0][1]); + angle[2]=atan2(s[1][2],s[0][2]); + angle[3]=atan2(s[1][3],s[0][3]); + + double f[2][4]; + f[0][0]=K[0][0]*sin(B[0][0]*(1-exp(-abs(s[0][0])/A[0][0]))); + f[0][1]=K[0][1]*sin(B[0][1]*(1-exp(-abs(s[0][1])/A[0][1]))); + f[0][2]=K[0][2]*sin(B[0][2]*(1-exp(-abs(s[0][2])/A[0][2]))); + f[0][3]=K[0][3]*sin(B[0][3]*(1-exp(-abs(s[0][3])/A[0][3]))); + + f[1][0]=K[1][0]*sin(B[1][0]*(1-exp(-abs(s[1][0])/A[1][0]))); + f[1][1]=K[1][1]*sin(B[1][1]*(1-exp(-abs(s[1][1])/A[1][1]))); + f[1][2]=K[1][2]*sin(B[1][2]*(1-exp(-abs(s[1][2])/A[1][2]))); + f[1][3]=K[1][3]*sin(B[1][3]*(1-exp(-abs(s[1][3])/A[1][3]))); + + double f_norm[4]; + f_norm[0]=0.5*(f[0][0]+f[1][0]+(f[0][0]-f[1][0])*cos(2*angle[0])); + f_norm[1]=0.5*(f[0][1]+f[1][1]+(f[0][1]-f[1][1])*cos(2*angle[1])); + f_norm[2]=0.5*(f[0][2]+f[1][2]+(f[0][2]-f[1][2])*cos(2*angle[2])); + f_norm[3]=0.5*(f[0][3]+f[1][3]+(f[0][3]-f[1][3])*cos(2*angle[3])); + + + Fi[0][0]=f_norm[0]*cos(angle[0]); + Fi[0][1]=f_norm[1]*cos(angle[1]); + Fi[0][2]=f_norm[2]*cos(angle[2]); + Fi[0][3]=f_norm[3]*cos(angle[3]); + + Fi[1][0]=f_norm[0]*sin(angle[0]); + Fi[1][1]=f_norm[1]*sin(angle[1]); + Fi[1][2]=f_norm[2]*sin(angle[2]); + Fi[1][3]=f_norm[3]*sin(angle[3]); + //////////// + Fi[1][1]=-Fi[1][1]; + Fi[1][3]=-Fi[1][3]; + + //pc.printf("Fi \n"); + //pc.printf("%f %f %f %f\n",Fi[0][0],Fi[0][1],Fi[0][2],Fi[0][3]); + //pc.printf("%f %f %f %f\n",Fi[1][0],Fi[1][1],Fi[1][2],Fi[1][3]); + //pc.printf("%.20f %.20f %.20f %.20f\n",Fi[2][0],Fi[2][1],Fi[2][2],Fi[2][3]); + + // right wheel should have antisymmetric + // characteristic compared to left -> right(x) = -left(-x) because generally + // left and right wheels of same axle have same characteristic but + // antisymmetric (implementation can be further optimized by calculating + // only front and rear frictions and than just distributing those)\ + + double aw[4]; + aw[0]=(Tfl-Fi[0][0]*r[0])/Iw[0]; + aw[1]=(Tfr-Fi[0][1]*r[1])/Iw[1]; + aw[2]=(Trl-Fi[0][2]*r[2])/Iw[2]; + aw[3]=(Trr-Fi[0][3]*r[3])/Iw[3]; + //pc.printf("aw \n"); + //pc.printf("%f %f %f %f\n",aw[0],aw[1],aw[2],aw[3]); + //////////// + double FL[4]; + FL[0]=Fi[0][0]; + FL[1]=Fi[1][0]; + FL[2]=tempF[0][0]; + + double FR[3]; + FR[0]=Fi[0][1]; + FR[1]=Fi[1][1]; + FR[2]=tempF[0][1]; + + double RL[3]; + RL[0]=Fi[0][2]; + RL[1]=Fi[1][2]; + RL[2]=tempF[0][2]; + + double RR[3]; + RR[0]=Fi[0][3]; + RR[1]=Fi[1][3]; + RR[2]=tempF[0][3]; + //////////// + + cosin=cos(-d); + sinus=sin(-d); + p1 = cosin*Fi[0][0]+sinus*Fi[1][0]; + p2 = cosin*Fi[0][1]+sinus*Fi[1][1]; + p3 = -sinus*Fi[0][0]+cosin*Fi[1][0]; + p4 = -sinus*Fi[0][1]+cosin*Fi[1][1]; + + Fi[0][0]=p1; + Fi[0][1]=p2; + Fi[1][0]=p3; + Fi[1][1]=p4; + + //pc.printf("Fi 2 put \n"); + //pc.printf("%f %f %f %f\n",Fi[0][0],Fi[0][1],Fi[0][2],Fi[0][3]); + //pc.printf("%f %f %f %f\n",Fi[1][0],Fi[1][1],Fi[1][2],Fi[1][3]); + //pc.printf("%.20f %.20f %.20f %.20f\n",Fi[2][0],Fi[2][1],Fi[2][2],Fi[2][3]); + /////////// + double Mi[3][4]; + Mi[0][0]=pw[1][0]*Fi[2][0]-pw[2][0]*Fi[1][0]; + Mi[1][0]=pw[0][0]*Fi[2][0]-pw[2][0]*Fi[0][0]; + Mi[2][0]=pw[0][0]*Fi[1][0]-pw[1][0]*Fi[0][0]; + + Mi[0][1]=pw[1][1]*Fi[2][1]-pw[2][1]*Fi[1][1]; + Mi[1][1]=pw[0][1]*Fi[2][1]-pw[2][1]*Fi[0][1]; + Mi[2][1]=pw[0][1]*Fi[1][1]-pw[1][1]*Fi[0][1]; + + Mi[0][2]=pw[1][2]*Fi[2][2]-pw[2][2]*Fi[1][2]; + Mi[1][2]=pw[0][2]*Fi[2][2]-pw[2][2]*Fi[0][2]; + Mi[2][2]=pw[0][2]*Fi[1][2]-pw[1][2]*Fi[0][2]; + + Mi[0][3]=pw[1][3]*Fi[2][3]-pw[2][3]*Fi[1][3]; + Mi[1][3]=pw[0][3]*Fi[2][3]-pw[2][3]*Fi[0][3]; + Mi[2][3]=pw[0][3]*Fi[1][3]-pw[1][3]*Fi[0][3]; + + //pc.printf("Mi \n"); + //pc.printf("%f %f %f %f\n",Mi[0][0],Mi[0][1],Mi[0][2],Mi[0][3]); + //pc.printf("%f %f %f %f\n",Mi[1][0],Mi[1][1],Mi[1][2],Mi[1][3]); + //pc.printf("%f %f %f %f\n",Mi[2][0],Mi[2][1],Mi[2][2],Mi[2][3]); + + + /////////// + double F[3]; + F[0]=Fi[0][0]+Fi[0][1]+Fi[0][2]+Fi[0][3]; + F[1]=Fi[1][0]+Fi[1][1]+Fi[1][2]+Fi[1][3]; + F[2]=Fi[2][0]+Fi[2][1]+Fi[2][2]+Fi[2][3]; + //pc.printf("F \n"); + //pc.printf("%f %f %f\n",F[0],F[1],F[2]); + /////////// + double M[3]; + M[0]=Mi[0][0]+Mi[0][1]+Mi[0][2]+Mi[0][3]; + M[1]=Mi[1][0]+Mi[1][1]+Mi[1][2]+Mi[1][3]; + M[2]=Mi[2][0]+Mi[2][1]+Mi[2][2]+Mi[2][3]; + //pc.printf("M \n"); + //pc.printf("%f %f %f\n",M[0],M[1],M[2]); + /////////// + double acp[3]; + acp[0]=va[1]*v[2]-va[2]*v[1]; + acp[1]=va[0]*v[2]-va[2]*v[0]; + acp[2]=va[0]*v[1]-va[1]*v[0]; + //pc.printf("acp \n"); + //pc.printf("%f %f %f\n",acp[0],acp[1],acp[2]); + /////////// + double a[3]; + a[0]=F[0]/m[0]-acp[0]; + a[1]=F[1]/m[1]-acp[1]; + a[2]=F[2]/m[2]-acp[2]; + //pc.printf("a \n"); + //pc.printf("%f %f %f\n",a[0],a[1],a[2]); + /////////// + /////////// aa = I_inv * (M - cross(va, I*va)); % Euler's equation + double pom1[3]; + pom1[0]=I[0][0]*va[0]+I[0][1]*va[1]+I[0][2]*va[2]; + pom1[1]=I[1][0]*va[0]+I[1][1]*va[1]+I[1][2]*va[2]; + pom1[2]=I[2][0]*va[0]+I[2][1]*va[1]+I[2][2]*va[2]; + double pom2[3]; + pom2[0]=va[1]*pom1[2]-va[2]*pom1[1]; + pom2[1]=va[0]*pom1[2]-va[2]*pom1[0]; + pom2[2]=va[0]*pom1[1]-va[1]*pom1[0]; + double pom3[3]; + pom3[0]=M[0]-pom2[0]; + pom3[1]=M[1]-pom2[1]; + pom3[2]=M[2]-pom2[2]; + double aa[3]; + aa[0]=I_inv[0][0]*pom3[0]+I_inv[0][1]*pom3[1]+I_inv[0][2]*pom3[2]; + aa[1]=I_inv[1][0]*pom3[0]+I_inv[1][1]*pom3[1]+I_inv[1][2]*pom3[2]; + aa[2]=I_inv[2][0]*pom3[0]+I_inv[2][1]*pom3[1]+I_inv[2][2]*pom3[2]; + //pc.printf("aa \n"); + //pc.printf("%f %f %f\n",aa[0],aa[1],aa[2]); + //////////// + v[0]=v[0]+a[0]*Ts; + v[1]=v[1]+a[1]*Ts; + v[2]=v[2]+a[2]*Ts; + //pc.printf("v \n"); + //pc.printf("%f %f %f\n",v[0],v[1],v[2]); + //////////// + va[0]=va[0]+aa[0]*Ts; + va[1]=va[1]+aa[1]*Ts; + va[2]=va[2]+aa[2]*Ts; + //pc.printf("va \n"); + //pc.printf("%f %f %f\n",va[0],va[1],va[2]); + //////////// + w[0]=w[0]+aw[0]*Ts; + w[1]=w[1]+aw[1]*Ts; + w[2]=w[2]+aw[2]*Ts; + w[3]=w[3]+aw[3]*Ts; + //pc.printf("w \n"); + //pc.printf("%f %f %f %f\n",w[0],w[1],w[2],w[3]); + //////////// R3D_xyz (Rx=I, Ry=I) + double Rz[3][3]={ + {cos(pa[2]), -sin(pa[2]), 0}, + {sin(pa[2]), cos(pa[2]), 0}, + {0, 0, 1} + }; + p[0]=p[0]+(Rz[0][0]*v[0]+Rz[0][1]*v[1]+Rz[0][2]*v[2])*Ts; + p[1]=p[1]+(Rz[1][0]*v[0]+Rz[1][1]*v[1]+Rz[1][2]*v[2])*Ts; + p[2]=p[2]+(Rz[2][0]*v[0]+Rz[2][1]*v[1]+Rz[2][2]*v[2])*Ts; + //pc.printf("p \n"); + pc.printf("%f %f %f\n",p[0],p[1],p[2]); + ///////////// + pa[0]=pa[0]+va[0]*Ts; + pa[1]=pa[1]+va[1]*Ts; + pa[2]=pa[2]+va[2]*Ts; + //pc.printf("pa \n"); + //pc.printf("%f %f %f\n",pa[0],pa[1],pa[2]); + + + } + + + + + +CANMessage msg; + +void callbackCAN(){ + flag=1; + can1.read(msg); + if(msg.id==0x0184){ + T_1=(double)((int16_t)((msg.data[4]<<8)+msg.data[5]))*Mn/1000; + + } + else if(msg.id==0x0185){ + T_2=(double)((int16_t)((msg.data[4]<<8)+msg.data[5]))*Mn/1000; + } + else if(msg.id==0x0188){ + T_3=(double)((int16_t)((msg.data[4]<<8)+msg.data[5]))*Mn/1000; + } + else if(msg.id==0x0189){ + T_4=(double)((int16_t)((msg.data[4]<<8)+msg.data[5]))*Mn/1000; + } + + + cnt++; + + } + +Timer t; + +int main() { + initialize(); + //can1.filter(RX_ID, 0x03F0,CANStandard, 0); + + //messageOutVel1.id = 0x282; + //messageOutVel2.id = 0x283; + //messageOutVel3.id = 0x287; + //messageOutVel4.id = 0x288; + + + + //messageOut2.format = CANStandard;// or CANExtended; // standard or extended ID (can be skipped for standard) + // messageOut2.len = 1;//length in bytes (1 to 8); + //messageOut1.format = CANStandard;// or CANExtended; // standard or extended ID (can be skipped for standard) + //messageOut1.len = 1;//length in bytes (1 to 8); + + //can1.frequency(500000); + + // tick1.attach(&send, 0.0005); + + //can1.attach(callbackCAN); + + + + //while(1){ + // while(!flag); + // pc.printf("V: %f",v*3.6); + // pc.printf(" T1: %f",T_1); + // pc.printf(" T2: %f",T_2); + // pc.printf(" T3: %f",T_3); + // pc.printf(" T4: %f",T_4); + // pc.printf(" Cnt: %d",cnt); + // pc.printf("\n"); + // + // flag=0; + //} + + //t.start(); + while(1){ + model(450,450,450,450,0); + wait(0.01); + } + + + +} \ No newline at end of file