Diff: main.cpp

Revision:

3:5388a3ec580a

Parent:

2:035fc65548f6

Child:

4:52685928a7c3

--- a/main.cpp	Mon May 08 16:26:16 2017 +0000
+++ b/main.cpp	Tue May 09 12:51:23 2017 +0000
@@ -72,44 +72,12 @@
         valInG = Sensor1*100.0;
         valInR = Sensor2*100.0;
         
-        if (valInG>=70.5 && valInG<=72.0 && valInR>=53 && valInR<=54){
-            zustand = gruen;
-            }
-            else{
-                zustand = rot;
-            }
-        if(valInG>=77.8 && valInG<=79 && valInR>=72 && valInR<=74){
-            zustand = gruen;
-            }
-            else {
-                zustand = rot;
-            }
-        if(valInG>=76 && valInG<=77 && valInR>=64 && valInR<=68){
-            zustand = gruen;
-            }
-            else{
-                zustand = rot;
-            }
+     
             
         
         
         
 
-      /*  if ((valInG >= 596 && valInG <= 665) && (valInR >=370 && valInR <= 470)) {
-            //roter Klotz
-            zustand = 1;
-             Servo1 =0.8f;      //Grenze 0..7.4
-        } else {
-            if ((valInG >= 370 && valInG <= 380) && (valInR >=400 && valInR <= 610)) {
-                //gruener Klotz
-                zustand = 2;
-                 Servo1 =0.3f;      //Grenze 0..7.4
-            } else {
-                //kein Klotz oder falscher Klotz
-                 Servo1 =0.8f;      //Grenze 0..7.4
-                zustand = 0;
-            }
-        }*/
 
 //State setzen****************************************************************
         switch (robot_state) {
@@ -249,7 +217,27 @@
         }
         
     // Auswurf ansteuern******************************************************* 
-       
+       if (valInG>=70.5 && valInG<=77.0 && valInR>=52.5 && valInR<=54.5){      //Lego Normal
+            zustand = gruen;
+                        }
+            else{
+                zustand = rot;
+            }
+            /*
+        else if (valInG>=77.8 && valInG<=79 && valInR>=72 && valInR<=74){         // Lego Aufrecht Boden
+            zustand = gruen;
+            }
+            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
             merker_gruen ++;
             robot_state=Lego_G;
@@ -266,8 +254,15 @@
             Servo1 =0.1f;      //Grenze 0..7.4
             robot_state=Lego_R;
             }
-            
-            
+           */
+           if(zustand == gruen){
+             Servo1 = 1.1f;
+             robot_state=Lego_G;
+             }
+             else{
+                 Servo1 = 0.1f;
+             robot_state=Lego_R;
+            }
             
     //Ausgaben an Konsole******************************************************
 
@@ -277,6 +272,7 @@
         //printf("left:%f\n\r", sensors[5].read());
         printf("Gruen:%f\n\r", valInG);
         printf("Rot:%f\n\r", valInR);
+        printf("Status:%d",zustand);
 
         wait(0.001);