HIL 14
Dependencies: mbed MatrixMath Matrix
main.cpp@1:a12ef11ea8d6, 2021-04-11 (annotated)
- Committer:
- aleksa98
- Date:
- Sun Apr 11 10:36:55 2021 +0000
- Revision:
- 1:a12ef11ea8d6
- Parent:
- 0:19aa346c5a6a
Nova biblioteka dodata
Who changed what in which revision?
User | Revision | Line number | New 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 | } |