Gruppe 3 / Mbed 2 deprecated PES1

für holdor

Dependencies:   Servo mbed pixy

Fork of PES by Gruppe 3

readCamera.cpp@12:b9faf8637183, 2017-04-19 (annotated)

Committer:
itslinear
Date:
Wed Apr 19 15:38:56 2017 +0000
Revision:
12:b9faf8637183
Parent:
11:7c5598781280
Child:
14:7e330f65f26e
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
Shukle 10:75a18bf17c86 4 Pixy pixy(I2C_SDA, I2C_SCL);
Shukle 10:75a18bf17c86 5 Serial pc(SERIAL_TX, SERIAL_RX);
Shukle 10:75a18bf17c86 6
Shukle 10:75a18bf17c86 7 static int i = 0;
Shukle 10:75a18bf17c86 8 static int k = 0;
Shukle 10:75a18bf17c86 9 static int range = 1; // 1: fährt strahl an, 2: fährt kegel an
Shukle 10:75a18bf17c86 10 static int state = 0;
Shukle 10:75a18bf17c86 11 int j;
Shukle 10:75a18bf17c86 12 uint16_t blocks;
Shukle 10:75a18bf17c86 13
Shukle 10:75a18bf17c86 14
itslinear 12:b9faf8637183 15 int readCamera()
Shukle 10:75a18bf17c86 16 {
Shukle 10:75a18bf17c86 17 pixy.setSerialOutput(&pc);
Shukle 10:75a18bf17c86 18
Shukle 10:75a18bf17c86 19 while (1) {
Shukle 10:75a18bf17c86 20 blocks = pixy.getBlocks(1);
Shukle 10:75a18bf17c86 21
Shukle 10:75a18bf17c86 22 if (blocks) {
Shukle 10:75a18bf17c86 23 i++;
Shukle 10:75a18bf17c86 24 if (i % 5 == 0) {
Shukle 10:75a18bf17c86 25 for (j = 0; j < blocks; j++) {
Shukle 10:75a18bf17c86 26 int xAxis = pixy.blocks[j].x;
Shukle 10:75a18bf17c86 27 int yAxis = pixy.blocks[j].y;
Shukle 10:75a18bf17c86 28 //printf("%d, %d\n\r", xAxis, yAxis);
Shukle 10:75a18bf17c86 29
Shukle 10:75a18bf17c86 30
Shukle 10:75a18bf17c86 31
Shukle 10:75a18bf17c86 32 // soll drehen bis in stahl, fahren bis kegel, 25 pixel links rechts kegel
Shukle 10:75a18bf17c86 33 if(xAxis < 185 && xAxis > 135 && yAxis > 155 && yAxis < 190 ) { // wenn Klotz an Position zum aufnehmen
Shukle 10:75a18bf17c86 34 state = 4;
Shukle 10:75a18bf17c86 35 range = 1;
Shukle 10:75a18bf17c86 36 } else {
Shukle 10:75a18bf17c86 37 if (range == 1){ // fährt Strahl an
Shukle 10:75a18bf17c86 38 if (xAxis < 155){
Shukle 10:75a18bf17c86 39 state = 1;
Shukle 10:75a18bf17c86 40 } else if (xAxis > 165){
Shukle 10:75a18bf17c86 41 state = 2;
Shukle 10:75a18bf17c86 42 } else{ // fährt gerade
Shukle 10:75a18bf17c86 43 state = 3;
Shukle 10:75a18bf17c86 44 range = 2;
Shukle 10:75a18bf17c86 45 }
Shukle 10:75a18bf17c86 46 } else { // fährt solange bis Kegelrand
Shukle 10:75a18bf17c86 47 if (xAxis > 185 || xAxis < 135) {
Shukle 10:75a18bf17c86 48 range = 1;
Shukle 10:75a18bf17c86 49 }
Shukle 10:75a18bf17c86 50 }
Shukle 10:75a18bf17c86 51 }
Shukle 10:75a18bf17c86 52
Shukle 10:75a18bf17c86 53
Shukle 10:75a18bf17c86 54
Shukle 10:75a18bf17c86 55 }
Shukle 10:75a18bf17c86 56 k=0;
Shukle 10:75a18bf17c86 57 }
Shukle 10:75a18bf17c86 58 } else{
Shukle 10:75a18bf17c86 59 k++;
Shukle 10:75a18bf17c86 60 if (k % 20 == 0) {
Shukle 10:75a18bf17c86 61 i=0;
Shukle 10:75a18bf17c86 62 state = 0;
Shukle 10:75a18bf17c86 63 }
Shukle 10:75a18bf17c86 64 }
Shukle 10:75a18bf17c86 65 printf("%d\n\r",state);
itslinear 12:b9faf8637183 66 return state;
Shukle 10:75a18bf17c86 67 }
Shukle 10:75a18bf17c86 68 }