Gruppe 3 / Mbed 2 deprecated PES1

für holdor

Dependencies:   Servo mbed pixy

Fork of PES by Gruppe 3

readCamera.cpp@22:ffd36f07c046, 2017-05-01 (annotated)

Committer:
Shukle
Date:
Mon May 01 10:18:19 2017 +0000
Revision:
22:ffd36f07c046
Parent:
21:d05dee0d1a7d
Child:
23:6feed75772b6
?berarbeited;

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 21:d05dee0d1a7d 11 static double distance = 0;
Shukle 22:ffd36f07c046 12 static int yLimit = 40;
Shukle 10:75a18bf17c86 13 int j;
Shukle 10:75a18bf17c86 14 uint16_t blocks;
Shukle 10:75a18bf17c86 15
Shukle 10:75a18bf17c86 16
itslinear 12:b9faf8637183 17 int readCamera()
Shukle 10:75a18bf17c86 18 {
Shukle 10:75a18bf17c86 19 pixy.setSerialOutput(&pc);
Shukle 10:75a18bf17c86 20
Shukle 10:75a18bf17c86 21 while (1) {
Shukle 10:75a18bf17c86 22 blocks = pixy.getBlocks(1);
Shukle 10:75a18bf17c86 23
Shukle 10:75a18bf17c86 24 if (blocks) {
Shukle 10:75a18bf17c86 25 i++;
Shukle 10:75a18bf17c86 26 if (i % 5 == 0) {
Shukle 10:75a18bf17c86 27 for (j = 0; j < blocks; j++) {
Shukle 10:75a18bf17c86 28 int xAxis = pixy.blocks[j].x;
Shukle 10:75a18bf17c86 29 int yAxis = pixy.blocks[j].y;
Shukle 22:ffd36f07c046 30 //printf("%d, %d\n\r", xAxis, yAxis);
Shukle 10:75a18bf17c86 31
Shukle 10:75a18bf17c86 32
Shukle 10:75a18bf17c86 33
Shukle 10:75a18bf17c86 34 // soll drehen bis in stahl, fahren bis kegel, 25 pixel links rechts kegel
Shukle 22:ffd36f07c046 35 if(xAxis < 180 && xAxis > 140 && yAxis > 0 && yAxis < yLimit ) { // wenn Klotz an Position zum aufnehmen
Shukle 21:d05dee0d1a7d 36 state = 400;
Shukle 10:75a18bf17c86 37 range = 1;
Shukle 10:75a18bf17c86 38 } else {
Shukle 10:75a18bf17c86 39 if (range == 1){ // fährt Strahl an
Shukle 21:d05dee0d1a7d 40 if (xAxis < 140){
Shukle 21:d05dee0d1a7d 41 distance = ((xAxis - 160)*(-0.625))-1;
Shukle 21:d05dee0d1a7d 42 state = 100 + (int) distance;
Shukle 21:d05dee0d1a7d 43 } else if (xAxis > 180){
Shukle 21:d05dee0d1a7d 44 distance = ((xAxis - 160)*(0.625))-1;
Shukle 21:d05dee0d1a7d 45 state = 200 + (int) distance;
Shukle 10:75a18bf17c86 46 } else{ // fährt gerade
Shukle 21:d05dee0d1a7d 47 distance = (((yAxis - yLimit)/(199-yLimit))*100);
Shukle 21:d05dee0d1a7d 48 state = 300 + (int) distance;
Shukle 21:d05dee0d1a7d 49 if (state == 400){
Shukle 21:d05dee0d1a7d 50 state=399;
Shukle 21:d05dee0d1a7d 51 }
Shukle 10:75a18bf17c86 52 range = 2;
Shukle 10:75a18bf17c86 53 }
Shukle 10:75a18bf17c86 54 } else { // fährt solange bis Kegelrand
Shukle 10:75a18bf17c86 55 if (xAxis > 185 || xAxis < 135) {
Shukle 10:75a18bf17c86 56 range = 1;
Shukle 10:75a18bf17c86 57 }
Shukle 10:75a18bf17c86 58 }
Shukle 10:75a18bf17c86 59 }
Shukle 10:75a18bf17c86 60
Shukle 10:75a18bf17c86 61
Shukle 10:75a18bf17c86 62
Shukle 10:75a18bf17c86 63 }
Shukle 10:75a18bf17c86 64 k=0;
Shukle 10:75a18bf17c86 65 }
Shukle 10:75a18bf17c86 66 } else{
Shukle 10:75a18bf17c86 67 k++;
Shukle 10:75a18bf17c86 68 if (k % 20 == 0) {
Shukle 10:75a18bf17c86 69 i=0;
Shukle 10:75a18bf17c86 70 state = 0;
Shukle 10:75a18bf17c86 71 }
Shukle 10:75a18bf17c86 72 }
itslinear 14:7e330f65f26e 73 //printf("%d\n\r",state);
Shukle 21:d05dee0d1a7d 74 return (double) state;
Shukle 10:75a18bf17c86 75 }
Shukle 10:75a18bf17c86 76 }