Gruppe 3 / Mbed 2 deprecated PES1

für holdor

Dependencies:   Servo mbed pixy

Fork of PES by Gruppe 3

readCamera.cpp@26:c2f7c6cdeece, 2017-05-01 (annotated)

Committer:
Shukle
Date:
Mon May 01 15:37:23 2017 +0000
Revision:
26:c2f7c6cdeece
Parent:
24:be4fd3005611 
final vom 1.5.2017

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 24:be4fd3005611 12 static int yLimit = 130;
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 24:be4fd3005611 35 if(xAxis < 180 && xAxis > 140 && yAxis < 199 && yAxis > yLimit ) { // wenn Klotz an Position zum aufnehmen
Shukle 21:d05dee0d1a7d 36 state = 400;
Shukle 10:75a18bf17c86 37 range = 1;
Shukle 23:6feed75772b6 38 printf("\n\raufnehmen\n\r");
Shukle 10:75a18bf17c86 39 } else {
Shukle 10:75a18bf17c86 40 if (range == 1){ // fährt Strahl an
Shukle 21:d05dee0d1a7d 41 if (xAxis < 140){
Shukle 21:d05dee0d1a7d 42 distance = ((xAxis - 160)*(-0.625))-1;
Shukle 21:d05dee0d1a7d 43 state = 100 + (int) distance;
Shukle 23:6feed75772b6 44 printf("\n\rfahre links\n\r");
Shukle 21:d05dee0d1a7d 45 } else if (xAxis > 180){
Shukle 21:d05dee0d1a7d 46 distance = ((xAxis - 160)*(0.625))-1;
Shukle 21:d05dee0d1a7d 47 state = 200 + (int) distance;
Shukle 23:6feed75772b6 48 printf("\n\rfahre rechts\n\r");
Shukle 10:75a18bf17c86 49 } else{ // fährt gerade
Shukle 24:be4fd3005611 50 distance = (((double) yAxis - (double) yLimit)/(double) yLimit)*-100.0;
Shukle 21:d05dee0d1a7d 51 state = 300 + (int) distance;
Shukle 21:d05dee0d1a7d 52 if (state == 400){
Shukle 21:d05dee0d1a7d 53 state=399;
Shukle 21:d05dee0d1a7d 54 }
Shukle 10:75a18bf17c86 55 range = 2;
Shukle 23:6feed75772b6 56 printf("\n\rgerade\n\r");
Shukle 10:75a18bf17c86 57 }
Shukle 10:75a18bf17c86 58 } else { // fährt solange bis Kegelrand
Shukle 26:c2f7c6cdeece 59 if (xAxis > 200 || xAxis < 120) {
Shukle 10:75a18bf17c86 60 range = 1;
Shukle 10:75a18bf17c86 61 }
Shukle 24:be4fd3005611 62 distance = (((double) yAxis - (double) yLimit)/(double) yLimit)*-100.0;
Shukle 24:be4fd3005611 63 state = 300 + (int) distance;
Shukle 10:75a18bf17c86 64 }
Shukle 10:75a18bf17c86 65 }
Shukle 10:75a18bf17c86 66
Shukle 10:75a18bf17c86 67
Shukle 10:75a18bf17c86 68
Shukle 10:75a18bf17c86 69 }
Shukle 10:75a18bf17c86 70 k=0;
Shukle 10:75a18bf17c86 71 }
Shukle 10:75a18bf17c86 72 } else{
Shukle 10:75a18bf17c86 73 k++;
Shukle 10:75a18bf17c86 74 if (k % 20 == 0) {
Shukle 10:75a18bf17c86 75 i=0;
Shukle 10:75a18bf17c86 76 state = 0;
Shukle 10:75a18bf17c86 77 }
Shukle 10:75a18bf17c86 78 }
itslinear 14:7e330f65f26e 79 //printf("%d\n\r",state);
Shukle 21:d05dee0d1a7d 80 return (double) state;
Shukle 10:75a18bf17c86 81 }
Shukle 10:75a18bf17c86 82 }