readCamera.cpp

00001 #include "readCamera.h"
00002 
00003 Pixy pixy(I2C_SDA, I2C_SCL);
00004 Serial pc(SERIAL_TX, SERIAL_RX);
00005 
00006 static int i = 0;
00007 static int k = 0;
00008 static float distance = 0;        // für dynamische Geschwindigkeit
00009 static float yLimit = 140;        // Punkt an dem Roboter anhalten soll
00010 int j;
00011 uint16_t blocks;
00012 float state;
00013 
00014 int readCamera(int now)
00015 {    
00016     state = now;
00017     pixy.setSerialOutput(&pc);
00018     blocks = pixy.getBlocks(1);
00019 
00020     if (blocks) {
00021         i++;
00022         if (i % 5 == 0) {                                               // Jedes 5. Bild wird ausgewertet
00023             for (j = 0; j < blocks; j++) {
00024                 float xAxis = pixy.blocks[j].x;                         // Auswertung der x-Koordinate
00025                 float yAxis = pixy.blocks[j].y;                         // Auswertung der y-Koordinate
00026                 //printf("%f y\n", yAxis);
00027                 //printf("%f x\n", xAxis);
00028 
00029                 if(yAxis < yLimit) {
00030 
00031                     if (xAxis < 130) {                                  // links fahren
00032                         distance = (129 - xAxis)*(99.0/129.0);
00033                         state = 100 + distance;
00034 
00035                     } else if (xAxis > 190) {                           // rechts fahren
00036                         distance = (xAxis - 191)*(99.0/128.0);
00037                         state = 200 + distance;
00038 
00039                     } else {                                            // fahre gerade
00040                         distance = ((yAxis - yLimit) / yLimit)*(-99);
00041                         state = 300 + distance;
00042                     }
00043                 }
00044 
00045                 if(yAxis > yLimit) {
00046 
00047                     if (xAxis < 140) {                                  // links fahren
00048                         state = 100;
00049 
00050                     } else if (xAxis > 180) {                           // rechts fahren
00051                         state = 200;
00052 
00053                     } else {                                            // Klotz bereit zum Aufnehmen
00054                         state = 400;
00055                     }
00056                 }
00057             }
00058             i = 0;
00059             k = 0;
00060         }
00061     } else {
00062         k++;
00063         if (k % 20 == 0) {
00064             k = 0;
00065             state = 0;                                                  // kein Klotz in Sichtweite
00066         }
00067     }
00068     //printf("%f\n\r",state);
00069     return state;
00070 }