Diff: main.cpp

Revision:

9:d9e46f9c9e40

Parent:

8:6d9cd5ad332d

Child:

12:b9faf8637183

diff -r 6d9cd5ad332d -r d9e46f9c9e40 main.cpp
--- a/main.cpp	Tue Apr 18 12:09:46 2017 +0000
+++ b/main.cpp	Wed Apr 19 10:07:55 2017 +0000
@@ -40,78 +40,96 @@
     pwmL.period(0.00005f); // Setzt die Periode auf 50 μs
     pwmR.period(0.00005f);
     pwmL = 0.5f;
-    pwmR = 0.5f; 
-   
+    pwmR = 0.5f;
+
 
     int state = 1; // Diese Variable gibt an in welchem State man sich befindet
 
-    /*if ( kamera() == 1) {
-        state = 4;
-        } // Die Kamera erkennt ein grünen Klotz*/
-        while(1) {
+
+    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 1: // Roboter Anschalten mit Knopf
 
-                    if (switchOnOff== 0){
-                        state = 5;
-                    }
+                if (switchOnOff== 0) {
+                    state = 2;
+                }
 
-                    break;
+                break;
 
-                case 2:
-                    roboter1.bandeAusweichen();
-                    static int time1 = 0;
-                    if(time1 < 10) {       // Im Kreis drehen für 1s
-                        pwmL = 0.4f;
-                        pwmR = 0.4f;
-                        time1 ++;
-                    } else {
-                        time1 = 0;
-                        pwmL = 0.5f;
-                        pwmR = 0.5f;
-                        state = 3;
-                    }
-                    break;
+            case 2:
+                static int time1 = 0;
+                if(time1 < 20) {       // Im Kreis drehen für 1s
+                    pwmL = 0.4f;
+                    pwmR = 0.4f;
+                    time1 ++;
+                } else {
+                    time1 = 0;
+                    pwmL = 0.5f;
+                    pwmR = 0.5f;
+                    state = 3;
+                }
+                break;
 
 
-                case 3:
-                    roboter1.bandeAusweichen();
-                    static int time2 = 0;
-                    if(time2 < 10) {    // gerade aus fahren
-                        pwmL = 0.6f;
-                        pwmR = 0.4f;
-                        time2 ++;
-                    } else {
-                        time2 = 0;
-                        pwmL = 0.5f;
-                        pwmR = 0.5f;
-                        state = 2;
-                    }
-                    break;
+            case 3:
+                roboter1.bandeAusweichen();
+                static int time2 = 0;
+                if(time2 < 40) {    // gerade aus fahren
+                    pwmL = 0.6f;
+                    pwmR = 0.4f;
+                    time2 ++;
+                } else {
+                    time2 = 0;
+                    pwmL = 0.5f;
+                    pwmR = 0.5f;
+                    state = 2;
+                }
+                break;
 
 
-                case 4: // Roboter erkennt gründer Klotz, Klotz wird angefahren
-
-                    
-                    break;
-
-                case 5: // Aufnehmen des Klotzes
+            case 4: // Roboter erkennt gründer Klotz, Klotz wird angefahren
+                roboter1.bandeAusweichen();
+                if(kamera() == 1) {     // links fahren
+                    pwmL = 0.45f;
+                    pwmR = 0.45f;
+                }
+                if(kamera() == 2) {     // rechts fahren
+                    pwmL = 0.55f;
+                    pwmR = 0.55f;
+                }
+                if(kamera() == 3) {     // gerade fahren
+                    pwmL = 0.55f;
+                    pwmR = 0.45f;
+                }
 
- roboter1.bandeAusweichen();
-                    
-                    break;
-                    
-                    
-                default: break;  
+                break;
+
+            case 5: // Aufnehmen des Klotzes
+
+                
 
-                    
-            }
+                break;
+
 
-            wait(0.1f);
+            default:
+                break;
+
 
         }
 
+        wait(0.1f);
+
     }
 
+}
+