Mechatronics
Dydaktyka
Dependencies: FastPWM mbed-src
Fork of
2015_04_17_quadro_bez_sterowania
by 
Quadrocopter
Diff: main.cpp
- Revision:
- 10:605b0bfadc2e
- Parent:
- 9:4e94a16ca90c
- Child:
- 11:38db3ed41f13
diff -r 4e94a16ca90c -r 605b0bfadc2e main.cpp
--- a/main.cpp Thu Jan 29 14:17:43 2015 +0000
+++ b/main.cpp Tue Feb 10 15:08:04 2015 +0000
@@ -49,13 +49,15 @@
float o[3];
float O[3];
char buff[160];
-float r,katx,katy;
+float r,katx,katy,katz;
float rbut,katxbut,katybut;
-float pitch, roll;
+float pitch, roll, yaw;
float pitch2, roll2;
-float pitchE, rollE;
+float pitchE, rollE, yawE;
double i;
float offsetGyro[3];
+float offsetMagneto;
+float yawOff;
char odczyt[20];
char znak;
char znak2;
@@ -110,13 +112,22 @@
int RPMtachWynPoprz;
int iTach;
bool TachFlaga;
+bool FlagaPom;
+
+int Omegazad[4];
+int Omegamax[4][9];
+float a1buf[4][9];
+float a2buf[4][9];
+int ister;
+int jster;
+bool flagaster;
+float a1[4];
+float a2[4];
void rise(void)
{
//myled = !myled;
-
if(TachFlaga==1)impulsA++;
-
}
@@ -132,6 +143,18 @@
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;
+
+ yaw = atan2(d[7],d[6])+4.98333*M_PI/180;
+ katz = katz+(o[2]*dt);
+ off.offsetMagneto(&yaw,&offsetMagneto,&yawOff);
+ // Correct for heading < 0deg and heading > 360deg
+ /*if(yawOff < 0){
+ yawOff+= 2 * M_PI;
+ }
+
+ if(yawOff > 2 * M_PI){
+ yawOff-= 2 * M_PI;
+ }*/
/*rbut = sqrt(pow(D[3],2) + pow(D[4],2) + pow(D[5],2));
katxbut = acos(D[4]/rbut)-M_PI2;
@@ -466,10 +489,13 @@
void task_tachometer()
{
+
TachFlaga=0;
- //PWM4zad+=100;
- //M4.pulsewidth_us(PWM4zad);
- wait(1.5f);
+ if(FlagaPom==1){
+ PWM4zad+=5;
+ M3.pulsewidth_us(PWM4zad);
+ }
+ wait(0.5f);
TachFlaga=1;
//myled2=!myled2;
RPMtach=impulsA*10; // dla T=3s
@@ -502,7 +528,6 @@
pc.printf(buff);
*/
-
sprintf(buff, "%f,%i\n\r", PWM4zad,RPMtach);
pc.printf(buff);
//iTach++;
@@ -525,21 +550,21 @@
kalman_init(&roll_data);
kalman_init(&pitch_data2);
kalman_init(&roll_data2);
- event.rise(&rise);
- event.mode(PullUp);
- event.enable_irq();
+ //event.rise(&rise);
+ //event.mode(PullUp);
+ //event.enable_irq();
sprintf(buff, "Hello: \n\r");
pc.printf(buff);
- //off.setOffsets(offsetGyro, bluetooth, imu);
+ off.setOffsets(offsetGyro,&offsetMagneto, pc, 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);
+ //tachometer.attach(&task_tachometer, 3.5);
i=1000;
@@ -599,6 +624,119 @@
iTach=0;
TachFlaga=0;
+a1buf[0][0]= 1.438602213;
+a1buf[0][1]= 1.130064065;
+a1buf[0][2]= 0.958928363;
+a1buf[0][3]= 0.836156086;
+a1buf[0][4]= 0.735165987;
+a1buf[0][5]= 0.619988352;
+a1buf[0][6]= 0.580966803;
+a1buf[0][7]= 0.580966803;
+a1buf[0][8]= 0.580966803;
+a1buf[1][0]= 1.55790332;
+a1buf[1][1]= 1.181479324;
+a1buf[1][2]= 1.001071637;
+a1buf[1][3]= 0.872754805;
+a1buf[1][4]= 0.699592312;
+a1buf[1][5]= 0.644682586;
+a1buf[1][6]= 0.596575422;
+a1buf[1][7]= 0.596575422;
+a1buf[1][8]= 0.596575422;
+a1buf[2][0]= 1.475317414;
+a1buf[2][1]= 1.159883518;
+a1buf[2][2]= 0.965894001;
+a1buf[2][3]= 0.85553873;
+a1buf[2][4]= 0.738311008;
+a1buf[2][5]= 0.627093768;
+a1buf[2][6]= 0.627093768;
+a1buf[2][7]= 0.627093768;
+a1buf[2][8]= 0.627093768;
+a1buf[3][0]= 1.40570763;
+a1buf[3][1]= 1.143366337;
+a1buf[3][2]= 0.953383809;
+a1buf[3][3]= 0.850413512;
+a1buf[3][4]= 0.734304019;
+a1buf[3][5]= 0.634804892;
+a1buf[3][6]= 0.588398369;
+a1buf[3][7]= 0.588305183;
+a1buf[3][8]= 0.588305183;
+
+a2buf[0][0]= -14020.22132;
+a2buf[0][1]= -10629.35935;
+a2buf[0][2]= -8667.80431;
+a2buf[0][3]= -7195.189284;
+a2buf[0][4]= -5935.445545;
+a2buf[0][5]= -4432.964473;
+a2buf[0][6]= -3899.481654;
+a2buf[0][7]= -3899.481654;
+a2buf[0][8]= -3899.481654;
+a2buf[1][0]= -15394.5894;
+a2buf[1][1]= -11261.14735;
+a2buf[1][2]= -9187.344205;
+a2buf[1][3]= -7648.375073;
+a2buf[1][4]= -5478.811881;
+a2buf[1][5]= -4780.460105;
+a2buf[1][6]= -4131.426907;
+a2buf[1][7]= -4131.426907;
+a2buf[1][8]= -4131.426907;
+a2buf[2][0]= -14446.09785;
+a2buf[2][1]= -10984.43215;
+a2buf[2][2]= -8760.145603;
+a2buf[2][3]= -7434.705882;
+a2buf[2][4]= -5971.980198;
+a2buf[2][5]= -4525.428072;
+a2buf[2][6]= -4525.428072;
+a2buf[2][7]= -4525.428072;
+a2buf[2][8]= -4525.428072;
+a2buf[3][0]= -13645.61444;
+a2buf[3][1]= -10761.08911;
+a2buf[3][2]= -8579.586488;
+a2buf[3][3]= -7342.912056;
+a2buf[3][4]= -5896.039604;
+a2buf[3][5]= -4603.83809;
+a2buf[3][6]= -3978.10134;
+a2buf[3][7]= -3976.928363;
+a2buf[3][8]= -3976.928363;
+
+Omegamax[0][0]= 1790;
+Omegamax[0][1]= 2360;
+Omegamax[0][2]= 2830;
+Omegamax[0][3]= 3250;
+Omegamax[0][4]= 3620;
+Omegamax[0][5]= 3950;
+Omegamax[0][6]= 4230;
+Omegamax[0][7]= 4270;
+Omegamax[0][8]= 20000;
+Omegamax[1][0]= 1710;
+Omegamax[1][1]= 2300;
+Omegamax[1][2]= 2820;
+Omegamax[1][3]= 3260;
+Omegamax[1][4]= 3590;
+Omegamax[1][5]= 3920;
+Omegamax[1][6]= 4220;
+Omegamax[1][7]= 4240;
+Omegamax[1][8]= 20000;
+Omegamax[2][0]= 1760;
+Omegamax[2][1]= 2360;
+Omegamax[2][2]= 2810;
+Omegamax[2][3]= 3250;
+Omegamax[2][4]= 3630;
+Omegamax[2][5]= 3940;
+Omegamax[2][6]= 4110;
+Omegamax[2][7]= 4110;
+Omegamax[2][8]= 20000;
+Omegamax[3][0]= 1790;
+Omegamax[3][1]= 2380;
+Omegamax[3][2]= 2880;
+Omegamax[3][3]= 3270;
+Omegamax[3][4]= 3650;
+Omegamax[3][5]= 3970;
+Omegamax[3][6]= 4250;
+Omegamax[3][7]= 4300;
+Omegamax[3][8]= 20000;
+
+
+
while(1) {
//myled2 = !myled2;
@@ -633,8 +771,13 @@
//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);
if(pc.readable()){
@@ -654,36 +797,43 @@
case 't':
PWM4zad++;
- M4.pulsewidth_us(PWM4zad);
+ M3.pulsewidth_us(PWM4zad);
break;
case 'g':
PWM4zad--;
- M4.pulsewidth_us(PWM4zad);
+ M3.pulsewidth_us(PWM4zad);
break;
case 'y':
PWM4zad+=10;
- M4.pulsewidth_us(PWM4zad);
+ M3.pulsewidth_us(PWM4zad);
break;
case 'h':
PWM4zad-=10;
- M4.pulsewidth_us(PWM4zad);
+ M3.pulsewidth_us(PWM4zad);
break;
case 'u':
PWM4zad+=100;
- M4.pulsewidth_us(PWM4zad);
+ M3.pulsewidth_us(PWM4zad);
break;
case 'j':
PWM4zad-=100;
- M4.pulsewidth_us(PWM4zad);
+ M3.pulsewidth_us(PWM4zad);
break;
case 's':
PWM4zad=10000;
- M4.pulsewidth_us(PWM4zad);
+ M3.pulsewidth_us(PWM4zad);
break;
case 'b':
PWM4zad=10000;
- M4.pulsewidth_us(PWM4zad);
+ M3.pulsewidth_us(PWM4zad);
+ break;
+ case 'z':
+ FlagaPom=1;
break;
+ case 'x':
+ FlagaPom=0;
+ break;
+
/*case 'u':
PWM1zad+=10;
M1.pulsewidth_us(PWM1zad);
@@ -696,7 +846,7 @@
}
znak=0;
- }
+ }
if(bluetooth.readable()){
@@ -704,7 +854,14 @@
znak2=bluetooth.getc();
switch (znak2){
- case 'a':
+
+
+ case 'a':
+ for(jster=0;jster<4;jster++){
+ Omegazad[jster]-=50;
+ }
+ flagaster=1;
+
/*T1zad-=0.05;
T2zad-=0.05;
T3zad-=0.05;
@@ -714,6 +871,7 @@
PWM3zad=1295.740776*T3zad-(-514.6040317)+10000;
PWM4zad=1269.412072*T4zad-(-568.7123367)+10000;*/
+ /*
//PWM3zad-=50;
PWM4zad-=50;
//PWM2zad=1.045*(PWM4zad-10000)+10000;
@@ -732,10 +890,16 @@
M2.pulsewidth_us(PWM2zad);
M3.pulsewidth_us(PWM3zad);
M4.pulsewidth_us(PWM4zad);
+ */
znak2=0;
+
break;
case 'b':
+ for(jster=0;jster<4;jster++){
+ Omegazad[jster]+=50;
+ }
+ flagaster=1;
/*T1zad+=0.05;
T2zad+=0.05;
T3zad+=0.05;
@@ -744,6 +908,7 @@
PWM2zad=1245.207965*T2zad-(-600.7566265)+10000;
PWM3zad=1295.740776*T3zad-(-514.6040317)+10000;
PWM4zad=1269.412072*T4zad-(-568.7123367)+10000;*/
+ /*
//PWM3zad+=50;
PWM4zad+=50;
//PWM2zad=1.045*(PWM4zad-10000)+10000;
@@ -762,6 +927,7 @@
M2.pulsewidth_us(PWM2zad);
M3.pulsewidth_us(PWM3zad);
M4.pulsewidth_us(PWM4zad);
+ */
znak2=0;
break;
@@ -769,6 +935,9 @@
//sprintf(buff,"Nastawy: %f,%f\n\r",Kp2,Kd2);
//pc.printf(buff);
//wait(1.0f);
+ for(jster=0;jster<4;jster++){
+ Omegazad[jster]=0;
+ }
PWM1zad=10000;
PWM2zad=10000;
PWM3zad=10000;
@@ -823,8 +992,46 @@
Ki1+=0.1;
Ki2+=0.1;
break;
- }
+ }
+
+
+ if(flagaster==1){
+
+ for(int jster=0;jster<4;jster++){
+ ister=-1;
+ do{
+ ister++;
+ }while(Omegazad[jster] > Omegamax[jster][ister]);
+ a1[jster]=a1buf[jster][ister];
+ a2[jster]=a2buf[jster][ister];
+ }
+
+ PWM1zad=(Omegazad[0]-a2[0])/a1[0];
+ PWM2zad=(Omegazad[1]-a2[1])/a1[1];
+ PWM3zad=(Omegazad[2]-a2[2])/a1[2];
+ PWM4zad=(Omegazad[3]-a2[3])/a1[3];
+
+ if(PWM1zad<10000){
+ PWM1zad=10000;
+ }
+ if(PWM2zad<10000){
+ PWM2zad=10000;
+ }
+ if(PWM3zad<10000){
+ PWM3zad=10000;
+ }
+ if(PWM4zad<10000){
+ PWM4zad=10000;
+ }
+ M1.pulsewidth_us(PWM1zad);
+ M2.pulsewidth_us(PWM2zad);
+ M3.pulsewidth_us(PWM3zad);
+ M4.pulsewidth_us(PWM4zad);
+
+ flagaster=0;
}
+
+ }//switch
//myled2 = !myled2;
- }
-}
\ No newline at end of file
+ }//while(1)
+}//main
\ No newline at end of file