Gruppe 3
/
PES3
hallo
Fork of PES1 by
readCamera.cpp@19:adae367247d4, 2017-05-12 (annotated)
- Committer:
- itslinear
- Date:
- Fri May 12 12:25:34 2017 +0000
- Revision:
- 19:adae367247d4
- Parent:
- 18:3ee1b02ed3aa
- Child:
- 20:a90e5b54bf7b
hallo balz
Who changed what in which revision?
User | Revision | Line number | New 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 | 19:adae367247d4 | 9 | static float yLimit = 155; // 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 | 18:3ee1b02ed3aa | 29 | if(yAxis < yLimit && yAxis > 30) { |
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 | } |
itslinear | 19:adae367247d4 | 57 | if(yAxis <= 20) { // Zone mit vielen Fehlern |
itslinear | 18:3ee1b02ed3aa | 58 | state = 0; // deshalb wird alles, was dort drin ist als "kein Klotz in Sichtweite"" gewertet |
itslinear | 18:3ee1b02ed3aa | 59 | } |
Shukle | 10:75a18bf17c86 | 60 | } |
itslinear | 18:3ee1b02ed3aa | 61 | i = 0; |
Shukle | 10:75a18bf17c86 | 62 | } |
itslinear | 18:3ee1b02ed3aa | 63 | } else { |
itslinear | 18:3ee1b02ed3aa | 64 | k++; |
itslinear | 19:adae367247d4 | 65 | if (k % 5 == 0) { |
itslinear | 18:3ee1b02ed3aa | 66 | k = 0; |
itslinear | 18:3ee1b02ed3aa | 67 | state = 0; // kein Klotz in Sichtweite |
itslinear | 18:3ee1b02ed3aa | 68 | } |
Shukle | 10:75a18bf17c86 | 69 | } |
itslinear | 18:3ee1b02ed3aa | 70 | printf("%f\n\r",state); |
itslinear | 18:3ee1b02ed3aa | 71 | return state; |
Shukle | 10:75a18bf17c86 | 72 | } |