Revision 12:e955cc086c42, committed 2015-04-17

Comitter:

Michu90

Date:

Fri Apr 17 14:25:25 2015 +0000

Parent:

11:38db3ed41f13

Commit message:

Dydaktyka

Changed in this revision

--- a/Offsets.cpp	Fri Apr 17 07:48:05 2015 +0000
+++ b/Offsets.cpp	Fri Apr 17 14:25:25 2015 +0000
@@ -31,7 +31,7 @@
     float d[9];
     float yaw;
     
-    sprintf(buff, "Hello!\n\rPlace Qudrocopter motionlessly and press o\n\r");
+    sprintf(buff, "Hello!\n\rPlace Qudrocopter motionlessly and press a\n\r");
     serial.printf(buff);
     do{
         if(serial.readable()){
@@ -56,10 +56,6 @@
         offsetGyr[2]/=1000;
         yaw = yaw/1000;
         
-        //o[0] = offsetGyr[0];
-        //o[1] = offsetGyr[1];
-        //o[2] = offsetGyr[2];
-        // lub tak:
         *(o+0) = offsetGyr[0];
         *(o+1) = offsetGyr[1];
         *(o+2) = offsetGyr[2];
@@ -87,9 +83,6 @@
 
 void Offsets::offsetData(float *d, float *D, float *O) 
 {
-    /*for (int i=0; i<3; i++) {
-         O[i] = d[i]-D[i]; 
-    }*/
     O[0] = d[0]-D[0];
     O[1] = d[1]-D[1];
     O[2] = d[2]-D[2];
@@ -97,9 +90,6 @@
 
 void Offsets::offsetData2(double *d, float *D, float *O) 
 {
-    /*for (int i=0; i<3; i++) {
-         O[i] = d[i]-D[i]; 
-    }*/
     O[0] = d[0]-D[0];
     O[1] = d[1]-D[1];
     O[2] = d[2]-D[2];
@@ -107,8 +97,5 @@
 
 void Offsets::offsetMagneto(float *d, float *D, float *O) 
 {
-    /*for (int i=0; i<3; i++) {
-         O[i] = d[i]-D[i]; 
-    }*/
     O[0] = d[0]-D[0];
 }
\ No newline at end of file

--- a/Offsets.h	Fri Apr 17 07:48:05 2015 +0000
+++ b/Offsets.h	Fri Apr 17 14:25:25 2015 +0000
@@ -1,3 +1,9 @@
+/*
+2014_12_15
+Author: Michał Szewc
+Free to use
+*/
+
 #ifndef OFFSETS_H
 #define OFFSETS_H
 

--- a/kalman.h	Fri Apr 17 07:48:05 2015 +0000
+++ b/kalman.h	Fri Apr 17 14:25:25 2015 +0000
@@ -16,13 +16,13 @@
 //#define DT 0.002472 //klaman2 + filtr dolno x2
 //#define DT 0.00223 //klaman2 + filtr dolno x2 bez kompl
 //  Q matrix elements
-#define Q1 0.2f //0.2f            //5  //10
-#define Q2 2.0f //2.0f      //80       //120
-#define Q3 0.01 //0.01f
+#define Q1 0.2f         //0.2f            //5  //10
+#define Q2 2.0f         //2.0f            //80 //120
+#define Q3 0.01         //0.01f
 
 // R matrix elements
-#define R1 120    //500.0f  //900.0f       //90      //tu duzo   //10000  //500
-#define R2 150  //600.0f  //1500.0f      //150      //tu mniej  //12000  //600
+#define R1 120      //500.0f  //900.0f       //90      //tu duzo   //10000  //500
+#define R2 150      //600.0f  //1500.0f      //150      //tu mniej  //12000  //600
 
 struct _kalman_data
 {
@@ -36,6 +36,4 @@
 void kalman_innovate(kalman_data *data, float z1, float z2);
 void kalman_init(kalman_data *data);
 
-
-
 #endif /* KALMAN_H_ */

--- a/main.cpp	Fri Apr 17 07:48:05 2015 +0000
+++ b/main.cpp	Fri Apr 17 14:25:25 2015 +0000
@@ -17,6 +17,10 @@
 #define filtr3Sygnal d[5]
 
 
+
+#define zad 1
+
+
 //kalman
 // Structs for containing filter data
 kalman_data pitch_data;
@@ -29,8 +33,11 @@
 DigitalOut myled2(PTA1);
 Serial pc(USBTX, USBRX);
 Serial bluetooth(D1, D0);
+
 IMU imu(PTE25,PTE24);
+
 Offsets off;
+
 InterruptIn event(PTB23);
 
 FastPWM M1(D10);
@@ -39,8 +46,8 @@
 FastPWM M4(D13);
 
 Ticker triger1; //przerwanie filtracji
-Ticker triger2; //kalibracja accelero
-Ticker triger3; //kalibracja zyro
+//Ticker triger2; //kalibracja accelero
+//Ticker triger3; //kalibracja zyro
 Ticker tachometer;
 
 float d[9];
@@ -48,39 +55,33 @@
 float o[3];
 float O[3];
 char buff[160];
-float r,katx,katy,katz;
+float r,katx,katy,katz,katzyro;
 float rbut,katxbut,katybut;
 float pitch, roll, yaw;
 float pitch2, roll2;
-float pitchE, rollE, yawE;
+float pitchma, rollma;
+
 double i;
 float offsetGyro[3];
 float offsetMagneto;
 float yawOff;
 char odczyt[20];
+
+
 char znak;
 char znak2;
-float Kp1,Td1,Kd1,Kp2,Td2,Kd2,Kp3,Td3,Kd3,Ti1,Ki1,Ti2,Ki2,Ti3,Ki3,T;
-float U1,U2,U3;
-float Om1,Om2,Om3,Om4;
-float wyp1,wyp2,wyp3,wyp4;
+
 int impulsA;
-
 double PWM1zad;
 double PWM2zad;
 double PWM3zad;
 double PWM4zad;
-double valPWM1;
-double valPWM2;
-double valPWM3;
-double valPWM4;
-float T1zad,T2zad,T3zad,T4zad;
+
 
 //zmienne tymczasowe
 float katxzyro,katyzyro,katzzyro;
 float fkompl;
-float xmin, ymin, zmin;
-float xmax, ymax, zmax;
+
 float filtrBufor[filtrWymiar];
 int   filtrZmienna=0;
 float filtrSuma=0;
@@ -101,6 +102,8 @@
 float kalmanpitchdryf;
 
 int RPMtach;
+int zadanie;
+/*
 float PWM4zadBuf[5];
 float PWM4zadPoprz;
 float PWM4zadWyn;
@@ -109,31 +112,24 @@
 int RPMtachAktualny;
 float RPMtachWyn;
 int RPMtachWynPoprz;
+*/
+
 int iTach;
 bool TachFlaga;
 bool FlagaPom;
 
-int Omegazad[4];
-int Omegamax[4][9];
-float a1buf[4][9];
-float a2buf[4][9];
-int ister;
-int jster;
 bool flagaster;
-float a1[4];
-float a2[4];
 
 void rise(void)
 {
-    //myled = !myled;
     if(TachFlaga==1)impulsA++;
 }
 
-
 void task1()
 {
-    //myled = !myled;
     imu.readData(d);
+    
+    
     imu.filterData(d, D);
     off.offsetData(d,offsetGyro,o);
     off.offsetData2(D,offsetGyro,O);
@@ -144,7 +140,11 @@
        
     yaw = atan2(d[7],d[6])+4.98333*M_PI/180;
     katz = katz+(o[2]*dt);
+    
+    katzyro=katzyro+o[4]*dt;
+    
     off.offsetMagneto(&yaw,&offsetMagneto,&yawOff);
+    
     // Correct for heading < 0deg and heading > 360deg
     /*if(yawOff < 0){
     yawOff+= 2 * M_PI;
@@ -153,14 +153,7 @@
     if(yawOff > 2 * M_PI){
     yawOff-= 2 * M_PI;
     }*/
-    
-    /*rbut = sqrt(pow(D[3],2) + pow(D[4],2) + pow(D[5],2));
-    katxbut = acos(D[4]/rbut)-M_PI2;
-    katybut = acos(D[3]/rbut)-M_PI2;*/
-    
-    //katxzyro = katxzyro + o[0]*dt;
-    //fkompl = katyzyro*0.95+ (-katy)*0.05;
-    
+
     //Filtr Kalmana
     kalman_innovate(&pitch_data, katx, o[0]);
     kalman_innovate(&roll_data, -katy, o[1]);
@@ -168,17 +161,10 @@
     kalmanpitchzyro = pitch_data.x2;
     roll = roll_data.x1;
     kalmanrollzyro = roll_data.x2;
-   
-    /*
-    //Filtr Kalmana butterworth 2nd
-    kalman_innovate(&pitch_data2, katxbut, O[0]);
-    kalman_innovate(&roll_data2, -katybut, O[1]);
-    pitch2 = pitch_data2.x1;
-    kalmanpitchzyrobut = pitch_data2.x2;
-    roll2 = roll_data2.x1;
-    kalmanrollzyrobut = roll_data2.x2;*/
+  
 
-                if (filtrZmienna < 0) {
+    //moving avarage
+        if (filtrZmienna < 0) {
             filtrWynik = filtrSygnal;
             filtrBufor[filtrZmienna + filtrWymiar] = filtrSygnal;
             filtrSuma = filtrSuma + filtrBufor[filtrZmienna + filtrWymiar];
@@ -199,220 +185,7 @@
         if (filtrZmienna == filtrWymiar) {
             filtrZmienna = 0;
         }
-    
-    roll = filtrWynik;
-    
-    if (filtr2Zmienna < 0) {
-            filtr2Wynik = filtr2Sygnal;
-            filtr2Bufor[filtr2Zmienna + filtr2Wymiar] = filtr2Sygnal;
-            filtr2Suma = filtr2Suma + filtr2Bufor[filtr2Zmienna + filtr2Wymiar];
-        } else {
-            if (filtr2Zmienna == filtr2Wymiar - 1) {
-                filtr2Suma = filtr2Suma - filtr2Bufor[filtr2Zmienna];
-                filtr2Bufor[filtr2Zmienna] = filtr2Sygnal;
-                filtr2Suma = filtr2Suma + filtr2Bufor[filtr2Zmienna];
-                filtr2Wynik = filtr2Suma / filtr2Wymiar;
-            } else {
-                filtr2Suma = filtr2Suma - filtr2Bufor[filtr2Zmienna];
-                filtr2Bufor[filtr2Zmienna] = filtr2Sygnal;
-                filtr2Suma = filtr2Suma + filtr2Bufor[filtr2Zmienna];
-                filtr2Wynik = filtr2Suma / filtr2Wymiar;
-            }
-        }
-        filtr2Zmienna++;
-        if (filtr2Zmienna == filtr2Wymiar) {
-            filtr2Zmienna = 0;
-        }
-    pitch=filtr2Wynik;
-    
-    //U1 = 0.0173*(Kp1*(-pitch2)+Kd1*(0-O[0]));
-    //U2 = 0.0169*(Kp2*((-20*M_PI/180)-roll2)+Kd2*(0-O[1]));
-    //U3 = 0.0333*(/*Kp3*((105*M_PI/180)-fYaw)+*/Kd3*(0-O[2])*180/M_PI);
-    
-    //Om1 = 0.00576066*pow((PWM1zad-10000),2) - U2/0.000024768 + U3/0.000132 ;             //kwadraty
-    //Om2 = 0.00576066*pow((PWM2zad-10000),2) + U1/0.000024768 - U3/0.000132 ;
-    //Om3 = 0.00576066*pow((PWM3zad-10000),2) + U2/0.000024768 + U3/0.000132 ;
-    //Om4 = 0.00576066*pow((PWM4zad-10000),2) - U1/0.000024768 - U3/0.000132 ;
-    
-    /*wyp1 = sqrt(Om1)*13.17523+10000;
-    wyp2 = sqrt(Om2)*13.17523+10000;
-    wyp3 = sqrt(Om3)*13.17523+10000;
-    wyp4 = sqrt(Om4)*13.17523+10000;*/
-    
-    
-    // ***********************  DISCRETE PID CONTROLLER  **********************
-    //stale    
-    //b=0.000015
-    //B1=0.000776646
-    //C1=-0.334958973
-    //l=0.3
-    //d=0.000033
-    //4d=0.000132
-    
-    //B1/b1=50.22498189
-    //C1/b1=-21611.4954
-    //b1/B1=0.01991041
-    //C1/B1=-431.2892625
-    //1/B1=1287.588322
-
-    //2b2l=0.000009424
-    //B2/b2=51.130533
-    //C2/b2=-30717.00651
-    //b2/B2=0.019557786
-    //C2/B2=-600.7566265
-    
-    //2b3l=0.000009139
-    //B3/b3=50.66613192
-    //C3/b3=-26072.99576
-    //b3/B3=0.01973705
-    //C3/B3=-514.6040317
-    
-    //2b4l=0.000009276
-    //B4/b4=50.95680893
-    //C4/b4=-28979.76588
-    //b4/B4=0.019624463
-    //C4/B4=-568.7123367
-    
-    //stare 2b*l
-    //0.0000092780
-    //0.000009424
-    //0.000009139
-    //0.000009276
-    
-    //2b1l=0.000000096789585
-    //2b2l=0.000000095889323
-    //2b3l=0.000000097529031
-    //2b4l=0.000000093579744
-    
-    // U1=Ixx*(Kp1*(katzad-kat)+Kd1*(omegazad-omega)+Ki1*sumauchyb)
-    // U2=Iyy*(Kp2*(katzad-kat)+Kd2*(omegazad-omega)+Ki2*sumauchyb)
-    // U3=Izz*(Kp3*(katzad-kat)+Kd3*(omegazad-omega)+Ki3*sumauchyb)
-    //omega1^2=(B1/b1*PWM1zad+C1/b1)-U2/2b1l+U3/4d
-    //omega2^2=(B2/b2*PWM2zad+C2/b2)+U1/2b2l-U3/4d
-    //omega3^2=(B3/b3*PWM3zad+C3/b3)+U2/2b3l+U3/4d
-    //omega4^2=(B4/b4*PWM4zad+C4/b4)-U1/2b4l-U3/4d
-    //wyp1=b/B1*omega1^2-C1/B1
-    //wyp2=b/B*omega2^2-C/B
-    //wyp3=b/B3*omega3^2-C3/B3
-    //wyp4=b/B*omega4^2-C/B
-    
-    //B*PWMzad+C=T
-    //PWMzad=1/B*T-C/B
-    /*
-    PWMzad1=1287.588322*T1zad-(-431.2892625);
-    PWMzad2=1245.207965*T2zad-(-600.7566265);
-    PWMzad3=1295.740776*T3zad-(-514.6040317);
-    PWMzad4=1269.412072*T4zad-(-568.7123367);*/
-    /*
-    wyp1=0.01991041*Om1-(-431.2892625)+PWM1zad;
-    wyp2=0.019557786*Om2-(-600.7566265)+PWM2zad;
-    wyp3=0.01973705*Om3-(-514.6040317)+PWM3zad;
-    wyp4=0.019624463*Om4-(-568.7123367)+PWM4zad;
-    */
-
-    pitchE=pitchE+(0-pitch);
-    if(pitchE>3) pitchE=3;
-    if(pitchE<-3) pitchE=-3;
-    //Kd1=Kp1*Td1/T;
-    //if(Ti1==0){Ki1=0;}else Ki1=Kp1*T/Ti1;
-    
-    rollE=rollE+(0-roll2);
-    if(rollE>5) rollE=5;
-    if(rollE<-5) rollE=-5;
-    //Kd2=Kp2*Td2/T;
-    //if(Ti2==0){Ki2=0;}else Ki2=Kp2*T/Ti2;
-    
-    /* yawE=yawE+(0-roll2);
-    if(yawE>3) yawE=3;*/
-    Kd3=Kp3*Td3/T;
-    //if(Ti3==0){Ki3=0;}else Ki3=Kp3*T/Ti3;
-    
-    //U1 = 0.0346*(Kp1*(0-pitch)+Kd1*(0-(-kalmanpitchzyro))+Ki1*pitchE);
-    //U2 = 0.0338*(Kp2*(0-roll)+Kd2*(0-o[1])+Ki2*rollE);
-    //U3 = 0.0666*(/*Kp3*((105*M_PI/180)-fYaw)+*/Kd3*(0-O[2]));//+Ki3*rollE);
-    
-    U1 = 0.0173*(Kp1*(0-pitch)+Kd1*(0+kalmanpitchzyro)+Ki1*pitchE);
-    U2 = 0.0169*(Kp2*(0-roll)+Kd2*(0-kalmanrollzyro)+Ki2*rollE);
-    U3 = 0.0333*(/*Kp3*((105*M_PI/180)-fYaw)+*/Kd3*(0-o[2]));//+Ki3*rollE);
-        
-    Om1 = Omegazad[0]*Omegazad[0]-U2/0.000000096789585 + U3/0.000132 ;             //kwadraty
-    Om2 = Omegazad[1]*Omegazad[1]+U1/0.000000095889323 - U3/0.000132 ; 
-    Om3 = Omegazad[2]*Omegazad[2]+U2/0.000000097529031 + U3/0.000132 ;
-    Om4 = Omegazad[3]*Omegazad[3]-U1/0.000000093579744 - U3/0.000132 ;
-
-    wyp1=(sqrt(Om1)-a2[0])/a1[0];
-    wyp2=(sqrt(Om2)-a2[1])/a1[1];
-    wyp3=(sqrt(Om3)-a2[2])/a1[2];
-    wyp4=(sqrt(Om4)-a2[3])/a1[3];
-        
-    if(wyp1<=10001 || wyp1>40001) valPWM1=10000;
-    if(wyp1>=20000 && wyp1<40000) valPWM1=20000;
-    if(wyp1>10001 && wyp1<20000) valPWM1=(int)wyp1;
-
-    if(wyp2<=10001 || wyp2>40001) valPWM2=10000;
-    if(wyp2>=20000 && wyp2<40000) valPWM2=20000;
-    if(wyp2>10001 && wyp2<20000) valPWM2=(int)wyp2;
-
-    if(wyp3<=10001 || wyp3>40001) valPWM3=10000;
-    if(wyp3>=20000 && wyp3<40000) valPWM3=20000;
-    if(wyp3>10001 && wyp3<20000) valPWM3=(int)wyp3;
-
-    if(wyp4<=10001 || wyp4>40001) valPWM4=10000;
-    if(wyp4>=20000 && wyp4<40000) valPWM4=20000;
-    if(wyp4>10001 && wyp4<20000) valPWM4=(int)wyp4;    
-    
-    
-    //sprintf(buff, "%f,%f,%f,%f\n\r", -katy*180/M_PI, roll*180/M_PI, -katybut*180/M_PI, roll2*180/M_PI);
-    //pc.printf(buff);
-    //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));
-    //pc.printf(buff);    
-    //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));
-    //pc.printf(buff);
-    //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));
-    //pc.printf(buff);
-    
-    M1.pulsewidth_us(valPWM1);
-    M2.pulsewidth_us(valPWM2);
-    M3.pulsewidth_us(valPWM3);
-    M4.pulsewidth_us(valPWM4);
-    
-    //myled = !myled;
-}
-
-/*
-void task2()
-{
-    
-    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));
-    pc.printf(buff);
-    myled2 = !myled2;
-    imu.readData(d);
-    
-            if (filtrZmienna < 0) {
-            filtrWynik = filtrSygnal;
-            filtrBufor[filtrZmienna + filtrWymiar] = filtrSygnal;
-            filtrSuma = filtrSuma + filtrBufor[filtrZmienna + filtrWymiar];
-        } else {
-            if (filtrZmienna == filtrWymiar - 1) {
-                filtrSuma = filtrSuma - filtrBufor[filtrZmienna];
-                filtrBufor[filtrZmienna] = filtrSygnal;
-                filtrSuma = filtrSuma + filtrBufor[filtrZmienna];
-                filtrWynik = filtrSuma / filtrWymiar;
-            } else {
-                filtrSuma = filtrSuma - filtrBufor[filtrZmienna];
-                filtrBufor[filtrZmienna] = filtrSygnal;
-                filtrSuma = filtrSuma + filtrBufor[filtrZmienna];
-                filtrWynik = filtrSuma / filtrWymiar;
-            }
-        }
-        filtrZmienna++;
-        if (filtrZmienna == filtrWymiar) {
-            filtrZmienna = 0;
-        }
-    
-    if (filtrWynik < xmin) xmin=filtrWynik;
-    if (filtrWynik > xmax) xmax=filtrWynik;
-    
+    rollma = filtrWynik;
     
     if (filtr2Zmienna < 0) {
             filtr2Wynik = filtr2Sygnal;
@@ -435,132 +208,64 @@
         if (filtr2Zmienna == filtr2Wymiar) {
             filtr2Zmienna = 0;
         }
-    
-    if (filtr2Wynik < ymin) ymin=filtr2Wynik;
-    if (filtr2Wynik > ymax) ymax=filtr2Wynik;
+    pitchma=filtr2Wynik;
+            
+}
+
+/*
+void task2()
+{
     
-                if (filtr3Zmienna < 0) {
-            filtr3Wynik = filtr3Sygnal;
-            filtr3Bufor[filtr3Zmienna + filtr3Wymiar] = filtr3Sygnal;
-            filtr3Suma = filtr3Suma + filtr3Bufor[filtr3Zmienna + filtr3Wymiar];
-        } else {
-            if (filtr3Zmienna == filtr3Wymiar - 1) {
-                filtr3Suma = filtr3Suma - filtr3Bufor[filtr3Zmienna];
-                filtr3Bufor[filtr3Zmienna] = filtr3Sygnal;
-                filtr3Suma = filtr3Suma + filtr3Bufor[filtr3Zmienna];
-                filtr3Wynik = filtr3Suma / filtr3Wymiar;
-            } else {
-                filtr3Suma = filtr3Suma - filtr3Bufor[filtr3Zmienna];
-                filtr3Bufor[filtr3Zmienna] = filtr3Sygnal;
-                filtr3Suma = filtr3Suma + filtr3Bufor[filtr3Zmienna];
-                filtr3Wynik = filtr3Suma / filtr3Wymiar;
-            }
-        }
-        filtr3Zmienna++;
-        if (filtr3Zmienna == filtr3Wymiar) {
-            filtr3Zmienna = 0;
-        }
-    
-    if (filtr3Wynik < zmin) zmin=filtr3Wynik;
-    if (filtr3Wynik > zmax) zmax=filtr3Wynik;
-    
-    //sprintf(buff, "%f,%f,%f,%f\n\r", d[4],filtrWynik,ymin,ymax);
-    //pc.printf(buff);
 }
 
 
 void task3()
 {
-    imu.readData(d);
-    off.offsetData(d,offsetGyro,o);
-    katxzyro = katxzyro + o[0]*dt;
-    katyzyro = katyzyro + o[1]*dt;
-    katzzyro = katzzyro + o[2]*dt;
     
-    r = sqrt(pow(d[3],2) + pow(d[4],2) + pow(d[5],2));
-    katx = acos(d[4]/r)-M_PI2;
-    katy = acos(d[3]/r)-M_PI2;
-    
-    kalman_innovate(&pitch_data, katx, o[0]);
-    kalman_innovate(&roll_data, -katy, o[1]);
-    pitch = pitch_data.x1;
-    kalmanpitchzyro = pitch_data.x2;
-    kalmanpitchdryf = pitch_data.x3;
-    roll = roll_data.x1;
 }
 */
 
+
+
 void task_tachometer()
 {
     
     TachFlaga=0;
-    if(FlagaPom==1){
-    PWM4zad+=5;
-    M3.pulsewidth_us(PWM4zad);
-    }
-    wait(0.5f);
-    TachFlaga=1;
-    //myled2=!myled2;
-    RPMtach=impulsA*10; // dla T=3s
-    //RPMtach=impulsA*6;   //dla T=5s
+    
+    //RPMtach=impulsA*10; // dla T=3s
+    RPMtach=impulsA*6;   //dla T=5s
     //RPMtach=impulsA*3;   //dla T=10s
-    //RPMtachAktualny=impulsA*3;
-    /*
-    if (TachFlaga==1){
-        RPMtachBuf[iTach]=RPMtachAktualny;
-        iTach++;
-        if (iTach>3){
-            RPMtachWyn=(RPMtachBuf[iTach-1]+RPMtachBuf[iTach-2]+RPMtachBuf[iTach-3])/3;
-            RPMtachWynPoprz=RPMtachWyn;
-            PWM4zadWyn=PWM4zad;
-            TachFlaga=0;
-            iTach=0;
-            RPMtach=RPMtachAktualny;
-        }
-    }else{
-    if(abs(PWM4zadPoprz-PWM4zad)>0 && abs(RPMtachAktualny-RPMtachWynPoprz)>20){
-     TachFlaga=1;
-    }
-    //RPMtach=RPMtachAktualny;
-    PWM4zadPoprz=PWM4zad;
-    PWM4zad+=1;
-    M4.pulsewidth_us(PWM4zad);
-    //if(PWM4zad>14000)PWM4zad=10000;     
-     }
-    sprintf(buff, "%f,%i,%f,%f\n\r", PWM4zad,RPMtachAktualny,PWM4zadWyn,RPMtachWyn);
+    
+    sprintf(buff, "%f,%i\n\r", PWM1zad,RPMtach);
     pc.printf(buff);
-    */
-    sprintf(buff, "%f,%i\n\r", PWM4zad,RPMtach);
-    pc.printf(buff);
-    //iTach++;
-    //if(iTach%2>0)PWM4zad+=1;
-    //M4.pulsewidth_us(PWM4zad);
     
-    //RPMtachWyn=0;
-    //PWM4zadWyn=0;
     impulsA=0;
+    TachFlaga=1;
 }
 
 
+
 int main() {
     
+    zadanie=zad;
+    
     pc.baud(115200);
     bluetooth.baud(19200);
     imu.init();
+    
+    
     kalman_init(&pitch_data);
     kalman_init(&roll_data);
     kalman_init(&pitch_data2);
     kalman_init(&roll_data2);
-    //event.rise(&rise);
-    //event.mode(PullUp);
-    //event.enable_irq();
     
+
     sprintf(buff, "Hello: \n\r");
     pc.printf(buff);
     
-    off.setOffsets(offsetGyro,&offsetMagneto, bluetooth, imu);
+    off.setOffsets(offsetGyro,&offsetMagneto, pc, imu);
     i=1000;
-    
+
     PWM1zad=10000;
     PWM2zad=10000;
     PWM3zad=10000;
@@ -575,212 +280,42 @@
     M4.period_us(PWM_period);
     M4.pulsewidth_us(PWM4zad);
     
-    /*Kp1=10;
-    Kp2=8;
-    Kp3=0;
-    Kd1=8;
-    Kd2=16;
-    Kd3=5000;
-    Ki1=0.1;
-    Ki2=0.2;
-    Ki3=0;
-    Td1=0;
-    Td2=0;
-    Td3=0;
-    Ti1=0;
-    Ti2=0;
-    Ti3=0;
-    T=0.005;*/
-    Kp1=12;
-    Kp2=12;
-    Kp3=0;
-    Kd1=2;
-    Kd2=2;
-    Kd3=0;
-    Ki1=0;
-    Ki2=0;
-    Ki3=0;
-    Td1=0;
-    Td2=0;
-    Td3=0;
-    Ti1=0;
-    Ti2=0;
-    Ti3=0;
-    T=0.005;
-    
-    rollE=0;
-    
+    /*rollE=0;
     katxzyro = 0;
     katyzyro = 0;
-    katzzyro = 0;
+    katzzyro = 0;*/
     
     iTach=0;
     TachFlaga=0;
-    
-a1buf[0][0]=    1.438602213;
-a1buf[0][1]=    1.130064065;
-a1buf[0][2]=    0.958928363;
-a1buf[0][3]=    0.836156086;
-a1buf[0][4]=    0.735165987;
-a1buf[0][5]=    0.619988352;
-a1buf[0][6]=    0.580966803;
-a1buf[0][7]=    0.580966803;
-a1buf[0][8]=    0.580966803;
-a1buf[1][0]=    1.55790332;
-a1buf[1][1]=    1.181479324;
-a1buf[1][2]=    1.001071637;
-a1buf[1][3]=    0.872754805;
-a1buf[1][4]=    0.699592312;
-a1buf[1][5]=    0.644682586;
-a1buf[1][6]=    0.596575422;
-a1buf[1][7]=    0.596575422;
-a1buf[1][8]=    0.596575422;
-a1buf[2][0]=    1.475317414;
-a1buf[2][1]=    1.159883518;
-a1buf[2][2]=    0.965894001;
-a1buf[2][3]=    0.85553873;
-a1buf[2][4]=    0.738311008;
-a1buf[2][5]=    0.627093768;
-a1buf[2][6]=    0.627093768;
-a1buf[2][7]=    0.627093768;
-a1buf[2][8]=    0.627093768;
-a1buf[3][0]=    1.40570763;
-a1buf[3][1]=    1.143366337;
-a1buf[3][2]=    0.953383809;
-a1buf[3][3]=    0.850413512;
-a1buf[3][4]=    0.734304019;
-a1buf[3][5]=    0.634804892;
-a1buf[3][6]=    0.588398369;
-a1buf[3][7]=    0.588305183;
-a1buf[3][8]=    0.588305183;
+
 
-a2buf[0][0]=    -14020.22132;
-a2buf[0][1]=    -10629.35935;
-a2buf[0][2]=    -8667.80431;
-a2buf[0][3]=    -7195.189284;
-a2buf[0][4]=    -5935.445545;
-a2buf[0][5]=    -4432.964473;
-a2buf[0][6]=    -3899.481654;
-a2buf[0][7]=    -3899.481654;
-a2buf[0][8]=    -3899.481654;
-a2buf[1][0]=    -15394.5894;
-a2buf[1][1]=    -11261.14735;
-a2buf[1][2]=    -9187.344205;
-a2buf[1][3]=    -7648.375073;
-a2buf[1][4]=    -5478.811881;
-a2buf[1][5]=    -4780.460105;
-a2buf[1][6]=    -4131.426907;
-a2buf[1][7]=    -4131.426907;
-a2buf[1][8]=    -4131.426907;
-a2buf[2][0]=    -14446.09785;
-a2buf[2][1]=    -10984.43215;
-a2buf[2][2]=    -8760.145603;
-a2buf[2][3]=    -7434.705882;
-a2buf[2][4]=    -5971.980198;
-a2buf[2][5]=    -4525.428072;
-a2buf[2][6]=    -4525.428072;
-a2buf[2][7]=    -4525.428072;
-a2buf[2][8]=    -4525.428072;
-a2buf[3][0]=    -13645.61444;
-a2buf[3][1]=    -10761.08911;
-a2buf[3][2]=    -8579.586488;
-a2buf[3][3]=    -7342.912056;
-a2buf[3][4]=    -5896.039604;
-a2buf[3][5]=    -4603.83809;
-a2buf[3][6]=    -3978.10134;
-a2buf[3][7]=    -3976.928363;
-a2buf[3][8]=    -3976.928363;
-
-Omegamax[0][0]= 1790;
-Omegamax[0][1]= 2360;
-Omegamax[0][2]= 2830;
-Omegamax[0][3]= 3250;
-Omegamax[0][4]= 3620;
-Omegamax[0][5]= 3950;
-Omegamax[0][6]= 4230;
-Omegamax[0][7]= 4270;
-Omegamax[0][8]= 20000;
-Omegamax[1][0]= 1710;
-Omegamax[1][1]= 2300;
-Omegamax[1][2]= 2820;
-Omegamax[1][3]= 3260;
-Omegamax[1][4]= 3590;
-Omegamax[1][5]= 3920;
-Omegamax[1][6]= 4220;
-Omegamax[1][7]= 4240;
-Omegamax[1][8]= 20000;
-Omegamax[2][0]= 1760;
-Omegamax[2][1]= 2360;
-Omegamax[2][2]= 2810;
-Omegamax[2][3]= 3250;
-Omegamax[2][4]= 3630;
-Omegamax[2][5]= 3940;
-Omegamax[2][6]= 4110;
-Omegamax[2][7]= 4110;
-Omegamax[2][8]= 20000;
-Omegamax[3][0]= 1790;
-Omegamax[3][1]= 2380;
-Omegamax[3][2]= 2880;
-Omegamax[3][3]= 3270;
-Omegamax[3][4]= 3650;
-Omegamax[3][5]= 3970;
-Omegamax[3][6]= 4250;
-Omegamax[3][7]= 4300;
-Omegamax[3][8]= 20000;
-
-    a1[0]=a1buf[0][0];
-    a2[0]=a2buf[0][0];
-    a1[1]=a1buf[1][0];
-    a2[1]=a2buf[1][0];
-    a1[2]=a1buf[2][0];
-    a2[2]=a2buf[2][0];
-    a1[3]=a1buf[3][0];
-    a2[3]=a2buf[3][0];
-
-    triger1.attach(&task1, 0.005);
-    //triger2.attach(&task2, 0.005);
-    //triger3.attach(&task3, 0.005);
-    //tachometer.attach(&task_tachometer, 3.5);
+    switch (zadanie){
+        case 1:
+        triger1.attach(&task1, 0.005);
+        break;
+        
+        case 2:       
+        event.rise(&rise);
+        event.mode(PullUp);
+        event.enable_irq();
+        tachometer.attach(&task_tachometer, 5);
+        break;
+    };
     
     
     
     while(1) {
+//WYSYLANIE DANYCH PO PC UART 
     
-    //myled2 = !myled2;
-    //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);
-    //pc.printf(buff);
-    //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);
-    //pc.printf(buff);
-    //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);
-    //pc.printf(buff);
-    //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);
-    //pc.printf(buff);
-    //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);
-    //pc.printf(buff);
-    //sprintf(buff, "%f,%f,%f\n\r", d[0],d[1],d[2]);
-    //pc.printf(buff);  
-    //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);
-    //pc.printf(buff);  
-    //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);
-    //pc.printf(buff);
-    //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);
-    //pc.printf(buff);
-    //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);
-    //pc.printf(buff);     
-    //sprintf(buff, "%f,%f,%f,%f,%f\n\r", valPWM4,RPMtach,valPWM3,valPWM2,valPWM1);
-    //pc.printf(buff);  
-    //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);
-    //pc.printf(buff);    
-    //sprintf(buff, "%f,%f,%f,%f,%f,%f,%f\n\r", yaw*180/M_PI,yawOff*180/M_PI,d[6],d[7],d[8],o[2]*180/M_PI,katz*180/M_PI);
-    //pc.printf(buff);  
-    //myled2 = !myled2;
-    //sprintf(buff, "%i,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n\r", Omegazad[0],PWM1zad,PWM2zad,PWM3zad,PWM4zad,a1[0],a1[1],a1[2],a1[3],a2[0],a2[1],a2[2],a2[3]);
-    //pc.printf(buff);
-    
-    
-    
-    sprintf(buff, "%f,%f,%f,%f,%f,%f,%f,%f\n\r", roll*180/M_PI,valPWM1,valPWM2,valPWM3,valPWM4,Kp1,Kd1,Ki1);
-    pc.printf(buff);  
+    switch (zadanie){
+        case 1:
+        sprintf(buff, "0,%f,%f,%f,%f\n\r", -katy*180/M_PI, katzyro*180/M_PI, roll*180/M_PI);
+        pc.printf(buff);  
+        break;
+        
+        case 2:       
+        break;
+    };
          
     
     if(pc.readable()){
@@ -788,166 +323,48 @@
         switch (znak){
             
             case 'r':
-            sprintf(buff, "Resetuje zmienne %c\n\r",znak);
-            pc.printf(buff);
-            xmin=0;
-            ymin=0;
-            zmin=0;
-            xmax=0;
-            ymax=0;
-            zmax=0;
             break;
             
             case 't':
-            PWM4zad++;
-            M3.pulsewidth_us(PWM4zad);
-            break;
-            case 'g':
-            PWM4zad--;
-            M3.pulsewidth_us(PWM4zad);
-            break;
-            case 'y':
-            PWM4zad+=10;
-            M3.pulsewidth_us(PWM4zad);
-            break;
-            case 'h':
-            PWM4zad-=10;
-            M3.pulsewidth_us(PWM4zad);
-            break;
-            case 'u':
-            PWM4zad+=100;
-            M3.pulsewidth_us(PWM4zad);
-            break;
-            case 'j':
-            PWM4zad-=100;
-            M3.pulsewidth_us(PWM4zad);
-            break;
-            case 's':
-            PWM4zad=10000;
-            M3.pulsewidth_us(PWM4zad);
-            break;
-            case 'b':
-            PWM4zad=10000;
-            M3.pulsewidth_us(PWM4zad);
-            break;
-            case 'z':
-            FlagaPom=1;
-            break;
-            case 'x':
-            FlagaPom=0;
+            PWM1zad+=50;
+            if(PWM1zad>=20000){
+            PWM1zad=20000;
+            }
+            M1.pulsewidth_us(PWM1zad);
             break;
             
-            /*case 'u':
-            PWM1zad+=10;
+            case 'g':
+            PWM1zad-=50;
+            if(PWM1zad<10000){
+            PWM1zad=10000;
+            }
             M1.pulsewidth_us(PWM1zad);
             break;
-            case 'j':
-            PWM1zad-=10;
-            M1.pulsewidth_us(PWM1zad);
-            break;*/
-            }
             
-        znak=0;
-    }
-
-        
-        if(bluetooth.readable()){
-        
-        znak2=bluetooth.getc();
-        
-        switch (znak2){
+            case 'y':
+            break;
             
+            case 'h':
+            break;
             
-            case 'a':    
-            for(jster=0;jster<4;jster++){
-                Omegazad[jster]-=50;
-            }
-            flagaster=1;
-
-            /*T1zad-=0.05;
-            T2zad-=0.05;
-            T3zad-=0.05;
-            T4zad-=0.05;
-            PWM1zad=1287.588322*T1zad-(-431.2892625)+10000;
-            PWM2zad=1245.207965*T2zad-(-600.7566265)+10000;
-            PWM3zad=1295.740776*T3zad-(-514.6040317)+10000;
-            PWM4zad=1269.412072*T4zad-(-568.7123367)+10000;*/
+            case 'u':
+            break;
             
-            /*
-            //PWM3zad-=50;
-            PWM4zad-=50;
-            //PWM2zad=1.045*(PWM4zad-10000)+10000;
-            //PWM1zad=1.026*(PWM3zad-10000)+10000;
+            case 'j':
+            break;
             
-            //PWM1zad=1.026*(PWM3zad-10000)+10000;
-            //PWM1zad-=50;
-            if(PWM1zad<10000){
-                PWM1zad=10000;
-                PWM2zad=10000;
-                PWM3zad=10000;
-                PWM4zad=10000;
-            }
-            //ustawianie
-            M1.pulsewidth_us(PWM1zad);
-            M2.pulsewidth_us(PWM2zad);
-            M3.pulsewidth_us(PWM3zad);
-            M4.pulsewidth_us(PWM4zad);
-            */
-            znak2=0;
-            
+            case 's':
             break;
             
             case 'b':
-            for(jster=0;jster<4;jster++){
-            Omegazad[jster]+=50;
-            }
-            flagaster=1;
-            /*T1zad+=0.05;
-            T2zad+=0.05;
-            T3zad+=0.05;
-            T4zad+=0.05;
-            PWM1zad=1287.588322*T1zad-(-431.2892625)+10000;
-            PWM2zad=1245.207965*T2zad-(-600.7566265)+10000;
-            PWM3zad=1295.740776*T3zad-(-514.6040317)+10000;
-            PWM4zad=1269.412072*T4zad-(-568.7123367)+10000;*/
-            /*
-            //PWM3zad+=50;
-            PWM4zad+=50;
-            //PWM2zad=1.045*(PWM4zad-10000)+10000;
-            //PWM3zad+=50;
-            //PWM1zad=1.026*(PWM3zad-10000)+10000;
+            break;
             
-            //PWM1zad+=50;
-            if(PWM1zad>=20000){
-                PWM1zad=20000;
-                PWM2zad=20000;
-                PWM3zad=20000;
-                PWM4zad=20000;
-            }
-            //ustawianie
-            M1.pulsewidth_us(PWM1zad);
-            M2.pulsewidth_us(PWM2zad);
-            M3.pulsewidth_us(PWM3zad);
-            M4.pulsewidth_us(PWM4zad);
-            */
-            znak2=0;
+            case 'z':
             break;
             
             case 'x':
-            //sprintf(buff,"Nastawy: %f,%f\n\r",Kp2,Kd2);
-            //pc.printf(buff);
-            //wait(1.0f);
-            for(jster=0;jster<4;jster++){
-            Omegazad[jster]=0;
-            }
             PWM1zad=10000;
-            PWM2zad=10000;
-            PWM3zad=10000;
-            PWM4zad=10000;
             M1.pulsewidth_us(PWM1zad);
-            M2.pulsewidth_us(PWM2zad);
-            M3.pulsewidth_us(PWM3zad);
-            M4.pulsewidth_us(PWM4zad);
             sprintf(buff,"Odlacz silniki\n\r");
             pc.printf(buff);
             wait(1.0f);
@@ -970,66 +387,116 @@
             pc.printf(buff);
             break;
             
+            case 0x20:
+            PWM1zad=10000;
+            M1.pulsewidth_us(PWM1zad);
+            sprintf(buff,"Odlacz silniki\n\r");
+            pc.printf(buff);
+            wait(1.0f);
+            sprintf(buff,"5 \n\r");
+            pc.printf(buff);
+            wait(1.0f);
+            sprintf(buff,"4 \n\r");
+            pc.printf(buff);
+            wait(1.0f);
+            sprintf(buff,"3 \n\r");
+            pc.printf(buff);           
+            wait(1.0f);
+            sprintf(buff,"2 \n\r");
+            pc.printf(buff);
+            wait(1.0f);
+            sprintf(buff,"1 \n\r");
+            pc.printf(buff);
+            wait(1.0f);
+            sprintf(buff,"GO! \n\r");
+            pc.printf(buff);
+            break;
+
+            }
+            
+        znak=0;
+    }
+
+        
+        if(bluetooth.readable()){
+        
+        znak2=bluetooth.getc();
+        
+        switch (znak2){
+          
+            case 'a':    
+            PWM1zad-=50;
+            flagaster=1;
+            znak2=0;
+            break;
+            
+            case 'b':
+            PWM1zad+=50;
+            flagaster=1;
+            znak2=0;
+            break;
+            
+            case 'x':
+            PWM1zad=10000;
+            M1.pulsewidth_us(PWM1zad);
+            
+            sprintf(buff,"Odlacz silniki\n\r");
+            pc.printf(buff);
+            wait(1.0f);
+            sprintf(buff,"5 \n\r");
+            pc.printf(buff);
+            wait(1.0f);
+            sprintf(buff,"4 \n\r");
+            pc.printf(buff);
+            wait(1.0f);
+            sprintf(buff,"3 \n\r");
+            pc.printf(buff);           
+            wait(1.0f);
+            sprintf(buff,"2 \n\r");
+            pc.printf(buff);
+            wait(1.0f);
+            sprintf(buff,"1 \n\r");
+            pc.printf(buff);
+            wait(1.0f);
+            sprintf(buff,"GO! \n\r");
+            pc.printf(buff);
+            break;
+            
+            
             case 'c':
-            Kd1-=0.5;
-            Kd2-=0.5;
+
             break;
             case 'd':
-            Kd1+=0.5;
-            Kd2+=0.5;
+
             break;    
             case 'e':
-            Kp1-=0.5;
-            Kp2-=0.5;
+
             break;
             case 'f':
-            Kp1+=0.5;
-            Kp2+=0.5;
+
             break;   
             case 'g':
-            Ki1-=0.1;
-            Ki2-=0.1;
+
             break;  
             case 'h':
-            Ki1+=0.1;
-            Ki2+=0.1;
+
             break;                    
         }    
         
         
         if(flagaster==1){
 
-            for(int jster=0;jster<4;jster++){
-            ister=-1;
-                do{
-                    ister++;
-                }while(Omegazad[jster] > Omegamax[jster][ister]);
-            a1[jster]=a1buf[jster][ister];
-            a2[jster]=a2buf[jster][ister];
-            }    
 
-        PWM1zad=(Omegazad[0]-a2[0])/a1[0];
-        PWM2zad=(Omegazad[1]-a2[1])/a1[1];
-        PWM3zad=(Omegazad[2]-a2[2])/a1[2];
-        PWM4zad=(Omegazad[3]-a2[3])/a1[3];
         
         if(PWM1zad<10000){
         PWM1zad=10000;
-            }
-        if(PWM2zad<10000){
-        PWM2zad=10000;
-            }
-        if(PWM3zad<10000){
-        PWM3zad=10000;
-            }
-        if(PWM4zad<10000){
-        PWM4zad=10000;
-            }
-        M1.pulsewidth_us(PWM1zad);
-        M2.pulsewidth_us(PWM2zad);
-        M3.pulsewidth_us(PWM3zad);
-        M4.pulsewidth_us(PWM4zad);
-        
+        }
+
+        if(PWM1zad>=20000){
+        PWM1zad=20000;
+        }
+
+        M1.pulsewidth_us(PWM1zad);      
         flagaster=0;
         }