Diff: readCamera.cpp

Revision:

18:3ee1b02ed3aa

Parent:

15:7e9ecc4d7217

Child:

19:adae367247d4

--- a/readCamera.cpp	Tue Apr 25 12:26:04 2017 +0000
+++ b/readCamera.cpp	Tue May 09 15:25:54 2017 +0000
@@ -1,68 +1,73 @@
 #include "readCamera.h"
 
-
 Pixy pixy(I2C_SDA, I2C_SCL);
 Serial pc(SERIAL_TX, SERIAL_RX);
 
 static int i = 0;
 static int k = 0;
-static int range = 1;           // 1: fährt strahl an, 2: fährt kegel an
-static int state = 0;
+static float distance = 0;        // für dynamische Geschwindigkeit
+static float yLimit = 165;        // Punkt an dem Roboter anhalten soll
 int j;
 uint16_t blocks;
 
+float state;
 
 int readCamera()
 {
     pixy.setSerialOutput(&pc);
+    blocks = pixy.getBlocks(1);
 
-    while (1) {
-        blocks = pixy.getBlocks(1);
+    if (blocks) {
+        i++;
+        if (i % 5 == 0) {                                               // Jedes 5. Bild wird ausgewertet
+            for (j = 0; j < blocks; j++) {
+                float xAxis = pixy.blocks[j].x;                         // Auswertung der x-Koordinate
+                float yAxis = pixy.blocks[j].y;                         // Auswertung der y-Koordinate
+                //printf("%f y\n", yAxis);
+                //printf("%f x\n", xAxis);
+
+                if(yAxis < yLimit && yAxis > 30) {
+
+                    if (xAxis < 130) {                                  // links fahren
+                        distance = (129 - xAxis)*(99.0/129.0);
+                        state = 100 + distance;
+
+                    } else if (xAxis > 190) {                           // rechts fahren
+                        distance = (xAxis - 191)*(99.0/128.0);
+                        state = 200 + distance;
+
+                    } else {                                            // fahre gerade
+                        distance = ((yAxis - yLimit) / yLimit)*(-99);
+                        state = 300 + distance;
+                    }
+                }
 
-        if (blocks) {
-            i++;
-            if (i % 5 == 0) {
-                for (j = 0; j < blocks; j++) {
-                    int xAxis = pixy.blocks[j].x;
-                    int yAxis = pixy.blocks[j].y;
-                    
-                     
-        
-                    // soll drehen bis in stahl, fahren bis kegel, 25 pixel links rechts kegel
-                    if(xAxis < 185 && xAxis > 135  &&  yAxis > 155 && yAxis < 190 ) {              // wenn Klotz an Position zum aufnehmen
-                        state = 4;
-                        range = 1;
-                    } else {
-                        if (range == 1){         // fährt Strahl an
-                            if (xAxis < 155){
-                                state = 1;
-                            } else if (xAxis > 165){
-                                state = 2;
-                            } else{   // fährt gerade
-                                state = 3;
-                                range = 2;
-                            }
-                        } else {                // fährt solange bis Kegelrand
-                            if (xAxis > 185 || xAxis < 135) {
-                                range = 1;
-                            }
-                        }
+                if(yAxis > yLimit) {
+
+                    if (xAxis < 145) {                                  // links fahren
+                        state = 100;
+
+                    } else if (xAxis > 175) {                           // rechts fahren
+                        state = 200;
+
+                    } else {                                            // Klotz bereit zum Aufnehmen
+                        state = 400;
                     }
-                    
-                    
-                    
                 }
-                k=0;
+                if(yAxis <= 30) {                                       // Zone mit vielen Fehlern
+                    state = 0;                                          // deshalb wird alles, was dort drin ist als "kein Klotz in Sichtweite"" gewertet 
+                }
             }
-        } else{
-            k++;
-            if (k % 20 == 0) {
-                i=0;
-                state = 0;
-            }
+            i = 0;
         }
-        //printf("%d\n\r",state);
-        //printf("%d, %d\n\r", xAxis, yAxis);
-        return state;
+    } else {
+        k++;
+        if (k % 20 == 0) {
+            i=0;
+            k = 0;
+            state = 0;                                                  // kein Klotz in Sichtweite
+        }
     }
+    printf("%f\n\r",state);
+    return state;
 }
\ No newline at end of file