Gruppe 3
/
PES3
hallo
Fork of PES1 by
readCamera.cpp
- Committer:
- itslinear
- Date:
- 2017-05-13
- Revision:
- 20:a90e5b54bf7b
- Parent:
- 19:adae367247d4
- Child:
- 21:69ee872b8ee9
File content as of revision 20:a90e5b54bf7b:
#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 = 155; // 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; } } /*if(yAxis <= 20) { // Zone mit vielen Fehlern state = 0; // deshalb wird alles, was dort drin ist als "kein Klotz in Sichtweite"" gewertet }*/ } 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; }