diff --git a/IO.hpp b/IO.hpp
index 755d822..f70e42e 100644
--- a/IO.hpp
+++ b/IO.hpp
@@ -112,24 +112,33 @@ class IO {
     m_motors[1].setSpeed(0);
   }
 
-  void setDir(float dir)
+  void setDirWithAngularSpeed(float dir, float angular_speed = ANGULAR_SPEED)
   {
     static unsigned long prev_time {millis()};
     float err_dir = dir - m_dir;
-    float max_angular_displacement = ANGULAR_SPEED * (millis() - prev_time);
+    float angular_displacement = angular_speed * static_cast<float>(millis() / 1000 - prev_time / 1000);
 
-    if(std::abs(err_dir) < max_angular_displacement)
+    Serial.printf("Angular Speed : %d\nStatic Cast : %d\nprev_time : %d\n millis : %d\n", angular_speed, static_cast<float>(millis() / 1000 - prev_time / 1000), prev_time, millis());
+
+    if(std::abs(err_dir) < angular_displacement)
     {
       m_dir = dir;
     }
     else
     {
-      m_dir += max_angular_displacement * (err_dir < 0) ? -1 : 1;
+      m_dir += angular_displacement * (err_dir < 0) ? -1 : 1;
     }
 
+    Serial.printf("Angle : %d\nAngular Displacement : %d\n", m_dir, angular_displacement);
+
     prev_time = millis();
   }
 
+  void setDir(float dir)
+  {
+    m_dir = dir;
+  }
+
   void setSpeed(float speed)
   {
     m_speed = speed;
diff --git a/Main.ino b/Main.ino
index 3830f75..ce680fa 100644
--- a/Main.ino
+++ b/Main.ino
@@ -55,13 +55,13 @@ void setup()
     while(true);
   }
 
-  my_IO.getScreen()->drawBitmap(0, 0, Imgs::riombotique, 52, 64, WHITE);
-  my_IO.getScreen()->drawBitmap(75, 0, Imgs::poivron_robotique, 52, 64, WHITE);
-
-  //my_IO.motorControlOn();
+  my_IO.getScreen()->drawBitmap(38, 0, Res::Imgs::riombotique, 52, 64, WHITE);
+  //my_IO.getScreen()->drawBitmap(75, 0, Imgs::poivron_robotique, 52, 64, WHITE);
 
   Serial.println("Initalisation Finished");
 
+  my_IO.motorControlOn();
+
 #ifdef UNIT_TESTS
   Serial.println("UNIT_TESTS");
 #endif