Mechatronics
Dydaktyka
Dependencies: FastPWM mbed-src
Fork of
2015_04_17_quadro_bez_sterowania
by 
Quadrocopter
Diff: main.cpp
- Revision:
- 9:4e94a16ca90c
- Parent:
- 8:dc48ce79ad59
- Child:
- 10:605b0bfadc2e
diff -r dc48ce79ad59 -r 4e94a16ca90c main.cpp
--- a/main.cpp Wed Jan 21 16:30:33 2015 +0000
+++ b/main.cpp Thu Jan 29 14:17:43 2015 +0000
@@ -9,10 +9,10 @@
#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 filtrWymiar 5
+#define filtrSygnal roll
+#define filtr2Wymiar 5
+#define filtr2Sygnal pitch
#define filtr3Wymiar 100
#define filtr3Sygnal d[5]
@@ -31,6 +31,7 @@
Serial bluetooth(D1, D0);
IMU imu(PTE25,PTE24);
Offsets off;
+InterruptIn event(PTB23);
FastPWM M1(D10);
FastPWM M2(D11);
@@ -41,6 +42,7 @@
Ticker triger1; //przerwanie filtracji
Ticker triger2; //kalibracja accelero
Ticker triger3; //kalibracja zyro
+Ticker tachometer;
float d[9];
double D[9];
@@ -61,6 +63,7 @@
float U1,U2,U3;
float Om1,Om2,Om3,Om4;
float wyp1,wyp2,wyp3,wyp4;
+int impulsA;
double PWM1zad;
double PWM2zad;
@@ -91,10 +94,33 @@
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;
+
+void rise(void)
+{
+ //myled = !myled;
+
+ if(TachFlaga==1)impulsA++;
+
+}
+
+
+
void task1()
{
//myled = !myled;
@@ -107,9 +133,9 @@
katx = acos(d[4]/r)-M_PI2;
katy = acos(d[3]/r)-M_PI2;
- rbut = sqrt(pow(D[3],2) + pow(D[4],2) + pow(D[5],2));
+ /*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;
+ katybut = acos(D[3]/rbut)-M_PI2;*/
//katxzyro = katxzyro + o[0]*dt;
//fkompl = katyzyro*0.95+ (-katy)*0.05;
@@ -117,16 +143,68 @@
//Filtr Kalmana
kalman_innovate(&pitch_data, katx, o[0]);
kalman_innovate(&roll_data, -katy, o[1]);
- pitch = pitch_data.x1;
+ 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;
+ 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]));
@@ -147,10 +225,10 @@
// 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/b*PWM1zad+C1/b)-U2/2bl+U3/4d
- //omega2^2=(B/b*PWM2zad+C/b)+U1/2bl-U3/4d
- //omega3^2=(B3/b*PWM3zad+C3/b)+U2/2bl+U3/4d
- //omega4^2=(B/b*PWM4zad+C/b)-U1/2bl-U3/4d
+ //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
@@ -161,30 +239,50 @@
//C1=-0.334958973
//l=0.3
//d=0.000033
- //2bl=0.0000092780
+ //2b1l=0.0000092780
//4d=0.000132
- //B1/b=50.22498189
- //C1/b=-21611.4954
- //b/B1=0.01991041
+
+ //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
- //B3/b=49.90897978
- //C3/b=-25683.36221
- //b/B3=0.020036474
+ //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=1287.588322*T2zad-(-431.2892625);
- PWMzad3=1287.588322*T3zad-(-431.2892625);
- PWMzad4=1287.588322*T4zad-(-431.2892625);*/
+ PWMzad2=1245.207965*T2zad-(-600.7566265);
+ PWMzad3=1295.740776*T3zad-(-514.6040317);
+ PWMzad4=1269.412072*T4zad-(-568.7123367);*/
+
+
+
- pitchE=pitchE+(0-pitch2);
+
+
+
+ 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;
@@ -199,35 +297,39 @@
Kd3=Kp3*Td3/T;
//if(Ti3==0){Ki3=0;}else Ki3=Kp3*T/Ti3;
- U1 = 0.0173*(Kp1*(0-pitch2)+Kd1*(0-O[0])+Ki1*pitchE);
+ 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);
+ 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 = 50.22498189*PWM2zad+(-21611.4954)+U1/0.0000092780 - U3/0.000132 ;
- Om3 = 49.90897978*PWM3zad+(-25683.36221)+U2/0.0000092780 + U3/0.000132 ;
- Om4 = 50.22498189*PWM4zad+(-21611.4954)-U1/0.0000092780 - U3/0.000132 ;
+ 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.01991041*Om2-(-431.2892625);
- wyp3=0.020036474*Om3-(-514.6040317);
- wyp4=0.01991041*Om4-(-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>10000 & wyp2<20000) valPWM2=(int)wyp2;
+ 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>10100 & wyp3<20000) valPWM3=(int)wyp3;
+ 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>10000 & wyp4<20000) valPWM4=(int)wyp4;
+ 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);
@@ -248,13 +350,13 @@
//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;*/
+ myled2 = !myled2;
imu.readData(d);
if (filtrZmienna < 0) {
@@ -360,7 +462,58 @@
roll = roll_data.x1;
}
-
+*/
+
+void task_tachometer()
+{
+ TachFlaga=0;
+ //PWM4zad+=100;
+ //M4.pulsewidth_us(PWM4zad);
+ wait(1.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() {
@@ -372,17 +525,21 @@
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, pc, imu);
+ //off.setOffsets(offsetGyro, bluetooth, imu);
- triger1.attach(&task1, 0.005);
+ //triger1.attach(&task1, 0.005);
//triger2.attach(&task2, 0.005);
//triger3.attach(&task3, 0.005);
+ tachometer.attach(&task_tachometer, 4.5);
i=1000;
@@ -400,6 +557,22 @@
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;
@@ -423,6 +596,9 @@
katyzyro = 0;
katzzyro = 0;
+ iTach=0;
+ TachFlaga=0;
+
while(1) {
//myled2 = !myled2;
@@ -445,8 +621,14 @@
//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, "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);
@@ -459,10 +641,6 @@
znak=pc.getc();
switch (znak){
- case 'p':
- 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);
@@ -473,6 +651,39 @@
ymax=0;
zmax=0;
break;
+
+ case 't':
+ PWM4zad++;
+ M4.pulsewidth_us(PWM4zad);
+ break;
+ case 'g':
+ PWM4zad--;
+ M4.pulsewidth_us(PWM4zad);
+ break;
+ case 'y':
+ PWM4zad+=10;
+ M4.pulsewidth_us(PWM4zad);
+ break;
+ case 'h':
+ PWM4zad-=10;
+ M4.pulsewidth_us(PWM4zad);
+ break;
+ case 'u':
+ PWM4zad+=100;
+ M4.pulsewidth_us(PWM4zad);
+ break;
+ case 'j':
+ PWM4zad-=100;
+ M4.pulsewidth_us(PWM4zad);
+ break;
+ case 's':
+ PWM4zad=10000;
+ M4.pulsewidth_us(PWM4zad);
+ break;
+ case 'b':
+ PWM4zad=10000;
+ M4.pulsewidth_us(PWM4zad);
+ break;
/*case 'u':
PWM1zad+=10;
M1.pulsewidth_us(PWM1zad);
@@ -494,12 +705,21 @@
switch (znak2){
case 'a':
- PWM3zad-=50;
- //PWM2zad-=50;
+ /*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;
- //PWM1zad-=50;
+ PWM4zad-=50;
+ //PWM2zad=1.045*(PWM4zad-10000)+10000;
//PWM1zad=1.026*(PWM3zad-10000)+10000;
- PWM1zad=1.035*(PWM3zad-10000)+10000;
+
+ //PWM1zad=1.026*(PWM3zad-10000)+10000;
//PWM1zad-=50;
if(PWM1zad<10000){
PWM1zad=10000;
@@ -516,12 +736,19 @@
break;
case 'b':
- PWM3zad+=50;
- //PWM2zad+=50;
+ /*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;
- //PWM1zad+=50;
+ PWM4zad+=50;
+ //PWM2zad=1.045*(PWM4zad-10000)+10000;
+ //PWM3zad+=50;
//PWM1zad=1.026*(PWM3zad-10000)+10000;
- PWM1zad=1.035*(PWM3zad-10000)+10000;
//PWM1zad+=50;
if(PWM1zad>=20000){
@@ -539,15 +766,9 @@
break;
case 'x':
- sprintf(buff,"Nastawy: %f,%f\n\r",Kp2,Kd2);
- pc.printf(buff);
- wait(1.0f);
- Kp1=0;
- Kd1=0;
- Kp2=0;
- Kd2=0;
- Ki2=0;
- Ki2=0;
+ //sprintf(buff,"Nastawy: %f,%f\n\r",Kp2,Kd2);
+ //pc.printf(buff);
+ //wait(1.0f);
PWM1zad=10000;
PWM2zad=10000;
PWM3zad=10000;