File content as of revision 15:7e9ecc4d7217:

#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;
int j;
uint16_t blocks;


int readCamera()
{
    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;
                    
                     
        
                    // 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);
        //printf("%d, %d\n\r", xAxis, yAxis);
        return state;
    }
}