Mechatronics
Dydaktyka
Dependencies: FastPWM mbed-src
Fork of
2015_04_17_quadro_bez_sterowania
by 
Quadrocopter
Diff: main.cpp
- Revision:
- 11:38db3ed41f13
- Parent:
- 10:605b0bfadc2e
- Child:
- 12:e955cc086c42
--- a/main.cpp Tue Feb 10 15:08:04 2015 +0000
+++ b/main.cpp Fri Apr 17 07:48:05 2015 +0000
@@ -38,7 +38,6 @@
FastPWM M3(D12);
FastPWM M4(D13);
-
Ticker triger1; //przerwanie filtracji
Ticker triger2; //kalibracja accelero
Ticker triger3; //kalibracja zyro
@@ -131,7 +130,6 @@
}
-
void task1()
{
//myled = !myled;
@@ -180,7 +178,6 @@
roll2 = roll_data2.x1;
kalmanrollzyrobut = roll_data2.x2;*/
-
if (filtrZmienna < 0) {
filtrWynik = filtrSygnal;
filtrBufor[filtrZmienna + filtrWymiar] = filtrSygnal;
@@ -226,7 +223,6 @@
if (filtr2Zmienna == filtr2Wymiar) {
filtr2Zmienna = 0;
}
-
pitch=filtr2Wynik;
//U1 = 0.0173*(Kp1*(-pitch2)+Kd1*(0-O[0]));
@@ -245,24 +241,12 @@
// *********************** 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
-
+ //stale
//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
@@ -289,6 +273,29 @@
//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
/*
@@ -296,12 +303,12 @@
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;
@@ -320,23 +327,23 @@
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);
+
+ 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 = 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);
+ 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;
@@ -369,7 +376,6 @@
M3.pulsewidth_us(valPWM3);
M4.pulsewidth_us(valPWM4);
-
//myled = !myled;
}
@@ -460,8 +466,6 @@
//sprintf(buff, "%f,%f,%f,%f\n\r", d[4],filtrWynik,ymin,ymax);
//pc.printf(buff);
-
-
}
@@ -483,7 +487,6 @@
kalmanpitchzyro = pitch_data.x2;
kalmanpitchdryf = pitch_data.x3;
roll = roll_data.x1;
-
}
*/
@@ -527,7 +530,6 @@
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++;
@@ -537,7 +539,6 @@
//RPMtachWyn=0;
//PWM4zadWyn=0;
impulsA=0;
-
}
@@ -557,17 +558,9 @@
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);
+ off.setOffsets(offsetGyro,&offsetMagneto, bluetooth, imu);
i=1000;
-
PWM1zad=10000;
PWM2zad=10000;
PWM3zad=10000;
@@ -598,11 +591,11 @@
Ti2=0;
Ti3=0;
T=0.005;*/
- Kp1=0;
- Kp2=0;
+ Kp1=12;
+ Kp2=12;
Kp3=0;
- Kd1=0;
- Kd2=0;
+ Kd1=2;
+ Kd2=2;
Kd3=0;
Ki1=0;
Ki2=0;
@@ -734,13 +727,26 @@
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);
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);
@@ -749,14 +755,10 @@
//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);
@@ -767,17 +769,18 @@
//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, "%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);
if(pc.readable()){
@@ -842,7 +845,6 @@
PWM1zad-=10;
M1.pulsewidth_us(PWM1zad);
break;*/
-
}
znak=0;