File content as of revision 0:422088ad7fc5:

#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 = rot;
}

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

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

void Farbauswertung::auswertung() {
    
    if (farbsensor.readg()>=70.5 && farbsensor.readg()<=77.0 && farbsensor.readr()>=52.5 && farbsensor.readr()<=54.5){      //Lego Normal
        zustand = gruen;
    } else {
            zustand = rot;
            }
            /*
        else if (farbsensor.readg()>=77.8 && farbsensor.readg()<=79 && farbsensor.readr()>=72 && farbsensor.readr()<=74){         // Lego Aufrecht Boden
            zustand = gruen;
            }
            else {
                zustand = rot;
            }
         else if(farbsensor.readg()>=76 && farbsensor.readg()<=77 && farbsensor.readr()>=64 && farbsensor.readr()<=68){           // Lego Aufrecht Deckel
            zustand = gruen;
            }
            else{
                zustand = rot;
            }
            */
      /* 
        if(zustand = gruen || (merker_gruen >=1 && merker_gruen < 300)){ //Einschaltverzögerung
            merker_gruen ++;
            robot_state=Lego_G;
        }
        
        else if(merker_gruen == 300 || (merker_gruen1 >=1 && merker_gruen1 < 300)){ // Ausschaltverzögerung
            merker_gruen =0;
            merker_gruen1 ++;
             Servo1 = 1.1f;
             robot_state=Lego_G;
            }
            
        else if(zustand = rot && merker_gruen == 0 && merker_gruen1 ==0){
            Servo1 =0.1f;      //Grenze 0..7.4
            robot_state=Lego_R;
            }
           */
           if(zustand == gruen){
             servoAusw->write(1.1f);
             //robot_state=Lego_G;
             }
             else{
                 servoAusw->write(0.1f);
             //robot_state=Lego_R;
            }
            
    //Ausgaben an Konsole******************************************************
    //if (pc) {
        pc->printf("\n\r");
       // pc.printf("front:%f\n\r", sensors[0].read());
        //pc.printf("right:%f\n\r", sensors[1].read());
        //pc.printf("left:%f\n\r", sensors[5].read());
        pc->printf("Gruen:%f\n\r", farbsensor.readg());
        //pc->printf("Zusatzzeile");
        pc->printf("Rot:%f\n\r", farbsensor.readr());
        pc->printf("Status:%d",zustand);
    //}
    
}