diff --git a/lib/ServoMotorComponent/src/ServoMotorComponent.cpp b/lib/ServoMotorComponent/src/ServoMotorComponent.cpp
index 2b9a515..d23dd40 100644
--- a/lib/ServoMotorComponent/src/ServoMotorComponent.cpp
+++ b/lib/ServoMotorComponent/src/ServoMotorComponent.cpp
@@ -1,22 +1,29 @@
 #include "ServoMotorComponent.h"
 
-ServoMotorComponent::ServoMotorComponent(int PIN,  unsigned long updatePeriod) {
+
+/////////////////////////////////////////////////////////////////////////////////////
+// Functions for setting up the ServoMotor
+/////////////////////////////////////////////////////////////////////////////////////
+
+/**
+ * Constructor.
+ * Prepares the ServoMotor.
+ */
+ServoMotorComponent::ServoMotorComponent(int PIN,  unsigned long updatePeriod, float step) {
     this->PIN = PIN;
     this->myservo.attach(PIN);
     this->desiredposition = Position::MIDDLE;
-    this->currentPosition = 0;
+    this->currentPosition = 32;
     this->lastUpTime = millis();
     this->updatePeriod = updatePeriod;
+    this->step = step;
+    this->myservo.write(this->PIN, this->currentPosition);
 }
 
-int ServoMotorComponent::getCurrentPosition() {
-    return this->myservo.read(this->PIN);
-}
-
-void ServoMotorComponent::setCurrentPosition() {
-    this->currentPosition = this->myservo.read(this->PIN);
-}
-
+/**
+ * Set the desired position
+ * @desiredPosition: Give desired position
+ */
 void ServoMotorComponent::setDesiredPosition(Position desiredPosition) {
     switch (desiredPosition) {
         case Position::LEFT:
@@ -34,6 +41,9 @@ void ServoMotorComponent::setDesiredPosition(Position desiredPosition) {
 }
 
 
+/**
+ * Write a new servoMotor position when it's necessary
+ */
 void ServoMotorComponent::refresh() {
     if (this->desiredposition == this->currentPosition 
     ||  millis() - this->lastUpTime <= this->updatePeriod) return;
@@ -46,11 +56,4 @@ void ServoMotorComponent::refresh() {
     }
     this->lastUpTime = millis();
     this->myservo.write(this->PIN, this->currentPosition);
-}
-
-
-void ServoMotorComponent::setPin(int PIN) {
-    this->PIN = PIN;
-    this->myservo.attach(PIN);
-    return;
 }
\ No newline at end of file
diff --git a/lib/ServoMotorComponent/src/ServoMotorComponent.h b/lib/ServoMotorComponent/src/ServoMotorComponent.h
index 4ea850d..e493743 100644
--- a/lib/ServoMotorComponent/src/ServoMotorComponent.h
+++ b/lib/ServoMotorComponent/src/ServoMotorComponent.h
@@ -12,21 +12,18 @@ enum Position {
 class ServoMotorComponent
 {
 public:
-    ServoMotorComponent(int PIN, unsigned long updatePeriod);
-    ~ServoMotorComponent() = default;
-    int getCurrentPosition();
-    void setCurrentPosition();
+    ServoMotorComponent(int PIN, unsigned long updatePeriod, float step = 1);
     void setDesiredPosition(Position desiredPosition);
-    void setPin(int PIN);
     void refresh();
 
 private:
     int PIN;
-    int currentPosition;
-    int desiredposition;
+    float currentPosition;
+    float desiredposition;
     Servo myservo;
     unsigned long lastUpTime;
     unsigned long updatePeriod;
+    float step;
 };
 
 
diff --git a/src/Program.cpp b/src/Program.cpp
index 3d7ebe7..8454899 100644
--- a/src/Program.cpp
+++ b/src/Program.cpp
@@ -2,17 +2,28 @@
 #include "Arduino.h"
 #include "DolibarrClient.h"
 
+int initialize_wifi(WifiConfig wifi) {
+    WiFiClass::mode(WIFI_STA); //Optional
+    WiFi.setSleep(false);
+    WiFi.begin(wifi.ssid, wifi.password);
+    Serial.print("Connecting ");
+    while(WiFiClass::status() != WL_CONNECTED){
+        delay(WAITING_WIFI_DELAY);
+        Serial.print(".");
+    }
+    Serial.println("Connected to the WiFi network");
+    return 0;
+}
+
 Program::Program() {
-    this->servo = new ServoMotorComponent(2);
+    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);
 }
 
 void Program::loop() {
-    // this->servo->goLeft();
-    // delay(3000);
-    this->servo->goMiddle();
-    // delay(3000);
-    // this->servo->goRight();
-    // delay(3000);
 }