Gruppe 3 / Mbed 2 deprecated PES3

hallo

Dependencies:   Servo mbed pixy

Fork of PES1 by Gruppe 3

readCamera.cpp@18:3ee1b02ed3aa, 2017-05-09 (annotated)

Committer:
itslinear
Date:
Tue May 09 15:25:54 2017 +0000
Revision:
18:3ee1b02ed3aa
Parent:
15:7e9ecc4d7217
Child:
19:adae367247d4
new

Who changed what in which revision?

UserRevisionLine numberNew contents of line
itslinear 11:7c5598781280 1 #include "readCamera.h"
itslinear 11:7c5598781280 2
Shukle 10:75a18bf17c86 3 Pixy pixy(I2C_SDA, I2C_SCL);
Shukle 10:75a18bf17c86 4 Serial pc(SERIAL_TX, SERIAL_RX);
Shukle 10:75a18bf17c86 5
Shukle 10:75a18bf17c86 6 static int i = 0;
Shukle 10:75a18bf17c86 7 static int k = 0;
itslinear 18:3ee1b02ed3aa 8 static float distance = 0; // für dynamische Geschwindigkeit
itslinear 18:3ee1b02ed3aa 9 static float yLimit = 165; // Punkt an dem Roboter anhalten soll
Shukle 10:75a18bf17c86 10 int j;
Shukle 10:75a18bf17c86 11 uint16_t blocks;
Shukle 10:75a18bf17c86 12
itslinear 18:3ee1b02ed3aa 13 float state;
Shukle 10:75a18bf17c86 14
itslinear 12:b9faf8637183 15 int readCamera()
Shukle 10:75a18bf17c86 16 {
Shukle 10:75a18bf17c86 17 pixy.setSerialOutput(&pc);
itslinear 18:3ee1b02ed3aa 18 blocks = pixy.getBlocks(1);
Shukle 10:75a18bf17c86 19
itslinear 18:3ee1b02ed3aa 20 if (blocks) {
itslinear 18:3ee1b02ed3aa 21 i++;
itslinear 18:3ee1b02ed3aa 22 if (i % 5 == 0) { // Jedes 5. Bild wird ausgewertet
itslinear 18:3ee1b02ed3aa 23 for (j = 0; j < blocks; j++) {
itslinear 18:3ee1b02ed3aa 24 float xAxis = pixy.blocks[j].x; // Auswertung der x-Koordinate
itslinear 18:3ee1b02ed3aa 25 float yAxis = pixy.blocks[j].y; // Auswertung der y-Koordinate
itslinear 18:3ee1b02ed3aa 26 //printf("%f y\n", yAxis);
itslinear 18:3ee1b02ed3aa 27 //printf("%f x\n", xAxis);
itslinear 18:3ee1b02ed3aa 28
itslinear 18:3ee1b02ed3aa 29 if(yAxis < yLimit && yAxis > 30) {
itslinear 18:3ee1b02ed3aa 30
itslinear 18:3ee1b02ed3aa 31 if (xAxis < 130) { // links fahren
itslinear 18:3ee1b02ed3aa 32 distance = (129 - xAxis)*(99.0/129.0);
itslinear 18:3ee1b02ed3aa 33 state = 100 + distance;
itslinear 18:3ee1b02ed3aa 34
itslinear 18:3ee1b02ed3aa 35 } else if (xAxis > 190) { // rechts fahren
itslinear 18:3ee1b02ed3aa 36 distance = (xAxis - 191)*(99.0/128.0);
itslinear 18:3ee1b02ed3aa 37 state = 200 + distance;
itslinear 18:3ee1b02ed3aa 38
itslinear 18:3ee1b02ed3aa 39 } else { // fahre gerade
itslinear 18:3ee1b02ed3aa 40 distance = ((yAxis - yLimit) / yLimit)*(-99);
itslinear 18:3ee1b02ed3aa 41 state = 300 + distance;
itslinear 18:3ee1b02ed3aa 42 }
itslinear 18:3ee1b02ed3aa 43 }
Shukle 10:75a18bf17c86 44
itslinear 18:3ee1b02ed3aa 45 if(yAxis > yLimit) {
itslinear 18:3ee1b02ed3aa 46
itslinear 18:3ee1b02ed3aa 47 if (xAxis < 145) { // links fahren
itslinear 18:3ee1b02ed3aa 48 state = 100;
itslinear 18:3ee1b02ed3aa 49
itslinear 18:3ee1b02ed3aa 50 } else if (xAxis > 175) { // rechts fahren
itslinear 18:3ee1b02ed3aa 51 state = 200;
itslinear 18:3ee1b02ed3aa 52
itslinear 18:3ee1b02ed3aa 53 } else { // Klotz bereit zum Aufnehmen
itslinear 18:3ee1b02ed3aa 54 state = 400;
Shukle 10:75a18bf17c86 55 }
Shukle 10:75a18bf17c86 56 }
itslinear 18:3ee1b02ed3aa 57 if(yAxis <= 30) { // Zone mit vielen Fehlern
itslinear 18:3ee1b02ed3aa 58 state = 0; // deshalb wird alles, was dort drin ist als "kein Klotz in Sichtweite"" gewertet
itslinear 18:3ee1b02ed3aa 59 }
Shukle 10:75a18bf17c86 60 }
itslinear 18:3ee1b02ed3aa 61 i = 0;
Shukle 10:75a18bf17c86 62 }
itslinear 18:3ee1b02ed3aa 63 } else {
itslinear 18:3ee1b02ed3aa 64 k++;
itslinear 18:3ee1b02ed3aa 65 if (k % 20 == 0) {
itslinear 18:3ee1b02ed3aa 66 i=0;
itslinear 18:3ee1b02ed3aa 67 k = 0;
itslinear 18:3ee1b02ed3aa 68 state = 0; // kein Klotz in Sichtweite
itslinear 18:3ee1b02ed3aa 69 }
Shukle 10:75a18bf17c86 70 }
itslinear 18:3ee1b02ed3aa 71 printf("%f\n\r",state);
itslinear 18:3ee1b02ed3aa 72 return state;
Shukle 10:75a18bf17c86 73 }