diff --git a/Code/src/main.c b/Code/src/main.c
index 4d93682..d3fe3ca 100644
--- a/Code/src/main.c
+++ b/Code/src/main.c
@@ -1,145 +1,155 @@
-#include <pico/stdlib.h>
-#include <stdio.h>
-#include "include/io.h"
-
-#define NORMAL_SPEED 2000.0f
-#define TURNING_SPEED 1300.0f
-#define ON_STAGE_SPEED 1100.0f
-
-#define RAMPE_ANGLE -7.82907651006f
-#define RAMPE_ANGLE_OFFSET 1.0f
-#define RAMPE_START_ANGLE (RAMPE_ANGLE / 2 - RAMPE_ANGLE_OFFSET)
-#define RAMPE_END_ANGLE (RAMPE_ANGLE / 2 + RAMPE_ANGLE_OFFSET)
- 
-// Unit tests activation
-//#define UNIT_TESTS
-
-/*
-  Etapes :
-   - 1ere etape : avancer jusqu'a la montée / condition : si montée detecté : etape suivante
-   - 2eme etape : avancer j'usqua la fin de la montée / condition : si fin de montée detecté : etape suivante
-   - 3eme etape : tourner de 90* / condition : si action terminé : etape suivante
-   - 4eme etape : avancer jusqu'au bors du plateau / condition : si choc de fin detecté : etape suivante
-   - 5eme etape : faire tourner actionneurs pour figurine
-*/
-
-/*
-  Modules necessaires :
-   - angle          // Asservissement et control du robot
-   - choc           // Controle du robot (fin !)
-(?)- accelerometre  // Asservissement
- */
-
-// Movement states
-typedef enum state_t {
-  WaitingForTirette,
-  WaitingTimer,
-  ForwardToRamp,
-  ForwardToScene,
-  Turn90Blue, // If blue team
-  Turn90Yellow, // If yellow team => this is the only action where you have to do something different depending on your team
-  ForwardToSceneEdge,
-  Dancing
-} state_t;
-
-io_t io;
-
-void main(void)
-{
-    stdio_init_all();
-
-    // Initialise IO
-    init_io();
-
-    motor_control_activated(true);
-
-    io.target_dir = 0.0f;
-    io.target_speed = 0.0f;
-    //my_IO.startDancingAction(15);
-
-    #ifdef UNIT_TESTS
-    Serial.println("UNIT_TESTS");
-    #endif
-
-    printf("Rampe Angle         : %f\n", RAMPE_ANGLE);
-    printf("Rampe Angle Offset  : %f\n", RAMPE_ANGLE_OFFSET);
-    printf("Rampe Start Angle   : %f\n", RAMPE_START_ANGLE);
-    printf("Rampe End Angle     : %f\n", RAMPE_END_ANGLE);
-
-    state_t actual_state = WaitingForTirette;
-
-    while(true)
-    {
-        update_io();
-
-        switch(actual_state)
-        {
-          case WaitingForTirette:
-            if(io.is_tirette_pulled)
-            {
-                actual_state = WaitingTimer;
-            }
-            break;
-
-          case WaitingTimer:
-            unsigned long initial_time = io.time_ms;
-
-            if(io.time_ms - initial_time >= 87000)
-            {
-                actual_state = ForwardToRamp;
-            }
-            break;
-
-          case ForwardToRamp:
-            motor_control_activated(true);
-            io.target_speed = NORMAL_SPEED;
-
-            if(io.gyro_data.y_angle < RAMPE_START_ANGLE)
-            {
-                actual_state = ForwardToScene;
-            }
-            break;
-
-          case ForwardToScene:
-            if(io.gyro_data.y_angle > RAMPE_END_ANGLE)
-            {
-                io.target_speed = TURNING_SPEED;
-                actual_state = io.is_color_blue ? Turn90Blue : Turn90Yellow;
-            }
-            break;
-
-          case Turn90Blue:
-            set_dir_with_angular_speed(90.0f);
-
-            if(io.gyro_data.z_angle >= 80.0f)
-            {
-                actual_state = ForwardToSceneEdge;
-            }
-            break;
-
-          case Turn90Yellow:
-            set_dir_with_angular_speed(-90.0f);
-
-            if(io.gyro_data.z_angle <= -80.0f)
-            {
-                actual_state = ForwardToSceneEdge;
-            }
-            break;
-
-          case ForwardToSceneEdge:
-            io.target_speed = ON_STAGE_SPEED;
-
-            if(io.gyro_data.y_angle > 3.0f)
-            {
-                actual_state = Dancing;
-            }
-            break;
-
-          case Dancing:
-            motor_control_activated(false);
-            io.target_speed = 0.0f;
-            io.is_dancing = true;
-            break;
-        }
-    }
-}
+#include <pico/stdlib.h>
+#include <stdio.h>
+#include "include/io.h"
+
+#define NORMAL_SPEED 2000.0f
+#define TURNING_SPEED 1300.0f
+#define ON_STAGE_SPEED 1500.0f
+
+#define RAMPE_ANGLE -7.82907651006f
+#define RAMPE_ANGLE_OFFSET 1.0f
+#define RAMPE_START_ANGLE (RAMPE_ANGLE / 2 - RAMPE_ANGLE_OFFSET)
+#define RAMPE_END_ANGLE (RAMPE_ANGLE / 2 + RAMPE_ANGLE_OFFSET)
+ 
+// Unit tests activation
+//#define UNIT_TESTS
+
+/*
+  Etapes :
+   - 1ere etape : avancer jusqu'a la montée / condition : si montée detecté : etape suivante
+   - 2eme etape : avancer j'usqua la fin de la montée / condition : si fin de montée detecté : etape suivante
+   - 3eme etape : tourner de 90* / condition : si action terminé : etape suivante
+   - 4eme etape : avancer jusqu'au bors du plateau / condition : si choc de fin detecté : etape suivante
+   - 5eme etape : faire tourner actionneurs pour figurine
+*/
+
+/*
+  Modules necessaires :
+   - angle          // Asservissement et control du robot
+   - choc           // Controle du robot (fin !)
+(?)- accelerometre  // Asservissement
+ */
+
+// Movement states
+typedef enum state_t {
+  WaitingForTirette,
+  WaitingTimer,
+  ForwardToRamp,
+  ForwardToScene,
+  SceneTimeout,
+  Turn90Blue, // If blue team
+  Turn90Yellow, // If yellow team => this is the only action where you have to do something different depending on your team
+  ForwardToSceneEdge,
+  Dancing
+} state_t;
+
+io_t io;
+
+void main(void)
+{
+    stdio_init_all();
+
+    // Initialise IO
+    init_io();
+
+    motor_control_activated(true);
+
+    io.target_dir = 0.0f;
+    io.target_speed = 0.0f;
+    //my_IO.startDancingAction(15);
+
+    #ifdef UNIT_TESTS
+    Serial.println("UNIT_TESTS");
+    #endif
+
+    printf("Rampe Angle         : %f\n", RAMPE_ANGLE);
+    printf("Rampe Angle Offset  : %f\n", RAMPE_ANGLE_OFFSET);
+    printf("Rampe Start Angle   : %f\n", RAMPE_START_ANGLE);
+    printf("Rampe End Angle     : %f\n", RAMPE_END_ANGLE);
+
+    state_t actual_state = WaitingForTirette;
+
+    while(true)
+    {
+        update_io();
+
+        switch(actual_state)
+        {
+          case WaitingForTirette:
+            if(io.is_tirette_pulled)
+            {
+                actual_state = WaitingTimer;
+            }
+            break;
+
+          case WaitingTimer:
+            unsigned long initial_time = io.time_ms;
+
+            if(io.time_ms - initial_time >= 87000)
+            {
+                actual_state = ForwardToRamp;
+            }
+            break;
+
+          case ForwardToRamp:
+            motor_control_activated(true);
+            io.target_speed = NORMAL_SPEED;
+
+            if(io.gyro_data.y_angle < RAMPE_START_ANGLE)
+            {
+                actual_state = SceneTimeout;
+            }
+            break;
+
+          case SceneTimeout:
+            unsigned long initial_time = io.time_ms;
+
+            if(io.time_ms - initial_time >= 500)
+            {
+                actual_state = ForwardToScene;
+            }
+            break;
+
+          case ForwardToScene:
+            if(io.gyro_data.y_angle > RAMPE_END_ANGLE)
+            {
+                io.target_speed = TURNING_SPEED;
+                actual_state = io.is_color_blue ? Turn90Blue : Turn90Yellow;
+            }
+            break;
+
+          case Turn90Blue:
+            set_dir_with_angular_speed(90.0f);
+
+            if(io.gyro_data.z_angle >= 80.0f)
+            {
+                actual_state = ForwardToSceneEdge;
+            }
+            break;
+
+          case Turn90Yellow:
+            set_dir_with_angular_speed(-90.0f);
+
+            if(io.gyro_data.z_angle <= -80.0f)
+            {
+                actual_state = ForwardToSceneEdge;
+            }
+            break;
+
+          case ForwardToSceneEdge:
+            io.target_speed = ON_STAGE_SPEED;
+
+            if(io.gyro_data.y_angle > 3.0f)
+            {
+                actual_state = Dancing;
+            }
+            break;
+
+          case Dancing:
+            motor_control_activated(false);
+            io.target_speed = 0.0f;
+            io.is_dancing = true;
+            break;
+        }
+    }
+}