Diff: Farbauswertung.cpp

Revision:

3:017c85c4b14b

Parent:

1:5c44e2462a8b

Child:

6:d611637e7cad

--- a/Farbauswertung.cpp	Wed May 10 18:12:34 2017 +0000
+++ b/Farbauswertung.cpp	Sat May 13 12:52:51 2017 +0000
@@ -24,11 +24,12 @@
     this->SensorR = SensorR;
     this->servoAusw = servoAusw;
     farbsensor.init(SensorG, SensorR);
-    zustand = rot;
-    merker_gruen = 0;
-    merker_gruen1 = 0;
+    zustand = gruen;
+    merker_rot = 0;
+    merker_rot1 = 0;
+    merker_gruen =0;
     ausschaltZeit = 0;
-    einschlatZeit = 2;
+    einschlatZeit = 6;
 }
 
 void Farbauswertung::setSerialOutput(Serial *pc)
@@ -36,57 +37,57 @@
     this->pc = pc;
 }
 
-int Farbauswertung::getState() {
+int Farbauswertung::getState()
+{
     return zustand;
 }
 
-void Farbauswertung::auswertung() {
-    
-    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;
-            }
-            zustand = gruen;
-            if(merker_gruen==0){
-            merker_gruen=1;
+void Farbauswertung::auswertung()
+{
+
+    if ((farbsensor.readg()>=68 && farbsensor.readg()<=70 && 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+=2;
+            if(ausschaltZeit !=0) { //Wenn die einschaltzeit nicht gleich Null ist muss die zeit plus die Einschaltverzögerung verlängert werden
+                ausschaltZeit+=(einschlatZeit+2);
             }
         }
-            else{
-                zustand = rot;
-            }
-           
-           
+        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 ++;
         
-        if(zustand == gruen || (merker_gruen >=1 && merker_gruen < einschlatZeit)){ //Einschaltverzögerung
-            merker_gruen ++;
-            //robot_state=Lego_G;
-        }
-        if (merker_gruen1 == ausschaltZeit){                            //Merker zurücksetzen
-            merker_gruen1=0;
-            }
-        
-        else if(merker_gruen == einschlatZeit || (merker_gruen1 >=1 && merker_gruen1 < ausschaltZeit)){ // Ausschaltverzögerung
-            merker_gruen =0;
-            merker_gruen1 ++;
-             servoAusw->write(1.0f);
-             //robot_state=Lego_G;
-            }
-            
-        else if(zustand == rot && merker_gruen == 0 && merker_gruen1 ==0){
-            servoAusw->write(0.1f);      //Grenze 0..7.4
-            //robot_state=Lego_R;
-            ausschaltZeit = 0;
-            }
-            
+        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);
+    }
+
     //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:%d\n\r",merker_gruen);
-        pc->printf("Status Merker1:%d\n\r",merker_gruen1);
+        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);
     }
-    
+
 }
\ No newline at end of file