     }
   }
   else if (servo_state == 3) {
-    if (servo_pos >= 0) {
-      servo_pos -= 1;
+    if (servo_pos < 90) {
+      servo_pos += 1;
       servo.write(servo_pos);
     } 
     else {
+      servo_state = 4;
+      servo_delay = 0;
+    }
+  } 
+  else if (servo_state == 4) {
+    if (servo_delay > 200) {
       servo_state = 1;
+    } 
+    else {
+      servo_delay += 1;
     }
   }
 }

    }
  }
  else if (servo_state == 2) {
    if (servo_delay >= 200) {
      servo_state = 3;
    } 
    else {

    if (servo_state != 0) {
      Serial.println("Servo disabled");
      digitalWrite(LED_PIN, LOW);
      beep(50);
      servo_state = 0;
    } 
    else {
      Serial.println("Servo enabled");
      digitalWrite(LED_PIN, HIGH);
      beep(50);
      servo_state = 1;
    }

    while (digitalRead(BUTTON_PIN) == HIGH);
  }
}

void beep(unsigned char delayms) {
  analogWrite(SPEAKER_PIN, 20);
  delay(delayms);
  analogWrite(SPEAKER_PIN, 0);
  delay(delayms);
}


