Mechatronics / 2015_04_17_dydakt

Dydaktyka

Dependencies:   FastPWM mbed-src

Fork of 2015_04_17_quadro_bez_sterowania by Quadrocopter

main.cpp@9:4e94a16ca90c, 2015-01-29 (annotated)

Committer:
Michu90
Date:
Thu Jan 29 14:17:43 2015 +0000
Revision:
9:4e94a16ca90c
Parent:
8:dc48ce79ad59
Child:
10:605b0bfadc2e
pomiar omegi

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mbed_official 0:50d2b9c62765 1 #include "mbed.h"
Igor_W 3:1425359662e4 2 #include "FastPWM.h"
Michu90 5:c3caf8b83e6b 3 #include "kalman.h"
Michu90 5:c3caf8b83e6b 4 #include "Offsets.h"
Michu90 5:c3caf8b83e6b 5 #include "stdio.h"
Michu90 5:c3caf8b83e6b 6 #include "IMU.h"
Michu90 5:c3caf8b83e6b 7
Igor_W 2:9e6ed6302c78 8 #define PWM_period 2500
Michu90 5:c3caf8b83e6b 9 #define M_PI 3.141592
Michu90 5:c3caf8b83e6b 10 #define M_PI2 1.570796
Michu90 5:c3caf8b83e6b 11 #define dt 0.005
Michu90 9:4e94a16ca90c 12 #define filtrWymiar 5
Michu90 9:4e94a16ca90c 13 #define filtrSygnal roll
Michu90 9:4e94a16ca90c 14 #define filtr2Wymiar 5
Michu90 9:4e94a16ca90c 15 #define filtr2Sygnal pitch
Michu90 8:dc48ce79ad59 16 #define filtr3Wymiar 100
Michu90 8:dc48ce79ad59 17 #define filtr3Sygnal d[5]
Michu90 5:c3caf8b83e6b 18
Michu90 5:c3caf8b83e6b 19
Michu90 5:c3caf8b83e6b 20 //kalman
Michu90 5:c3caf8b83e6b 21 // Structs for containing filter data
Michu90 5:c3caf8b83e6b 22 kalman_data pitch_data;
Michu90 5:c3caf8b83e6b 23 kalman_data roll_data;
Michu90 5:c3caf8b83e6b 24
Michu90 5:c3caf8b83e6b 25 kalman_data pitch_data2;
Michu90 5:c3caf8b83e6b 26 kalman_data roll_data2;
Michu90 5:c3caf8b83e6b 27
Michu90 5:c3caf8b83e6b 28 DigitalOut myled(PTA2);
Michu90 5:c3caf8b83e6b 29 DigitalOut myled2(PTA1);
Michu90 5:c3caf8b83e6b 30 Serial pc(USBTX, USBRX);
Michu90 5:c3caf8b83e6b 31 Serial bluetooth(D1, D0);
Michu90 5:c3caf8b83e6b 32 IMU imu(PTE25,PTE24);
Michu90 5:c3caf8b83e6b 33 Offsets off;
Michu90 9:4e94a16ca90c 34 InterruptIn event(PTB23);
Michu90 5:c3caf8b83e6b 35
Michu90 5:c3caf8b83e6b 36 FastPWM M1(D10);
Michu90 5:c3caf8b83e6b 37 FastPWM M2(D11);
Michu90 5:c3caf8b83e6b 38 FastPWM M3(D12);
Michu90 5:c3caf8b83e6b 39 FastPWM M4(D13);
Igor_W 2:9e6ed6302c78 40
Igor_W 2:9e6ed6302c78 41
Michu90 5:c3caf8b83e6b 42 Ticker triger1; //przerwanie filtracji
Michu90 8:dc48ce79ad59 43 Ticker triger2; //kalibracja accelero
Michu90 8:dc48ce79ad59 44 Ticker triger3; //kalibracja zyro
Michu90 9:4e94a16ca90c 45 Ticker tachometer;
Igor_W 2:9e6ed6302c78 46
Michu90 5:c3caf8b83e6b 47 float d[9];
Michu90 5:c3caf8b83e6b 48 double D[9];
Michu90 5:c3caf8b83e6b 49 float o[3];
Michu90 5:c3caf8b83e6b 50 float O[3];
Michu90 5:c3caf8b83e6b 51 char buff[160];
Michu90 5:c3caf8b83e6b 52 float r,katx,katy;
Michu90 5:c3caf8b83e6b 53 float rbut,katxbut,katybut;
Michu90 5:c3caf8b83e6b 54 float pitch, roll;
Michu90 5:c3caf8b83e6b 55 float pitch2, roll2;
Michu90 7:2ba30a0cdc16 56 float pitchE, rollE;
Michu90 5:c3caf8b83e6b 57 double i;
Michu90 5:c3caf8b83e6b 58 float offsetGyro[3];
Michu90 5:c3caf8b83e6b 59 char odczyt[20];
Michu90 4:a5b51a651db7 60 char znak;
Michu90 4:a5b51a651db7 61 char znak2;
Michu90 7:2ba30a0cdc16 62 float Kp1,Td1,Kd1,Kp2,Td2,Kd2,Kp3,Td3,Kd3,Ti1,Ki1,Ti2,Ki2,Ti3,Ki3,T;
Michu90 6:8cc6df266363 63 float U1,U2,U3;
Michu90 6:8cc6df266363 64 float Om1,Om2,Om3,Om4;
Michu90 6:8cc6df266363 65 float wyp1,wyp2,wyp3,wyp4;
Michu90 9:4e94a16ca90c 66 int impulsA;
Michu90 5:c3caf8b83e6b 67
Michu90 5:c3caf8b83e6b 68 double PWM1zad;
Michu90 5:c3caf8b83e6b 69 double PWM2zad;
Michu90 5:c3caf8b83e6b 70 double PWM3zad;
Michu90 5:c3caf8b83e6b 71 double PWM4zad;
Michu90 5:c3caf8b83e6b 72 double valPWM1;
Michu90 5:c3caf8b83e6b 73 double valPWM2;
Michu90 5:c3caf8b83e6b 74 double valPWM3;
Michu90 5:c3caf8b83e6b 75 double valPWM4;
Michu90 8:dc48ce79ad59 76 float T1zad,T2zad,T3zad,T4zad;
Michu90 8:dc48ce79ad59 77
Michu90 8:dc48ce79ad59 78 //zmienne tymczasowe
Michu90 8:dc48ce79ad59 79 float katxzyro,katyzyro,katzzyro;
Michu90 8:dc48ce79ad59 80 float fkompl;
Michu90 8:dc48ce79ad59 81 float xmin, ymin, zmin;
Michu90 8:dc48ce79ad59 82 float xmax, ymax, zmax;
Michu90 8:dc48ce79ad59 83 float filtrBufor[filtrWymiar];
Michu90 8:dc48ce79ad59 84 int filtrZmienna=0;
Michu90 8:dc48ce79ad59 85 float filtrSuma=0;
Michu90 8:dc48ce79ad59 86 float filtrWynik;
Michu90 8:dc48ce79ad59 87 float filtr2Bufor[filtrWymiar];
Michu90 8:dc48ce79ad59 88 int filtr2Zmienna=0;
Michu90 8:dc48ce79ad59 89 float filtr2Suma=0;
Michu90 8:dc48ce79ad59 90 float filtr2Wynik;
Michu90 8:dc48ce79ad59 91 float filtr3Bufor[filtrWymiar];
Michu90 8:dc48ce79ad59 92 int filtr3Zmienna=0;
Michu90 8:dc48ce79ad59 93 float filtr3Suma=0;
Michu90 8:dc48ce79ad59 94 float filtr3Wynik;
Michu90 8:dc48ce79ad59 95
Michu90 8:dc48ce79ad59 96 float kalmanpitchzyro;
Michu90 9:4e94a16ca90c 97 float kalmanpitchzyrobut;
Michu90 8:dc48ce79ad59 98 float kalmanrollzyro;
Michu90 8:dc48ce79ad59 99 float kalmanrollzyrobut;
Michu90 8:dc48ce79ad59 100 float kalmanpitchdryf;
Michu90 5:c3caf8b83e6b 101
Michu90 9:4e94a16ca90c 102 int RPMtach;
Michu90 9:4e94a16ca90c 103 float PWM4zadBuf[5];
Michu90 9:4e94a16ca90c 104 float PWM4zadPoprz;
Michu90 9:4e94a16ca90c 105 float PWM4zadWyn;
Michu90 9:4e94a16ca90c 106 float RPMtachBuf[5];
Michu90 9:4e94a16ca90c 107 int RPMtachPoprz;
Michu90 9:4e94a16ca90c 108 int RPMtachAktualny;
Michu90 9:4e94a16ca90c 109 float RPMtachWyn;
Michu90 9:4e94a16ca90c 110 int RPMtachWynPoprz;
Michu90 9:4e94a16ca90c 111 int iTach;
Michu90 9:4e94a16ca90c 112 bool TachFlaga;
Michu90 9:4e94a16ca90c 113
Michu90 9:4e94a16ca90c 114 void rise(void)
Michu90 9:4e94a16ca90c 115 {
Michu90 9:4e94a16ca90c 116 //myled = !myled;
Michu90 9:4e94a16ca90c 117
Michu90 9:4e94a16ca90c 118 if(TachFlaga==1)impulsA++;
Michu90 9:4e94a16ca90c 119
Michu90 9:4e94a16ca90c 120 }
Michu90 9:4e94a16ca90c 121
Michu90 9:4e94a16ca90c 122
Michu90 9:4e94a16ca90c 123
Michu90 5:c3caf8b83e6b 124 void task1()
Michu90 5:c3caf8b83e6b 125 {
Michu90 7:2ba30a0cdc16 126 //myled = !myled;
Michu90 5:c3caf8b83e6b 127 imu.readData(d);
Michu90 5:c3caf8b83e6b 128 imu.filterData(d, D);
Michu90 5:c3caf8b83e6b 129 off.offsetData(d,offsetGyro,o);
Michu90 5:c3caf8b83e6b 130 off.offsetData2(D,offsetGyro,O);
Michu90 5:c3caf8b83e6b 131
Michu90 5:c3caf8b83e6b 132 r = sqrt(pow(d[3],2) + pow(d[4],2) + pow(d[5],2));
Michu90 5:c3caf8b83e6b 133 katx = acos(d[4]/r)-M_PI2;
Michu90 5:c3caf8b83e6b 134 katy = acos(d[3]/r)-M_PI2;
Michu90 5:c3caf8b83e6b 135
Michu90 9:4e94a16ca90c 136 /*rbut = sqrt(pow(D[3],2) + pow(D[4],2) + pow(D[5],2));
Michu90 5:c3caf8b83e6b 137 katxbut = acos(D[4]/rbut)-M_PI2;
Michu90 9:4e94a16ca90c 138 katybut = acos(D[3]/rbut)-M_PI2;*/
Michu90 5:c3caf8b83e6b 139
Michu90 8:dc48ce79ad59 140 //katxzyro = katxzyro + o[0]*dt;
Michu90 8:dc48ce79ad59 141 //fkompl = katyzyro*0.95+ (-katy)*0.05;
Michu90 5:c3caf8b83e6b 142
Michu90 5:c3caf8b83e6b 143 //Filtr Kalmana
Michu90 5:c3caf8b83e6b 144 kalman_innovate(&pitch_data, katx, o[0]);
Michu90 5:c3caf8b83e6b 145 kalman_innovate(&roll_data, -katy, o[1]);
Michu90 9:4e94a16ca90c 146 pitch = -pitch_data.x1;
Michu90 9:4e94a16ca90c 147 kalmanpitchzyro = pitch_data.x2;
Michu90 5:c3caf8b83e6b 148 roll = roll_data.x1;
Michu90 8:dc48ce79ad59 149 kalmanrollzyro = roll_data.x2;
Michu90 9:4e94a16ca90c 150
Michu90 9:4e94a16ca90c 151 /*
Michu90 5:c3caf8b83e6b 152 //Filtr Kalmana butterworth 2nd
Michu90 5:c3caf8b83e6b 153 kalman_innovate(&pitch_data2, katxbut, O[0]);
Michu90 5:c3caf8b83e6b 154 kalman_innovate(&roll_data2, -katybut, O[1]);
Michu90 5:c3caf8b83e6b 155 pitch2 = pitch_data2.x1;
Michu90 9:4e94a16ca90c 156 kalmanpitchzyrobut = pitch_data2.x2;
Michu90 5:c3caf8b83e6b 157 roll2 = roll_data2.x1;
Michu90 9:4e94a16ca90c 158 kalmanrollzyrobut = roll_data2.x2;*/
Michu90 9:4e94a16ca90c 159
Michu90 9:4e94a16ca90c 160
Michu90 9:4e94a16ca90c 161 if (filtrZmienna < 0) {
Michu90 9:4e94a16ca90c 162 filtrWynik = filtrSygnal;
Michu90 9:4e94a16ca90c 163 filtrBufor[filtrZmienna + filtrWymiar] = filtrSygnal;
Michu90 9:4e94a16ca90c 164 filtrSuma = filtrSuma + filtrBufor[filtrZmienna + filtrWymiar];
Michu90 9:4e94a16ca90c 165 } else {
Michu90 9:4e94a16ca90c 166 if (filtrZmienna == filtrWymiar - 1) {
Michu90 9:4e94a16ca90c 167 filtrSuma = filtrSuma - filtrBufor[filtrZmienna];
Michu90 9:4e94a16ca90c 168 filtrBufor[filtrZmienna] = filtrSygnal;
Michu90 9:4e94a16ca90c 169 filtrSuma = filtrSuma + filtrBufor[filtrZmienna];
Michu90 9:4e94a16ca90c 170 filtrWynik = filtrSuma / filtrWymiar;
Michu90 9:4e94a16ca90c 171 } else {
Michu90 9:4e94a16ca90c 172 filtrSuma = filtrSuma - filtrBufor[filtrZmienna];
Michu90 9:4e94a16ca90c 173 filtrBufor[filtrZmienna] = filtrSygnal;
Michu90 9:4e94a16ca90c 174 filtrSuma = filtrSuma + filtrBufor[filtrZmienna];
Michu90 9:4e94a16ca90c 175 filtrWynik = filtrSuma / filtrWymiar;
Michu90 9:4e94a16ca90c 176 }
Michu90 9:4e94a16ca90c 177 }
Michu90 9:4e94a16ca90c 178 filtrZmienna++;
Michu90 9:4e94a16ca90c 179 if (filtrZmienna == filtrWymiar) {
Michu90 9:4e94a16ca90c 180 filtrZmienna = 0;
Michu90 9:4e94a16ca90c 181 }
Michu90 9:4e94a16ca90c 182
Michu90 9:4e94a16ca90c 183 roll = filtrWynik;
Michu90 9:4e94a16ca90c 184
Michu90 9:4e94a16ca90c 185 if (filtr2Zmienna < 0) {
Michu90 9:4e94a16ca90c 186 filtr2Wynik = filtr2Sygnal;
Michu90 9:4e94a16ca90c 187 filtr2Bufor[filtr2Zmienna + filtr2Wymiar] = filtr2Sygnal;
Michu90 9:4e94a16ca90c 188 filtr2Suma = filtr2Suma + filtr2Bufor[filtr2Zmienna + filtr2Wymiar];
Michu90 9:4e94a16ca90c 189 } else {
Michu90 9:4e94a16ca90c 190 if (filtr2Zmienna == filtr2Wymiar - 1) {
Michu90 9:4e94a16ca90c 191 filtr2Suma = filtr2Suma - filtr2Bufor[filtr2Zmienna];
Michu90 9:4e94a16ca90c 192 filtr2Bufor[filtr2Zmienna] = filtr2Sygnal;
Michu90 9:4e94a16ca90c 193 filtr2Suma = filtr2Suma + filtr2Bufor[filtr2Zmienna];
Michu90 9:4e94a16ca90c 194 filtr2Wynik = filtr2Suma / filtr2Wymiar;
Michu90 9:4e94a16ca90c 195 } else {
Michu90 9:4e94a16ca90c 196 filtr2Suma = filtr2Suma - filtr2Bufor[filtr2Zmienna];
Michu90 9:4e94a16ca90c 197 filtr2Bufor[filtr2Zmienna] = filtr2Sygnal;
Michu90 9:4e94a16ca90c 198 filtr2Suma = filtr2Suma + filtr2Bufor[filtr2Zmienna];
Michu90 9:4e94a16ca90c 199 filtr2Wynik = filtr2Suma / filtr2Wymiar;
Michu90 9:4e94a16ca90c 200 }
Michu90 9:4e94a16ca90c 201 }
Michu90 9:4e94a16ca90c 202 filtr2Zmienna++;
Michu90 9:4e94a16ca90c 203 if (filtr2Zmienna == filtr2Wymiar) {
Michu90 9:4e94a16ca90c 204 filtr2Zmienna = 0;
Michu90 9:4e94a16ca90c 205 }
Michu90 9:4e94a16ca90c 206
Michu90 9:4e94a16ca90c 207 pitch=filtr2Wynik;
Michu90 5:c3caf8b83e6b 208
Michu90 7:2ba30a0cdc16 209 //U1 = 0.0173*(Kp1*(-pitch2)+Kd1*(0-O[0]));
Michu90 7:2ba30a0cdc16 210 //U2 = 0.0169*(Kp2*((-20*M_PI/180)-roll2)+Kd2*(0-O[1]));
Michu90 7:2ba30a0cdc16 211 //U3 = 0.0333*(/*Kp3*((105*M_PI/180)-fYaw)+*/Kd3*(0-O[2])*180/M_PI);
Michu90 6:8cc6df266363 212
Michu90 7:2ba30a0cdc16 213 //Om1 = 0.00576066*pow((PWM1zad-10000),2) - U2/0.000024768 + U3/0.000132 ; //kwadraty
Michu90 7:2ba30a0cdc16 214 //Om2 = 0.00576066*pow((PWM2zad-10000),2) + U1/0.000024768 - U3/0.000132 ;
Michu90 7:2ba30a0cdc16 215 //Om3 = 0.00576066*pow((PWM3zad-10000),2) + U2/0.000024768 + U3/0.000132 ;
Michu90 7:2ba30a0cdc16 216 //Om4 = 0.00576066*pow((PWM4zad-10000),2) - U1/0.000024768 - U3/0.000132 ;
Michu90 6:8cc6df266363 217
Michu90 7:2ba30a0cdc16 218 /*wyp1 = sqrt(Om1)*13.17523+10000;
Michu90 6:8cc6df266363 219 wyp2 = sqrt(Om2)*13.17523+10000;
Michu90 6:8cc6df266363 220 wyp3 = sqrt(Om3)*13.17523+10000;
Michu90 7:2ba30a0cdc16 221 wyp4 = sqrt(Om4)*13.17523+10000;*/
Michu90 6:8cc6df266363 222
Michu90 6:8cc6df266363 223
Michu90 7:2ba30a0cdc16 224 // *********************** DISCRETE PID CONTROLLER **********************
Michu90 7:2ba30a0cdc16 225 // U1=Ixx*(Kp1*(katzad-kat)+Kd1*(omegazad-omega)+Ki1*sumauchyb)
Michu90 7:2ba30a0cdc16 226 // U2=Iyy*(Kp2*(katzad-kat)+Kd2*(omegazad-omega)+Ki2*sumauchyb)
Michu90 7:2ba30a0cdc16 227 // U3=Izz*(Kp3*(katzad-kat)+Kd3*(omegazad-omega)+Ki3*sumauchyb)
Michu90 9:4e94a16ca90c 228 //omega1^2=(B1/b1*PWM1zad+C1/b1)-U2/2b1l+U3/4d
Michu90 9:4e94a16ca90c 229 //omega2^2=(B2/b2*PWM2zad+C2/b2)+U1/2b2l-U3/4d
Michu90 9:4e94a16ca90c 230 //omega3^2=(B3/b3*PWM3zad+C3/b3)+U2/2b3l+U3/4d
Michu90 9:4e94a16ca90c 231 //omega4^2=(B4/b4*PWM4zad+C4/b4)-U1/2b4l-U3/4d
Michu90 7:2ba30a0cdc16 232 //wyp1=b/B1*omega1^2-C1/B1
Michu90 7:2ba30a0cdc16 233 //wyp2=b/B*omega2^2-C/B
Michu90 7:2ba30a0cdc16 234 //wyp3=b/B3*omega3^2-C3/B3
Michu90 7:2ba30a0cdc16 235 //wyp4=b/B*omega4^2-C/B
Michu90 7:2ba30a0cdc16 236
Michu90 7:2ba30a0cdc16 237 //b=0.000015
Michu90 7:2ba30a0cdc16 238 //B1=0.000776646
Michu90 7:2ba30a0cdc16 239 //C1=-0.334958973
Michu90 7:2ba30a0cdc16 240 //l=0.3
Michu90 7:2ba30a0cdc16 241 //d=0.000033
Michu90 9:4e94a16ca90c 242 //2b1l=0.0000092780
Michu90 7:2ba30a0cdc16 243 //4d=0.000132
Michu90 9:4e94a16ca90c 244
Michu90 9:4e94a16ca90c 245 //B1/b1=50.22498189
Michu90 9:4e94a16ca90c 246 //C1/b1=-21611.4954
Michu90 9:4e94a16ca90c 247 //b1/B1=0.01991041
Michu90 7:2ba30a0cdc16 248 //C1/B1=-431.2892625
Michu90 8:dc48ce79ad59 249 //1/B1=1287.588322
Michu90 8:dc48ce79ad59 250
Michu90 9:4e94a16ca90c 251 //2b2l=0.000009424
Michu90 9:4e94a16ca90c 252 //B2/b2=51.130533
Michu90 9:4e94a16ca90c 253 //C2/b2=-30717.00651
Michu90 9:4e94a16ca90c 254 //b2/B2=0.019557786
Michu90 9:4e94a16ca90c 255 //C2/B2=-600.7566265
Michu90 7:2ba30a0cdc16 256
Michu90 9:4e94a16ca90c 257 //2b3l=0.000009139
Michu90 9:4e94a16ca90c 258 //B3/b3=50.66613192
Michu90 9:4e94a16ca90c 259 //C3/b3=-26072.99576
Michu90 9:4e94a16ca90c 260 //b3/B3=0.01973705
Michu90 7:2ba30a0cdc16 261 //C3/B3=-514.6040317
Michu90 8:dc48ce79ad59 262
Michu90 9:4e94a16ca90c 263 //2b4l=0.000009276
Michu90 9:4e94a16ca90c 264 //B4/b4=50.95680893
Michu90 9:4e94a16ca90c 265 //C4/b4=-28979.76588
Michu90 9:4e94a16ca90c 266 //b4/B4=0.019624463
Michu90 9:4e94a16ca90c 267 //C4/B4=-568.7123367
Michu90 9:4e94a16ca90c 268
Michu90 8:dc48ce79ad59 269 //B*PWMzad+C=T
Michu90 8:dc48ce79ad59 270 //PWMzad=1/B*T-C/B
Michu90 8:dc48ce79ad59 271 /*
Michu90 8:dc48ce79ad59 272 PWMzad1=1287.588322*T1zad-(-431.2892625);
Michu90 9:4e94a16ca90c 273 PWMzad2=1245.207965*T2zad-(-600.7566265);
Michu90 9:4e94a16ca90c 274 PWMzad3=1295.740776*T3zad-(-514.6040317);
Michu90 9:4e94a16ca90c 275 PWMzad4=1269.412072*T4zad-(-568.7123367);*/
Michu90 9:4e94a16ca90c 276
Michu90 9:4e94a16ca90c 277
Michu90 9:4e94a16ca90c 278
Michu90 7:2ba30a0cdc16 279
Michu90 9:4e94a16ca90c 280
Michu90 9:4e94a16ca90c 281
Michu90 9:4e94a16ca90c 282
Michu90 9:4e94a16ca90c 283 pitchE=pitchE+(0-pitch);
Michu90 7:2ba30a0cdc16 284 if(pitchE>3) pitchE=3;
Michu90 9:4e94a16ca90c 285 if(pitchE<-3) pitchE=-3;
Michu90 8:dc48ce79ad59 286 //Kd1=Kp1*Td1/T;
Michu90 8:dc48ce79ad59 287 //if(Ti1==0){Ki1=0;}else Ki1=Kp1*T/Ti1;
Michu90 6:8cc6df266363 288
Michu90 7:2ba30a0cdc16 289 rollE=rollE+(0-roll2);
Michu90 8:dc48ce79ad59 290 if(rollE>5) rollE=5;
Michu90 8:dc48ce79ad59 291 if(rollE<-5) rollE=-5;
Michu90 8:dc48ce79ad59 292 //Kd2=Kp2*Td2/T;
Michu90 8:dc48ce79ad59 293 //if(Ti2==0){Ki2=0;}else Ki2=Kp2*T/Ti2;
Michu90 7:2ba30a0cdc16 294
Michu90 7:2ba30a0cdc16 295 /* yawE=yawE+(0-roll2);
Michu90 7:2ba30a0cdc16 296 if(yawE>3) yawE=3;*/
Michu90 7:2ba30a0cdc16 297 Kd3=Kp3*Td3/T;
Michu90 8:dc48ce79ad59 298 //if(Ti3==0){Ki3=0;}else Ki3=Kp3*T/Ti3;
Michu90 7:2ba30a0cdc16 299
Michu90 9:4e94a16ca90c 300 U1 = 0.0173*(Kp1*(0-pitch)+Kd1*(0+kalmanpitchzyro)+Ki1*pitchE);
Michu90 8:dc48ce79ad59 301 U2 = 0.0169*(Kp2*(0-roll)+Kd2*(0-o[1])+Ki2*rollE);
Michu90 9:4e94a16ca90c 302 U3 = 0.0333*(/*Kp3*((105*M_PI/180)-fYaw)+*/Kd3*(0-o[2]));//+Ki3*rollE);
Michu90 9:4e94a16ca90c 303
Michu90 9:4e94a16ca90c 304 //U1 = 0.0346*(Kp1*(0-pitch)+Kd1*(0-(-kalmanpitchzyro))+Ki1*pitchE);
Michu90 9:4e94a16ca90c 305 //U2 = 0.0338*(Kp2*(0-roll)+Kd2*(0-o[1])+Ki2*rollE);
Michu90 9:4e94a16ca90c 306 //U3 = 0.0666*(/*Kp3*((105*M_PI/180)-fYaw)+*/Kd3*(0-O[2]));//+Ki3*rollE);
Michu90 7:2ba30a0cdc16 307
Michu90 7:2ba30a0cdc16 308 Om1 = 50.22498189*PWM1zad+(-21611.4954)-U2/0.0000092780 + U3/0.000132 ; //kwadraty
Michu90 9:4e94a16ca90c 309 Om2 = 51.130533*PWM2zad+(-30717.00651)+U1/0.000009424 - U3/0.000132 ;
Michu90 9:4e94a16ca90c 310 Om3 = 50.66613192*PWM3zad+(-26072.99576)+U2/0.000009139 + U3/0.000132 ;
Michu90 9:4e94a16ca90c 311 Om4 = 50.95680893*PWM4zad+(-28979.76588)-U1/0.000009276 - U3/0.000132 ;
Michu90 7:2ba30a0cdc16 312
Michu90 8:dc48ce79ad59 313 wyp1=0.01991041*Om1-(-431.2892625);
Michu90 9:4e94a16ca90c 314 wyp2=0.019557786*Om2-(-600.7566265);
Michu90 9:4e94a16ca90c 315 wyp3=0.01973705*Om3-(-514.6040317);
Michu90 9:4e94a16ca90c 316 wyp4=0.019624463*Om4-(-568.7123367);
Michu90 7:2ba30a0cdc16 317
Michu90 6:8cc6df266363 318 if(wyp1<=10001 || wyp1>40001) valPWM1=10000;
Michu90 6:8cc6df266363 319 if(wyp1>=20000 && wyp1<40000) valPWM1=20000;
Michu90 6:8cc6df266363 320 if(wyp1>10001 && wyp1<20000) valPWM1=(int)wyp1;
Michu90 6:8cc6df266363 321
Michu90 9:4e94a16ca90c 322 if(wyp2<=10001 || wyp2>40001) valPWM2=10000;
Michu90 9:4e94a16ca90c 323 if(wyp2>=20000 && wyp2<40000) valPWM2=20000;
Michu90 9:4e94a16ca90c 324 if(wyp2>10001 && wyp2<20000) valPWM2=(int)wyp2;
Michu90 6:8cc6df266363 325
Michu90 9:4e94a16ca90c 326 if(wyp3<=10001 || wyp3>40001) valPWM3=10000;
Michu90 9:4e94a16ca90c 327 if(wyp3>=20000 && wyp3<40000) valPWM3=20000;
Michu90 9:4e94a16ca90c 328 if(wyp3>10001 && wyp3<20000) valPWM3=(int)wyp3;
Michu90 6:8cc6df266363 329
Michu90 9:4e94a16ca90c 330 if(wyp4<=10001 || wyp4>40001) valPWM4=10000;
Michu90 9:4e94a16ca90c 331 if(wyp4>=20000 && wyp4<40000) valPWM4=20000;
Michu90 9:4e94a16ca90c 332 if(wyp4>10001 && wyp4<20000) valPWM4=(int)wyp4;
Michu90 6:8cc6df266363 333
Michu90 5:c3caf8b83e6b 334
Michu90 5:c3caf8b83e6b 335 //sprintf(buff, "%f,%f,%f,%f\n\r", -katy*180/M_PI, roll*180/M_PI, -katybut*180/M_PI, roll2*180/M_PI);
Michu90 5:c3caf8b83e6b 336 //pc.printf(buff);
Michu90 5:c3caf8b83e6b 337 //sprintf(buff, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n\r", -katy, roll, -katybut, roll2, (d[0]*180/M_PI),(D[0]*180/M_PI),(d[1]*180/M_PI),(D[1]*180/M_PI),(o[0]*180/M_PI),(O[0]*180/M_PI),(o[1]*180/M_PI),(O[1]*180/M_PI));
Michu90 5:c3caf8b83e6b 338 //pc.printf(buff);
Michu90 5:c3caf8b83e6b 339 //sprintf(buff, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n\r", offsetGyro[0]*180/M_PI, offsetGyro[1]*180/M_PI, offsetGyro[2]*180/M_PI, roll2, (d[0]*180/M_PI),(D[0]*180/M_PI),(d[1]*180/M_PI),(D[1]*180/M_PI),(o[0]*180/M_PI),(O[0]*180/M_PI),(o[1]*180/M_PI),(O[1]*180/M_PI));
Michu90 5:c3caf8b83e6b 340 //pc.printf(buff);
Michu90 5:c3caf8b83e6b 341 //sprintf(buff, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n\r", -katy, roll, -katybut, roll2, (d[0]*180/M_PI),(D[0]*180/M_PI),(d[1]*180/M_PI),(D[1]*180/M_PI),(o[0]*180/M_PI),(O[0]*180/M_PI),(o[1]*180/M_PI),(O[1]*180/M_PI));
Michu90 5:c3caf8b83e6b 342 //pc.printf(buff);
Michu90 5:c3caf8b83e6b 343
Michu90 6:8cc6df266363 344 M1.pulsewidth_us(valPWM1);
Michu90 6:8cc6df266363 345 M2.pulsewidth_us(valPWM2);
Michu90 6:8cc6df266363 346 M3.pulsewidth_us(valPWM3);
Michu90 6:8cc6df266363 347 M4.pulsewidth_us(valPWM4);
Michu90 6:8cc6df266363 348
Michu90 6:8cc6df266363 349
Michu90 7:2ba30a0cdc16 350 //myled = !myled;
Michu90 5:c3caf8b83e6b 351 }
Michu90 5:c3caf8b83e6b 352
Michu90 9:4e94a16ca90c 353 /*
Michu90 5:c3caf8b83e6b 354 void task2()
Michu90 5:c3caf8b83e6b 355 {
Michu90 9:4e94a16ca90c 356
Michu90 5:c3caf8b83e6b 357 sprintf(buff, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n\r", -katy*180/M_PI, roll*180/M_PI, -katybut*180/M_PI, roll2*180/M_PI, katx*180/M_PI, pitch*180/M_PI, katxbut*180/M_PI, pitch2*180/M_PI,(o[0]*180/M_PI),(O[0]*180/M_PI),(o[1]*180/M_PI),(O[1]*180/M_PI));
Michu90 5:c3caf8b83e6b 358 pc.printf(buff);
Michu90 9:4e94a16ca90c 359 myled2 = !myled2;
Michu90 8:dc48ce79ad59 360 imu.readData(d);
Michu90 8:dc48ce79ad59 361
Michu90 8:dc48ce79ad59 362 if (filtrZmienna < 0) {
Michu90 8:dc48ce79ad59 363 filtrWynik = filtrSygnal;
Michu90 8:dc48ce79ad59 364 filtrBufor[filtrZmienna + filtrWymiar] = filtrSygnal;
Michu90 8:dc48ce79ad59 365 filtrSuma = filtrSuma + filtrBufor[filtrZmienna + filtrWymiar];
Michu90 8:dc48ce79ad59 366 } else {
Michu90 8:dc48ce79ad59 367 if (filtrZmienna == filtrWymiar - 1) {
Michu90 8:dc48ce79ad59 368 filtrSuma = filtrSuma - filtrBufor[filtrZmienna];
Michu90 8:dc48ce79ad59 369 filtrBufor[filtrZmienna] = filtrSygnal;
Michu90 8:dc48ce79ad59 370 filtrSuma = filtrSuma + filtrBufor[filtrZmienna];
Michu90 8:dc48ce79ad59 371 filtrWynik = filtrSuma / filtrWymiar;
Michu90 8:dc48ce79ad59 372 } else {
Michu90 8:dc48ce79ad59 373 filtrSuma = filtrSuma - filtrBufor[filtrZmienna];
Michu90 8:dc48ce79ad59 374 filtrBufor[filtrZmienna] = filtrSygnal;
Michu90 8:dc48ce79ad59 375 filtrSuma = filtrSuma + filtrBufor[filtrZmienna];
Michu90 8:dc48ce79ad59 376 filtrWynik = filtrSuma / filtrWymiar;
Michu90 8:dc48ce79ad59 377 }
Michu90 8:dc48ce79ad59 378 }
Michu90 8:dc48ce79ad59 379 filtrZmienna++;
Michu90 8:dc48ce79ad59 380 if (filtrZmienna == filtrWymiar) {
Michu90 8:dc48ce79ad59 381 filtrZmienna = 0;
Michu90 8:dc48ce79ad59 382 }
Michu90 8:dc48ce79ad59 383
Michu90 8:dc48ce79ad59 384 if (filtrWynik < xmin) xmin=filtrWynik;
Michu90 8:dc48ce79ad59 385 if (filtrWynik > xmax) xmax=filtrWynik;
Michu90 8:dc48ce79ad59 386
Michu90 8:dc48ce79ad59 387
Michu90 8:dc48ce79ad59 388 if (filtr2Zmienna < 0) {
Michu90 8:dc48ce79ad59 389 filtr2Wynik = filtr2Sygnal;
Michu90 8:dc48ce79ad59 390 filtr2Bufor[filtr2Zmienna + filtr2Wymiar] = filtr2Sygnal;
Michu90 8:dc48ce79ad59 391 filtr2Suma = filtr2Suma + filtr2Bufor[filtr2Zmienna + filtr2Wymiar];
Michu90 8:dc48ce79ad59 392 } else {
Michu90 8:dc48ce79ad59 393 if (filtr2Zmienna == filtr2Wymiar - 1) {
Michu90 8:dc48ce79ad59 394 filtr2Suma = filtr2Suma - filtr2Bufor[filtr2Zmienna];
Michu90 8:dc48ce79ad59 395 filtr2Bufor[filtr2Zmienna] = filtr2Sygnal;
Michu90 8:dc48ce79ad59 396 filtr2Suma = filtr2Suma + filtr2Bufor[filtr2Zmienna];
Michu90 8:dc48ce79ad59 397 filtr2Wynik = filtr2Suma / filtr2Wymiar;
Michu90 8:dc48ce79ad59 398 } else {
Michu90 8:dc48ce79ad59 399 filtr2Suma = filtr2Suma - filtr2Bufor[filtr2Zmienna];
Michu90 8:dc48ce79ad59 400 filtr2Bufor[filtr2Zmienna] = filtr2Sygnal;
Michu90 8:dc48ce79ad59 401 filtr2Suma = filtr2Suma + filtr2Bufor[filtr2Zmienna];
Michu90 8:dc48ce79ad59 402 filtr2Wynik = filtr2Suma / filtr2Wymiar;
Michu90 8:dc48ce79ad59 403 }
Michu90 8:dc48ce79ad59 404 }
Michu90 8:dc48ce79ad59 405 filtr2Zmienna++;
Michu90 8:dc48ce79ad59 406 if (filtr2Zmienna == filtr2Wymiar) {
Michu90 8:dc48ce79ad59 407 filtr2Zmienna = 0;
Michu90 8:dc48ce79ad59 408 }
Michu90 8:dc48ce79ad59 409
Michu90 8:dc48ce79ad59 410 if (filtr2Wynik < ymin) ymin=filtr2Wynik;
Michu90 8:dc48ce79ad59 411 if (filtr2Wynik > ymax) ymax=filtr2Wynik;
Michu90 8:dc48ce79ad59 412
Michu90 8:dc48ce79ad59 413 if (filtr3Zmienna < 0) {
Michu90 8:dc48ce79ad59 414 filtr3Wynik = filtr3Sygnal;
Michu90 8:dc48ce79ad59 415 filtr3Bufor[filtr3Zmienna + filtr3Wymiar] = filtr3Sygnal;
Michu90 8:dc48ce79ad59 416 filtr3Suma = filtr3Suma + filtr3Bufor[filtr3Zmienna + filtr3Wymiar];
Michu90 8:dc48ce79ad59 417 } else {
Michu90 8:dc48ce79ad59 418 if (filtr3Zmienna == filtr3Wymiar - 1) {
Michu90 8:dc48ce79ad59 419 filtr3Suma = filtr3Suma - filtr3Bufor[filtr3Zmienna];
Michu90 8:dc48ce79ad59 420 filtr3Bufor[filtr3Zmienna] = filtr3Sygnal;
Michu90 8:dc48ce79ad59 421 filtr3Suma = filtr3Suma + filtr3Bufor[filtr3Zmienna];
Michu90 8:dc48ce79ad59 422 filtr3Wynik = filtr3Suma / filtr3Wymiar;
Michu90 8:dc48ce79ad59 423 } else {
Michu90 8:dc48ce79ad59 424 filtr3Suma = filtr3Suma - filtr3Bufor[filtr3Zmienna];
Michu90 8:dc48ce79ad59 425 filtr3Bufor[filtr3Zmienna] = filtr3Sygnal;
Michu90 8:dc48ce79ad59 426 filtr3Suma = filtr3Suma + filtr3Bufor[filtr3Zmienna];
Michu90 8:dc48ce79ad59 427 filtr3Wynik = filtr3Suma / filtr3Wymiar;
Michu90 8:dc48ce79ad59 428 }
Michu90 8:dc48ce79ad59 429 }
Michu90 8:dc48ce79ad59 430 filtr3Zmienna++;
Michu90 8:dc48ce79ad59 431 if (filtr3Zmienna == filtr3Wymiar) {
Michu90 8:dc48ce79ad59 432 filtr3Zmienna = 0;
Michu90 8:dc48ce79ad59 433 }
Michu90 8:dc48ce79ad59 434
Michu90 8:dc48ce79ad59 435 if (filtr3Wynik < zmin) zmin=filtr3Wynik;
Michu90 8:dc48ce79ad59 436 if (filtr3Wynik > zmax) zmax=filtr3Wynik;
Michu90 8:dc48ce79ad59 437
Michu90 8:dc48ce79ad59 438 //sprintf(buff, "%f,%f,%f,%f\n\r", d[4],filtrWynik,ymin,ymax);
Michu90 8:dc48ce79ad59 439 //pc.printf(buff);
Michu90 8:dc48ce79ad59 440
Michu90 8:dc48ce79ad59 441
Michu90 5:c3caf8b83e6b 442 }
Michu90 5:c3caf8b83e6b 443
Michu90 8:dc48ce79ad59 444
Michu90 8:dc48ce79ad59 445 void task3()
Michu90 8:dc48ce79ad59 446 {
Michu90 8:dc48ce79ad59 447 imu.readData(d);
Michu90 8:dc48ce79ad59 448 off.offsetData(d,offsetGyro,o);
Michu90 8:dc48ce79ad59 449 katxzyro = katxzyro + o[0]*dt;
Michu90 8:dc48ce79ad59 450 katyzyro = katyzyro + o[1]*dt;
Michu90 8:dc48ce79ad59 451 katzzyro = katzzyro + o[2]*dt;
Michu90 8:dc48ce79ad59 452
Michu90 8:dc48ce79ad59 453 r = sqrt(pow(d[3],2) + pow(d[4],2) + pow(d[5],2));
Michu90 8:dc48ce79ad59 454 katx = acos(d[4]/r)-M_PI2;
Michu90 8:dc48ce79ad59 455 katy = acos(d[3]/r)-M_PI2;
Michu90 8:dc48ce79ad59 456
Michu90 8:dc48ce79ad59 457 kalman_innovate(&pitch_data, katx, o[0]);
Michu90 8:dc48ce79ad59 458 kalman_innovate(&roll_data, -katy, o[1]);
Michu90 8:dc48ce79ad59 459 pitch = pitch_data.x1;
Michu90 8:dc48ce79ad59 460 kalmanpitchzyro = pitch_data.x2;
Michu90 8:dc48ce79ad59 461 kalmanpitchdryf = pitch_data.x3;
Michu90 8:dc48ce79ad59 462 roll = roll_data.x1;
Michu90 8:dc48ce79ad59 463
Michu90 8:dc48ce79ad59 464 }
Michu90 9:4e94a16ca90c 465 */
Michu90 9:4e94a16ca90c 466
Michu90 9:4e94a16ca90c 467 void task_tachometer()
Michu90 9:4e94a16ca90c 468 {
Michu90 9:4e94a16ca90c 469 TachFlaga=0;
Michu90 9:4e94a16ca90c 470 //PWM4zad+=100;
Michu90 9:4e94a16ca90c 471 //M4.pulsewidth_us(PWM4zad);
Michu90 9:4e94a16ca90c 472 wait(1.5f);
Michu90 9:4e94a16ca90c 473 TachFlaga=1;
Michu90 9:4e94a16ca90c 474 //myled2=!myled2;
Michu90 9:4e94a16ca90c 475 RPMtach=impulsA*10; // dla T=3s
Michu90 9:4e94a16ca90c 476 //RPMtach=impulsA*6; //dla T=5s
Michu90 9:4e94a16ca90c 477 //RPMtach=impulsA*3; //dla T=10s
Michu90 9:4e94a16ca90c 478 //RPMtachAktualny=impulsA*3;
Michu90 9:4e94a16ca90c 479 /*
Michu90 9:4e94a16ca90c 480 if (TachFlaga==1){
Michu90 9:4e94a16ca90c 481 RPMtachBuf[iTach]=RPMtachAktualny;
Michu90 9:4e94a16ca90c 482 iTach++;
Michu90 9:4e94a16ca90c 483 if (iTach>3){
Michu90 9:4e94a16ca90c 484 RPMtachWyn=(RPMtachBuf[iTach-1]+RPMtachBuf[iTach-2]+RPMtachBuf[iTach-3])/3;
Michu90 9:4e94a16ca90c 485 RPMtachWynPoprz=RPMtachWyn;
Michu90 9:4e94a16ca90c 486 PWM4zadWyn=PWM4zad;
Michu90 9:4e94a16ca90c 487 TachFlaga=0;
Michu90 9:4e94a16ca90c 488 iTach=0;
Michu90 9:4e94a16ca90c 489 RPMtach=RPMtachAktualny;
Michu90 9:4e94a16ca90c 490 }
Michu90 9:4e94a16ca90c 491 }else{
Michu90 9:4e94a16ca90c 492 if(abs(PWM4zadPoprz-PWM4zad)>0 && abs(RPMtachAktualny-RPMtachWynPoprz)>20){
Michu90 9:4e94a16ca90c 493 TachFlaga=1;
Michu90 9:4e94a16ca90c 494 }
Michu90 9:4e94a16ca90c 495 //RPMtach=RPMtachAktualny;
Michu90 9:4e94a16ca90c 496 PWM4zadPoprz=PWM4zad;
Michu90 9:4e94a16ca90c 497 PWM4zad+=1;
Michu90 9:4e94a16ca90c 498 M4.pulsewidth_us(PWM4zad);
Michu90 9:4e94a16ca90c 499 //if(PWM4zad>14000)PWM4zad=10000;
Michu90 9:4e94a16ca90c 500 }
Michu90 9:4e94a16ca90c 501 sprintf(buff, "%f,%i,%f,%f\n\r", PWM4zad,RPMtachAktualny,PWM4zadWyn,RPMtachWyn);
Michu90 9:4e94a16ca90c 502 pc.printf(buff);
Michu90 9:4e94a16ca90c 503 */
Michu90 9:4e94a16ca90c 504
Michu90 9:4e94a16ca90c 505
Michu90 9:4e94a16ca90c 506 sprintf(buff, "%f,%i\n\r", PWM4zad,RPMtach);
Michu90 9:4e94a16ca90c 507 pc.printf(buff);
Michu90 9:4e94a16ca90c 508 //iTach++;
Michu90 9:4e94a16ca90c 509 //if(iTach%2>0)PWM4zad+=1;
Michu90 9:4e94a16ca90c 510 //M4.pulsewidth_us(PWM4zad);
Michu90 9:4e94a16ca90c 511
Michu90 9:4e94a16ca90c 512 //RPMtachWyn=0;
Michu90 9:4e94a16ca90c 513 //PWM4zadWyn=0;
Michu90 9:4e94a16ca90c 514 impulsA=0;
Michu90 9:4e94a16ca90c 515
Michu90 9:4e94a16ca90c 516 }
Michu90 5:c3caf8b83e6b 517
Michu90 4:a5b51a651db7 518
mbed_official 0:50d2b9c62765 519 int main() {
Igor_W 2:9e6ed6302c78 520
Michu90 4:a5b51a651db7 521 pc.baud(115200);
Michu90 4:a5b51a651db7 522 bluetooth.baud(19200);
Michu90 5:c3caf8b83e6b 523 imu.init();
Michu90 5:c3caf8b83e6b 524 kalman_init(&pitch_data);
Michu90 5:c3caf8b83e6b 525 kalman_init(&roll_data);
Michu90 5:c3caf8b83e6b 526 kalman_init(&pitch_data2);
Michu90 5:c3caf8b83e6b 527 kalman_init(&roll_data2);
Michu90 9:4e94a16ca90c 528 event.rise(&rise);
Michu90 9:4e94a16ca90c 529 event.mode(PullUp);
Michu90 9:4e94a16ca90c 530 event.enable_irq();
Igor_W 3:1425359662e4 531
Michu90 4:a5b51a651db7 532 sprintf(buff, "Hello: \n\r");
Michu90 4:a5b51a651db7 533 pc.printf(buff);
Michu90 4:a5b51a651db7 534
Michu90 9:4e94a16ca90c 535 //off.setOffsets(offsetGyro, bluetooth, imu);
Michu90 5:c3caf8b83e6b 536
Michu90 5:c3caf8b83e6b 537
Michu90 5:c3caf8b83e6b 538
Michu90 9:4e94a16ca90c 539 //triger1.attach(&task1, 0.005);
Michu90 5:c3caf8b83e6b 540 //triger2.attach(&task2, 0.005);
Michu90 8:dc48ce79ad59 541 //triger3.attach(&task3, 0.005);
Michu90 9:4e94a16ca90c 542 tachometer.attach(&task_tachometer, 4.5);
Michu90 5:c3caf8b83e6b 543 i=1000;
Michu90 5:c3caf8b83e6b 544
Igor_W 3:1425359662e4 545
Michu90 5:c3caf8b83e6b 546 PWM1zad=10000;
Michu90 5:c3caf8b83e6b 547 PWM2zad=10000;
Michu90 5:c3caf8b83e6b 548 PWM3zad=10000;
Michu90 5:c3caf8b83e6b 549 PWM4zad=10000;
Michu90 5:c3caf8b83e6b 550
Michu90 5:c3caf8b83e6b 551 M1.period_us(PWM_period);
Michu90 5:c3caf8b83e6b 552 M1.pulsewidth_us(PWM1zad);
Michu90 5:c3caf8b83e6b 553 M2.period_us(PWM_period);
Michu90 5:c3caf8b83e6b 554 M2.pulsewidth_us(PWM2zad);
Michu90 5:c3caf8b83e6b 555 M3.period_us(PWM_period);
Michu90 5:c3caf8b83e6b 556 M3.pulsewidth_us(PWM3zad);
Michu90 5:c3caf8b83e6b 557 M4.period_us(PWM_period);
Michu90 5:c3caf8b83e6b 558 M4.pulsewidth_us(PWM4zad);
Michu90 4:a5b51a651db7 559
Michu90 9:4e94a16ca90c 560 /*Kp1=10;
Michu90 9:4e94a16ca90c 561 Kp2=8;
Michu90 9:4e94a16ca90c 562 Kp3=0;
Michu90 9:4e94a16ca90c 563 Kd1=8;
Michu90 9:4e94a16ca90c 564 Kd2=16;
Michu90 9:4e94a16ca90c 565 Kd3=5000;
Michu90 9:4e94a16ca90c 566 Ki1=0.1;
Michu90 9:4e94a16ca90c 567 Ki2=0.2;
Michu90 9:4e94a16ca90c 568 Ki3=0;
Michu90 9:4e94a16ca90c 569 Td1=0;
Michu90 9:4e94a16ca90c 570 Td2=0;
Michu90 9:4e94a16ca90c 571 Td3=0;
Michu90 9:4e94a16ca90c 572 Ti1=0;
Michu90 9:4e94a16ca90c 573 Ti2=0;
Michu90 9:4e94a16ca90c 574 Ti3=0;
Michu90 9:4e94a16ca90c 575 T=0.005;*/
Michu90 6:8cc6df266363 576 Kp1=0;
Michu90 6:8cc6df266363 577 Kp2=0;
Michu90 6:8cc6df266363 578 Kp3=0;
Michu90 8:dc48ce79ad59 579 Kd1=0;
Michu90 8:dc48ce79ad59 580 Kd2=0;
Michu90 8:dc48ce79ad59 581 Kd3=0;
Michu90 8:dc48ce79ad59 582 Ki1=0;
Michu90 8:dc48ce79ad59 583 Ki2=0;
Michu90 8:dc48ce79ad59 584 Ki3=0;
Michu90 7:2ba30a0cdc16 585 Td1=0;
Michu90 7:2ba30a0cdc16 586 Td2=0;
Michu90 7:2ba30a0cdc16 587 Td3=0;
Michu90 7:2ba30a0cdc16 588 Ti1=0;
Michu90 7:2ba30a0cdc16 589 Ti2=0;
Michu90 7:2ba30a0cdc16 590 Ti3=0;
Michu90 7:2ba30a0cdc16 591 T=0.005;
Michu90 6:8cc6df266363 592
Michu90 8:dc48ce79ad59 593 rollE=0;
Michu90 8:dc48ce79ad59 594
Michu90 8:dc48ce79ad59 595 katxzyro = 0;
Michu90 8:dc48ce79ad59 596 katyzyro = 0;
Michu90 8:dc48ce79ad59 597 katzzyro = 0;
Michu90 6:8cc6df266363 598
Michu90 9:4e94a16ca90c 599 iTach=0;
Michu90 9:4e94a16ca90c 600 TachFlaga=0;
Michu90 9:4e94a16ca90c 601
Igor_W 2:9e6ed6302c78 602 while(1) {
Michu90 6:8cc6df266363 603
Michu90 7:2ba30a0cdc16 604 //myled2 = !myled2;
Michu90 8:dc48ce79ad59 605
Michu90 8:dc48ce79ad59 606 //sprintf(buff, "%f,%f,%f,0,%f,%f,%f,%f,%f,%f,\n\r", -katybut*180/M_PI, roll2*180/M_PI, (O[1]*180/M_PI),valPWM1,valPWM3,Kp2,Kd2,Ki2,rollE);
Michu90 8:dc48ce79ad59 607 //pc.printf(buff);
Michu90 8:dc48ce79ad59 608 //sprintf(buff, "%f,%f,%f,%f,%f,%f,%f\n\r", -katybut*180/M_PI, roll2*180/M_PI, (O[1]*180/M_PI), -katy*180/M_PI, roll*180/M_PI, o[1]*180/M_PI,valPWM3);
Michu90 5:c3caf8b83e6b 609 //pc.printf(buff);
Michu90 8:dc48ce79ad59 610 //sprintf(buff, "%f,%f,%f,%f,%f\n\r", -katy*180/M_PI, roll*180/M_PI, katzyro*180/M_PI, fkompl*180/M_PI,roll2*180/M_PI);
Michu90 8:dc48ce79ad59 611 //pc.printf(buff);
Michu90 8:dc48ce79ad59 612 //sprintf(buff, "%f,%f,%f,%f,%f,%f,%f,%f,%f\n\r", d[3],d[4],d[5],xmin,xmax,ymin,ymax,zmin,zmax);
Michu90 8:dc48ce79ad59 613 //pc.printf(buff);
Michu90 8:dc48ce79ad59 614
Michu90 8:dc48ce79ad59 615 //sprintf(buff, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n\r", d[3],filtrWynik,d[4],filtr2Wynik,d[5],filtr3Wynik,xmin,xmax,ymin,ymax,zmin,zmax);
Michu90 7:2ba30a0cdc16 616 //pc.printf(buff);
Michu90 8:dc48ce79ad59 617
Michu90 8:dc48ce79ad59 618 //sprintf(buff, "%f,%f,%f\n\r", d[0],d[1],d[2]);
Michu90 8:dc48ce79ad59 619 //pc.printf(buff);
Michu90 8:dc48ce79ad59 620
Michu90 8:dc48ce79ad59 621
Michu90 8:dc48ce79ad59 622 //sprintf(buff, "0,%f,%f,%f,%f,%f,%f,\n\r", -katy*180/M_PI,roll*180/M_PI,-katybut*180/M_PI,roll2*180/M_PI,kalmanrollzyro*180/M_PI,kalmanrollzyrobut*180/M_PI);
Michu90 8:dc48ce79ad59 623 //pc.printf(buff);
Michu90 9:4e94a16ca90c 624 //sprintf(buff, "0,%f,%f,%f,%f,%f,%f,%f,%f,%f\n\r", pitch*180/M_PI,pitch2*180/M_PI,o[0]*180/M_PI,valPWM2,valPWM4,Kp1,Kd1,Ki1,pitchE*180/M_PI);
Michu90 9:4e94a16ca90c 625 //pc.printf(buff);
Michu90 9:4e94a16ca90c 626 //sprintf(buff, "0,%f,%f,%f,%f,%f,%f,%f\n\r", katx*180/M_PI,pitch*180/M_PI,o[0]*180/M_PI,PWM2zad,valPWM2,PWM4zad,valPWM4);
Michu90 9:4e94a16ca90c 627 //pc.printf(buff);
Michu90 9:4e94a16ca90c 628 //sprintf(buff, "0,%f,%f,%f,%f,%f,%f,%f,%f,%f\n\r", pitch*180/M_PI, -kalmanpitchzyro*180/M_PI,Kd1,Kp1,Ki1);
Michu90 9:4e94a16ca90c 629 //pc.printf(buff);
Michu90 9:4e94a16ca90c 630 //sprintf(buff, "%f,%f,%f,%f,%f\n\r", valPWM4,RPMtach,valPWM3,valPWM2,valPWM1);
Michu90 9:4e94a16ca90c 631 //pc.printf(buff);
Michu90 8:dc48ce79ad59 632
Michu90 8:dc48ce79ad59 633 //sprintf(buff, "%f,%f,%f,%f,%f,%f\n\r", katx*180/M_PI,pitch*180/M_PI,katxzyro*180/M_PI,o[0]*180/M_PI,kalmanpitchzyro*180/M_PI,kalmanpitchdryf*180/M_PI);
Michu90 8:dc48ce79ad59 634 //pc.printf(buff);
Michu90 6:8cc6df266363 635
Michu90 7:2ba30a0cdc16 636 //myled2 = !myled2;
Michu90 6:8cc6df266363 637
Michu90 5:c3caf8b83e6b 638
Michu90 4:a5b51a651db7 639
Michu90 4:a5b51a651db7 640 if(pc.readable()){
Michu90 4:a5b51a651db7 641 znak=pc.getc();
Michu90 4:a5b51a651db7 642 switch (znak){
Michu90 5:c3caf8b83e6b 643
Michu90 8:dc48ce79ad59 644 case 'r':
Michu90 8:dc48ce79ad59 645 sprintf(buff, "Resetuje zmienne %c\n\r",znak);
Michu90 8:dc48ce79ad59 646 pc.printf(buff);
Michu90 8:dc48ce79ad59 647 xmin=0;
Michu90 8:dc48ce79ad59 648 ymin=0;
Michu90 8:dc48ce79ad59 649 zmin=0;
Michu90 8:dc48ce79ad59 650 xmax=0;
Michu90 8:dc48ce79ad59 651 ymax=0;
Michu90 8:dc48ce79ad59 652 zmax=0;
Michu90 8:dc48ce79ad59 653 break;
Michu90 9:4e94a16ca90c 654
Michu90 9:4e94a16ca90c 655 case 't':
Michu90 9:4e94a16ca90c 656 PWM4zad++;
Michu90 9:4e94a16ca90c 657 M4.pulsewidth_us(PWM4zad);
Michu90 9:4e94a16ca90c 658 break;
Michu90 9:4e94a16ca90c 659 case 'g':
Michu90 9:4e94a16ca90c 660 PWM4zad--;
Michu90 9:4e94a16ca90c 661 M4.pulsewidth_us(PWM4zad);
Michu90 9:4e94a16ca90c 662 break;
Michu90 9:4e94a16ca90c 663 case 'y':
Michu90 9:4e94a16ca90c 664 PWM4zad+=10;
Michu90 9:4e94a16ca90c 665 M4.pulsewidth_us(PWM4zad);
Michu90 9:4e94a16ca90c 666 break;
Michu90 9:4e94a16ca90c 667 case 'h':
Michu90 9:4e94a16ca90c 668 PWM4zad-=10;
Michu90 9:4e94a16ca90c 669 M4.pulsewidth_us(PWM4zad);
Michu90 9:4e94a16ca90c 670 break;
Michu90 9:4e94a16ca90c 671 case 'u':
Michu90 9:4e94a16ca90c 672 PWM4zad+=100;
Michu90 9:4e94a16ca90c 673 M4.pulsewidth_us(PWM4zad);
Michu90 9:4e94a16ca90c 674 break;
Michu90 9:4e94a16ca90c 675 case 'j':
Michu90 9:4e94a16ca90c 676 PWM4zad-=100;
Michu90 9:4e94a16ca90c 677 M4.pulsewidth_us(PWM4zad);
Michu90 9:4e94a16ca90c 678 break;
Michu90 9:4e94a16ca90c 679 case 's':
Michu90 9:4e94a16ca90c 680 PWM4zad=10000;
Michu90 9:4e94a16ca90c 681 M4.pulsewidth_us(PWM4zad);
Michu90 9:4e94a16ca90c 682 break;
Michu90 9:4e94a16ca90c 683 case 'b':
Michu90 9:4e94a16ca90c 684 PWM4zad=10000;
Michu90 9:4e94a16ca90c 685 M4.pulsewidth_us(PWM4zad);
Michu90 9:4e94a16ca90c 686 break;
Michu90 7:2ba30a0cdc16 687 /*case 'u':
Michu90 7:2ba30a0cdc16 688 PWM1zad+=10;
Michu90 7:2ba30a0cdc16 689 M1.pulsewidth_us(PWM1zad);
Michu90 7:2ba30a0cdc16 690 break;
Michu90 7:2ba30a0cdc16 691 case 'j':
Michu90 7:2ba30a0cdc16 692 PWM1zad-=10;
Michu90 7:2ba30a0cdc16 693 M1.pulsewidth_us(PWM1zad);
Michu90 7:2ba30a0cdc16 694 break;*/
Michu90 4:a5b51a651db7 695
Michu90 4:a5b51a651db7 696 }
Michu90 4:a5b51a651db7 697
Michu90 5:c3caf8b83e6b 698 znak=0;
Michu90 4:a5b51a651db7 699 }
Michu90 5:c3caf8b83e6b 700
Michu90 4:a5b51a651db7 701
Michu90 4:a5b51a651db7 702 if(bluetooth.readable()){
Michu90 4:a5b51a651db7 703
Michu90 4:a5b51a651db7 704 znak2=bluetooth.getc();
Michu90 4:a5b51a651db7 705
Michu90 5:c3caf8b83e6b 706 switch (znak2){
Michu90 4:a5b51a651db7 707 case 'a':
Michu90 9:4e94a16ca90c 708 /*T1zad-=0.05;
Michu90 9:4e94a16ca90c 709 T2zad-=0.05;
Michu90 9:4e94a16ca90c 710 T3zad-=0.05;
Michu90 9:4e94a16ca90c 711 T4zad-=0.05;
Michu90 9:4e94a16ca90c 712 PWM1zad=1287.588322*T1zad-(-431.2892625)+10000;
Michu90 9:4e94a16ca90c 713 PWM2zad=1245.207965*T2zad-(-600.7566265)+10000;
Michu90 9:4e94a16ca90c 714 PWM3zad=1295.740776*T3zad-(-514.6040317)+10000;
Michu90 9:4e94a16ca90c 715 PWM4zad=1269.412072*T4zad-(-568.7123367)+10000;*/
Michu90 9:4e94a16ca90c 716
Michu90 8:dc48ce79ad59 717 //PWM3zad-=50;
Michu90 9:4e94a16ca90c 718 PWM4zad-=50;
Michu90 9:4e94a16ca90c 719 //PWM2zad=1.045*(PWM4zad-10000)+10000;
Michu90 8:dc48ce79ad59 720 //PWM1zad=1.026*(PWM3zad-10000)+10000;
Michu90 9:4e94a16ca90c 721
Michu90 9:4e94a16ca90c 722 //PWM1zad=1.026*(PWM3zad-10000)+10000;
Michu90 8:dc48ce79ad59 723 //PWM1zad-=50;
Michu90 5:c3caf8b83e6b 724 if(PWM1zad<10000){
Michu90 5:c3caf8b83e6b 725 PWM1zad=10000;
Michu90 5:c3caf8b83e6b 726 PWM2zad=10000;
Michu90 5:c3caf8b83e6b 727 PWM3zad=10000;
Michu90 5:c3caf8b83e6b 728 PWM4zad=10000;
Michu90 5:c3caf8b83e6b 729 }
Michu90 5:c3caf8b83e6b 730 //ustawianie
Michu90 5:c3caf8b83e6b 731 M1.pulsewidth_us(PWM1zad);
Michu90 5:c3caf8b83e6b 732 M2.pulsewidth_us(PWM2zad);
Michu90 5:c3caf8b83e6b 733 M3.pulsewidth_us(PWM3zad);
Michu90 5:c3caf8b83e6b 734 M4.pulsewidth_us(PWM4zad);
Michu90 5:c3caf8b83e6b 735 znak2=0;
Michu90 4:a5b51a651db7 736 break;
Michu90 4:a5b51a651db7 737
Michu90 4:a5b51a651db7 738 case 'b':
Michu90 9:4e94a16ca90c 739 /*T1zad+=0.05;
Michu90 9:4e94a16ca90c 740 T2zad+=0.05;
Michu90 9:4e94a16ca90c 741 T3zad+=0.05;
Michu90 9:4e94a16ca90c 742 T4zad+=0.05;
Michu90 9:4e94a16ca90c 743 PWM1zad=1287.588322*T1zad-(-431.2892625)+10000;
Michu90 9:4e94a16ca90c 744 PWM2zad=1245.207965*T2zad-(-600.7566265)+10000;
Michu90 9:4e94a16ca90c 745 PWM3zad=1295.740776*T3zad-(-514.6040317)+10000;
Michu90 9:4e94a16ca90c 746 PWM4zad=1269.412072*T4zad-(-568.7123367)+10000;*/
Michu90 8:dc48ce79ad59 747 //PWM3zad+=50;
Michu90 9:4e94a16ca90c 748 PWM4zad+=50;
Michu90 9:4e94a16ca90c 749 //PWM2zad=1.045*(PWM4zad-10000)+10000;
Michu90 9:4e94a16ca90c 750 //PWM3zad+=50;
Michu90 8:dc48ce79ad59 751 //PWM1zad=1.026*(PWM3zad-10000)+10000;
Michu90 8:dc48ce79ad59 752
Michu90 8:dc48ce79ad59 753 //PWM1zad+=50;
Michu90 5:c3caf8b83e6b 754 if(PWM1zad>=20000){
Michu90 5:c3caf8b83e6b 755 PWM1zad=20000;
Michu90 5:c3caf8b83e6b 756 PWM2zad=20000;
Michu90 5:c3caf8b83e6b 757 PWM3zad=20000;
Michu90 5:c3caf8b83e6b 758 PWM4zad=20000;
Michu90 5:c3caf8b83e6b 759 }
Michu90 5:c3caf8b83e6b 760 //ustawianie
Michu90 5:c3caf8b83e6b 761 M1.pulsewidth_us(PWM1zad);
Michu90 5:c3caf8b83e6b 762 M2.pulsewidth_us(PWM2zad);
Michu90 5:c3caf8b83e6b 763 M3.pulsewidth_us(PWM3zad);
Michu90 5:c3caf8b83e6b 764 M4.pulsewidth_us(PWM4zad);
Michu90 5:c3caf8b83e6b 765 znak2=0;
Michu90 5:c3caf8b83e6b 766 break;
Michu90 5:c3caf8b83e6b 767
Michu90 5:c3caf8b83e6b 768 case 'x':
Michu90 9:4e94a16ca90c 769 //sprintf(buff,"Nastawy: %f,%f\n\r",Kp2,Kd2);
Michu90 9:4e94a16ca90c 770 //pc.printf(buff);
Michu90 9:4e94a16ca90c 771 //wait(1.0f);
Michu90 5:c3caf8b83e6b 772 PWM1zad=10000;
Michu90 5:c3caf8b83e6b 773 PWM2zad=10000;
Michu90 5:c3caf8b83e6b 774 PWM3zad=10000;
Michu90 5:c3caf8b83e6b 775 PWM4zad=10000;
Michu90 5:c3caf8b83e6b 776 M1.pulsewidth_us(PWM1zad);
Michu90 5:c3caf8b83e6b 777 M2.pulsewidth_us(PWM2zad);
Michu90 5:c3caf8b83e6b 778 M3.pulsewidth_us(PWM3zad);
Michu90 5:c3caf8b83e6b 779 M4.pulsewidth_us(PWM4zad);
Michu90 5:c3caf8b83e6b 780 sprintf(buff,"Odlacz silniki\n\r");
Michu90 5:c3caf8b83e6b 781 pc.printf(buff);
Michu90 5:c3caf8b83e6b 782 wait(1.0f);
Michu90 5:c3caf8b83e6b 783 sprintf(buff,"5 \n\r");
Michu90 5:c3caf8b83e6b 784 pc.printf(buff);
Michu90 5:c3caf8b83e6b 785 wait(1.0f);
Michu90 5:c3caf8b83e6b 786 sprintf(buff,"4 \n\r");
Michu90 5:c3caf8b83e6b 787 pc.printf(buff);
Michu90 5:c3caf8b83e6b 788 wait(1.0f);
Michu90 5:c3caf8b83e6b 789 sprintf(buff,"3 \n\r");
Michu90 5:c3caf8b83e6b 790 pc.printf(buff);
Michu90 5:c3caf8b83e6b 791 wait(1.0f);
Michu90 5:c3caf8b83e6b 792 sprintf(buff,"2 \n\r");
Michu90 5:c3caf8b83e6b 793 pc.printf(buff);
Michu90 5:c3caf8b83e6b 794 wait(1.0f);
Michu90 5:c3caf8b83e6b 795 sprintf(buff,"1 \n\r");
Michu90 5:c3caf8b83e6b 796 pc.printf(buff);
Michu90 5:c3caf8b83e6b 797 wait(1.0f);
Michu90 5:c3caf8b83e6b 798 sprintf(buff,"GO! \n\r");
Michu90 4:a5b51a651db7 799 pc.printf(buff);
Michu90 4:a5b51a651db7 800 break;
Michu90 4:a5b51a651db7 801
Michu90 6:8cc6df266363 802 case 'c':
Michu90 8:dc48ce79ad59 803 Kd1-=0.5;
Michu90 8:dc48ce79ad59 804 Kd2-=0.5;
Michu90 6:8cc6df266363 805 break;
Michu90 6:8cc6df266363 806 case 'd':
Michu90 8:dc48ce79ad59 807 Kd1+=0.5;
Michu90 8:dc48ce79ad59 808 Kd2+=0.5;
Michu90 6:8cc6df266363 809 break;
Michu90 6:8cc6df266363 810 case 'e':
Michu90 6:8cc6df266363 811 Kp1-=0.5;
Michu90 6:8cc6df266363 812 Kp2-=0.5;
Michu90 6:8cc6df266363 813 break;
Michu90 6:8cc6df266363 814 case 'f':
Michu90 6:8cc6df266363 815 Kp1+=0.5;
Michu90 6:8cc6df266363 816 Kp2+=0.5;
Michu90 8:dc48ce79ad59 817 break;
Michu90 8:dc48ce79ad59 818 case 'g':
Michu90 8:dc48ce79ad59 819 Ki1-=0.1;
Michu90 8:dc48ce79ad59 820 Ki2-=0.1;
Michu90 8:dc48ce79ad59 821 break;
Michu90 8:dc48ce79ad59 822 case 'h':
Michu90 8:dc48ce79ad59 823 Ki1+=0.1;
Michu90 8:dc48ce79ad59 824 Ki2+=0.1;
Michu90 8:dc48ce79ad59 825 break;
Michu90 5:c3caf8b83e6b 826 }
Michu90 4:a5b51a651db7 827 }
Michu90 6:8cc6df266363 828 //myled2 = !myled2;
Igor_W 2:9e6ed6302c78 829 }
mbed_official 0:50d2b9c62765 830 }