diff --git a/prog/prog.ino b/prog/prog.ino
index 501ec39..8766946 100644
--- a/prog/prog.ino
+++ b/prog/prog.ino
@@ -9,30 +9,18 @@ Servo servo1;
 const int SDL = 22;
 const int SDA2 = 21; // useless in the code
 
-const int minHeight = 5;
-const int maxHeight = 600;
+const int minHeight = 50;
+const int maxHeight = 455;
 
-int minServo = 0;
-int maxServo = 90;
-
-float curHeight = 0; 
-
-void setup() {
-  Serial.begin(115200); // Starts the serial communication
-  while (! Serial) {
-    delay(1);
-  }
-  servo1.attach(servoPin);
-
-  Serial.println("Adafruit VL53L0X test");
-  if (!lox.begin()) {
-    Serial.println(F("Failed to boot VL53L0X"));
-    while(1);
-  }
-  Serial.println(F("VL53L0X API Simple Ranging example\n\n")); 
-}
+int minServo = 30;
+int maxServo = 60;
+int lastAngle = -1;
 
 
+float curHeight = 50;
+float targetHeight = 250;
+
+float equilibrium = 0;
 
 auto mesureHeight(){
   VL53L0X_RangingMeasurementData_t measure;
@@ -53,8 +41,45 @@ auto mesureHeight(){
   }
 }
 
+void setup() {
+  Serial.begin(115200); // Starts the serial communication
+  while (! Serial) {
+    delay(1);
+  }
+  servo1.attach(servoPin);
+
+  Serial.println("Adafruit VL53L0X test");
+  if (!lox.begin()) {
+    Serial.println(F("Failed to boot VL53L0X"));
+    while(1);
+  }
+  Serial.println(F("VL53L0X API Simple Ranging example\n\n")); 
+
+  Serial.println("Setuping");
+  servo1.write(minServo);
+  delay(2000);
+  float h = mesureHeight();
+  if (h < maxHeight) {
+    Serial.println("CHANGE FAN SPEED");
+    while (1);
+  }
+  Serial.println("Setuping for downwards");
+  int angle = minServo;
+  while (h > maxHeight) {
+    ++angle;
+    servo1.write(angle);
+    delay(100);
+    h = mesureHeight();
+  }
+  angle -= 2;
+  servo1.write(angle);
+  equilibrium = angle;
+  Serial.println("Done setuping have funning");
+}
+
+
 auto heightRatio(float height){
-  return min(1.0f, (height - minHeight) / (maxHeight-minHeight));
+  return max(min(1.0f, (height - minHeight) / (maxHeight-minHeight)), 0.0f);
 }
 
 int servoPosition(float ratio){
@@ -62,20 +87,13 @@ int servoPosition(float ratio){
 }
 
 void loop() {
-  
    
   float h = mesureHeight();
-  float ratio = heightRatio(h);
-  int servoRot = servoPosition(ratio);
-  
-  /*
-  Serial.print("h: ");
-  Serial.print(h);
-  Serial.print(" ratio: ");
-  Serial.print(ratio);
-  Serial.print("rotation: ");
-  Serial.println(servoRot);
-*/
-  servo1.write(servoRot);
+  Serial.print("Measure = ");
+  Serial.println(h);
+  if (abs(h - targetHeight) > 1.0f) {
+    servo1.write(equilibrium + 5);
+  }
+
   delay(1);
 }
\ No newline at end of file