Prom_Roebi_0.1
Dependencies: Farbsensor IRSensorLib PID_Control Servo mbed PixyLib
Farbauswertung.cpp@16:ad45ef4fee04, 2017-05-26 (annotated)
- Committer:
- ZHAW_Prometheus
- Date:
- Fri May 26 09:11:49 2017 +0000
- Revision:
- 16:ad45ef4fee04
- Parent:
- 15:26dbcd6ff48d
Vers. 26.05.2017 11:10
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| ZHAW_Prometheus | 0:422088ad7fc5 | 1 | #include "mbed.h" |
| ZHAW_Prometheus | 0:422088ad7fc5 | 2 | #include "cstdlib" |
| ZHAW_Prometheus | 0:422088ad7fc5 | 3 | //#include <cmath> |
| ZHAW_Prometheus | 0:422088ad7fc5 | 4 | #include "Farbauswertung.h" |
| ZHAW_Prometheus | 0:422088ad7fc5 | 5 | |
| ZHAW_Prometheus | 12:472b26872a42 | 6 | //Konstruktor |
| ZHAW_Prometheus | 16:ad45ef4fee04 | 7 | Farbauswertung::Farbauswertung(AnalogIn* SensorG, AnalogIn* SensorR, Servo& _servoAusw, Servo& _servoFoerder, LiftAnsteuerung& _lift, int verzEin, int verzAus) : servoAusw(_servoAusw), servoFoerder(_servoFoerder), lift(_lift) |
| ZHAW_Prometheus | 0:422088ad7fc5 | 8 | { |
| ZHAW_Prometheus | 0:422088ad7fc5 | 9 | this->SensorG = SensorG; |
| ZHAW_Prometheus | 0:422088ad7fc5 | 10 | this->SensorR = SensorR; |
| ZHAW_Prometheus | 13:7ce78048b733 | 11 | m_verzEin = verzEin; |
| ZHAW_Prometheus | 13:7ce78048b733 | 12 | m_verzAus = verzAus; |
| ZHAW_Prometheus | 13:7ce78048b733 | 13 | p1 = 0; |
| ZHAW_Prometheus | 13:7ce78048b733 | 14 | p2 = merkerSize - verzEin; |
| ZHAW_Prometheus | 0:422088ad7fc5 | 15 | farbsensor.init(SensorG, SensorR); |
| schuema4 | 3:017c85c4b14b | 16 | zustand = gruen; |
| ZHAW_Prometheus | 13:7ce78048b733 | 17 | memset(merker, 0, merkerSize); |
| ZHAW_Prometheus | 0:422088ad7fc5 | 18 | } |
| ZHAW_Prometheus | 0:422088ad7fc5 | 19 | |
| ZHAW_Prometheus | 12:472b26872a42 | 20 | //Destruktor |
| ZHAW_Prometheus | 12:472b26872a42 | 21 | Farbauswertung::~Farbauswertung() {} |
| ZHAW_Prometheus | 12:472b26872a42 | 22 | |
| ZHAW_Prometheus | 15:26dbcd6ff48d | 23 | void Farbauswertung::setSerialOutput(Serial *pc) |
| ZHAW_Prometheus | 15:26dbcd6ff48d | 24 | { |
| ZHAW_Prometheus | 0:422088ad7fc5 | 25 | this->pc = pc; |
| ZHAW_Prometheus | 0:422088ad7fc5 | 26 | } |
| ZHAW_Prometheus | 0:422088ad7fc5 | 27 | |
| ZHAW_Prometheus | 12:472b26872a42 | 28 | //Methoden |
| ZHAW_Prometheus | 15:26dbcd6ff48d | 29 | void Farbauswertung::printState() |
| ZHAW_Prometheus | 15:26dbcd6ff48d | 30 | { |
| ZHAW_Prometheus | 12:472b26872a42 | 31 | if (pc) { |
| ZHAW_Prometheus | 13:7ce78048b733 | 32 | pc->printf("Gruen: %f\tRot: %f\tStatus: %d\n\r", farbsensor.readg(), farbsensor.readr(), zustand); |
| ZHAW_Prometheus | 12:472b26872a42 | 33 | } |
| ZHAW_Prometheus | 12:472b26872a42 | 34 | } |
| ZHAW_Prometheus | 12:472b26872a42 | 35 | |
| ZHAW_Prometheus | 15:26dbcd6ff48d | 36 | void Farbauswertung::auswertung() |
| ZHAW_Prometheus | 15:26dbcd6ff48d | 37 | { |
| ZHAW_Prometheus | 15:26dbcd6ff48d | 38 | servoFoerder = 0.2f; |
| ZHAW_Prometheus | 15:26dbcd6ff48d | 39 | |
| ZHAW_Prometheus | 15:26dbcd6ff48d | 40 | valInG = farbsensor.readg(); |
| ZHAW_Prometheus | 15:26dbcd6ff48d | 41 | valInR = farbsensor.readr(); |
| ZHAW_Prometheus | 15:26dbcd6ff48d | 42 | //Zustände am Farbsensor ins Array schreiben |
| ZHAW_Prometheus | 15:26dbcd6ff48d | 43 | if ((valInG=68.0 && valInG<=70.0 && valInR>=70.0 && valInR<=71.6)|| (valInG>=77.2 && valInG<=77.39 && valInR>=75.0 && valInR<=77.0) || (valInG>=75.0 && valInG<=76.0 && valInR>=73.0 && valInR<=74.0)) { |
| ZHAW_Prometheus | 15:26dbcd6ff48d | 44 | zustand = rot; |
| ZHAW_Prometheus | 15:26dbcd6ff48d | 45 | } else { |
| ZHAW_Prometheus | 15:26dbcd6ff48d | 46 | zustand = gruen; |
| ZHAW_Prometheus | 15:26dbcd6ff48d | 47 | } |
| ZHAW_Prometheus | 15:26dbcd6ff48d | 48 | merker[p1] = zustand; |
| ZHAW_Prometheus | 15:26dbcd6ff48d | 49 | |
| ZHAW_Prometheus | 15:26dbcd6ff48d | 50 | //Zustände an der Klappe abrufen und Klappe öffnen und schliessen |
| ZHAW_Prometheus | 15:26dbcd6ff48d | 51 | sum = 0; |
| ZHAW_Prometheus | 16:ad45ef4fee04 | 52 | uint8_t i = 0; |
| ZHAW_Prometheus | 16:ad45ef4fee04 | 53 | while (i<m_verzAus) { |
| ZHAW_Prometheus | 15:26dbcd6ff48d | 54 | if ((p2+i) < merkerSize) { |
| ZHAW_Prometheus | 15:26dbcd6ff48d | 55 | sum += merker[p2+i]; |
| ZHAW_Prometheus | 13:7ce78048b733 | 56 | } else { |
| ZHAW_Prometheus | 15:26dbcd6ff48d | 57 | sum += merker[p2+i-merkerSize]; |
| ZHAW_Prometheus | 1:5c44e2462a8b | 58 | } |
| ZHAW_Prometheus | 16:ad45ef4fee04 | 59 | i++; |
| ZHAW_Prometheus | 16:ad45ef4fee04 | 60 | } |
| ZHAW_Prometheus | 16:ad45ef4fee04 | 61 | |
| ZHAW_Prometheus | 16:ad45ef4fee04 | 62 | //Klappe öffnen oder schliessen |
| ZHAW_Prometheus | 16:ad45ef4fee04 | 63 | if (sum > 0) { |
| ZHAW_Prometheus | 16:ad45ef4fee04 | 64 | servoAusw = 1.0f; |
| ZHAW_Prometheus | 16:ad45ef4fee04 | 65 | } else { |
| ZHAW_Prometheus | 16:ad45ef4fee04 | 66 | servoAusw = 0.01f; |
| ZHAW_Prometheus | 16:ad45ef4fee04 | 67 | } |
| ZHAW_Prometheus | 16:ad45ef4fee04 | 68 | |
| ZHAW_Prometheus | 16:ad45ef4fee04 | 69 | //Liftmodul alle vier Durchgänge aufrufen |
| ZHAW_Prometheus | 16:ad45ef4fee04 | 70 | if (p1%4 == 0) { |
| ZHAW_Prometheus | 16:ad45ef4fee04 | 71 | lift.steuerung(); |
| ZHAW_Prometheus | 16:ad45ef4fee04 | 72 | } |
| ZHAW_Prometheus | 16:ad45ef4fee04 | 73 | |
| ZHAW_Prometheus | 16:ad45ef4fee04 | 74 | //Zähler aufsummieren oder zurücksetzen. |
| ZHAW_Prometheus | 16:ad45ef4fee04 | 75 | if (p1 < (merkerSize-1)) { |
| ZHAW_Prometheus | 16:ad45ef4fee04 | 76 | p1++; |
| ZHAW_Prometheus | 16:ad45ef4fee04 | 77 | } else { |
| ZHAW_Prometheus | 16:ad45ef4fee04 | 78 | p1 = 0; |
| ZHAW_Prometheus | 15:26dbcd6ff48d | 79 | } |
| ZHAW_Prometheus | 15:26dbcd6ff48d | 80 | if (p2 < (merkerSize-1)) { |
| ZHAW_Prometheus | 15:26dbcd6ff48d | 81 | p2++; |
| ZHAW_Prometheus | 13:7ce78048b733 | 82 | } else { |
| ZHAW_Prometheus | 15:26dbcd6ff48d | 83 | p2 = 0; |
| schuema4 | 3:017c85c4b14b | 84 | } |
| ZHAW_Prometheus | 13:7ce78048b733 | 85 | } |