Mechatronics
Dydaktyka
Dependencies: FastPWM mbed-src
Fork of
2015_04_17_quadro_bez_sterowania
by 
Quadrocopter
Diff: main.cpp
- Revision:
- 7:2ba30a0cdc16
- Parent:
- 6:8cc6df266363
- Child:
- 8:dc48ce79ad59
--- a/main.cpp Wed Jan 07 13:44:34 2015 +0000
+++ b/main.cpp Thu Jan 15 07:46:43 2015 +0000
@@ -44,12 +44,13 @@
float rbut,katxbut,katybut;
float pitch, roll;
float pitch2, roll2;
+float pitchE, rollE;
double i;
float offsetGyro[3];
char odczyt[20];
char znak;
char znak2;
-float Kp1,Kd1,Kp2,Kd2,Kp3,Kd3;
+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;
@@ -65,7 +66,7 @@
void task1()
{
- myled = !myled;
+ //myled = !myled;
imu.readData(d);
imu.filterData(d, D);
off.offsetData(d,offsetGyro,o);
@@ -92,45 +93,86 @@
pitch2 = pitch_data2.x1;
roll2 = roll_data2.x1;
- 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);
+ //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 ;
+ //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;
+ /*wyp1 = sqrt(Om1)*13.17523+10000;
wyp2 = sqrt(Om2)*13.17523+10000;
wyp3 = sqrt(Om3)*13.17523+10000;
- wyp4 = sqrt(Om4)*13.17523+10000;
-
- /*
- valPWM1 = (wyp1<10000) ? 10000 : (int)wyp1;
- valPWM1 = (wyp1>20000) ? 20000 : (int)wyp1;
- valPWM2 = (wyp2<10000) ? 10000 : (int)wyp2;
- valPWM2 = (wyp2>20000) ? 20000 : (int)wyp2;
- valPWM3 = (wyp3<10000) ? 10000 : (int)wyp3;
- valPWM3 = (wyp3>20000) ? 20000 : (int)wyp3;
- valPWM4 = (wyp4<10000) ? 10000 : (int)wyp4;
- valPWM4 = (wyp4>20000) ? 20000 : (int)wyp4;*/
+ wyp4 = sqrt(Om4)*13.17523+10000;*/
- /*valPWM1 = (int)wyp1;
- if (wyp1<10000) valPWM1=10000;
- if (wyp1>20000) valPWM1=20000;
- valPWM2 = (int)wyp2;
- if (wyp2<10000) valPWM2=10000;
- if (wyp2>20000) valPWM2=20000;
- valPWM3 = (int)wyp3;
- if (wyp3<10000) valPWM3=10000;
- if (wyp3>20000) valPWM3=20000;
- valPWM4 = (int)wyp4;
- if (wyp4<10000) valPWM4=10000;
- if (wyp4>20000) valPWM4=20000;*/
+ // *********************** 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/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
+ //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
+ //2bl=0.0000092780
+ //4d=0.000132
+ //B1/b=50.22498189
+ //C1/b=-21611.4954
+ //b/B1=0.01991041
+ //C1/B1=-431.2892625
+
+ //B3/b=49.90897978
+ //C3/b=-25683.36221
+ //b/B3=0.020036474
+ //C3/B3=-514.6040317
+
+
+
+ pitchE=pitchE+(0-pitch2);
+ if(pitchE>3) pitchE=3;
+ Kd1=Kp1*Td1/T;
+ Ki1=Kp1*T/Ti1;
+ if(Ti1==0)Ki1=0;
+ rollE=rollE+(0-roll2);
+ if(rollE>3) rollE=3;
+ Kd2=Kp2*Td2/T;
+ Ki2=Kp2*T/Ti2;
+ if(Ti2==0)Ki2=0;
+
+ /* yawE=yawE+(0-roll2);
+ if(yawE>3) yawE=3;*/
+ Kd3=Kp3*Td3/T;
+ Ki3=Kp3*T/Ti3;
+ if(Ti3==0)Ki3=0;
+
+ 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);
+ 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 = 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 ;
+
+ 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;
if(wyp1>10001 && wyp1<20000) valPWM1=(int)wyp1;
@@ -163,7 +205,7 @@
M4.pulsewidth_us(valPWM4);
- myled = !myled;
+ //myled = !myled;
}
@@ -215,26 +257,32 @@
M4.pulsewidth_us(PWM4zad);
Kp1=0;
- Kd1=0;
Kp2=0;
- Kd2=0;
Kp3=0;
- Kd3=0;
+ Td1=0;
+ Td2=0;
+ Td3=0;
+ Ti1=0;
+ Ti2=0;
+ Ti3=0;
+ T=0.005;
while(1) {
- myled2 = !myled2;
+ //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));
//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,%f,%f\n\r", -katybut*180/M_PI, roll2*180/M_PI, (O[1]*180/M_PI),valPWM1, valPWM2, valPWM3, valPWM4);
+ //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);
- myled2 = !myled2;
+ //myled2 = !myled2;
@@ -246,6 +294,14 @@
sprintf(buff, "odczytany znak: %c\n\r",znak);
pc.printf(buff);
break;
+ /*case 'u':
+ PWM1zad+=10;
+ M1.pulsewidth_us(PWM1zad);
+ break;
+ case 'j':
+ PWM1zad-=10;
+ M1.pulsewidth_us(PWM1zad);
+ break;*/
}
@@ -297,13 +353,13 @@
break;
case 'x':
- sprintf(buff,"Nastawy: %f,%f\n\r",Kp2,Kd2);
+ sprintf(buff,"Nastawy: %f,%f\n\r",Kp2,Td2);
pc.printf(buff);
wait(1.0f);
Kp1=0;
- Kd1=0;
+ Td1=0;
Kp2=0;
- Kd2=0;
+ Td2=0;
PWM1zad=10000;
PWM2zad=10000;
PWM3zad=10000;
@@ -335,12 +391,12 @@
break;
case 'c':
- Kd1-=0.5;
- Kd2-=0.5;
+ Td1-=0.001;
+ Td2-=0.001;
break;
case 'd':
- Kd1+=0.5;
- Kd2+=0.5;
+ Td1+=0.001;
+ Td2+=0.001;
break;
case 'e':
Kp1-=0.5;