diff --git a/IO.hpp b/IO.hpp
index e961541..5131870 100644
--- a/IO.hpp
+++ b/IO.hpp
@@ -276,6 +276,8 @@ class IO {
       //m1_speed = static_cast<int>(m_speed + 2 * correction);
     }
 
+    Serial.printf(">Actual Angle: %f\n", actual_angle);
+
     m_motors[0].setSpeed(m1_speed);
     m_motors[1].setSpeed(m2_speed);
   }
diff --git a/Main.ino b/Main.ino
index b646cc3..3699d98 100644
--- a/Main.ino
+++ b/Main.ino
@@ -7,7 +7,7 @@
 #include "Ressources.hpp"
 
 // Unit tests activation
-#define UNIT_TESTS
+//#define UNIT_TESTS
 
 /*
   Etapes :
@@ -65,7 +65,11 @@ void setup()
 
   Serial.println("Initalisation Finished");
 
-  my_IO.motorControlOn();
+  my_IO.motorControlOff();
+
+  my_IO.setDir(0);
+  my_IO.setSpeed(0.0f);
+  //my_IO.startDancingAction(15);
 
 #ifdef UNIT_TESTS
   Serial.println("UNIT_TESTS");
@@ -78,18 +82,19 @@ void loop()
 {
   my_IO.update();
 
-  //Serial.printf("Is Selected Color Blue : %d\n", my_IO.isSelectedColorBlue());
-  //Serial.printf("Is Tirette Pulled : %d\n", my_IO.isTirettePulled());
+  Serial.printf("Is Ta Mere Selected : %d\n", my_IO.isSelectedColorBlue());
+  Serial.printf("Is Tirette Pulled : %d\n", my_IO.isTirettePulled());
 
-  my_IO.setDir(0);
-  my_IO.setSpeed(2048.0f);
+  my_IO.startDancingAction();
 }
 
 #else
 
 void loop()
 {
-  static State actual_state {State::ForwardToRamp};
+  static State actual_state {State::WaitingForTirette};
+
+  my_IO.update();
 
   switch(actual_state)
   {
@@ -103,59 +108,64 @@ void loop()
     case State::WaitingTimer:
       static unsigned long initial_time = millis();
 
-      if(millis() - initial_time >= 90000)
+      //if(millis() - initial_time >= 90000)
+      if(millis() - initial_time >= 2000)
       {
         actual_state = State::ForwardToRamp;
       }
       break;
     
     case State::ForwardToRamp:
-      my_IO.forward();
+      my_IO.motorControlOn();
+      my_IO.setSpeed(2048.0f);
 
-      if(my_IO.getAngle(Axes::Y) > 9)
+      if(my_IO.getAngle(Axes::Y) < -5)
       {
         actual_state = State::ForwardToScene;
       }
       break;
 
     case State::ForwardToScene:
-      IO::motors.forward();
+      my_IO.setSpeed(2048.0f);
 
-      if(IO::gyro.getAngle(Axes::Y) < 2)
+      if(my_IO.getAngle(Axes::Y) > -5.0f)
       {
-        actual_state = IO::ext_controls.isSelectedColorBlue() ? State::Turn90Blue : State::Turn90Yellow;
+        //actual_state = my_IO.isSelectedColorBlue() ? State::Turn90Blue : State::Turn90Yellow;
+        actual_state = State::Dancing;
       }
       break;
 
     case State::Turn90Blue:
-      IO::motors.setDir(90);
+      my_IO.setDirWithAngularSpeed(90.0f);
 
-      if(IO::gyro.getAngle() > 90)
+      if(my_IO.getAngle(Axes::Z) > 90.0f)
       {
         actual_state = State::ForwardToSceneEdge;
       }
       break;
 
     case State::Turn90Yellow:
-      IO::motors.setAngle(-90);
+      my_IO.setDirWithAngularSpeed(-90);
 
-      if(IO::gyro.getAngle() < -90)
+      if(my_IO.getAngle(Axes::Z) < -90)
       {
         actual_state = State::ForwardToSceneEdge;
       }
       break;
 
     case State::ForwardToSceneEdge:
-      IO::motors.forward();
+      my_IO.setSpeed(2048);
 
-      if(IO::gyro.getAngle() < 0)
+      if(my_IO.getAngle(Axes::Y) < 2)
       {
         actual_state = State::Dancing;
       }
       break;
 
     case State::Dancing:
-      IO::ext_controls.startSpecialAction();
+      my_IO.setSpeed(0);
+      my_IO.motorControlOff();
+      my_IO.startDancingAction();
       break;
   }
 }