Aleksa Katic / Mbed 2 deprecated HIL_14_dof_copy

HIL 14

Dependencies:   mbed MatrixMath Matrix

main.cpp@0:19aa346c5a6a, 2021-04-11 (annotated)

Committer:
aleksa98
Date:
Sun Apr 11 10:32:29 2021 +0000
Revision:
0:19aa346c5a6a
Verzija koda 1 (warning)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aleksa98 0:19aa346c5a6a 1 #include "mbed.h"
aleksa98 0:19aa346c5a6a 2 #include "Matrix.h"
aleksa98 0:19aa346c5a6a 3 #include "MatrixMath.h"
aleksa98 0:19aa346c5a6a 4
aleksa98 0:19aa346c5a6a 5 CAN can1(PD_0, PD_1); // rd, td Transmitter
aleksa98 0:19aa346c5a6a 6
aleksa98 0:19aa346c5a6a 7 //Ticker tick1;
aleksa98 0:19aa346c5a6a 8 //Ticker tick2;
aleksa98 0:19aa346c5a6a 9 //Ticker tick3;
aleksa98 0:19aa346c5a6a 10
aleksa98 0:19aa346c5a6a 11
aleksa98 0:19aa346c5a6a 12 const double Mn=9.8; //Nominalni moment
aleksa98 0:19aa346c5a6a 13 const double mi_x=1.2l; //Koeficijent trenja gume po x osi
aleksa98 0:19aa346c5a6a 14 const double mi_y=1.2l; //Koeficijent trenja gume po y osi
aleksa98 0:19aa346c5a6a 15 const double g=9.81;
aleksa98 0:19aa346c5a6a 16 const double m_sprung=1100; //sprung masa vozila
aleksa98 0:19aa346c5a6a 17 const double m_unsprung=110; //unsprung masa vozila
aleksa98 0:19aa346c5a6a 18 const double L=2.64; //Medjuosovinsko rastojanje
aleksa98 0:19aa346c5a6a 19 const double lf=1.35; //Rastojanje centra mase do prednje osovine
aleksa98 0:19aa346c5a6a 20 const double lr=L-lf; //Rastojanje centra mase do zadnje osovine
aleksa98 0:19aa346c5a6a 21 const double Wfl=1.458/2; //Rastojanje levog tocka od centra prednje osovine
aleksa98 0:19aa346c5a6a 22 const double Wfr=1.458/2; //Rastojanje desnog tocka od centra prednje osovine
aleksa98 0:19aa346c5a6a 23 const double Wrl=1.455/2; //Rastojanje levog tocka od centra zadnje osovine
aleksa98 0:19aa346c5a6a 24 const double Wrr=1.455/2; //Rastojanje desnog tocka od centra zadnje osovine
aleksa98 0:19aa346c5a6a 25 const double h=0.507; //Visina centra mase
aleksa98 0:19aa346c5a6a 26 const double Ixx=441; //Moment inercije oko x ose
aleksa98 0:19aa346c5a6a 27 const double Iyy=1748; //Moment inercije oko y ose
aleksa98 0:19aa346c5a6a 28 const double Izz=1945; //Moment inercije oko z ose
aleksa98 0:19aa346c5a6a 29 const double Ixy=0;
aleksa98 0:19aa346c5a6a 30 const double Iyz=0;
aleksa98 0:19aa346c5a6a 31 const double Ixz=0;
aleksa98 0:19aa346c5a6a 32 const double It=2; //Moment inercije tocka oko ose rotacije(uracunati i motor i planetarac)
aleksa98 0:19aa346c5a6a 33 const double rt=0.3; //poluprecnik pneumatika
aleksa98 0:19aa346c5a6a 34 const double Cv=1.14; //koeficijent otpora vazduha
aleksa98 0:19aa346c5a6a 35 const double Ap=1.23; //ref. ceona povrs
aleksa98 0:19aa346c5a6a 36 const double fo=0.011; //koef. kotrljanja staticki
aleksa98 0:19aa346c5a6a 37 const double ro=1.2; //gustina vazduha
aleksa98 0:19aa346c5a6a 38 const double kp=0.9; //Koeficijent efikasnosti planetarca PROVERITI
aleksa98 0:19aa346c5a6a 39 const double sw_ratio=12.0; //Prenosni odnos volana
aleksa98 0:19aa346c5a6a 40 const double N=12.3; //PROVERITI
aleksa98 0:19aa346c5a6a 41 const double Ts=0.002;
aleksa98 0:19aa346c5a6a 42
aleksa98 0:19aa346c5a6a 43
aleksa98 0:19aa346c5a6a 44 const double m[3]={m_sprung+m_unsprung, m_sprung+m_unsprung, m_sprung};// matrica mase
aleksa98 0:19aa346c5a6a 45
aleksa98 0:19aa346c5a6a 46 const double I[3][3]={
aleksa98 0:19aa346c5a6a 47 {Ixx, Ixy, Ixz},
aleksa98 0:19aa346c5a6a 48 {Ixy, Iyy, Iyz},
aleksa98 0:19aa346c5a6a 49 {Ixz, Iyz, Izz}
aleksa98 0:19aa346c5a6a 50 };// tenzor inercije
aleksa98 0:19aa346c5a6a 51
aleksa98 0:19aa346c5a6a 52 double I_inv[3][3];// inverzija tenzora inercije
aleksa98 0:19aa346c5a6a 53
aleksa98 0:19aa346c5a6a 54 const double Iw[4]={It, It, It, It};//momenti inercije tockova
aleksa98 0:19aa346c5a6a 55
aleksa98 0:19aa346c5a6a 56 const double mi[2][4]={
aleksa98 0:19aa346c5a6a 57 {1.2, 1.2, 1.3, 1.3}, // flx frx rlx rrx
aleksa98 0:19aa346c5a6a 58 {1.2, 1.2, 1.3, 1.3} //fly fry rly rry
aleksa98 0:19aa346c5a6a 59 };//koeficijenti trenja gume po x i y osi za sve tockove
aleksa98 0:19aa346c5a6a 60
aleksa98 0:19aa346c5a6a 61 const double in[2][4]={
aleksa98 0:19aa346c5a6a 62 {0.8, 0.8, 0.9, 0.9},
aleksa98 0:19aa346c5a6a 63 {0.8, 0.8, 0.9, 0.9}
aleksa98 0:19aa346c5a6a 64 };// Coefficient of friction with infinite slip [no unit]
aleksa98 0:19aa346c5a6a 65
aleksa98 0:19aa346c5a6a 66 const double C[2][4]={
aleksa98 0:19aa346c5a6a 67 {70000, 70000, 100000, 100000},
aleksa98 0:19aa346c5a6a 68 {70000, 70000, 100000, 100000}
aleksa98 0:19aa346c5a6a 69 };//Cornering stiffness [x - N; y - N/rad]
aleksa98 0:19aa346c5a6a 70
aleksa98 0:19aa346c5a6a 71 const double r[4]={rt, rt, rt, rt};// Matrica poluprecnika tockova [rfl rfr rrl rrr]
aleksa98 0:19aa346c5a6a 72
aleksa98 0:19aa346c5a6a 73 const double cs[4]={26125, 26125, 26125, 26125};//Spring effective stiffness [N/m]
aleksa98 0:19aa346c5a6a 74
aleksa98 0:19aa346c5a6a 75 const double ca[2]={12000, 15000};// Antiroll bar stiffnes [N/m]
aleksa98 0:19aa346c5a6a 76
aleksa98 0:19aa346c5a6a 77 const double cd[4]={2000, 2000, 2000, 2000};//Damper effective dampening [N*s/m]
aleksa98 0:19aa346c5a6a 78
aleksa98 0:19aa346c5a6a 79 const double p_init[3]={0, 0, h};// inital position of CM in global fixed coord sys [m]
aleksa98 0:19aa346c5a6a 80
aleksa98 0:19aa346c5a6a 81 const double v_init[3]={0, 0, 0};// inital velocity in body fixed coord sys [m/s]
aleksa98 0:19aa346c5a6a 82
aleksa98 0:19aa346c5a6a 83 const double pa_init[3]={0, 0, 0};// Initial roll pitch and yaw [rad]
aleksa98 0:19aa346c5a6a 84
aleksa98 0:19aa346c5a6a 85 const double va_init[3]={0, 0, 0};// initial roll pitch and yaw velocities [rad/s]
aleksa98 0:19aa346c5a6a 86
aleksa98 0:19aa346c5a6a 87 const double pw_init[3][4]={
aleksa98 0:19aa346c5a6a 88 {lf, lf, -lr, -lr}, //xfl xfr xrl xrr
aleksa98 0:19aa346c5a6a 89 {Wfl, -Wfr, Wrl, -Wrr}, //yfl yfr yrl yrr
aleksa98 0:19aa346c5a6a 90 {h, h, h, h} //zfl zfr zrl zrr
aleksa98 0:19aa346c5a6a 91 };// initial wheel positions in body fixed coord sys
aleksa98 0:19aa346c5a6a 92
aleksa98 0:19aa346c5a6a 93 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]
aleksa98 0:19aa346c5a6a 94
aleksa98 0:19aa346c5a6a 95 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]
aleksa98 0:19aa346c5a6a 96
aleksa98 0:19aa346c5a6a 97 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]
aleksa98 0:19aa346c5a6a 98
aleksa98 0:19aa346c5a6a 99 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};
aleksa98 0:19aa346c5a6a 100
aleksa98 0:19aa346c5a6a 101
aleksa98 0:19aa346c5a6a 102 double p[3]; //position in global coordinate system
aleksa98 0:19aa346c5a6a 103 double v[3]; //velocity in CM fixed coordinate system
aleksa98 0:19aa346c5a6a 104 double pa[3]; //angular position - x and y are in vehicle local and z is in global coordinate system
aleksa98 0:19aa346c5a6a 105 double va[3]; //angular velocity
aleksa98 0:19aa346c5a6a 106 double pw[3][4]; //wheel contact patch position in CM fixed coordinate system
aleksa98 0:19aa346c5a6a 107 double vw[3][4]; //wheel contact patch speed in CM fixed coordinate system
aleksa98 0:19aa346c5a6a 108 double w[4]; //wheel angular speed
aleksa98 0:19aa346c5a6a 109 double Fs[4]; //spring force from each wheel
aleksa98 0:19aa346c5a6a 110 double Fa[4]; //antiroll bar force from each wheel
aleksa98 0:19aa346c5a6a 111 double Fd[4]; //damper force from each wheel
aleksa98 0:19aa346c5a6a 112 double Fi[3][4]; //force of each wheel in CM fixed coordinate system
aleksa98 0:19aa346c5a6a 113
aleksa98 0:19aa346c5a6a 114 double FL[3];
aleksa98 0:19aa346c5a6a 115
aleksa98 0:19aa346c5a6a 116
aleksa98 0:19aa346c5a6a 117
aleksa98 0:19aa346c5a6a 118 double d=0;
aleksa98 0:19aa346c5a6a 119
aleksa98 0:19aa346c5a6a 120
aleksa98 0:19aa346c5a6a 121 double T_1=0, T_2=0, T_3=0, T_4=0;
aleksa98 0:19aa346c5a6a 122 double T_1p=0, T_2p=0, T_3p=0, T_4p=0;
aleksa98 0:19aa346c5a6a 123
aleksa98 0:19aa346c5a6a 124 int cnt=0;
aleksa98 0:19aa346c5a6a 125 volatile bool flag=0;
aleksa98 0:19aa346c5a6a 126
aleksa98 0:19aa346c5a6a 127
aleksa98 0:19aa346c5a6a 128 int brojac=0;
aleksa98 0:19aa346c5a6a 129 const unsigned int RX_ID = 0x0184;
aleksa98 0:19aa346c5a6a 130 CANMessage messageOutVel1;
aleksa98 0:19aa346c5a6a 131 CANMessage messageOutVel2;
aleksa98 0:19aa346c5a6a 132 CANMessage messageOutVel3;
aleksa98 0:19aa346c5a6a 133 CANMessage messageOutVel4;
aleksa98 0:19aa346c5a6a 134
aleksa98 0:19aa346c5a6a 135
aleksa98 0:19aa346c5a6a 136
aleksa98 0:19aa346c5a6a 137 Serial pc(USBTX, USBRX);
aleksa98 0:19aa346c5a6a 138
aleksa98 0:19aa346c5a6a 139 void initialize(){
aleksa98 0:19aa346c5a6a 140 Matrix I_mat(3,3);
aleksa98 0:19aa346c5a6a 141 I_mat<<Ixx<<Ixy<<Ixz
aleksa98 0:19aa346c5a6a 142 <<Ixy<<Iyy<<Iyz
aleksa98 0:19aa346c5a6a 143 <<Ixz<<Iyz<<Izz;
aleksa98 0:19aa346c5a6a 144
aleksa98 0:19aa346c5a6a 145 Matrix I_mat_inv(3,3);
aleksa98 0:19aa346c5a6a 146 I_mat_inv=MatrixMath::Inv(I_mat);
aleksa98 0:19aa346c5a6a 147
aleksa98 0:19aa346c5a6a 148 I_inv[0][0]=I_mat_inv(1,1);
aleksa98 0:19aa346c5a6a 149 I_inv[0][1]=I_mat_inv(1,2);
aleksa98 0:19aa346c5a6a 150 I_inv[0][2]=I_mat_inv(1,3);
aleksa98 0:19aa346c5a6a 151
aleksa98 0:19aa346c5a6a 152 I_inv[1][0]=I_mat_inv(2,1);
aleksa98 0:19aa346c5a6a 153 I_inv[1][1]=I_mat_inv(2,2);
aleksa98 0:19aa346c5a6a 154 I_inv[1][2]=I_mat_inv(2,3);
aleksa98 0:19aa346c5a6a 155
aleksa98 0:19aa346c5a6a 156 I_inv[2][0]=I_mat_inv(3,1);
aleksa98 0:19aa346c5a6a 157 I_inv[2][1]=I_mat_inv(3,2);
aleksa98 0:19aa346c5a6a 158 I_inv[2][2]=I_mat_inv(3,3);
aleksa98 0:19aa346c5a6a 159
aleksa98 0:19aa346c5a6a 160 p[0]=p_init[0];
aleksa98 0:19aa346c5a6a 161 p[1]=p_init[1];
aleksa98 0:19aa346c5a6a 162 p[2]=p_init[2];
aleksa98 0:19aa346c5a6a 163
aleksa98 0:19aa346c5a6a 164 v[0]=v_init[0];
aleksa98 0:19aa346c5a6a 165 v[1]=v_init[1];
aleksa98 0:19aa346c5a6a 166 v[2]=v_init[2];
aleksa98 0:19aa346c5a6a 167
aleksa98 0:19aa346c5a6a 168 pa[0]=pa_init[0];
aleksa98 0:19aa346c5a6a 169 pa[1]=pa_init[1];
aleksa98 0:19aa346c5a6a 170 pa[2]=pa_init[2];
aleksa98 0:19aa346c5a6a 171
aleksa98 0:19aa346c5a6a 172 va[0]=va_init[0];
aleksa98 0:19aa346c5a6a 173 va[1]=va_init[1];
aleksa98 0:19aa346c5a6a 174 va[2]=va_init[2];
aleksa98 0:19aa346c5a6a 175
aleksa98 0:19aa346c5a6a 176 pw[0][0]=pw_init[0][0];
aleksa98 0:19aa346c5a6a 177 pw[0][1]=pw_init[0][1];
aleksa98 0:19aa346c5a6a 178 pw[0][2]=pw_init[0][2];
aleksa98 0:19aa346c5a6a 179 pw[0][3]=pw_init[0][3];
aleksa98 0:19aa346c5a6a 180
aleksa98 0:19aa346c5a6a 181 pw[1][0]=pw_init[1][0];
aleksa98 0:19aa346c5a6a 182 pw[1][1]=pw_init[1][1];
aleksa98 0:19aa346c5a6a 183 pw[1][2]=pw_init[1][2];
aleksa98 0:19aa346c5a6a 184 pw[1][3]=pw_init[1][3];
aleksa98 0:19aa346c5a6a 185
aleksa98 0:19aa346c5a6a 186 pw[2][0]=pw_init[2][0];
aleksa98 0:19aa346c5a6a 187 pw[2][1]=pw_init[2][1];
aleksa98 0:19aa346c5a6a 188 pw[2][2]=pw_init[2][2];
aleksa98 0:19aa346c5a6a 189 pw[2][3]=pw_init[2][3];
aleksa98 0:19aa346c5a6a 190
aleksa98 0:19aa346c5a6a 191 w[0]=w_init[0];
aleksa98 0:19aa346c5a6a 192 w[1]=w_init[1];
aleksa98 0:19aa346c5a6a 193 w[2]=w_init[2];
aleksa98 0:19aa346c5a6a 194 w[3]=w_init[3];
aleksa98 0:19aa346c5a6a 195
aleksa98 0:19aa346c5a6a 196
aleksa98 0:19aa346c5a6a 197 }
aleksa98 0:19aa346c5a6a 198
aleksa98 0:19aa346c5a6a 199
aleksa98 0:19aa346c5a6a 200 void model(double Tfl, double Tfr, double Trl, double Trr, double Steering_angle){
aleksa98 0:19aa346c5a6a 201 double d=Steering_angle/sw_ratio;
aleksa98 0:19aa346c5a6a 202 //////////////
aleksa98 0:19aa346c5a6a 203 pw[2][0]=-(p[2]+pw[1][0]*sin(pa[0])-pw[0][0]*sin(pa[1]));
aleksa98 0:19aa346c5a6a 204 pw[2][1]=-(p[2]+pw[1][1]*sin(pa[0])-pw[0][1]*sin(pa[1]));
aleksa98 0:19aa346c5a6a 205 pw[2][2]=-(p[2]+pw[1][2]*sin(pa[0])-pw[0][2]*sin(pa[1]));
aleksa98 0:19aa346c5a6a 206 pw[2][3]=-(p[2]+pw[1][3]*sin(pa[0])-pw[0][3]*sin(pa[1]));
aleksa98 0:19aa346c5a6a 207
aleksa98 0:19aa346c5a6a 208 //pc.printf("pw \n");
aleksa98 0:19aa346c5a6a 209 //pc.printf("%f %f %f %f\n",pw[0][0],pw[0][1],pw[0][2],pw[0][3]);
aleksa98 0:19aa346c5a6a 210 //pc.printf("%f %f %f %f\n",pw[1][0],pw[1][1],pw[1][2],pw[1][3]);
aleksa98 0:19aa346c5a6a 211 //pc.printf("%f %f %f %f\n",pw[2][0],pw[2][1],pw[2][2],pw[2][3]);
aleksa98 0:19aa346c5a6a 212 //pc.printf("aaaaaaaa \n");
aleksa98 0:19aa346c5a6a 213 /////////////
aleksa98 0:19aa346c5a6a 214 vw[0][0]=v[0]-va[2]*pw[1][0];
aleksa98 0:19aa346c5a6a 215 vw[0][1]=v[0]-va[2]*pw[1][1];
aleksa98 0:19aa346c5a6a 216 vw[0][2]=v[0]-va[2]*pw[1][2];
aleksa98 0:19aa346c5a6a 217 vw[0][3]=v[0]-va[2]*pw[1][3];
aleksa98 0:19aa346c5a6a 218
aleksa98 0:19aa346c5a6a 219 vw[1][0]=v[1]+va[2]*pw[0][0];
aleksa98 0:19aa346c5a6a 220 vw[1][1]=v[1]+va[2]*pw[0][1];
aleksa98 0:19aa346c5a6a 221 vw[1][2]=v[1]+va[2]*pw[0][2];
aleksa98 0:19aa346c5a6a 222 vw[1][3]=v[1]+va[2]*pw[0][3];
aleksa98 0:19aa346c5a6a 223
aleksa98 0:19aa346c5a6a 224 vw[2][0]=v[2]+va[0]*pw[1][0]-va[1]*pw[0][0];
aleksa98 0:19aa346c5a6a 225 vw[2][1]=v[2]+va[0]*pw[1][1]-va[1]*pw[0][1];
aleksa98 0:19aa346c5a6a 226 vw[2][2]=v[2]+va[0]*pw[1][2]-va[1]*pw[0][2];
aleksa98 0:19aa346c5a6a 227 vw[2][3]=v[2]+va[0]*pw[1][3]-va[1]*pw[0][3];
aleksa98 0:19aa346c5a6a 228
aleksa98 0:19aa346c5a6a 229 //pc.printf("vw \n");
aleksa98 0:19aa346c5a6a 230 //pc.printf("%f %f %f %f\n",vw[0][0],vw[0][1],vw[0][2],vw[0][3]);
aleksa98 0:19aa346c5a6a 231 //pc.printf("%f %f %f %f\n",vw[1][0],vw[1][1],vw[1][2],vw[1][3]);
aleksa98 0:19aa346c5a6a 232 //pc.printf("%f %f %f %f\n",vw[2][0],vw[2][1],vw[2][2],vw[2][3]);
aleksa98 0:19aa346c5a6a 233
aleksa98 0:19aa346c5a6a 234 ////////////
aleksa98 0:19aa346c5a6a 235 double cosin=cos(d);
aleksa98 0:19aa346c5a6a 236 double sinus=sin(d);
aleksa98 0:19aa346c5a6a 237 double p1 = cosin*vw[0][0]+sinus*vw[1][0];
aleksa98 0:19aa346c5a6a 238 double p2 = cosin*vw[0][1]+sinus*vw[1][1];
aleksa98 0:19aa346c5a6a 239 double p3 = -sinus*vw[0][0]+cosin*vw[1][0];
aleksa98 0:19aa346c5a6a 240 double p4 = -sinus*vw[0][1]+cosin*vw[1][1];
aleksa98 0:19aa346c5a6a 241
aleksa98 0:19aa346c5a6a 242 vw[0][0]=p1;
aleksa98 0:19aa346c5a6a 243 vw[0][1]=p2;
aleksa98 0:19aa346c5a6a 244 vw[1][0]=p3;
aleksa98 0:19aa346c5a6a 245 vw[1][1]=p4;
aleksa98 0:19aa346c5a6a 246 //pc.printf("vw 2 put \n");
aleksa98 0:19aa346c5a6a 247 //pc.printf("%f %f %f %f\n",vw[0][0],vw[0][1],vw[0][2],vw[0][3]);
aleksa98 0:19aa346c5a6a 248 //pc.printf("%f %f %f %f\n",vw[1][0],vw[1][1],vw[1][2],vw[1][3]);
aleksa98 0:19aa346c5a6a 249 //pc.printf("%f %f %f %f\n",vw[2][0],vw[2][1],vw[2][2],vw[2][3]);
aleksa98 0:19aa346c5a6a 250
aleksa98 0:19aa346c5a6a 251 ////////////
aleksa98 0:19aa346c5a6a 252 double pw_dif[2]={0.5*(pw[2][0]-pw[2][1]),0.5*(pw[2][2]-pw[2][3])};
aleksa98 0:19aa346c5a6a 253
aleksa98 0:19aa346c5a6a 254 //pc.printf("pw_dif \n");
aleksa98 0:19aa346c5a6a 255 //pc.printf("%f %f\n",pw_dif[0],pw_dif[1]);
aleksa98 0:19aa346c5a6a 256 ///////////
aleksa98 0:19aa346c5a6a 257 Fs[0]=cs[0]*(h_spring[0]+pw[2][0]);
aleksa98 0:19aa346c5a6a 258 Fs[1]=cs[1]*(h_spring[1]+pw[2][1]);
aleksa98 0:19aa346c5a6a 259 Fs[2]=cs[2]*(h_spring[2]+pw[2][2]);
aleksa98 0:19aa346c5a6a 260 Fs[3]=cs[3]*(h_spring[3]+pw[2][3]);
aleksa98 0:19aa346c5a6a 261 //pc.printf("Fs \n");
aleksa98 0:19aa346c5a6a 262 //pc.printf("%.20f %.20f %.20f %.20f\n",Fs[0],Fs[1],Fs[2],Fs[3]);
aleksa98 0:19aa346c5a6a 263 ///////////
aleksa98 0:19aa346c5a6a 264 Fa[0]=ca[0]*pw_dif[0]*1+ca[1]*pw_dif[1]*0;
aleksa98 0:19aa346c5a6a 265 Fa[1]=ca[0]*pw_dif[0]*(-1)+ca[1]*pw_dif[1]*0;
aleksa98 0:19aa346c5a6a 266 Fa[2]=ca[0]*pw_dif[0]*0+ca[1]*pw_dif[1]*1;
aleksa98 0:19aa346c5a6a 267 Fa[3]=ca[0]*pw_dif[0]*0+ca[1]*pw_dif[1]*(-1);
aleksa98 0:19aa346c5a6a 268 //pc.printf("Fa \n");
aleksa98 0:19aa346c5a6a 269 //pc.printf("%.20f %.20f %.20f %.20f\n",Fa[0],Fa[1],Fa[2],Fa[3]);
aleksa98 0:19aa346c5a6a 270 //////////
aleksa98 0:19aa346c5a6a 271 Fd[0]=-cd[0]*vw[2][0];
aleksa98 0:19aa346c5a6a 272 Fd[1]=-cd[1]*vw[2][1];
aleksa98 0:19aa346c5a6a 273 Fd[2]=-cd[2]*vw[2][2];
aleksa98 0:19aa346c5a6a 274 Fd[3]=-cd[3]*vw[2][3];
aleksa98 0:19aa346c5a6a 275
aleksa98 0:19aa346c5a6a 276 //pc.printf("Fd \n");
aleksa98 0:19aa346c5a6a 277 //pc.printf("%.20f %.20f %.20f %.20f\n",Fd[0],Fd[1],Fd[2],Fd[3]);
aleksa98 0:19aa346c5a6a 278 //////////
aleksa98 0:19aa346c5a6a 279 Fi[2][0]=Fs[0]+Fd[0]+Fa[0]-mg_sprung[0];
aleksa98 0:19aa346c5a6a 280 Fi[2][1]=Fs[1]+Fd[1]+Fa[1]-mg_sprung[1];
aleksa98 0:19aa346c5a6a 281 Fi[2][2]=Fs[2]+Fd[2]+Fa[2]-mg_sprung[2];
aleksa98 0:19aa346c5a6a 282 Fi[2][3]=Fs[3]+Fd[3]+Fa[3]-mg_sprung[3];
aleksa98 0:19aa346c5a6a 283 //pc.printf("mg_sprung \n");
aleksa98 0:19aa346c5a6a 284 //pc.printf("%.20f %.20f %.20f %.20f\n",mg_sprung[0],mg_sprung[1],mg_sprung[2],mg_sprung[3]);
aleksa98 0:19aa346c5a6a 285 //pc.printf("Fi \n");
aleksa98 0:19aa346c5a6a 286 //pc.printf("%f %f %f %f\n",Fi[0][0],Fi[0][1],Fi[0][2],Fi[0][3]);
aleksa98 0:19aa346c5a6a 287 //pc.printf("%f %f %f %f\n",Fi[1][0],Fi[1][1],Fi[1][2],Fi[1][3]);
aleksa98 0:19aa346c5a6a 288 //pc.printf("%.20f %.20f %.20f %.20f\n",Fi[2][0],Fi[2][1],Fi[2][2],Fi[2][3]);
aleksa98 0:19aa346c5a6a 289 //////////
aleksa98 0:19aa346c5a6a 290 // if dealing with vx = 0, vy = 0 state, use these two lines
aleksa98 0:19aa346c5a6a 291 double sx[4];
aleksa98 0:19aa346c5a6a 292 sx[0]=(w[0]*r[0]-vw[0][0])/(abs(vw[0][0])+0.5);
aleksa98 0:19aa346c5a6a 293 sx[1]=(w[1]*r[1]-vw[0][1])/(abs(vw[0][1])+0.5);
aleksa98 0:19aa346c5a6a 294 sx[2]=(w[2]*r[2]-vw[0][2])/(abs(vw[0][2])+0.5);
aleksa98 0:19aa346c5a6a 295 sx[3]=(w[3]*r[3]-vw[0][3])/(abs(vw[0][3])+0.5);
aleksa98 0:19aa346c5a6a 296 //pc.printf("sx \n");
aleksa98 0:19aa346c5a6a 297 //pc.printf("%f %f %f %f\n",sx[0],sx[1],sx[2],sx[3]);
aleksa98 0:19aa346c5a6a 298 //////////
aleksa98 0:19aa346c5a6a 299 double sy[4];
aleksa98 0:19aa346c5a6a 300 sy[0]=atan2(vw[1][0],(abs(vw[0][0])+0.5));
aleksa98 0:19aa346c5a6a 301 sy[1]=atan2(vw[1][1],(abs(vw[0][1])+0.5));
aleksa98 0:19aa346c5a6a 302 sy[2]=atan2(vw[1][2],(abs(vw[0][2])+0.5));
aleksa98 0:19aa346c5a6a 303 sy[3]=atan2(vw[1][3],(abs(vw[0][3])+0.5));
aleksa98 0:19aa346c5a6a 304 //pc.printf("sy \n");
aleksa98 0:19aa346c5a6a 305 //pc.printf("%f %f %f %f\n",sy[0],sy[1],sy[2],sy[3]);
aleksa98 0:19aa346c5a6a 306 //////////
aleksa98 0:19aa346c5a6a 307 // if vx > e and e is larger than 0 use these
aleksa98 0:19aa346c5a6a 308 // sx = w.*r./vw(1, :) - 1;
aleksa98 0:19aa346c5a6a 309 // sy = atan2(vw(2, :), abs(vw(1, :)));
aleksa98 0:19aa346c5a6a 310 //////////
aleksa98 0:19aa346c5a6a 311 double tempF[2][4];
aleksa98 0:19aa346c5a6a 312 tempF[0][0]=Fs[0]+Fd[0]+Fa[0]+mg_unsprung[0];
aleksa98 0:19aa346c5a6a 313 tempF[0][1]=Fs[1]+Fd[1]+Fa[1]+mg_unsprung[1];
aleksa98 0:19aa346c5a6a 314 tempF[0][2]=Fs[2]+Fd[2]+Fa[2]+mg_unsprung[2];
aleksa98 0:19aa346c5a6a 315 tempF[0][3]=Fs[3]+Fd[3]+Fa[3]+mg_unsprung[3];
aleksa98 0:19aa346c5a6a 316
aleksa98 0:19aa346c5a6a 317 tempF[1][0]=tempF[0][0];
aleksa98 0:19aa346c5a6a 318 tempF[1][1]=tempF[0][1];
aleksa98 0:19aa346c5a6a 319 tempF[1][2]=tempF[0][2];
aleksa98 0:19aa346c5a6a 320 tempF[1][3]=tempF[0][3];
aleksa98 0:19aa346c5a6a 321
aleksa98 0:19aa346c5a6a 322 for (int i = 0; i < 2; i++){
aleksa98 0:19aa346c5a6a 323 for (int j = 0; j < 4; j++){
aleksa98 0:19aa346c5a6a 324 if(tempF[i][j]<1){
aleksa98 0:19aa346c5a6a 325 tempF[i][j]=1;
aleksa98 0:19aa346c5a6a 326 }
aleksa98 0:19aa346c5a6a 327 }
aleksa98 0:19aa346c5a6a 328 }
aleksa98 0:19aa346c5a6a 329
aleksa98 0:19aa346c5a6a 330 //pc.printf("tempF \n");
aleksa98 0:19aa346c5a6a 331 //pc.printf("%f %f %f %f\n",tempF[0][0],tempF[0][1],tempF[0][2],tempF[0][3]);
aleksa98 0:19aa346c5a6a 332 //pc.printf("%f %f %f %f\n",tempF[1][0],tempF[1][1],tempF[1][2],tempF[1][3]);
aleksa98 0:19aa346c5a6a 333 //pc.printf("%f %f %f %f\n",tempF[2][0],tempF[2][1],tempF[2][2],tempF[2][3]);
aleksa98 0:19aa346c5a6a 334
aleksa98 0:19aa346c5a6a 335 ////////// TM_easy_combined
aleksa98 0:19aa346c5a6a 336 double sy_pom[4];
aleksa98 0:19aa346c5a6a 337 sy_pom[0]=sy[0];
aleksa98 0:19aa346c5a6a 338 sy_pom[1]=-sy[1];
aleksa98 0:19aa346c5a6a 339 sy_pom[2]=sy[2];
aleksa98 0:19aa346c5a6a 340 sy_pom[3]=-sy[3];
aleksa98 0:19aa346c5a6a 341
aleksa98 0:19aa346c5a6a 342 double K[2][4]; //Isto sto i F_max
aleksa98 0:19aa346c5a6a 343 K[0][0]=mi[0][0]*tempF[0][0];
aleksa98 0:19aa346c5a6a 344 K[0][1]=mi[0][1]*tempF[0][1];
aleksa98 0:19aa346c5a6a 345 K[0][2]=mi[0][2]*tempF[0][2];
aleksa98 0:19aa346c5a6a 346 K[0][3]=mi[0][3]*tempF[0][3];
aleksa98 0:19aa346c5a6a 347
aleksa98 0:19aa346c5a6a 348 K[1][0]=mi[1][0]*tempF[1][0];
aleksa98 0:19aa346c5a6a 349 K[1][1]=mi[1][1]*tempF[1][1];
aleksa98 0:19aa346c5a6a 350 K[1][2]=mi[1][2]*tempF[1][2];
aleksa98 0:19aa346c5a6a 351 K[1][3]=mi[1][3]*tempF[1][3];
aleksa98 0:19aa346c5a6a 352
aleksa98 0:19aa346c5a6a 353 //pc.printf("K \n");
aleksa98 0:19aa346c5a6a 354 //pc.printf("%f %f %f %f\n",K[0][0],K[0][1],K[0][2],K[0][3]);
aleksa98 0:19aa346c5a6a 355 //pc.printf("%f %f %f %f\n",K[1][0],K[1][1],K[1][2],K[1][3]);
aleksa98 0:19aa346c5a6a 356 //pc.printf("%f %f %f %f\n",K[2][0],K[2][1],K[2][2],K[2][3]);
aleksa98 0:19aa346c5a6a 357
aleksa98 0:19aa346c5a6a 358 double F_inf[2][4];
aleksa98 0:19aa346c5a6a 359 F_inf[0][0]=in[0][0]*tempF[0][0];
aleksa98 0:19aa346c5a6a 360 F_inf[0][1]=in[0][1]*tempF[0][1];
aleksa98 0:19aa346c5a6a 361 F_inf[0][2]=in[0][2]*tempF[0][2];
aleksa98 0:19aa346c5a6a 362 F_inf[0][3]=in[0][3]*tempF[0][3];
aleksa98 0:19aa346c5a6a 363
aleksa98 0:19aa346c5a6a 364 F_inf[1][0]=in[1][0]*tempF[1][0];
aleksa98 0:19aa346c5a6a 365 F_inf[1][1]=in[1][1]*tempF[1][1];
aleksa98 0:19aa346c5a6a 366 F_inf[1][2]=in[1][2]*tempF[1][2];
aleksa98 0:19aa346c5a6a 367 F_inf[1][3]=in[1][3]*tempF[1][3];
aleksa98 0:19aa346c5a6a 368
aleksa98 0:19aa346c5a6a 369 //pc.printf("F_inf \n");
aleksa98 0:19aa346c5a6a 370 //pc.printf("%f %f %f %f\n",F_inf[0][0],F_inf[0][1],F_inf[0][2],F_inf[0][3]);
aleksa98 0:19aa346c5a6a 371 //pc.printf("%f %f %f %f\n",F_inf[1][0],F_inf[1][1],F_inf[1][2],F_inf[1][3]);
aleksa98 0:19aa346c5a6a 372 //pc.printf("%f %f %f %f\n",F_inf[2][0],F_inf[2][1],F_inf[2][2],F_inf[2][3]);
aleksa98 0:19aa346c5a6a 373
aleksa98 0:19aa346c5a6a 374 double B[2][4];
aleksa98 0:19aa346c5a6a 375 B[0][0]=3.14159265359-asin(F_inf[0][0]/K[0][0]);
aleksa98 0:19aa346c5a6a 376 B[0][1]=3.14159265359-asin(F_inf[0][1]/K[0][1]);
aleksa98 0:19aa346c5a6a 377 B[0][2]=3.14159265359-asin(F_inf[0][2]/K[0][2]);
aleksa98 0:19aa346c5a6a 378 B[0][3]=3.14159265359-asin(F_inf[0][3]/K[0][3]);
aleksa98 0:19aa346c5a6a 379
aleksa98 0:19aa346c5a6a 380 B[1][0]=3.14159265359-asin(F_inf[1][0]/K[1][0]);
aleksa98 0:19aa346c5a6a 381 B[1][1]=3.14159265359-asin(F_inf[1][1]/K[1][1]);
aleksa98 0:19aa346c5a6a 382 B[1][2]=3.14159265359-asin(F_inf[1][2]/K[1][2]);
aleksa98 0:19aa346c5a6a 383 B[1][3]=3.14159265359-asin(F_inf[1][3]/K[1][3]);
aleksa98 0:19aa346c5a6a 384
aleksa98 0:19aa346c5a6a 385
aleksa98 0:19aa346c5a6a 386 //pc.printf("B \n");
aleksa98 0:19aa346c5a6a 387 //pc.printf("%f %f %f %f\n",B[0][0],B[0][1],B[0][2],B[0][3]);
aleksa98 0:19aa346c5a6a 388 //pc.printf("%f %f %f %f\n",B[1][0],B[1][1],B[1][2],B[1][3]);
aleksa98 0:19aa346c5a6a 389 //pc.printf("%f %f %f %f\n",B[2][0],B[2][1],B[2][2],B[2][3]);
aleksa98 0:19aa346c5a6a 390
aleksa98 0:19aa346c5a6a 391 double A[2][4];
aleksa98 0:19aa346c5a6a 392 A[0][0]=K[0][0]*B[0][0]/C[0][0];
aleksa98 0:19aa346c5a6a 393 A[0][1]=K[0][1]*B[0][1]/C[0][1];
aleksa98 0:19aa346c5a6a 394 A[0][2]=K[0][2]*B[0][2]/C[0][2];
aleksa98 0:19aa346c5a6a 395 A[0][3]=K[0][3]*B[0][3]/C[0][3];
aleksa98 0:19aa346c5a6a 396
aleksa98 0:19aa346c5a6a 397 A[1][0]=K[1][0]*B[1][0]/C[1][0];
aleksa98 0:19aa346c5a6a 398 A[1][1]=K[1][1]*B[1][1]/C[1][1];
aleksa98 0:19aa346c5a6a 399 A[1][2]=K[1][2]*B[1][2]/C[1][2];
aleksa98 0:19aa346c5a6a 400 A[1][3]=K[1][3]*B[1][3]/C[1][3];
aleksa98 0:19aa346c5a6a 401
aleksa98 0:19aa346c5a6a 402
aleksa98 0:19aa346c5a6a 403 //pc.printf("A \n");
aleksa98 0:19aa346c5a6a 404 //pc.printf("%f %f %f %f\n",A[0][0],A[0][1],A[0][2],A[0][3]);
aleksa98 0:19aa346c5a6a 405 //pc.printf("%f %f %f %f\n",A[1][0],A[1][1],A[1][2],A[1][3]);
aleksa98 0:19aa346c5a6a 406 //pc.printf("%f %f %f %f\n",A[2][0],A[2][1],A[2][2],A[2][3]);
aleksa98 0:19aa346c5a6a 407
aleksa98 0:19aa346c5a6a 408 double G[4];
aleksa98 0:19aa346c5a6a 409 G[0]=(A[1][0]*K[0][0]*B[0][0])/(A[0][0]*K[1][0]*B[1][0]);
aleksa98 0:19aa346c5a6a 410 G[1]=(A[1][1]*K[0][1]*B[0][1])/(A[0][1]*K[1][1]*B[1][1]);
aleksa98 0:19aa346c5a6a 411 G[2]=(A[1][2]*K[0][2]*B[0][2])/(A[0][2]*K[1][2]*B[1][2]);
aleksa98 0:19aa346c5a6a 412 G[3]=(A[1][3]*K[0][3]*B[0][3])/(A[0][3]*K[1][3]*B[1][3]);
aleksa98 0:19aa346c5a6a 413
aleksa98 0:19aa346c5a6a 414 //p.printf("G \n");
aleksa98 0:19aa346c5a6a 415 //pc.printf("%f %f %f %f\n",G[0],G[1],G[2],G[3]);
aleksa98 0:19aa346c5a6a 416 double s[2][4];
aleksa98 0:19aa346c5a6a 417 s[0][0]=sx[0];
aleksa98 0:19aa346c5a6a 418 s[0][1]=sx[1];
aleksa98 0:19aa346c5a6a 419 s[0][2]=sx[2];
aleksa98 0:19aa346c5a6a 420 s[0][3]=sx[3];
aleksa98 0:19aa346c5a6a 421
aleksa98 0:19aa346c5a6a 422 s[1][0]=-sy_pom[0]/G[0];
aleksa98 0:19aa346c5a6a 423 s[1][1]=-sy_pom[1]/G[1];
aleksa98 0:19aa346c5a6a 424 s[1][2]=-sy_pom[2]/G[2];
aleksa98 0:19aa346c5a6a 425 s[1][3]=-sy_pom[3]/G[3];
aleksa98 0:19aa346c5a6a 426
aleksa98 0:19aa346c5a6a 427 //%s_norm = sqrt(s(1, :).^2 + s(2, :).^2);
aleksa98 0:19aa346c5a6a 428
aleksa98 0:19aa346c5a6a 429 double angle[4];
aleksa98 0:19aa346c5a6a 430 angle[0]=atan2(s[1][0],s[0][0]);
aleksa98 0:19aa346c5a6a 431 angle[1]=atan2(s[1][1],s[0][1]);
aleksa98 0:19aa346c5a6a 432 angle[2]=atan2(s[1][2],s[0][2]);
aleksa98 0:19aa346c5a6a 433 angle[3]=atan2(s[1][3],s[0][3]);
aleksa98 0:19aa346c5a6a 434
aleksa98 0:19aa346c5a6a 435 double f[2][4];
aleksa98 0:19aa346c5a6a 436 f[0][0]=K[0][0]*sin(B[0][0]*(1-exp(-abs(s[0][0])/A[0][0])));
aleksa98 0:19aa346c5a6a 437 f[0][1]=K[0][1]*sin(B[0][1]*(1-exp(-abs(s[0][1])/A[0][1])));
aleksa98 0:19aa346c5a6a 438 f[0][2]=K[0][2]*sin(B[0][2]*(1-exp(-abs(s[0][2])/A[0][2])));
aleksa98 0:19aa346c5a6a 439 f[0][3]=K[0][3]*sin(B[0][3]*(1-exp(-abs(s[0][3])/A[0][3])));
aleksa98 0:19aa346c5a6a 440
aleksa98 0:19aa346c5a6a 441 f[1][0]=K[1][0]*sin(B[1][0]*(1-exp(-abs(s[1][0])/A[1][0])));
aleksa98 0:19aa346c5a6a 442 f[1][1]=K[1][1]*sin(B[1][1]*(1-exp(-abs(s[1][1])/A[1][1])));
aleksa98 0:19aa346c5a6a 443 f[1][2]=K[1][2]*sin(B[1][2]*(1-exp(-abs(s[1][2])/A[1][2])));
aleksa98 0:19aa346c5a6a 444 f[1][3]=K[1][3]*sin(B[1][3]*(1-exp(-abs(s[1][3])/A[1][3])));
aleksa98 0:19aa346c5a6a 445
aleksa98 0:19aa346c5a6a 446 double f_norm[4];
aleksa98 0:19aa346c5a6a 447 f_norm[0]=0.5*(f[0][0]+f[1][0]+(f[0][0]-f[1][0])*cos(2*angle[0]));
aleksa98 0:19aa346c5a6a 448 f_norm[1]=0.5*(f[0][1]+f[1][1]+(f[0][1]-f[1][1])*cos(2*angle[1]));
aleksa98 0:19aa346c5a6a 449 f_norm[2]=0.5*(f[0][2]+f[1][2]+(f[0][2]-f[1][2])*cos(2*angle[2]));
aleksa98 0:19aa346c5a6a 450 f_norm[3]=0.5*(f[0][3]+f[1][3]+(f[0][3]-f[1][3])*cos(2*angle[3]));
aleksa98 0:19aa346c5a6a 451
aleksa98 0:19aa346c5a6a 452
aleksa98 0:19aa346c5a6a 453 Fi[0][0]=f_norm[0]*cos(angle[0]);
aleksa98 0:19aa346c5a6a 454 Fi[0][1]=f_norm[1]*cos(angle[1]);
aleksa98 0:19aa346c5a6a 455 Fi[0][2]=f_norm[2]*cos(angle[2]);
aleksa98 0:19aa346c5a6a 456 Fi[0][3]=f_norm[3]*cos(angle[3]);
aleksa98 0:19aa346c5a6a 457
aleksa98 0:19aa346c5a6a 458 Fi[1][0]=f_norm[0]*sin(angle[0]);
aleksa98 0:19aa346c5a6a 459 Fi[1][1]=f_norm[1]*sin(angle[1]);
aleksa98 0:19aa346c5a6a 460 Fi[1][2]=f_norm[2]*sin(angle[2]);
aleksa98 0:19aa346c5a6a 461 Fi[1][3]=f_norm[3]*sin(angle[3]);
aleksa98 0:19aa346c5a6a 462 ////////////
aleksa98 0:19aa346c5a6a 463 Fi[1][1]=-Fi[1][1];
aleksa98 0:19aa346c5a6a 464 Fi[1][3]=-Fi[1][3];
aleksa98 0:19aa346c5a6a 465
aleksa98 0:19aa346c5a6a 466 //pc.printf("Fi \n");
aleksa98 0:19aa346c5a6a 467 //pc.printf("%f %f %f %f\n",Fi[0][0],Fi[0][1],Fi[0][2],Fi[0][3]);
aleksa98 0:19aa346c5a6a 468 //pc.printf("%f %f %f %f\n",Fi[1][0],Fi[1][1],Fi[1][2],Fi[1][3]);
aleksa98 0:19aa346c5a6a 469 //pc.printf("%.20f %.20f %.20f %.20f\n",Fi[2][0],Fi[2][1],Fi[2][2],Fi[2][3]);
aleksa98 0:19aa346c5a6a 470
aleksa98 0:19aa346c5a6a 471 // right wheel should have antisymmetric
aleksa98 0:19aa346c5a6a 472 // characteristic compared to left -> right(x) = -left(-x) because generally
aleksa98 0:19aa346c5a6a 473 // left and right wheels of same axle have same characteristic but
aleksa98 0:19aa346c5a6a 474 // antisymmetric (implementation can be further optimized by calculating
aleksa98 0:19aa346c5a6a 475 // only front and rear frictions and than just distributing those)\
aleksa98 0:19aa346c5a6a 476
aleksa98 0:19aa346c5a6a 477 double aw[4];
aleksa98 0:19aa346c5a6a 478 aw[0]=(Tfl-Fi[0][0]*r[0])/Iw[0];
aleksa98 0:19aa346c5a6a 479 aw[1]=(Tfr-Fi[0][1]*r[1])/Iw[1];
aleksa98 0:19aa346c5a6a 480 aw[2]=(Trl-Fi[0][2]*r[2])/Iw[2];
aleksa98 0:19aa346c5a6a 481 aw[3]=(Trr-Fi[0][3]*r[3])/Iw[3];
aleksa98 0:19aa346c5a6a 482 //pc.printf("aw \n");
aleksa98 0:19aa346c5a6a 483 //pc.printf("%f %f %f %f\n",aw[0],aw[1],aw[2],aw[3]);
aleksa98 0:19aa346c5a6a 484 ////////////
aleksa98 0:19aa346c5a6a 485 double FL[4];
aleksa98 0:19aa346c5a6a 486 FL[0]=Fi[0][0];
aleksa98 0:19aa346c5a6a 487 FL[1]=Fi[1][0];
aleksa98 0:19aa346c5a6a 488 FL[2]=tempF[0][0];
aleksa98 0:19aa346c5a6a 489
aleksa98 0:19aa346c5a6a 490 double FR[3];
aleksa98 0:19aa346c5a6a 491 FR[0]=Fi[0][1];
aleksa98 0:19aa346c5a6a 492 FR[1]=Fi[1][1];
aleksa98 0:19aa346c5a6a 493 FR[2]=tempF[0][1];
aleksa98 0:19aa346c5a6a 494
aleksa98 0:19aa346c5a6a 495 double RL[3];
aleksa98 0:19aa346c5a6a 496 RL[0]=Fi[0][2];
aleksa98 0:19aa346c5a6a 497 RL[1]=Fi[1][2];
aleksa98 0:19aa346c5a6a 498 RL[2]=tempF[0][2];
aleksa98 0:19aa346c5a6a 499
aleksa98 0:19aa346c5a6a 500 double RR[3];
aleksa98 0:19aa346c5a6a 501 RR[0]=Fi[0][3];
aleksa98 0:19aa346c5a6a 502 RR[1]=Fi[1][3];
aleksa98 0:19aa346c5a6a 503 RR[2]=tempF[0][3];
aleksa98 0:19aa346c5a6a 504 ////////////
aleksa98 0:19aa346c5a6a 505
aleksa98 0:19aa346c5a6a 506 cosin=cos(-d);
aleksa98 0:19aa346c5a6a 507 sinus=sin(-d);
aleksa98 0:19aa346c5a6a 508 p1 = cosin*Fi[0][0]+sinus*Fi[1][0];
aleksa98 0:19aa346c5a6a 509 p2 = cosin*Fi[0][1]+sinus*Fi[1][1];
aleksa98 0:19aa346c5a6a 510 p3 = -sinus*Fi[0][0]+cosin*Fi[1][0];
aleksa98 0:19aa346c5a6a 511 p4 = -sinus*Fi[0][1]+cosin*Fi[1][1];
aleksa98 0:19aa346c5a6a 512
aleksa98 0:19aa346c5a6a 513 Fi[0][0]=p1;
aleksa98 0:19aa346c5a6a 514 Fi[0][1]=p2;
aleksa98 0:19aa346c5a6a 515 Fi[1][0]=p3;
aleksa98 0:19aa346c5a6a 516 Fi[1][1]=p4;
aleksa98 0:19aa346c5a6a 517
aleksa98 0:19aa346c5a6a 518 //pc.printf("Fi 2 put \n");
aleksa98 0:19aa346c5a6a 519 //pc.printf("%f %f %f %f\n",Fi[0][0],Fi[0][1],Fi[0][2],Fi[0][3]);
aleksa98 0:19aa346c5a6a 520 //pc.printf("%f %f %f %f\n",Fi[1][0],Fi[1][1],Fi[1][2],Fi[1][3]);
aleksa98 0:19aa346c5a6a 521 //pc.printf("%.20f %.20f %.20f %.20f\n",Fi[2][0],Fi[2][1],Fi[2][2],Fi[2][3]);
aleksa98 0:19aa346c5a6a 522 ///////////
aleksa98 0:19aa346c5a6a 523 double Mi[3][4];
aleksa98 0:19aa346c5a6a 524 Mi[0][0]=pw[1][0]*Fi[2][0]-pw[2][0]*Fi[1][0];
aleksa98 0:19aa346c5a6a 525 Mi[1][0]=pw[0][0]*Fi[2][0]-pw[2][0]*Fi[0][0];
aleksa98 0:19aa346c5a6a 526 Mi[2][0]=pw[0][0]*Fi[1][0]-pw[1][0]*Fi[0][0];
aleksa98 0:19aa346c5a6a 527
aleksa98 0:19aa346c5a6a 528 Mi[0][1]=pw[1][1]*Fi[2][1]-pw[2][1]*Fi[1][1];
aleksa98 0:19aa346c5a6a 529 Mi[1][1]=pw[0][1]*Fi[2][1]-pw[2][1]*Fi[0][1];
aleksa98 0:19aa346c5a6a 530 Mi[2][1]=pw[0][1]*Fi[1][1]-pw[1][1]*Fi[0][1];
aleksa98 0:19aa346c5a6a 531
aleksa98 0:19aa346c5a6a 532 Mi[0][2]=pw[1][2]*Fi[2][2]-pw[2][2]*Fi[1][2];
aleksa98 0:19aa346c5a6a 533 Mi[1][2]=pw[0][2]*Fi[2][2]-pw[2][2]*Fi[0][2];
aleksa98 0:19aa346c5a6a 534 Mi[2][2]=pw[0][2]*Fi[1][2]-pw[1][2]*Fi[0][2];
aleksa98 0:19aa346c5a6a 535
aleksa98 0:19aa346c5a6a 536 Mi[0][3]=pw[1][3]*Fi[2][3]-pw[2][3]*Fi[1][3];
aleksa98 0:19aa346c5a6a 537 Mi[1][3]=pw[0][3]*Fi[2][3]-pw[2][3]*Fi[0][3];
aleksa98 0:19aa346c5a6a 538 Mi[2][3]=pw[0][3]*Fi[1][3]-pw[1][3]*Fi[0][3];
aleksa98 0:19aa346c5a6a 539
aleksa98 0:19aa346c5a6a 540 //pc.printf("Mi \n");
aleksa98 0:19aa346c5a6a 541 //pc.printf("%f %f %f %f\n",Mi[0][0],Mi[0][1],Mi[0][2],Mi[0][3]);
aleksa98 0:19aa346c5a6a 542 //pc.printf("%f %f %f %f\n",Mi[1][0],Mi[1][1],Mi[1][2],Mi[1][3]);
aleksa98 0:19aa346c5a6a 543 //pc.printf("%f %f %f %f\n",Mi[2][0],Mi[2][1],Mi[2][2],Mi[2][3]);
aleksa98 0:19aa346c5a6a 544
aleksa98 0:19aa346c5a6a 545
aleksa98 0:19aa346c5a6a 546 ///////////
aleksa98 0:19aa346c5a6a 547 double F[3];
aleksa98 0:19aa346c5a6a 548 F[0]=Fi[0][0]+Fi[0][1]+Fi[0][2]+Fi[0][3];
aleksa98 0:19aa346c5a6a 549 F[1]=Fi[1][0]+Fi[1][1]+Fi[1][2]+Fi[1][3];
aleksa98 0:19aa346c5a6a 550 F[2]=Fi[2][0]+Fi[2][1]+Fi[2][2]+Fi[2][3];
aleksa98 0:19aa346c5a6a 551 //pc.printf("F \n");
aleksa98 0:19aa346c5a6a 552 //pc.printf("%f %f %f\n",F[0],F[1],F[2]);
aleksa98 0:19aa346c5a6a 553 ///////////
aleksa98 0:19aa346c5a6a 554 double M[3];
aleksa98 0:19aa346c5a6a 555 M[0]=Mi[0][0]+Mi[0][1]+Mi[0][2]+Mi[0][3];
aleksa98 0:19aa346c5a6a 556 M[1]=Mi[1][0]+Mi[1][1]+Mi[1][2]+Mi[1][3];
aleksa98 0:19aa346c5a6a 557 M[2]=Mi[2][0]+Mi[2][1]+Mi[2][2]+Mi[2][3];
aleksa98 0:19aa346c5a6a 558 //pc.printf("M \n");
aleksa98 0:19aa346c5a6a 559 //pc.printf("%f %f %f\n",M[0],M[1],M[2]);
aleksa98 0:19aa346c5a6a 560 ///////////
aleksa98 0:19aa346c5a6a 561 double acp[3];
aleksa98 0:19aa346c5a6a 562 acp[0]=va[1]*v[2]-va[2]*v[1];
aleksa98 0:19aa346c5a6a 563 acp[1]=va[0]*v[2]-va[2]*v[0];
aleksa98 0:19aa346c5a6a 564 acp[2]=va[0]*v[1]-va[1]*v[0];
aleksa98 0:19aa346c5a6a 565 //pc.printf("acp \n");
aleksa98 0:19aa346c5a6a 566 //pc.printf("%f %f %f\n",acp[0],acp[1],acp[2]);
aleksa98 0:19aa346c5a6a 567 ///////////
aleksa98 0:19aa346c5a6a 568 double a[3];
aleksa98 0:19aa346c5a6a 569 a[0]=F[0]/m[0]-acp[0];
aleksa98 0:19aa346c5a6a 570 a[1]=F[1]/m[1]-acp[1];
aleksa98 0:19aa346c5a6a 571 a[2]=F[2]/m[2]-acp[2];
aleksa98 0:19aa346c5a6a 572 //pc.printf("a \n");
aleksa98 0:19aa346c5a6a 573 //pc.printf("%f %f %f\n",a[0],a[1],a[2]);
aleksa98 0:19aa346c5a6a 574 ///////////
aleksa98 0:19aa346c5a6a 575 /////////// aa = I_inv * (M - cross(va, I*va)); % Euler's equation
aleksa98 0:19aa346c5a6a 576 double pom1[3];
aleksa98 0:19aa346c5a6a 577 pom1[0]=I[0][0]*va[0]+I[0][1]*va[1]+I[0][2]*va[2];
aleksa98 0:19aa346c5a6a 578 pom1[1]=I[1][0]*va[0]+I[1][1]*va[1]+I[1][2]*va[2];
aleksa98 0:19aa346c5a6a 579 pom1[2]=I[2][0]*va[0]+I[2][1]*va[1]+I[2][2]*va[2];
aleksa98 0:19aa346c5a6a 580 double pom2[3];
aleksa98 0:19aa346c5a6a 581 pom2[0]=va[1]*pom1[2]-va[2]*pom1[1];
aleksa98 0:19aa346c5a6a 582 pom2[1]=va[0]*pom1[2]-va[2]*pom1[0];
aleksa98 0:19aa346c5a6a 583 pom2[2]=va[0]*pom1[1]-va[1]*pom1[0];
aleksa98 0:19aa346c5a6a 584 double pom3[3];
aleksa98 0:19aa346c5a6a 585 pom3[0]=M[0]-pom2[0];
aleksa98 0:19aa346c5a6a 586 pom3[1]=M[1]-pom2[1];
aleksa98 0:19aa346c5a6a 587 pom3[2]=M[2]-pom2[2];
aleksa98 0:19aa346c5a6a 588 double aa[3];
aleksa98 0:19aa346c5a6a 589 aa[0]=I_inv[0][0]*pom3[0]+I_inv[0][1]*pom3[1]+I_inv[0][2]*pom3[2];
aleksa98 0:19aa346c5a6a 590 aa[1]=I_inv[1][0]*pom3[0]+I_inv[1][1]*pom3[1]+I_inv[1][2]*pom3[2];
aleksa98 0:19aa346c5a6a 591 aa[2]=I_inv[2][0]*pom3[0]+I_inv[2][1]*pom3[1]+I_inv[2][2]*pom3[2];
aleksa98 0:19aa346c5a6a 592 //pc.printf("aa \n");
aleksa98 0:19aa346c5a6a 593 //pc.printf("%f %f %f\n",aa[0],aa[1],aa[2]);
aleksa98 0:19aa346c5a6a 594 ////////////
aleksa98 0:19aa346c5a6a 595 v[0]=v[0]+a[0]*Ts;
aleksa98 0:19aa346c5a6a 596 v[1]=v[1]+a[1]*Ts;
aleksa98 0:19aa346c5a6a 597 v[2]=v[2]+a[2]*Ts;
aleksa98 0:19aa346c5a6a 598 //pc.printf("v \n");
aleksa98 0:19aa346c5a6a 599 //pc.printf("%f %f %f\n",v[0],v[1],v[2]);
aleksa98 0:19aa346c5a6a 600 ////////////
aleksa98 0:19aa346c5a6a 601 va[0]=va[0]+aa[0]*Ts;
aleksa98 0:19aa346c5a6a 602 va[1]=va[1]+aa[1]*Ts;
aleksa98 0:19aa346c5a6a 603 va[2]=va[2]+aa[2]*Ts;
aleksa98 0:19aa346c5a6a 604 //pc.printf("va \n");
aleksa98 0:19aa346c5a6a 605 //pc.printf("%f %f %f\n",va[0],va[1],va[2]);
aleksa98 0:19aa346c5a6a 606 ////////////
aleksa98 0:19aa346c5a6a 607 w[0]=w[0]+aw[0]*Ts;
aleksa98 0:19aa346c5a6a 608 w[1]=w[1]+aw[1]*Ts;
aleksa98 0:19aa346c5a6a 609 w[2]=w[2]+aw[2]*Ts;
aleksa98 0:19aa346c5a6a 610 w[3]=w[3]+aw[3]*Ts;
aleksa98 0:19aa346c5a6a 611 //pc.printf("w \n");
aleksa98 0:19aa346c5a6a 612 //pc.printf("%f %f %f %f\n",w[0],w[1],w[2],w[3]);
aleksa98 0:19aa346c5a6a 613 //////////// R3D_xyz (Rx=I, Ry=I)
aleksa98 0:19aa346c5a6a 614 double Rz[3][3]={
aleksa98 0:19aa346c5a6a 615 {cos(pa[2]), -sin(pa[2]), 0},
aleksa98 0:19aa346c5a6a 616 {sin(pa[2]), cos(pa[2]), 0},
aleksa98 0:19aa346c5a6a 617 {0, 0, 1}
aleksa98 0:19aa346c5a6a 618 };
aleksa98 0:19aa346c5a6a 619 p[0]=p[0]+(Rz[0][0]*v[0]+Rz[0][1]*v[1]+Rz[0][2]*v[2])*Ts;
aleksa98 0:19aa346c5a6a 620 p[1]=p[1]+(Rz[1][0]*v[0]+Rz[1][1]*v[1]+Rz[1][2]*v[2])*Ts;
aleksa98 0:19aa346c5a6a 621 p[2]=p[2]+(Rz[2][0]*v[0]+Rz[2][1]*v[1]+Rz[2][2]*v[2])*Ts;
aleksa98 0:19aa346c5a6a 622 //pc.printf("p \n");
aleksa98 0:19aa346c5a6a 623 pc.printf("%f %f %f\n",p[0],p[1],p[2]);
aleksa98 0:19aa346c5a6a 624 /////////////
aleksa98 0:19aa346c5a6a 625 pa[0]=pa[0]+va[0]*Ts;
aleksa98 0:19aa346c5a6a 626 pa[1]=pa[1]+va[1]*Ts;
aleksa98 0:19aa346c5a6a 627 pa[2]=pa[2]+va[2]*Ts;
aleksa98 0:19aa346c5a6a 628 //pc.printf("pa \n");
aleksa98 0:19aa346c5a6a 629 //pc.printf("%f %f %f\n",pa[0],pa[1],pa[2]);
aleksa98 0:19aa346c5a6a 630
aleksa98 0:19aa346c5a6a 631
aleksa98 0:19aa346c5a6a 632 }
aleksa98 0:19aa346c5a6a 633
aleksa98 0:19aa346c5a6a 634
aleksa98 0:19aa346c5a6a 635
aleksa98 0:19aa346c5a6a 636
aleksa98 0:19aa346c5a6a 637
aleksa98 0:19aa346c5a6a 638 CANMessage msg;
aleksa98 0:19aa346c5a6a 639
aleksa98 0:19aa346c5a6a 640 void callbackCAN(){
aleksa98 0:19aa346c5a6a 641 flag=1;
aleksa98 0:19aa346c5a6a 642 can1.read(msg);
aleksa98 0:19aa346c5a6a 643 if(msg.id==0x0184){
aleksa98 0:19aa346c5a6a 644 T_1=(double)((int16_t)((msg.data[4]<<8)+msg.data[5]))*Mn/1000;
aleksa98 0:19aa346c5a6a 645
aleksa98 0:19aa346c5a6a 646 }
aleksa98 0:19aa346c5a6a 647 else if(msg.id==0x0185){
aleksa98 0:19aa346c5a6a 648 T_2=(double)((int16_t)((msg.data[4]<<8)+msg.data[5]))*Mn/1000;
aleksa98 0:19aa346c5a6a 649 }
aleksa98 0:19aa346c5a6a 650 else if(msg.id==0x0188){
aleksa98 0:19aa346c5a6a 651 T_3=(double)((int16_t)((msg.data[4]<<8)+msg.data[5]))*Mn/1000;
aleksa98 0:19aa346c5a6a 652 }
aleksa98 0:19aa346c5a6a 653 else if(msg.id==0x0189){
aleksa98 0:19aa346c5a6a 654 T_4=(double)((int16_t)((msg.data[4]<<8)+msg.data[5]))*Mn/1000;
aleksa98 0:19aa346c5a6a 655 }
aleksa98 0:19aa346c5a6a 656
aleksa98 0:19aa346c5a6a 657
aleksa98 0:19aa346c5a6a 658 cnt++;
aleksa98 0:19aa346c5a6a 659
aleksa98 0:19aa346c5a6a 660 }
aleksa98 0:19aa346c5a6a 661
aleksa98 0:19aa346c5a6a 662 Timer t;
aleksa98 0:19aa346c5a6a 663
aleksa98 0:19aa346c5a6a 664 int main() {
aleksa98 0:19aa346c5a6a 665 initialize();
aleksa98 0:19aa346c5a6a 666 //can1.filter(RX_ID, 0x03F0,CANStandard, 0);
aleksa98 0:19aa346c5a6a 667
aleksa98 0:19aa346c5a6a 668 //messageOutVel1.id = 0x282;
aleksa98 0:19aa346c5a6a 669 //messageOutVel2.id = 0x283;
aleksa98 0:19aa346c5a6a 670 //messageOutVel3.id = 0x287;
aleksa98 0:19aa346c5a6a 671 //messageOutVel4.id = 0x288;
aleksa98 0:19aa346c5a6a 672
aleksa98 0:19aa346c5a6a 673
aleksa98 0:19aa346c5a6a 674
aleksa98 0:19aa346c5a6a 675 //messageOut2.format = CANStandard;// or CANExtended; // standard or extended ID (can be skipped for standard)
aleksa98 0:19aa346c5a6a 676 // messageOut2.len = 1;//length in bytes (1 to 8);
aleksa98 0:19aa346c5a6a 677 //messageOut1.format = CANStandard;// or CANExtended; // standard or extended ID (can be skipped for standard)
aleksa98 0:19aa346c5a6a 678 //messageOut1.len = 1;//length in bytes (1 to 8);
aleksa98 0:19aa346c5a6a 679
aleksa98 0:19aa346c5a6a 680 //can1.frequency(500000);
aleksa98 0:19aa346c5a6a 681
aleksa98 0:19aa346c5a6a 682 // tick1.attach(&send, 0.0005);
aleksa98 0:19aa346c5a6a 683
aleksa98 0:19aa346c5a6a 684 //can1.attach(callbackCAN);
aleksa98 0:19aa346c5a6a 685
aleksa98 0:19aa346c5a6a 686
aleksa98 0:19aa346c5a6a 687
aleksa98 0:19aa346c5a6a 688 //while(1){
aleksa98 0:19aa346c5a6a 689 // while(!flag);
aleksa98 0:19aa346c5a6a 690 // pc.printf("V: %f",v*3.6);
aleksa98 0:19aa346c5a6a 691 // pc.printf(" T1: %f",T_1);
aleksa98 0:19aa346c5a6a 692 // pc.printf(" T2: %f",T_2);
aleksa98 0:19aa346c5a6a 693 // pc.printf(" T3: %f",T_3);
aleksa98 0:19aa346c5a6a 694 // pc.printf(" T4: %f",T_4);
aleksa98 0:19aa346c5a6a 695 // pc.printf(" Cnt: %d",cnt);
aleksa98 0:19aa346c5a6a 696 // pc.printf("\n");
aleksa98 0:19aa346c5a6a 697 //
aleksa98 0:19aa346c5a6a 698 // flag=0;
aleksa98 0:19aa346c5a6a 699 //}
aleksa98 0:19aa346c5a6a 700
aleksa98 0:19aa346c5a6a 701 //t.start();
aleksa98 0:19aa346c5a6a 702 while(1){
aleksa98 0:19aa346c5a6a 703 model(450,450,450,450,0);
aleksa98 0:19aa346c5a6a 704 wait(0.01);
aleksa98 0:19aa346c5a6a 705 }
aleksa98 0:19aa346c5a6a 706
aleksa98 0:19aa346c5a6a 707
aleksa98 0:19aa346c5a6a 708
aleksa98 0:19aa346c5a6a 709 }