diff --git a/config.ini b/config.ini
index 89f0c25..5052dee 100644
--- a/config.ini
+++ b/config.ini
@@ -27,3 +27,8 @@ build_flags =
 
     ; nfc addr
     -D NFC_ADDR=0x28
+    ;;;  Servo config  ;;;
+    ;;;;;;;;;;;;;;;;;;;;;;
+    -D RIGHT_POS=18
+    -D MIDDLE_POS=32
+    -D LEFT_POS=52
diff --git a/lib/ServoMotorComponent/src/ServoMotorComponent.cpp b/lib/ServoMotorComponent/src/ServoMotorComponent.cpp
index d23dd40..950e1c7 100644
--- a/lib/ServoMotorComponent/src/ServoMotorComponent.cpp
+++ b/lib/ServoMotorComponent/src/ServoMotorComponent.cpp
@@ -13,7 +13,7 @@ ServoMotorComponent::ServoMotorComponent(int PIN,  unsigned long updatePeriod, f
     this->PIN = PIN;
     this->myservo.attach(PIN);
     this->desiredposition = Position::MIDDLE;
-    this->currentPosition = 32;
+    this->currentPosition = MIDDLE_POS;
     this->lastUpTime = millis();
     this->updatePeriod = updatePeriod;
     this->step = step;
@@ -27,13 +27,13 @@ ServoMotorComponent::ServoMotorComponent(int PIN,  unsigned long updatePeriod, f
 void ServoMotorComponent::setDesiredPosition(Position desiredPosition) {
     switch (desiredPosition) {
         case Position::LEFT:
-            this->desiredposition = 52;
+            this->desiredposition = LEFT_POS;
             break;
         case Position::MIDDLE:
-            this->desiredposition = 32;
+            this->desiredposition = MIDDLE_POS;
             break;
         case Position::RIGHT:
-            this->desiredposition = 18;
+            this->desiredposition = RIGHT_POS;
             break;
         default:
             break;
@@ -49,10 +49,10 @@ void ServoMotorComponent::refresh() {
     ||  millis() - this->lastUpTime <= this->updatePeriod) return;
 
     if (this->currentPosition > this->desiredposition) {
-        this->currentPosition--;
+        this->currentPosition -= this->step;
     }
     if (this->currentPosition < this->desiredposition) {
-        this->currentPosition++;
+        this->currentPosition += this->step;
     }
     this->lastUpTime = millis();
     this->myservo.write(this->PIN, this->currentPosition);
diff --git a/src/Program.cpp b/src/Program.cpp
index 8454899..0126fe1 100644
--- a/src/Program.cpp
+++ b/src/Program.cpp
@@ -1,6 +1,7 @@
 #include "Program.h"
 #include "Arduino.h"
 #include "DolibarrClient.h"
+#include "ServoMotorComponent.h"
 
 int initialize_wifi(WifiConfig wifi) {
     WiFiClass::mode(WIFI_STA); //Optional
@@ -17,13 +18,16 @@ int initialize_wifi(WifiConfig wifi) {
 
 Program::Program() {
     Serial.begin(MONITOR_SPEED);
-    struct WifiConfig wifi_c = {WIFI_SSID, WIFI_PASSWORD};
-    struct DolibarrConfig dolibarr = {DOLIBARR_URL, DOLIBARR_API_TOKEN};
-    initialize_wifi(wifi_c);
-    this->client = new DolibarrClient(dolibarr);
+    this->servo = new ServoMotorComponent(2, 1, 0.1);
+    //struct WifiConfig wifi_c = {WIFI_SSID, WIFI_PASSWORD};
+    //struct DolibarrConfig dolibarr = {DOLIBARR_URL, DOLIBARR_API_TOKEN};
+    //initialize_wifi(wifi_c);
+    //this->client = new DolibarrClient(dolibarr);
 }
 
 void Program::loop() {
+    this->servo->refresh();
+    this->servo->setDesiredPosition(Position::LEFT);
 }