   Serial.println(distance);
 }
 
+void 1draaiNaarBeneden(){
+  if (currentMillis - previousMillisStappenmotor >= intervalStappenmotor) {
+    previousMillisStappenmotor = currentMillis;
+    digitalWrite(coila1, HIGH);
+    digitalWrite(coila2, LOW);
+    digitalWrite(coilb1, LOW);
+    digitalWrite(coilb2, HIGH);
+    delay(10);
+  }
+
+  if (currentMillis - previousMillisStappenmotor >= intervalStappenmotor) {
+  previousMillisStappenmotor = currentMillis;
+  
+    digitalWrite(coila1, LOW);
+    digitalWrite(coila2, LOW);
+    digitalWrite(coilb1, LOW);
+    digitalWrite(coilb2, HIGH);
+  }
+  if (currentMillis - previousMillisStappenmotor >= intervalStappenmotor) {
+  previousMillisStappenmotor = currentMillis;
+    digitalWrite(coila1, LOW);
+    digitalWrite(coila2, HIGH);
+    digitalWrite(coilb1, LOW);
+    digitalWrite(coilb2, HIGH);
+    delay(10);
+  }
+  if (currentMillis - previousMillisStappenmotor >= intervalStappenmotor) {
+  previousMillisStappenmotor = currentMillis;
+    digitalWrite(coila1, LOW);
+    digitalWrite(coila2, HIGH);
+    digitalWrite(coilb1, LOW);
+    digitalWrite(coilb2, LOW);
+  }
+  if (currentMillis - previousMillisStappenmotor >= intervalStappenmotor) {
+  previousMillisStappenmotor = currentMillis;
+    digitalWrite(coila1, LOW);
+    digitalWrite(coila2, HIGH);
+    digitalWrite(coilb1, HIGH);
+    digitalWrite(coilb2, LOW);
+  }
+  if (currentMillis - previousMillisStappenmotor >= intervalStappenmotor) {
+  previousMillisStappenmotor = currentMillis;
+    digitalWrite(coila1, LOW);
+    digitalWrite(coila2, LOW);
+    digitalWrite(coilb1, HIGH);
+    digitalWrite(coilb2, LOW);
+  }
+  if (currentMillis - previousMillisStappenmotor >= intervalStappenmotor) {
+  previousMillisStappenmotor = currentMillis;
+    digitalWrite(coila1, HIGH);
+    digitalWrite(coila2, LOW);
+    digitalWrite(coilb1, HIGH);
+    digitalWrite(coilb2, LOW);
+  }
+  if (currentMillis - previousMillisStappenmotor >= intervalStappenmotor) {
+  previousMillisStappenmotor = currentMillis;
+    digitalWrite(coila1, HIGH);
+    digitalWrite(coila2, LOW);
+    digitalWrite(coilb1, LOW);
+    digitalWrite(coilb2, LOW);
+  }
+}
+
+void 1draaiNaarBoven(){
+  if (currentMillis - previousMillisStappenmotor >= intervalStappenmotor) {
+  previousMillisStappenmotor = currentMillis;
+    digitalWrite(coila1, HIGH);
+    digitalWrite(coila2, LOW);
+    digitalWrite(coilb1, LOW);
+    digitalWrite(coilb2, LOW);
+  }
+  if (currentMillis - previousMillisStappenmotor >= intervalStappenmotor) {
+  previousMillisStappenmotor = currentMillis;
+    digitalWrite(coila1, HIGH);
+    digitalWrite(coila2, LOW);
+    digitalWrite(coilb1, HIGH);
+    digitalWrite(coilb2, LOW);
+  }
+  if (currentMillis - previousMillisStappenmotor >= intervalStappenmotor) {
+  previousMillisStappenmotor = currentMillis;
+    digitalWrite(coila1, LOW);
+    digitalWrite(coila2, LOW);
+    digitalWrite(coilb1, HIGH);
+    digitalWrite(coilb2, LOW);
+  }
+  if (currentMillis - previousMillisStappenmotor >= intervalStappenmotor) {
+    previousMillisStappenmotor = currentMillis;
+    digitalWrite(coila1, LOW);
+    digitalWrite(coila2, HIGH);
+    digitalWrite(coilb1, HIGH);
+    digitalWrite(coilb2, LOW);
+  }
+  if (currentMillis - previousMillisStappenmotor >= intervalStappenmotor) {
+    previousMillisStappenmotor = currentMillis;
+    digitalWrite(coila1, LOW);
+    digitalWrite(coila2, HIGH);
+    digitalWrite(coilb1, LOW);
+    digitalWrite(coilb2, LOW);
+  }
+  if (currentMillis - previousMillisStappenmotor >= intervalStappenmotor) {
+    previousMillisStappenmotor = currentMillis;
+    digitalWrite(coila1, LOW);
+    digitalWrite(coila2, HIGH);
+    digitalWrite(coilb1, LOW);
+    digitalWrite(coilb2, HIGH);
+  }
+  if (currentMillis - previousMillisStappenmotor >= intervalStappenmotor) {
+    previousMillisStappenmotor = currentMillis;
+    digitalWrite(coila1, LOW);
+    digitalWrite(coila2, LOW);
+    digitalWrite(coilb1, LOW);
+    digitalWrite(coilb2, HIGH);
+  }
+  if (currentMillis - previousMillisStappenmotor >= intervalStappenmotor) {
+    previousMillisStappenmotor = currentMillis;
+    digitalWrite(coila1, HIGH);
+    digitalWrite(coila2, LOW);
+    digitalWrite(coilb1, LOW);
+    digitalWrite(coilb2, HIGH);
+  }
+}
+

int reader;
// Generally, you should use "unsigned long" for variables that hold time
// The value will quickly become too large for an int to store
unsigned long previousMillis = 0;        // will store last time LED was updated

// constants won't change :
const long interval = 500;           // interval at which to blink (milliseconds)

#define echoPin 5          // Echo Pin
#define trigPin 4          // Trigger Pin

  else if (reader == 3) { myservo.write(0); delay(550); reader = -1; }
  else if (reader == 4) { myservo.write(180); delay(550); reader = -1; }
  else{
    
     if (currentMillis - previousMillis >= interval) {
        Distance();
        previousMillis = currentMillis;
     }
    }  
}

void Distance() { // meet de afstand van de sensor
