Diff: main.cpp

Revision:

19:adae367247d4

Parent:

18:3ee1b02ed3aa

Child:

20:a90e5b54bf7b

--- a/main.cpp	Tue May 09 15:25:54 2017 +0000
+++ b/main.cpp	Fri May 12 12:25:34 2017 +0000
@@ -40,6 +40,7 @@
     enable = 1;            // Sensoren einschalten
     servo1.Enable(1500,20000);
     servo2.Enable(700,20000);
+    wait(1);
     float camValue;
 
     while(1) {
@@ -51,7 +52,7 @@
             case 0: // Roboter Anschalten mit Knopf
                 printf("0");
 
-                if (switchOnOff == 0) {     // Bei Betätigung des Userbuttons wird das Programm gestartet
+                if (switchOnOff == 0) { // Bei Betätigung des Userbuttons wird das Programm gestartet
                     state = 1;
                 }
 
@@ -59,7 +60,7 @@
 
             case 1:
                 //printf("1");
-                camValue = readCamera();
+                camValue = readCamera(state);
                 state = roboter1.stateAuswahl(camValue, tempState); // Case-Auswahl aufgrund von der Kamera
                 break;
 
@@ -72,7 +73,7 @@
 
             case 3:
                 //printf("3");
-                roboter1.bandeAusweichen();  // Hindernissen ausweichen
+                roboter1.ausweichen1();  // Hindernissen ausweichen
                 state = roboter1.geradeFahren(); // Gerade Fahren
                 tempState = 3;
                 break;
@@ -80,9 +81,8 @@
 
             case 4:
                 //printf("4");
-                roboter1.bandeAusweichen();  // Hindernissen ausweichen
-                roboter1.anfahren(camValue); // Roboter erkennt gründer Klotz, Klotz wird angefahren
-                state = 1;
+                roboter1.anfahren(camValue); // Roboter erkennt grünen Klotz, Klotz wird angefahren
+                state = roboter1.readSensors();
                 break;
 
             case 5:
@@ -90,11 +90,18 @@
                 state = roboter1.pickup(); // Aufnehmen des Klotzes
                 break;
 
+            case 6:
+                //printf("6");
+                state = roboter1.ausweichen2();  // Hindernissen ausweichen während Klotz angefahren wird
+            
+            case 7:
+                //printf("7"); 
+                //state = roboter1.back();
             default:
                 break;
 
         }
-        
-        wait(0.01f);
+
+        wait(0.0001f);
     }
 }