From 567f8e96a226950289f7387e445a8bde464020a9 Mon Sep 17 00:00:00 2001
From: Christoffer Martinsson <christoffer.martinsson@mildef.com>
Date: Tue, 13 Feb 2018 11:47:33 +0100
Subject: [PATCH] Updated test code

---
 zuno/zuno.ino | 15 +++++++++++----
 1 file changed, 11 insertions(+), 4 deletions(-)

diff --git a/zuno/zuno.ino b/zuno/zuno.ino
index 16743f2..b964fbb 100644
--- a/zuno/zuno.ino
+++ b/zuno/zuno.ino
@@ -37,7 +37,7 @@ void lock(int mode){
 	}
 	digitalWrite(SERVO_ENABLE_PIN, HIGH);
 	servo.begin();
-	lock_timestamp = current_timestamp + 1000;
+	lock_timestamp = current_timestamp + 500;
 	lock_status = mode;
 }
 
@@ -54,6 +54,7 @@ void setup() {
 	pinMode(SERVO_ENABLE_PIN, OUTPUT); // setup pin as output
 	digitalWrite(SERVO_ENABLE_PIN, HIGH);
 	
+	servo.setValue(90);
 	servo.begin();
 }
 // the loop routine runs over and over again forever:
@@ -63,9 +64,15 @@ void loop() {
 	// Lock disable timeout	
 	if (lock_status > 0) {
 		if (current_timestamp >= lock_timestamp) {
-			servo.end();
-			digitalWrite(SERVO_ENABLE_PIN, LOW);
-			lock_status = 0;
+			if (lock_status < 3) {
+				servo.setValue(90);
+				lock_timestamp = current_timestamp + 500;
+				lock_status = 3;
+			}else{	
+				servo.end();
+				digitalWrite(SERVO_ENABLE_PIN, LOW);
+				lock_status = 0;
+			}
 		}
 	}
 	// Heartbeat