Diff: main.cpp

Revision:

4:52685928a7c3

Parent:

3:5388a3ec580a

--- a/main.cpp	Tue May 09 12:51:23 2017 +0000
+++ b/main.cpp	Wed May 10 11:37:42 2017 +0000
@@ -34,7 +34,8 @@
 int merker_gruen1 = 0;
 double wenden =0.26;
 double wand =0.07;
-
+int einschlatZeit = 2;
+int ausschaltZeit =0;
 
 int main()
 {
@@ -217,64 +218,59 @@
         }
         
     // Auswurf ansteuern******************************************************* 
-       if (valInG>=70.5 && valInG<=77.0 && valInR>=52.5 && valInR<=54.5){      //Lego Normal
-            zustand = gruen;
-                        }
-            else{
-                zustand = rot;
+   
+    
+       if ((valInG>=70.5 && valInG<=77.0 && valInR>=52.5 && valInR<=55) || (valInG>=77.8 && valInG<=79 && valInR>=73 && valInR<=74) || (valInG>=76 && valInG<=78.2 && valInR>=64.6 && valInR<=65.5)){      
+           if(zustand==rot){
+            ausschaltZeit+=5;
             }
-            /*
-        else if (valInG>=77.8 && valInG<=79 && valInR>=72 && valInR<=74){         // Lego Aufrecht Boden
             zustand = gruen;
+            if(merker_gruen==0){
+            merker_gruen=1;
             }
-            else {
-                zustand = rot;
-            }
-         else if(valInG>=76 && valInG<=77 && valInR>=64 && valInR<=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;
         }
+        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;
+             Servo1 = 1.4f;
              robot_state=Lego_G;
             }
             
-        else if(zustand = rot && merker_gruen == 0 && merker_gruen1 ==0){
+        else if(zustand == rot && merker_gruen == 0 && merker_gruen1 ==0){
             Servo1 =0.1f;      //Grenze 0..7.4
             robot_state=Lego_R;
+            ausschaltZeit = 0;
             }
-           */
-           if(zustand == gruen){
-             Servo1 = 1.1f;
-             robot_state=Lego_G;
-             }
-             else{
-                 Servo1 = 0.1f;
-             robot_state=Lego_R;
-            }
-            
+         
     //Ausgaben an Konsole******************************************************
 
         printf("\n\r");
        // printf("front:%f\n\r", sensors[0].read());
         //printf("right:%f\n\r", sensors[1].read());
         //printf("left:%f\n\r", sensors[5].read());
-        printf("Gruen:%f\n\r", valInG);
-        printf("Rot:%f\n\r", valInR);
-        printf("Status:%d",zustand);
+       printf("Gruen:%f\n\r", valInG);
+       printf("Rot:%f\n\n\r", valInR);
+       printf("Status:%d\n\r",zustand);
+      // printf("Merker1:%d\n\r",merker_gruen);
+      // printf("Merker2:%d\n\r",merker_gruen1);
+      // printf("Merker2:%d\n\r",ausschaltZeit);
 
-        wait(0.001);
+        wait(0.5);
 
     }