Diff: readCamera.cpp

Revision:

10:75a18bf17c86

Child:

11:7c5598781280

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/readCamera.cpp	Wed Apr 19 10:13:52 2017 +0000
@@ -0,0 +1,68 @@
+#include "mbed.h"
+#include "Pixy.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;
+int j;
+uint16_t blocks;
+
+
+int main()
+{
+    pixy.setSerialOutput(&pc);
+
+    while (1) {
+        blocks = pixy.getBlocks(1);
+
+        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;
+                    //printf("%d, %d\n\r", xAxis, yAxis);
+                    
+                     
+        
+                    // 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;
+                            }
+                        }
+                    }
+                    
+                    
+                    
+                }
+                k=0;
+            }
+        } else{
+            k++;
+            if (k % 20 == 0) {
+                i=0;
+                state = 0;
+            }
+        }
+        printf("%d\n\r",state);
+        //return state;
+    }
+}
\ No newline at end of file