Gruppe 3 / Mbed 2 deprecated PES3

hallo

Dependencies:   Servo mbed pixy

Fork of PES1 by Gruppe 3

readCamera.cpp@21:69ee872b8ee9, 2017-05-16 (annotated)

Committer:
itslinear
Date:
Tue May 16 16:09:08 2017 +0000
Revision:
21:69ee872b8ee9
Parent:
20:a90e5b54bf7b 
sieg

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 21:69ee872b8ee9 9 static float yLimit = 140; // Punkt an dem Roboter anhalten soll
Shukle 10:75a18bf17c86 10 int j;
Shukle 10:75a18bf17c86 11 uint16_t blocks;
itslinear 18:3ee1b02ed3aa 12 float state;
Shukle 10:75a18bf17c86 13
itslinear 19:adae367247d4 14 int readCamera(int now)
itslinear 19:adae367247d4 15 {
itslinear 19:adae367247d4 16 state = now;
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 20:a90e5b54bf7b 29 if(yAxis < yLimit) {
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 19:adae367247d4 47 if (xAxis < 140) { // links fahren
itslinear 18:3ee1b02ed3aa 48 state = 100;
itslinear 18:3ee1b02ed3aa 49
itslinear 19:adae367247d4 50 } else if (xAxis > 180) { // 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 }
Shukle 10:75a18bf17c86 57 }
itslinear 18:3ee1b02ed3aa 58 i = 0;
itslinear 20:a90e5b54bf7b 59 k = 0;
Shukle 10:75a18bf17c86 60 }
itslinear 18:3ee1b02ed3aa 61 } else {
itslinear 18:3ee1b02ed3aa 62 k++;
itslinear 20:a90e5b54bf7b 63 if (k % 20 == 0) {
itslinear 18:3ee1b02ed3aa 64 k = 0;
itslinear 18:3ee1b02ed3aa 65 state = 0; // kein Klotz in Sichtweite
itslinear 18:3ee1b02ed3aa 66 }
Shukle 10:75a18bf17c86 67 }
itslinear 20:a90e5b54bf7b 68 //printf("%f\n\r",state);
itslinear 18:3ee1b02ed3aa 69 return state;
Shukle 10:75a18bf17c86 70 }