Mechatronics
Dydaktyka
Dependencies: FastPWM mbed-src
Fork of
2015_04_17_quadro_bez_sterowania
by 
Quadrocopter
Diff: main.cpp
- Revision:
- 6:8cc6df266363
- Parent:
- 5:c3caf8b83e6b
- Child:
- 7:2ba30a0cdc16
--- a/main.cpp Fri Dec 19 13:33:21 2014 +0000
+++ b/main.cpp Wed Jan 07 13:44:34 2015 +0000
@@ -49,6 +49,10 @@
char odczyt[20];
char znak;
char znak2;
+float Kp1,Kd1,Kp2,Kd2,Kp3,Kd3;
+float U1,U2,U3;
+float Om1,Om2,Om3,Om4;
+float wyp1,wyp2,wyp3,wyp4;
double PWM1zad;
double PWM2zad;
@@ -61,6 +65,7 @@
void task1()
{
+ myled = !myled;
imu.readData(d);
imu.filterData(d, D);
off.offsetData(d,offsetGyro,o);
@@ -87,6 +92,61 @@
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);
+
+ 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;
+
+ /*
+ 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;*/
+
+
+ /*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;*/
+
+
+ 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(wyp3<=10001 | wyp3>40001) valPWM3=10000;
+ if(wyp3>=20000 & wyp3<40000) valPWM3=20000;
+ if(wyp3>10100 & 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;
+
//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);
@@ -97,6 +157,12 @@
//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;
}
@@ -125,7 +191,7 @@
sprintf(buff, "Hello: \n\r");
pc.printf(buff);
- //off.setOffsets(offsetGyro, pc, imu);
+ off.setOffsets(offsetGyro, pc, imu);
@@ -148,11 +214,28 @@
M4.period_us(PWM_period);
M4.pulsewidth_us(PWM4zad);
+ Kp1=0;
+ Kd1=0;
+ Kp2=0;
+ Kd2=0;
+ Kp3=0;
+ Kd3=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));
//pc.printf(buff);
- //myled2 = !myled2;
+ 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\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;
+
if(pc.readable()){
@@ -162,52 +245,11 @@
case 'p':
sprintf(buff, "odczytany znak: %c\n\r",znak);
pc.printf(buff);
-
- i+=10;
- break;
-
- case 'm':
- i-=10;
- break;
-
- case 'u':
- i+=0.1;
- break;
-
- case 'd':
- i-=0.1;
break;
-
- case 'q':
- for(i=1000;i<1500;i++){
- M1.pulsewidth_us(i);
- wait(0.005);
- }
- break;
-
- case 'w':
- for(i=1500;i>1000;i--){
- M1.pulsewidth_us(i);
- wait(0.005);
- }
- break;
- case 'e':
- for(i=1000;i<1500;i=i+0.1){
- M1.pulsewidth_us(i);
- wait(0.001);
- }
- break;
- case 'r':
- for(i=1500;i>1000;i-=0.1){
- M1.pulsewidth_us(i);
- wait(0.001);
- }
- break;
}
znak=0;
- M1.pulsewidth_us(i);
}
@@ -255,6 +297,13 @@
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;
PWM1zad=10000;
PWM2zad=10000;
PWM3zad=10000;
@@ -285,8 +334,24 @@
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;
}
-
}
+ //myled2 = !myled2;
}
}
\ No newline at end of file