Diff: main.cpp

Revision:

12:4a4dad7a3432

Parent:

11:8457b851e3e1

Child:

13:5f0a2103c707

--- a/main.cpp	Thu Aug 10 18:04:27 2017 +0000
+++ b/main.cpp	Wed Aug 16 09:45:50 2017 +0000
@@ -2,6 +2,7 @@
 #include "stdio.h"
 #include <Timer.h>
 #include "messen.h"
+#include <math.h> 
 
 static Serial pc(SERIAL_TX, SERIAL_RX);
 static SPI spi(PE_6,PE_5,PE_2);  //mosi,miso,sclk
@@ -12,10 +13,7 @@
 static AnalogIn poti_3(PF_4);
 static AnalogIn poti_4(PF_5);
 
-static DigitalIn taster4(PG_4,PullDown);
-static DigitalIn taster3(PD_10,PullDown);
-static DigitalIn taster2(PG_14,PullDown);
-static DigitalIn taster1(PF_12,PullDown);
+
 
 static DigitalOut db0(PC_8);
 static DigitalOut db1(PC_9);
@@ -26,17 +24,19 @@
 static DigitalOut db6(PC_14);
 static DigitalOut db7(PC_15);
 
-static PwmOut Motor1 (PC_8);       //  Schwarz  QBRAIN: rot
-static PwmOut Motor2 (PC_9);       //  Weiß             orange
-static PwmOut Motor3 (PC_6);       //  Grau             weiß
-static PwmOut Motor4 (PB_9);       //  Blau             braun
-//  Gelb und Orange Vcc +5V
-//  Gnd Rot
+static PwmOut Motor1 (PC_8);        //  Schwarz  QBRAIN: rot
+static PwmOut Motor2 (PC_9);        //  Weiß             orange
+static PwmOut Motor3 (PC_6);        //  Grau             weiß
+static PwmOut Motor4 (PB_9);        //  Blau             braun
+                                    //  Gelb und Orange Vcc +5V
+                                    //  Gnd Rot
+                                    
 
-
+static int n1, n2, n3, n4;
 
+float k;
 float y_off, y_high_low_summe, y_winkel;
-int16_t n1, n2, n3, n4;
+
 uint16_t i, j;
 
 uint16_t zeit;
@@ -46,37 +46,68 @@
 Timer timer;
 Timer timer2;
 
+AnalogOut rauschen(PA_5);
+
 
 int main()
-{   i = j = 0;
-    //Motor auf Drehzahl null einstellen start
-    n1 = n2 = n3 = n4 =700;
-    Motor1.period_ms(2);
+{   Motor1.period_ms(2);
     Motor2.period_ms(2);
     Motor3.period_ms(2);
     Motor4.period_ms(2);
     wait_ms (10);
-
+    pc.printf("\n\r");
+    initialisierung_gyro();
+    initialisierung_acc();
+    wait_ms(20);    
+    if (taster2)
+    {   
+        rauschen = 1;
+        n1 = n2 = n3 = n4 = 700;
+        Motor1.pulsewidth_us(n1);
+        Motor2.pulsewidth_us(n2);
+        Motor3.pulsewidth_us(n3);
+        Motor4.pulsewidth_us(n4);
+        wait_ms(10000);
+        while(!taster4)
+        {
+            for(i = 1; i <= 1000; i++)
+            {
+            Motor1.pulsewidth_us(n1);
+            Motor2.pulsewidth_us(n2);
+            Motor3.pulsewidth_us(n3);
+            Motor4.pulsewidth_us(n4);
+            k = aktuell_acc_x()*aktuell_acc_x();
+            k = sqrt(k) * 0.0000438596491;
+            pc.printf("Winkel:%2.5f\n\r",k);
+            rauschen = k;
+            wait_ms(10);
+            }
+            for(i = 1, n1 = n2 = n3= 1400; i <= 3000; i++)
+            {
+            Motor1.pulsewidth_us(n1);
+            Motor2.pulsewidth_us(n2);
+            Motor3.pulsewidth_us(n3);
+            Motor4.pulsewidth_us(n4);
+            k = aktuell_acc_x()*aktuell_acc_x();
+            k = sqrt(k) * 0.0000438596491;
+            pc.printf("Winkel:%2.5f\n\r",k);
+            rauschen = k;
+            wait_ms(10);
+            }
+            rauschen = 0;
+            wait_ms(100000);
+        }
+    }  
+    
+    if (taster3)            
+    {         
+    n1 = n2 = n3 = n4 = 1900;
     Motor1.pulsewidth_us(n1);
     Motor2.pulsewidth_us(n2);
     Motor3.pulsewidth_us(n3);
     Motor4.pulsewidth_us(n4);
-    wait_ms (10);
-    pc.printf("\n\r");
-    pc.printf("Drehzahl aller Motoren: %d%%\n\r",(n1-700)*100/(1900-700));
-    //Motor auf Drehzahl null einstellen ende
-    initialisierung_gyro();
-    wait_ms(20);    
-    //Motor anlernen start
-    if (taster3)            
-    {         
-        n1 = n2 = n3 = n4 = 1900;
-        Motor1.pulsewidth_us(n1);
-        Motor2.pulsewidth_us(n2);
-        Motor3.pulsewidth_us(n3);
-        Motor4.pulsewidth_us(n4);
-        pc.printf("Nach einem langem PiepTon  Taste1 betaetigen\n\r");
-        pc.printf("Drehzahl aller Motoren: %d%%\n\r",(n1-700)*100/(1900-700));
+    pc.printf("Nach einem langem PiepTon  Taste1 betaetigen\n\r");
+    pc.printf("Drehzahl aller Motoren: %d%%\r",(n1-700)*100/(1900-700));
         while (!taster4) 
         {
             if (taster1) 
@@ -91,10 +122,11 @@
             Motor2.pulsewidth_us(n2);
             Motor3.pulsewidth_us(n3);
             Motor4.pulsewidth_us(n4);
-            pc.printf("Drehzahl aller Motoren: %d%%\n\r",(n1-700)*100/(1900-700));
+            pc.printf("Drehzahl aller Motoren: %d%%\r",(n1-700)*100/(1900-700));
         }
     }
-    //Motor anlernen ende  
+    wait_ms (10);
+    pc.printf("Drehzahl aller Motoren: %d%%\n\r",(n1-700)*100/(1900-700));
     pc.printf("Druecke Taster1 für den Start\n\r");
     while(1)
     {