 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;        
-unsigned long previousMillisStappenmotor = 0;        
-
-const long interval = 20;           
-const long intervalStappenmotor = 10;   
+unsigned long previousMillis = 0;              
+
+const long interval = 10;           
 
 #define echoPin 5          // Echo Pin
 #define trigPin 4          // Trigger Pin







void 1draaiNaarBoven(){
  if (currentMillis - previousMillisStappenmotor >= intervalStappenmotor && currentMillis - previousMillisStappenmotor < intervalStappenmotor+10) {

    digitalWrite(coila1, HIGH);
    digitalWrite(coila2, LOW);
    digitalWrite(coilb1, LOW);
    digitalWrite(coilb2, LOW);
  }
  if (currentMillis - previousMillisStappenmotor >= intervalStappenmotor && currentMillis - previousMillisStappenmotor < intervalStappenmotor+20) {

    digitalWrite(coila1, HIGH);
    digitalWrite(coila2, LOW);
    digitalWrite(coilb1, HIGH);
    digitalWrite(coilb2, LOW);
  }
  if (currentMillis - previousMillisStappenmotor >= intervalStappenmotor && currentMillis - previousMillisStappenmotor < intervalStappenmotor+30) {

    digitalWrite(coila1, LOW);
    digitalWrite(coila2, LOW);
    digitalWrite(coilb1, HIGH);
    digitalWrite(coilb2, LOW);
  }
  if (currentMillis - previousMillisStappenmotor >= intervalStappenmotor && currentMillis - previousMillisStappenmotor < intervalStappenmotor+40) {

    digitalWrite(coila1, LOW);
    digitalWrite(coila2, HIGH);
    digitalWrite(coilb1, HIGH);
    digitalWrite(coilb2, LOW);
  }
  if (currentMillis - previousMillisStappenmotor >= intervalStappenmotor && currentMillis - previousMillisStappenmotor < intervalStappenmotor+50) {

    digitalWrite(coila1, LOW);
    digitalWrite(coila2, HIGH);
    digitalWrite(coilb1, LOW);
    digitalWrite(coilb2, LOW);
  }
  if (currentMillis - previousMillisStappenmotor >= intervalStappenmotor && currentMillis - previousMillisStappenmotor < intervalStappenmotor+60) {

    digitalWrite(coila1, LOW);
    digitalWrite(coila2, HIGH);
    digitalWrite(coilb1, LOW);
    digitalWrite(coilb2, HIGH);
  }
  if (currentMillis - previousMillisStappenmotor >= intervalStappenmotor && currentMillis - previousMillisStappenmotor < intervalStappenmotor+70) {

    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);
  }
}


  else if (reader == 2) { myservo.write(100); delay(500); reader = -1; }
  else if (reader == 3) { myservo.write(0); delay(1000); reader = -1; }
  else if (reader == 4) { myservo.write(180); delay(1000); reader = -1; }
  else if (reader == 6) { myservo.write(180); draainaarbeneden = 10; reader = -1; }
  else if (reader == 7) { myservo.write(180); draainaarboven =10; reader = -1; }
  else{
    
     if (currentMillis - previousMillis >= interval) {
        Distance();
            if(draainaarboven>0){
              digitalWrite(enable, HIGH);
        
              switch(positie){
