File content as of revision 18:3ee1b02ed3aa:

#include "readCamera.h"

Pixy pixy(I2C_SDA, I2C_SCL);
Serial pc(SERIAL_TX, SERIAL_RX);

static int i = 0;
static int k = 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);

    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(yAxis > yLimit) {

                    if (xAxis < 145) {                                  // links fahren
                        state = 100;

                    } else if (xAxis > 175) {                           // rechts fahren
                        state = 200;

                    } else {                                            // Klotz bereit zum Aufnehmen
                        state = 400;
                    }
                }
                if(yAxis <= 30) {                                       // Zone mit vielen Fehlern
                    state = 0;                                          // deshalb wird alles, was dort drin ist als "kein Klotz in Sichtweite"" gewertet 
                }
            }
            i = 0;
        }
    } else {
        k++;
        if (k % 20 == 0) {
            i=0;
            k = 0;
            state = 0;                                                  // kein Klotz in Sichtweite
        }
    }
    printf("%f\n\r",state);
    return state;
}