Quadrocopter / 2015_04_17_quadro_bez_sterowania

bez sterowania

Dependencies:   FastPWM mbed-src

Fork of 2015_01_29_quadro2 by Quadrocopter

main.cpp

Committer:
Michu90
Date:
2015-02-10
Revision:
10:605b0bfadc2e
Parent:
9:4e94a16ca90c
Child:
11:38db3ed41f13

File content as of revision 10:605b0bfadc2e:

#include "mbed.h"
#include "FastPWM.h"
#include "kalman.h"
#include "Offsets.h"
#include "stdio.h"
#include "IMU.h"

#define PWM_period 2500
#define M_PI 3.141592
#define M_PI2 1.570796
#define dt 0.005
#define filtrWymiar 5
#define filtrSygnal roll
#define filtr2Wymiar 5
#define filtr2Sygnal pitch
#define filtr3Wymiar 100
#define filtr3Sygnal d[5]


//kalman
// Structs for containing filter data
kalman_data pitch_data;
kalman_data roll_data;

kalman_data pitch_data2;
kalman_data roll_data2;

DigitalOut myled(PTA2);
DigitalOut myled2(PTA1);
Serial pc(USBTX, USBRX);
Serial bluetooth(D1, D0);
IMU imu(PTE25,PTE24);
Offsets off;
InterruptIn event(PTB23);

FastPWM M1(D10);
FastPWM M2(D11);
FastPWM M3(D12);
FastPWM M4(D13);


Ticker triger1; //przerwanie filtracji
Ticker triger2; //kalibracja accelero
Ticker triger3; //kalibracja zyro
Ticker tachometer;

float d[9];
double D[9];
float o[3];
float O[3];
char buff[160];
float r,katx,katy,katz;
float rbut,katxbut,katybut;
float pitch, roll, yaw;
float pitch2, roll2;
float pitchE, rollE, yawE;
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;
float filtrWynik;
float filtr2Bufor[filtrWymiar];
int   filtr2Zmienna=0;
float filtr2Suma=0;
float filtr2Wynik;
float filtr3Bufor[filtrWymiar];
int   filtr3Zmienna=0;
float filtr3Suma=0;
float filtr3Wynik;

float kalmanpitchzyro;
float kalmanpitchzyrobut;
float kalmanrollzyro;
float kalmanrollzyrobut;
float kalmanpitchdryf;

int RPMtach;
float PWM4zadBuf[5];
float PWM4zadPoprz;
float PWM4zadWyn;
float RPMtachBuf[5];
int RPMtachPoprz;
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);

    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;
       
    yaw = atan2(d[7],d[6])+4.98333*M_PI/180;
    katz = katz+(o[2]*dt);
    off.offsetMagneto(&yaw,&offsetMagneto,&yawOff);
    // Correct for heading < 0deg and heading > 360deg
    /*if(yawOff < 0){
    yawOff+= 2 * M_PI;
    }
 
    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]);
    pitch = -pitch_data.x1;
    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) {
            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;
        }
    
    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  **********************
    // 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=0.000015
    //B1=0.000776646
    //C1=-0.334958973
    //l=0.3
    //d=0.000033
    //2b1l=0.0000092780
    //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
    
    //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);*/





    

    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.0173*(Kp1*(0-pitch)+Kd1*(0+kalmanpitchzyro)+Ki1*pitchE);
    U2 = 0.0169*(Kp2*(0-roll)+Kd2*(0-o[1])+Ki2*rollE);
    U3 = 0.0333*(/*Kp3*((105*M_PI/180)-fYaw)+*/Kd3*(0-o[2]));//+Ki3*rollE);
    
    //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);
        
    Om1 = 50.22498189*PWM1zad+(-21611.4954)-U2/0.0000092780 + U3/0.000132 ;             //kwadraty
    Om2 = 51.130533*PWM2zad+(-30717.00651)+U1/0.000009424 - U3/0.000132 ; 
    Om3 = 50.66613192*PWM3zad+(-26072.99576)+U2/0.000009139 + U3/0.000132 ;
    Om4 = 50.95680893*PWM4zad+(-28979.76588)-U1/0.000009276 - U3/0.000132 ;
    
    wyp1=0.01991041*Om1-(-431.2892625);
    wyp2=0.019557786*Om2-(-600.7566265);
    wyp3=0.01973705*Om3-(-514.6040317);
    wyp4=0.019624463*Om4-(-568.7123367);
        
    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;
    
    
    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;
        }
    
    if (filtr2Wynik < ymin) ymin=filtr2Wynik;
    if (filtr2Wynik > ymax) ymax=filtr2Wynik;
    
                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*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);
    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;
    
}


int main() {
    
    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, pc, imu);
    

    
    triger1.attach(&task1, 0.005);
    //triger2.attach(&task2, 0.005);
    //triger3.attach(&task3, 0.005);
    //tachometer.attach(&task_tachometer, 3.5);
    i=1000;
    
    
    PWM1zad=10000;
    PWM2zad=10000;
    PWM3zad=10000;
    PWM4zad=10000;
    
    M1.period_us(PWM_period);
    M1.pulsewidth_us(PWM1zad);
    M2.period_us(PWM_period);
    M2.pulsewidth_us(PWM2zad);
    M3.period_us(PWM_period);
    M3.pulsewidth_us(PWM3zad);
    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=0;
    Kp2=0;
    Kp3=0;
    Kd1=0;
    Kd2=0;
    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;
    
    katxzyro = 0;
    katyzyro = 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;
    
    
    
    while(1) {
    
    //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);
         
    
    if(pc.readable()){
        znak=pc.getc();
        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;
            break;
            
            /*case 'u':
            PWM1zad+=10;
            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 '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;*/
            
            /*
            //PWM3zad-=50;
            PWM4zad-=50;
            //PWM2zad=1.045*(PWM4zad-10000)+10000;
            //PWM1zad=1.026*(PWM3zad-10000)+10000;
            
            //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;
            
            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;
            
            //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;
            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);
            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);
        
        flagaster=0;
        }
        
        }//switch
    //myled2 = !myled2;
    }//while(1)
}//main