Diff: Farbauswertung.cpp

Revision:

1:5c44e2462a8b

Parent:

0:422088ad7fc5

Child:

3:017c85c4b14b

diff -r 422088ad7fc5 -r 5c44e2462a8b Farbauswertung.cpp
--- a/Farbauswertung.cpp	Wed May 10 08:59:22 2017 +0000
+++ b/Farbauswertung.cpp	Wed May 10 14:48:50 2017 +0000
@@ -25,6 +25,10 @@
     this->servoAusw = servoAusw;
     farbsensor.init(SensorG, SensorR);
     zustand = rot;
+    merker_gruen = 0;
+    merker_gruen1 = 0;
+    ausschaltZeit = 0;
+    einschlatZeit = 2;
 }
 
 void Farbauswertung::setSerialOutput(Serial *pc)
@@ -38,62 +42,51 @@
 
 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;
+    if ((farbsensor.readg()>=70.5 && farbsensor.readg()<=77.0 && farbsensor.readr()>=52.5 && farbsensor.readr()<=55) || (farbsensor.readg()>=77.8 && farbsensor.readg()<=79 && farbsensor.readr()>=73 && farbsensor.readr()<=74) || (farbsensor.readg()>=76 && farbsensor.readg()<=78.2 && farbsensor.readr()>=64.6 && farbsensor.readr()<=65.5)){      
+           if(zustand==rot){
+            ausschaltZeit+=5;
             }
-            /*
-        else if (farbsensor.readg()>=77.8 && farbsensor.readg()<=79 && farbsensor.readr()>=72 && farbsensor.readr()<=74){         // Lego Aufrecht Boden
             zustand = gruen;
-            }
-            else {
-                zustand = rot;
+            if(merker_gruen==0){
+            merker_gruen=1;
             }
-         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
+           
+           
+
+        
+        if(zustand == gruen || (merker_gruen >=1 && merker_gruen < einschlatZeit)){ //Einschaltverzögerung
             merker_gruen ++;
-            robot_state=Lego_G;
+            //robot_state=Lego_G;
         }
+        if (merker_gruen1 == ausschaltZeit){                            //Merker zurücksetzen
+            merker_gruen1=0;
+            }
         
-        else if(merker_gruen == 300 || (merker_gruen1 >=1 && merker_gruen1 < 300)){ // Ausschaltverzögerung
+        else if(merker_gruen == einschlatZeit || (merker_gruen1 >=1 && merker_gruen1 < ausschaltZeit)){ // Ausschaltverzögerung
             merker_gruen =0;
             merker_gruen1 ++;
-             Servo1 = 1.1f;
-             robot_state=Lego_G;
+             servoAusw->write(1.0f);
+             //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;
+        else if(zustand == rot && merker_gruen == 0 && merker_gruen1 ==0){
+            servoAusw->write(0.1f);      //Grenze 0..7.4
+            //robot_state=Lego_R;
+            ausschaltZeit = 0;
             }
             
     //Ausgaben an Konsole******************************************************
-    //if (pc) {
+    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);
-    //}
+        pc->printf("Status:%d\n\r",zustand);
+        pc->printf("Status Merker:%d\n\r",merker_gruen);
+        pc->printf("Status Merker1:%d\n\r",merker_gruen1);
+    }
     
 }
\ No newline at end of file