File content as of revision 21:69ee872b8ee9:

#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 = 140;        // Punkt an dem Roboter anhalten soll
int j;
uint16_t blocks;
float state;

int readCamera(int now)
{    
    state = now;
    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) {

                    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 < 140) {                                  // links fahren
                        state = 100;

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

                    } else {                                            // Klotz bereit zum Aufnehmen
                        state = 400;
                    }
                }
            }
            i = 0;
            k = 0;
        }
    } else {
        k++;
        if (k % 20 == 0) {
            k = 0;
            state = 0;                                                  // kein Klotz in Sichtweite
        }
    }
    //printf("%f\n\r",state);
    return state;
}