Diff: main.cpp

Revision:

12:b9faf8637183

Parent:

9:d9e46f9c9e40

Child:

13:7eba9911e196

--- a/main.cpp	Wed Apr 19 11:35:12 2017 +0000
+++ b/main.cpp	Wed Apr 19 15:38:56 2017 +0000
@@ -43,35 +43,42 @@
     pwmR = 0.5f;
 
 
-    int state = 1; // Diese Variable gibt an in welchem State man sich befindet
+    int state = 0; // Diese Variable gibt an in welchem State man sich befindet
 
 
     while(1) {
 
-        if (kamera() == 1 || kamera() == 2 || kamera() == 3) {           // Die Kamera erkennt ein grünen Klotz
-            state = 4;
-        }
-        if(kamera() == 4) {     // Roboter in Position
-            state = 5;
-        }
 
 
         switch(state) {
 
-            case 1: // Roboter Anschalten mit Knopf
+            case 0: // Roboter Anschalten mit Knopf
 
-                if (switchOnOff== 0) {
-                    state = 2;
+                if (switchOnOff == 0) {
+                    state = 1;
                 }
 
                 break;
 
+            case 1:
+                if(readCamera() == 1 || readCamera() == 2 || readCamera() == 3) {           // Die Kamera erkennt ein grünen Klotz
+                    state = 4;
+                }
+                if(readCamera() == 4) {     // Roboter in Position
+                    state = 5;
+                }
+                if(readCamera() == 0){
+                    state = 2;
+                    }
+
+
             case 2:
                 static int time1 = 0;
                 if(time1 < 20) {       // Im Kreis drehen für 1s
                     pwmL = 0.4f;
                     pwmR = 0.4f;
                     time1 ++;
+                    state = 1;
                 } else {
                     time1 = 0;
                     pwmL = 0.5f;
@@ -88,6 +95,7 @@
                     pwmL = 0.6f;
                     pwmR = 0.4f;
                     time2 ++;
+                    state = 1;
                 } else {
                     time2 = 0;
                     pwmL = 0.5f;
@@ -99,15 +107,15 @@
 
             case 4: // Roboter erkennt gründer Klotz, Klotz wird angefahren
                 roboter1.bandeAusweichen();
-                if(kamera() == 1) {     // links fahren
+                if(readCamera() == 1) {     // links fahren
                     pwmL = 0.45f;
                     pwmR = 0.45f;
                 }
-                if(kamera() == 2) {     // rechts fahren
+                if(readCamera() == 2) {     // rechts fahren
                     pwmL = 0.55f;
                     pwmR = 0.55f;
                 }
-                if(kamera() == 3) {     // gerade fahren
+                if(readCamera() == 3) {     // gerade fahren
                     pwmL = 0.55f;
                     pwmR = 0.45f;
                 }
@@ -115,8 +123,9 @@
                 break;
 
             case 5: // Aufnehmen des Klotzes
+                pwmL = 0.5f;
+                pwmR = 0.5f;
 
-                
 
                 break;