main.cpp

00001 #include <Timer.h>
00002 #include <math.h> 
00003 #include "mbed.h"
00004 #include "stdio.h"
00005 #include "deklaration.h"
00006 #include "messen.h"
00007 #include "filter/Kalman.h"
00008 
00009 double gyro_pitch;
00010 double gyro_yaw;
00011 double gyro_roll;
00012 
00013 #define RAD 57.29577951
00014                  
00015 int main()
00016 {  
00017     z_off = 0;
00018     drift_z = 0;
00019     gyro_pitch = 0;
00020     gyro_yaw = 0;
00021     gyro_roll = 0;
00022  
00023     Motor1.period_ms(2);
00024     Motor2.period_ms(2);
00025     Motor3.period_ms(2);
00026     Motor4.period_ms(2);
00027     initialisierung_gyro();
00028     initialisierung_acc();
00029     Kalman_pitch();
00030     Kalman_yaw();
00031     Kalman_roll();
00032     aktuell_roh(&z_g, &x_g, &y_g, &z_a, &x_a, &y_a);
00033     wait(1);   
00034     if (taster2)
00035     {   
00036         viberationen(&rauschen, &Motor1, &Motor2, &Motor3, &Motor4, &taster4);
00037     }  
00038     if (taster3)            
00039     {         
00040         anlernen(&Motor1, &Motor2, &Motor3, &Motor4, &taster1, &taster2, &taster4);
00041     }
00042     
00043     pc.printf("Druecke Taster1 fuer den Start und Taster2 fuers rauschen\n\r");   
00044     n1=n2=n3=n4=700;
00045     Motor1.pulsewidth_us(n1);
00046     Motor2.pulsewidth_us(n2);
00047     Motor3.pulsewidth_us(n3);
00048     Motor4.pulsewidth_us(n4); 
00049           
00050     while(1)
00051     {
00052         if (taster1) 
00053         {
00054             while(1) 
00055             {             
00056                 pc.printf("\n\rOffset und Driftberechnung wird durchgefuehrt, halte die Drohne still");
00057                 offset_gyro(&z_off, &x_off, &y_off);             
00058                 //drift_gyro(&drift_z, &drift_x, &drift_y, &timer, &timer2, &gain_g, &roll_g, &pitch_g, &z_off, &x_off, &y_off);
00059                 pc.printf("\n\rOffgesamt:\n\rZ = %3.5f\tY = %3.5f\tZ = %3.5f\t\n\r", z_off, x_off, y_off);  
00060                 pc.printf("\n\rDrift:Z: %3.10f\tX: %3.10f\tY: %3.10f\n\r", drift_z, drift_x, drift_y);   
00061                              
00062                 timer.reset();
00063                 timer.start();
00064                 timer2.reset();
00065                 timer2.start();
00066                 int i = 0;
00067                 while(1)
00068                 {    
00069                     i++;                   
00070                     dt = timer.read_us() * 0.000001;                    //Zeit zwischen zwei Messpunkten
00071                     timer.reset();
00072                     aktuell_roh(&z_g, &x_g, &y_g, &z_a, &x_a, &y_a);    //Rohdaten einlesen
00073                 
00074                     y = y_a / 16384.00;                                 //Umwandlung in G-Kraft
00075                     x = x_a / 16384.00;                                 //Umwandlung in G-Kraft
00076                     z = z_a / 16384.00;                                 //Umwandlung in G-Kraft
00077                     
00078                     newAngle_pitch = atan2(-x, z) * RAD;                    //Umwandlung der G-Kraft in °
00079                     newRate_pitch = ((y_g  - y_off) * 1/16.4);              //Offset subtrahiert +++ Umwandlung in °/s
00080                     
00081                     newAngle_roll = atan2(y, sqrt(x * x + z * z)) * RAD;    //Umwandlung der G-Kraft in °
00082                     newRate_roll = ((x_g - x_off)  * 1/16.4);               //Offset subtrahiert +++ Umwandlung in °/s
00083                     
00084                     newAngle_yaw = ((z_g - z_off) * 1/16.4);                //Umwandlung der G-Kraft in °
00085                     newRate_yaw = ((z_g - z_off) * 1/16.4);                 //Offset subtrahiert +++ Umwandlung in °/s
00086 
00087                     pitch = getPitch(&newAngle_pitch, &newRate_pitch, &dt);
00088                     yaw = getYaw(&newAngle_yaw, &newRate_yaw, &dt);
00089                     roll = getRoll(&newAngle_roll, &newRate_roll, &dt);
00090                     
00091                     if (i == 1000)
00092                     {
00093                         printf("%f2.3 \t%f2.3 \t%f2.3 \t%f2.3 \t%f2.3 \t%f2.3 \t%fd \t", pitch, yaw, roll, newAngle_pitch, newRate_pitch, newAngle_roll, newRate_roll, newAngle_yaw, n1);
00094                         i = 0;
00095                     }
00096                     Motorsteurung(&Motor1, &Motor2, &Motor3, &Motor4, &taster2, &taster3, &taster4, &n1, &n2, &n3, &n4);   
00097                 }
00098             }
00099         }
00100         if (taster2) 
00101         {
00102             pc.printf("\n\rOffset und Driftberechnung wird durchgefuehrt, halte die Drohne still");
00103                 printf("\n\rpitch, yaw, roll, newAngle_pitch, newAngle_roll, gyro_pitch, gyro_yaw, gyro_roll, n2\n\r"); 
00104             while(1) 
00105             {             
00106                 
00107                 offset_gyro(&z_off, &x_off, &y_off);             
00108                 //drift_gyro(&drift_z, &drift_x, &drift_y, &timer, &timer2, &gain_g, &roll_g, &pitch_g, &z_off, &x_off, &y_off);
00109                 //pc.printf("\n\rOffgesamt:\n\rZ = %3.5f\tY = %3.5f\tZ = %3.5f\t\n\r", z_off, x_off, y_off);  
00110                 //pc.printf("\n\rDrift:Z: %3.10f\tX: %3.10f\tY: %3.10f\n\r", drift_z, drift_x, drift_y);   
00111                              
00112                 timer.reset();
00113                 timer.start();
00114                 timer2.reset();
00115                 timer2.start();
00116                 int i = 0;
00117                 while(1)
00118                 {    
00119                     i++;                   
00120                     dt = timer.read_us() * 0.000001;                    //Zeit zwischen zwei Messpunkten
00121                     timer.reset();
00122                     aktuell_roh(&z_g, &x_g, &y_g, &z_a, &x_a, &y_a);    //Rohdaten einlesen
00123                 
00124                     y = y_a / 16384.00;                                 //Umwandlung in G-Kraft
00125                     x = x_a / 16384.00;                                 //Umwandlung in G-Kraft
00126                     z = z_a / 16384.00;                                 //Umwandlung in G-Kraft
00127                     
00128                     newAngle_pitch = atan2(-x, z) * RAD;                    //Umwandlung der G-Kraft in °
00129                     newRate_pitch = ((y_g  - y_off) * 1/16.4);              //Offset subtrahiert +++ Umwandlung in °/s
00130                     
00131                     newAngle_roll = atan2(y, sqrt(x * x + z * z)) * RAD;    //Umwandlung der G-Kraft in °
00132                     newRate_roll = ((x_g - x_off)  * 1/16.4);               //Offset subtrahiert +++ Umwandlung in °/s
00133                     
00134                     newAngle_yaw = ((z_g - z_off) * 1/16.4);                //Umwandlung der G-Kraft in °
00135                     newRate_yaw = ((z_g - z_off) * 1/16.4);                 //Offset subtrahiert +++ Umwandlung in °/s
00136 
00137                     pitch = getPitch(&newAngle_pitch, &newRate_pitch, &dt);
00138                     yaw = getYaw(&newAngle_yaw, &newRate_yaw, &dt);
00139                     roll = getRoll(&newAngle_roll, &newRate_roll, &dt);
00140                     
00141                     gyro_pitch += dt * newRate_pitch;
00142                     gyro_yaw += dt * newRate_yaw;
00143                     gyro_roll += dt * newRate_roll;
00144                     
00145                     if (i == 500)
00146                     {
00147                         printf(" %3.2f \t %3.2f \t %3.2f \t %3.2f \t\t %3.2f \t\t %3.2f \t\t %3.2f \t\t %3.2f \t\t %d\n\r", pitch, yaw, roll, newAngle_pitch, newAngle_roll, gyro_pitch, gyro_yaw, gyro_roll, n2); 
00148                         i = 0;
00149                     }
00150                     if (timer2.read_ms() >= 5000)
00151                     {
00152                             n1+=200;
00153                             n2+=200;
00154                             n3+=200;
00155                             n4+=200;
00156                             Motor1.pulsewidth_us(n1);
00157                             Motor2.pulsewidth_us(n2);
00158                             Motor3.pulsewidth_us(n3);
00159                             Motor4.pulsewidth_us(n4);
00160                             timer2.reset();
00161                     }
00162                     Motorsteurung(&Motor1, &Motor2, &Motor3, &Motor4, &taster2, &taster3, &taster4, &n1, &n2, &n3, &n4);
00163                     if (n1>1501)
00164                     {
00165                         n1=n2=n3=n4=700;
00166                         Motor1.pulsewidth_us(n1);
00167                         Motor2.pulsewidth_us(n2);
00168                         Motor3.pulsewidth_us(n3);
00169                         Motor4.pulsewidth_us(n4);
00170                         while(1);  
00171                     } 
00172                 }
00173             }
00174         }
00175     }
00176 }