File content as of revision 6:d611637e7cad:

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


Farbauswertung::Farbauswertung(AnalogIn* SensorG, AnalogIn* SensorR, Servo* servoAusw)
{
    init(SensorG, SensorR, servoAusw);
}


Farbauswertung::Farbauswertung() {}

/**
 * Deletes the IRSensor object.
 */
Farbauswertung::~Farbauswertung() {}


void Farbauswertung::init(AnalogIn* SensorG, AnalogIn* SensorR, Servo* servoAusw)
{
    this->SensorG = SensorG;
    this->SensorR = SensorR;
    this->servoAusw = servoAusw;
    farbsensor.init(SensorG, SensorR);
    zustand = gruen;
    merker_rot = 0;
    merker_rot1 = 0;
    merker_gruen =0;
    ausschaltZeit = 0;
    einschlatZeit = 120;
}

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

int Farbauswertung::getState()
{
    return zustand;
}

void Farbauswertung::auswertung()
{

    if ((farbsensor.readg()>=68 && farbsensor.readg()<=70.5 && farbsensor.readr()>=69 && farbsensor.readr()<=72) || (farbsensor.readg()>=77 && farbsensor.readg()<=78 && farbsensor.readr()>=74 && farbsensor.readr()<=77) || (farbsensor.readg()>=75 && farbsensor.readg()<=76 && farbsensor.readr()>=75 && farbsensor.readr()<=77)) {

        zustand =rot;
        if (merker_gruen==1) {   //Wenn wechsel Grün auf Rot Ausschaltzeit um X erhöhen
            ausschaltZeit+=100;
            if(ausschaltZeit !=0) { //Wenn die einschaltzeit nicht gleich Null ist muss die zeit plus die Einschaltverzögerung verlängert werden
                ausschaltZeit+=(einschlatZeit+100);
            }
        }
        merker_gruen = 0;
    } else {
        merker_gruen=1;
        zustand=gruen; //Grünes oder gar kein Lego
    }

    if(zustand == rot || (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);
    }

    static int timer = 0;
    ++timer;

    if( timer % 100 == 0) {
        //Ausgaben an Konsole******************************************************
        if (pc) {
            pc->printf("\n\r");
            pc->printf("Gruen:%f\n\r", farbsensor.readg());
            pc->printf("Rot:%f\n\r", farbsensor.readr());
            pc->printf("Status:%d\n\r",zustand);
            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);
        }
    }

}