File content as of revision 9:b83994ef4b08:

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


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


/**
 * 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 = 50;
}

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

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

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);
    }

    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 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);
        }
    }

}