Mechatronics
Dydaktyka
Dependencies: FastPWM mbed-src
Fork of
2015_04_17_quadro_bez_sterowania
by 
Quadrocopter
Diff: main.cpp
- Revision:
- 8:dc48ce79ad59
- Parent:
- 7:2ba30a0cdc16
- Child:
- 9:4e94a16ca90c
--- a/main.cpp Thu Jan 15 07:46:43 2015 +0000
+++ b/main.cpp Wed Jan 21 16:30:33 2015 +0000
@@ -9,6 +9,12 @@
#define M_PI 3.141592
#define M_PI2 1.570796
#define dt 0.005
+#define filtrWymiar 100
+#define filtrSygnal d[3]
+#define filtr2Wymiar 100
+#define filtr2Sygnal d[4]
+#define filtr3Wymiar 100
+#define filtr3Sygnal d[5]
//kalman
@@ -33,7 +39,8 @@
Ticker triger1; //przerwanie filtracji
-//Ticker triger2; //przerwanie wysyłania danych
+Ticker triger2; //kalibracja accelero
+Ticker triger3; //kalibracja zyro
float d[9];
double D[9];
@@ -63,6 +70,30 @@
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 kalmanrollzyro;
+float kalmanrollzyrobut;
+float kalmanpitchdryf;
void task1()
{
@@ -80,18 +111,22 @@
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;
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;
roll2 = roll_data2.x1;
+ kalmanrollzyrobut = roll_data2.x2;
//U1 = 0.0173*(Kp1*(-pitch2)+Kd1*(0-O[0]));
//U2 = 0.0169*(Kp2*((-20*M_PI/180)-roll2)+Kd2*(0-O[1]));
@@ -132,35 +167,40 @@
//C1/b=-21611.4954
//b/B1=0.01991041
//C1/B1=-431.2892625
+ //1/B1=1287.588322
+
//B3/b=49.90897978
//C3/b=-25683.36221
//b/B3=0.020036474
//C3/B3=-514.6040317
-
+
+ //B*PWMzad+C=T
+ //PWMzad=1/B*T-C/B
+ /*
+ PWMzad1=1287.588322*T1zad-(-431.2892625);
+ PWMzad2=1287.588322*T2zad-(-431.2892625);
+ PWMzad3=1287.588322*T3zad-(-431.2892625);
+ PWMzad4=1287.588322*T4zad-(-431.2892625);*/
-
-
pitchE=pitchE+(0-pitch2);
if(pitchE>3) pitchE=3;
- Kd1=Kp1*Td1/T;
- Ki1=Kp1*T/Ti1;
- if(Ti1==0)Ki1=0;
+ //Kd1=Kp1*Td1/T;
+ //if(Ti1==0){Ki1=0;}else Ki1=Kp1*T/Ti1;
rollE=rollE+(0-roll2);
- if(rollE>3) rollE=3;
- Kd2=Kp2*Td2/T;
- Ki2=Kp2*T/Ti2;
- if(Ti2==0)Ki2=0;
+ 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;
- Ki3=Kp3*T/Ti3;
- if(Ti3==0)Ki3=0;
+ //if(Ti3==0){Ki3=0;}else Ki3=Kp3*T/Ti3;
U1 = 0.0173*(Kp1*(0-pitch2)+Kd1*(0-O[0])+Ki1*pitchE);
- U2 = 0.0169*(Kp2*(0-roll2)+Kp2*Td2/T*(0-O[1])+Kp2*T/Ti2*rollE);
+ 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);
Om1 = 50.22498189*PWM1zad+(-21611.4954)-U2/0.0000092780 + U3/0.000132 ; //kwadraty
@@ -168,10 +208,10 @@
Om3 = 49.90897978*PWM3zad+(-25683.36221)+U2/0.0000092780 + U3/0.000132 ;
Om4 = 50.22498189*PWM4zad+(-21611.4954)-U1/0.0000092780 - U3/0.000132 ;
- wyp1=0.01991041*Om1+(-431.2892625);
- wyp2=0.01991041*Om2+(-431.2892625);
- wyp3=0.020036474*Om3+(-514.6040317);
- wyp4=0.01991041*Om4+(-431.2892625);
+ wyp1=0.01991041*Om1-(-431.2892625);
+ wyp2=0.01991041*Om2-(-431.2892625);
+ wyp3=0.020036474*Om3-(-514.6040317);
+ wyp4=0.01991041*Om4-(-431.2892625);
if(wyp1<=10001 || wyp1>40001) valPWM1=10000;
if(wyp1>=20000 && wyp1<40000) valPWM1=20000;
@@ -215,8 +255,111 @@
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;
+
+}
@@ -239,6 +382,7 @@
triger1.attach(&task1, 0.005);
//triger2.attach(&task2, 0.005);
+ //triger3.attach(&task3, 0.005);
i=1000;
@@ -259,6 +403,12 @@
Kp1=0;
Kp2=0;
Kp3=0;
+ Kd1=0;
+ Kd2=0;
+ Kd3=0;
+ Ki1=0;
+ Ki2=0;
+ Ki3=0;
Td1=0;
Td2=0;
Td3=0;
@@ -267,20 +417,39 @@
Ti3=0;
T=0.005;
+ rollE=0;
+
+ katxzyro = 0;
+ katyzyro = 0;
+ katzzyro = 0;
while(1) {
//myled2 = !myled2;
- //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));
+
+ //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,%f,%f\n\r", -katybut*180/M_PI, roll2*180/M_PI, (O[1]*180/M_PI),valPWM1, valPWM2, valPWM3, valPWM4);
+ //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,%f,%f,%f,%f,%f\n\r", -katybut*180/M_PI, roll2*180/M_PI, -katy*180/M_PI,(O[1]*180/M_PI),wyp1,wyp2,wyp3,wyp4);
- pc.printf(buff);
- //sprintf(buff, "%f,%f\n\r", -katy*180/M_PI, roll*180/M_PI);
- //pc.printf(buff);
- //sprintf(buff, "%f,%f,%f,%f\n\r", valPWM1, valPWM2, valPWM3, valPWM4);
- //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\n\r", roll*180/M_PI,kalmanrollzyro*180/M_PI,valPWM1,valPWM3,Kp2,Kd2,Ki2,rollE*180/M_PI);
+ 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);
//myled2 = !myled2;
@@ -294,6 +463,16 @@
sprintf(buff, "odczytany znak: %c\n\r",znak);
pc.printf(buff);
break;
+ 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 'u':
PWM1zad+=10;
M1.pulsewidth_us(PWM1zad);
@@ -315,10 +494,13 @@
switch (znak2){
case 'a':
- PWM1zad-=50;
- PWM2zad-=50;
PWM3zad-=50;
- PWM4zad-=50;
+ //PWM2zad-=50;
+ //PWM3zad-=50;
+ //PWM1zad-=50;
+ //PWM1zad=1.026*(PWM3zad-10000)+10000;
+ PWM1zad=1.035*(PWM3zad-10000)+10000;
+ //PWM1zad-=50;
if(PWM1zad<10000){
PWM1zad=10000;
PWM2zad=10000;
@@ -334,10 +516,14 @@
break;
case 'b':
- PWM1zad+=50;
- PWM2zad+=50;
PWM3zad+=50;
- PWM4zad+=50;
+ //PWM2zad+=50;
+ //PWM3zad+=50;
+ //PWM1zad+=50;
+ //PWM1zad=1.026*(PWM3zad-10000)+10000;
+ PWM1zad=1.035*(PWM3zad-10000)+10000;
+
+ //PWM1zad+=50;
if(PWM1zad>=20000){
PWM1zad=20000;
PWM2zad=20000;
@@ -353,13 +539,15 @@
break;
case 'x':
- sprintf(buff,"Nastawy: %f,%f\n\r",Kp2,Td2);
+ sprintf(buff,"Nastawy: %f,%f\n\r",Kp2,Kd2);
pc.printf(buff);
wait(1.0f);
Kp1=0;
- Td1=0;
+ Kd1=0;
Kp2=0;
- Td2=0;
+ Kd2=0;
+ Ki2=0;
+ Ki2=0;
PWM1zad=10000;
PWM2zad=10000;
PWM3zad=10000;
@@ -391,12 +579,12 @@
break;
case 'c':
- Td1-=0.001;
- Td2-=0.001;
+ Kd1-=0.5;
+ Kd2-=0.5;
break;
case 'd':
- Td1+=0.001;
- Td2+=0.001;
+ Kd1+=0.5;
+ Kd2+=0.5;
break;
case 'e':
Kp1-=0.5;
@@ -405,7 +593,15 @@
case 'f':
Kp1+=0.5;
Kp2+=0.5;
- break;
+ break;
+ case 'g':
+ Ki1-=0.1;
+ Ki2-=0.1;
+ break;
+ case 'h':
+ Ki1+=0.1;
+ Ki2+=0.1;
+ break;
}
}
//myled2 = !myled2;