      previousMillisSensor = currentMillisSensor; 
      Distance(); 
      
-     if (pos >= 55 && pos <= 125 && distance <= 10 && distance > 0) {
-        if(pos==55){
-        linksRijden==HIGH;
-         Serial.println(linksRijden);
+     if (pos >= 55 && pos <= 125 && distance <= 10 && distance > 0) {        
+       vooruitRijden = LOW;
+       //rondKijken = HIGH;
+       //vooruitKijken = LOW;
+       Serial.println("STOP");
       }
-        if(pos==125)rechtsRijden==HIGH;
-       // if(pos==90)vooruitRijden = LOW;
-
-     }  
+       
      else if (pos >= 55 && pos <= 125 && distance > 10 || distance == -1) {
 
        vooruitRijden = HIGH;
+       //rondKijken = LOW;
        Serial.println("DRIVE");
      }
-  }
+     }
     
   
   if(vooruitRijden){
          Snelheid = 2000;
          Stuur = 1500;
   }
-  if(rechtsRijden){
-      Serial.println("rechts");
-      Snelheid = 2000;
-      Stuur = 1300;
-
+  
+  if(rondKijken) {
+    pos += hoekServoRondkijken;
+       if(pos >= 180 || pos<= 0)hoekServoRondkijken = hoekServoRondkijken*-1;
+       //if (pos == 90 && distance > 10 || distance == -1) vooruitKijken = HIGH;
+       //if (pos == 0 && distance > 10 || distance == -1) { Stuur = 2000; Snelheid = 1500;}
     }
-  if(linksRijden){
-    Serial.println("links");
-    Snelheid = 2000;
-    Stuur = 1700;
-
-  }
-  
-  
-  
+    
    
     Pinkers(Stuur, currentMillis);
     //------------------------------------------------------------ Code voor RC inputs. ---------------------------------------------------------

bool achteruitRijden= LOW;
bool linksRijden = LOW;
bool rechtsRijden = LOW;

int LmotorA= 5;  // Left  motor H bridge, input A
int LmotorB = 6;  // Left  motor H bridge, input B

       previousMillisServo = currentMillisServo;     
       myservo.write(pos);  
       
       if(vooruitKijken){
       pos += hoekServo;
       if(pos >= 125 || pos<= 55)hoekServo = hoekServo*-1;
       }
       
       }
     }
