Diff: main.cpp

Revision:

16:0a1703438e8b

Parent:

14:7e330f65f26e

Child:

17:caf5ae550f2e

--- a/main.cpp	Thu Apr 20 09:19:20 2017 +0000
+++ b/main.cpp	Fri Apr 21 11:53:23 2017 +0000
@@ -45,6 +45,8 @@
 
     int state = 0; // Diese Variable gibt an in welchem State man sich befindet
     int tempState = 2;
+    int time1 = 0;
+    int time2 = 0;
 
     while(1) {
 
@@ -53,6 +55,7 @@
         switch(state) {
 
             case 0: // Roboter Anschalten mit Knopf
+                printf("0");
 
                 if (switchOnOff == 0) {
                     state = 1;
@@ -61,6 +64,7 @@
                 break;
 
             case 1:
+                printf("1");
                 if(camValue == 1 || camValue == 2 || camValue == 3) {           // Die Kamera erkennt ein grünen Klotz
                     state = 4;
                 }
@@ -69,11 +73,11 @@
                 }
                 if(camValue == 0){
                     state = tempState;
-                    }
-
+                }
+                break;
 
             case 2:
-                static int time1 = 0;
+                printf("2");
                 if(time1 < 20) {       // Im Kreis drehen für 1s
                     pwmL = 0.4f;
                     pwmR = 0.4f;
@@ -90,8 +94,8 @@
 
 
             case 3:
+                printf("3");
                 roboter1.bandeAusweichen();
-                static int time2 = 0;
                 if(time2 < 40) {    // gerade aus fahren
                     pwmL = 0.6f;
                     pwmR = 0.4f;
@@ -108,6 +112,7 @@
 
 
             case 4: // Roboter erkennt gründer Klotz, Klotz wird angefahren
+                printf("4");
                 roboter1.bandeAusweichen();
                 if(camValue == 1) {     // links fahren
                     pwmL = 0.45f;
@@ -126,10 +131,12 @@
                 break;
 
             case 5: // Aufnehmen des Klotzes
+                printf("5");
                 pwmL = 0.5f;
                 pwmR = 0.5f;
                 state = 1;
-
+                time1 = 0;
+                time2 = 0;
 
                 break;
 
@@ -139,9 +146,8 @@
 
 
         }
-
-        wait(0.1f);
-
+        
+        //wait(0.1f);
     }
 
 }