File content as of revision 12:472b26872a42:

#include "mbed.h"
#include "cstdlib"
//#include <cmath>
#include "Farbauswertung.h"

//Konstruktor
Farbauswertung::Farbauswertung(AnalogIn* SensorG, AnalogIn* SensorR, Servo* servoAusw, Servo* servoFoerder, Button& _onoff) : onoff(_onoff)
{
    this->SensorG = SensorG;
    this->SensorR = SensorR;
    this->servoAusw = servoAusw;
    this->servoFoerder = servoFoerder;
    farbsensor.init(SensorG, SensorR);
    zustand = gruen;
    merker_rot = 0;
    merker_rot1 = 0;
    merker_gruen =0;
    ausschaltZeit = 0;
    einschlatZeit = 50;
}

//Destruktor
Farbauswertung::~Farbauswertung() {}

void Farbauswertung::setSerialOutput(Serial *pc)
{
    this->pc = pc;
}

//Methoden
void Farbauswertung::printState()
{
    if (pc) {
        pc->printf("\n\r");
        pc->printf("Status:%d\n\r",zustand);
        pc->printf("Status Merker gruen:%d\n\r",merker_gruen);
        pc->printf("Status Merker rt:%d\n\r",merker_rot);
        pc->printf("Status Merker rot1:%d\n\r",merker_rot1);
        pc->printf("Ausschaltzeit:%d\n\r",ausschaltZeit);
    }
}

void Farbauswertung::printSens()
{
    if (pc) {
        pc->printf("Gruen:%f\n\r", farbsensor.readg());
        pc->printf("Rot:%f\n\r", farbsensor.readr());
    }
}


void Farbauswertung::auswertung()
{
    merker_gruen=0;
    if ((farbsensor.readg()>=68 && farbsensor.readg()<=70 && farbsensor.readr()>=70 && farbsensor.readr()<=71.6)|| (farbsensor.readg()>=77.2 && farbsensor.readg()<=77.39 && farbsensor.readr()>=75 && farbsensor.readr()<=77) || (farbsensor.readg()>=75 && farbsensor.readg()<=76 && farbsensor.readr()>=73 && farbsensor.readr()<=74)) {

        merker_gruen=1;


        if (zustand ==1 && ausschaltZeit ==0) {   //Wenn wechsel Grün auf Rot Ausschaltzeit um X erhöhen
            ausschaltZeit+=180;

            zustand =0;
        }
        if(zustand ==1 && ausschaltZeit !=0) { //Wenn die einschaltzeit nicht gleich Null ist muss die zeit plus die Einschaltverzögerung verlängert werden
            ausschaltZeit+=(180);

            zustand =0;

        }
        zustand =0;
        wait(0.5);

    }
    if(merker_gruen==0) {

        zustand=1; //Grünes oder gar kein Lego
    }

    if(zustand == 0 || (merker_rot >=1 && merker_rot < einschlatZeit)) { //Einschaltverzögerung
        merker_rot ++;
    }
    if (merker_rot == einschlatZeit) { //wenn einschaltzeit abgelaufen => MerkerRot1 setzen + MerkerRot zurücksetzen
        merker_rot1=1;
        merker_rot=0;
    }

    if(merker_rot1>=1 && merker_rot1<ausschaltZeit) { // Ausschaltverzögerung +  Auswurf schliessen
        merker_rot1 ++;

        servoAusw->write(1.0f);
    }
    if (merker_rot1 == ausschaltZeit) { //wenn Ausschaltzeit abgelaufen => MerkerRot1 zurücksetzen + Auswurf öffnen
        merker_rot1=0;
        ausschaltZeit=0;
        servoAusw->write(0.01f);
    }


    if (onoff.getState() != 0) {
        servoFoerder->write(1.2f);
    } else {
        servoFoerder->write(0.74f);
    }
}