File content as of revision 24:be4fd3005611:

#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 double distance = 0;
static int yLimit = 130;
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;
                    //printf("%d, %d\n\r", xAxis, yAxis);
                    
                     
        
                    // soll drehen bis in stahl, fahren bis kegel, 25 pixel links rechts kegel
                    if(xAxis < 180 && xAxis > 140  &&  yAxis < 199 && yAxis > yLimit ) {              // wenn Klotz an Position zum aufnehmen
                        state = 400;
                        range = 1;
                        printf("\n\raufnehmen\n\r");
                    } else {
                        if (range == 1){         // fährt Strahl an
                            if (xAxis < 140){
                                distance = ((xAxis - 160)*(-0.625))-1;
                                state = 100 + (int) distance;
                                printf("\n\rfahre links\n\r");
                            } else if (xAxis > 180){
                                distance = ((xAxis - 160)*(0.625))-1;
                                state = 200 + (int) distance;
                                printf("\n\rfahre rechts\n\r");
                            } else{   // fährt gerade
                                distance = (((double) yAxis - (double) yLimit)/(double) yLimit)*-100.0;
                                state = 300 + (int) distance;
                                if (state == 400){
                                    state=399;
                                }
                                range = 2;
                                printf("\n\rgerade\n\r");
                            }
                        } else {                // fährt solange bis Kegelrand
                            if (xAxis > 185 || xAxis < 135) {
                                range = 1;
                            }
                            distance = (((double) yAxis - (double) yLimit)/(double) yLimit)*-100.0;
                            state = 300 + (int) distance;
                        }
                    }
                    
                    
                    
                }
                k=0;
            }
        } else{
            k++;
            if (k % 20 == 0) {
                i=0;
                state = 0;
            }
        }
        //printf("%d\n\r",state);
        return (double) state;
    }
}