Gruppe 3
/
PES4
xx
Fork of PES1 by
Diff: readCamera.cpp
- Revision:
- 18:6547e54ac803
- Parent:
- 15:7e9ecc4d7217
--- a/readCamera.cpp Tue Apr 25 12:26:04 2017 +0000 +++ b/readCamera.cpp Tue May 09 13:21:49 2017 +0000 @@ -1,68 +1,73 @@ #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 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); - while (1) { - 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 (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; - } - } + 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; } - - - } - k=0; + if(yAxis <= 30) { // Zone mit vielen Fehlern + state = 0; // deshalb wird alles, was dort drin ist als "kein Klotz in Sichtweite"" gewertet + } } - } else{ - k++; - if (k % 20 == 0) { - i=0; - state = 0; - } + k = 0; + i = 0; } - //printf("%d\n\r",state); - //printf("%d, %d\n\r", xAxis, yAxis); - return state; + } else { + k++; + if (k % 20 == 0) { + i=0; + state = 0; // kein Klotz in Sichtweite + } } + //printf("%f\n\r",state); + return state; } \ No newline at end of file