Gruppe 3 / Mbed 2 deprecated PES1

für holdor

Dependencies:   Servo mbed pixy

Fork of PES by Gruppe 3

readCamera.cpp@19:2f44ecffaf30, 2017-05-01 (annotated)

Committer:
Shukle
Date:
Mon May 01 09:43:31 2017 +0000
Revision:
19:2f44ecffaf30
Parent:
18:ea4671f566d4 
readCamera angepasst

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 19:2f44ecffaf30 11 static double distance = 0;
Shukle 19:2f44ecffaf30 12 static int yLimit = 155;
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 10:75a18bf17c86 30
Shukle 10:75a18bf17c86 31
Shukle 10:75a18bf17c86 32
Shukle 10:75a18bf17c86 33 // soll drehen bis in stahl, fahren bis kegel, 25 pixel links rechts kegel
Shukle 19:2f44ecffaf30 34 if(xAxis < 180 && xAxis > 140 && yAxis > 155 && yAxis < yLimit ) { // wenn Klotz an Position zum aufnehmen
Shukle 19:2f44ecffaf30 35 state = 400;
Shukle 10:75a18bf17c86 36 range = 1;
Shukle 10:75a18bf17c86 37 } else {
Shukle 10:75a18bf17c86 38 if (range == 1){ // fährt Strahl an
Shukle 18:ea4671f566d4 39 if (xAxis < 140){
Shukle 19:2f44ecffaf30 40 distance = ((xAxis - 160)*(-0.625))-1;
Shukle 19:2f44ecffaf30 41 state = 100 + (int) distance;
Shukle 18:ea4671f566d4 42 } else if (xAxis > 180){
Shukle 19:2f44ecffaf30 43 distance = ((xAxis - 160)*(0.625))-1;
Shukle 19:2f44ecffaf30 44 state = 200 + (int) distance;
Shukle 10:75a18bf17c86 45 } else{ // fährt gerade
Shukle 19:2f44ecffaf30 46 distance = (((yAxis - yLimit)/(199-yLimit))*100);
Shukle 19:2f44ecffaf30 47 state = 300 + (int) distance;
Shukle 19:2f44ecffaf30 48 if (state == 400){
Shukle 19:2f44ecffaf30 49 state=399;
Shukle 19:2f44ecffaf30 50 }
Shukle 10:75a18bf17c86 51 range = 2;
Shukle 10:75a18bf17c86 52 }
Shukle 10:75a18bf17c86 53 } else { // fährt solange bis Kegelrand
Shukle 10:75a18bf17c86 54 if (xAxis > 185 || xAxis < 135) {
Shukle 10:75a18bf17c86 55 range = 1;
Shukle 10:75a18bf17c86 56 }
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 k=0;
Shukle 10:75a18bf17c86 64 }
Shukle 10:75a18bf17c86 65 } else{
Shukle 10:75a18bf17c86 66 k++;
Shukle 10:75a18bf17c86 67 if (k % 20 == 0) {
Shukle 10:75a18bf17c86 68 i=0;
Shukle 10:75a18bf17c86 69 state = 0;
Shukle 10:75a18bf17c86 70 }
Shukle 10:75a18bf17c86 71 }
itslinear 14:7e330f65f26e 72 //printf("%d\n\r",state);
Shukle 15:7e9ecc4d7217 73 //printf("%d, %d\n\r", xAxis, yAxis);
Shukle 19:2f44ecffaf30 74 return (double) state;
Shukle 10:75a18bf17c86 75 }
Shukle 10:75a18bf17c86 76 }