Diff: main.cpp

Revision:

23:c99a4bd60609

Parent:

21:f09823a13ac6

Child:

24:aaa5b4703555

diff -r 6a2b9696eea2 -r c99a4bd60609 main.cpp
--- a/main.cpp	Thu Sep 14 15:35:54 2017 +0000
+++ b/main.cpp	Thu Sep 14 15:51:15 2017 +0000
@@ -32,16 +32,6 @@
     initialisierung_gyro();
     initialisierung_acc();
     Kalman(&Q_angle, &Q_bias, &R_measure, &bias, P);
-    if (taster2)
-    {   
-        viberationen(&rauschen, &Motor1, &Motor2, &Motor3, &Motor4, &taster4);
-    }  
-    if (taster3)            
-    {         
-        anlernen(&Motor1, &Motor2, &Motor3, &Motor4, &taster1, &taster2, &taster4);
-    }
-    pc.printf("Druecke Taster1 fuer den Start\n\r");   
-    n1=n2=n3=n4=700;
     
     if (taster1)
     {
@@ -81,8 +71,24 @@
              
             pc.printf("Drehzahl=  %d\r= %d",n1);
         }    
-    }      
+    }
     
+    if (taster2)
+    {   
+        viberationen(&rauschen, &Motor1, &Motor2, &Motor3, &Motor4, &taster4);
+    }  
+    if (taster3)            
+    {         
+        anlernen(&Motor1, &Motor2, &Motor3, &Motor4, &taster1, &taster2, &taster4);
+    }
+    
+    pc.printf("Druecke Taster1 fuer den Start\n\r");   
+    n1=n2=n3=n4=700;
+    Motor1.pulsewidth_us(n1);
+    Motor2.pulsewidth_us(n2);
+    Motor3.pulsewidth_us(n3);
+    Motor4.pulsewidth_us(n4); 
+          
     while(1)
     {
         if (taster1) 
@@ -136,10 +142,21 @@
                     roll = getAngle(&newAngle, &newRate, &dt, &Q_angle, &Q_bias, &R_measure, &bias); 
                     if (i == 2000)
                     {
-                        pc.printf("gain: %2.5f\tpitch: %2.5f\troll: %2.5f\t\n\r",angle);
+                        pc.printf("gain: %2.5f\tpitch: %2.5f\troll: %2.5f\t\n\r",roll);
                         i = 0;
                     }
-                        
+                    n1=n2=n3=n4=800;
+                    Motor1.pulsewidth_us(n1);
+                    Motor2.pulsewidth_us(n2);
+                    Motor3.pulsewidth_us(n3);
+                    Motor4.pulsewidth_us(n4); 
+                    if(taster4){
+                            n1=n2=n3=n4=700;
+                    Motor1.pulsewidth_us(n1);
+                    Motor2.pulsewidth_us(n2);
+                    Motor3.pulsewidth_us(n3);
+                    Motor4.pulsewidth_us(n4); while(1);}
+                            
                 }
             }
         }