 
 void loop(){
   measure();
+  tone(beepPin, 2000, 500);
   Serial.println(arrayToString(measurements,stepsInFullTurn));
   delay(1000);
   stepsToCylinder = findCylinder();
+  Serial.println("Cylinder at index: "+String(stepsToCylinder));
+  tone(beepPin, 2000, 500);
   for(int i=0; i<stepsToCylinder; i++){
     rotateStep(counterClockwise);
+  }
+  tone(beepPin, 5000, 500);
+  while(measurePingDist()>10){
+    moveForward();
   }
   delay(4000);
 }

// Rotates one step clockwise or counter-clockwise
void rotateStep(bool clockwise) {
  if(clockwise){
    servoLeft.writeMicroseconds(1700);
    servoRight.writeMicroseconds(1700);
  } else {
    servoLeft.writeMicroseconds(1300);
    servoRight.writeMicroseconds(1300);
  }
  delay(100);
  stopMovement();


  while(true){
    int nearestIndex = getIndexMin();
    
    Serial.println("Nearest index found: "+String(nearestIndex));
    if(nearestIndex == -1){
      return getIndexOfLargestOpenInterval();
    }
    int iL = prevIndex(nearestIndex,l);
    int iR = nextIndex(nearestIndex,l);
    int intervalLength = 0;
