diff --git a/program/main_controller/CMakeLists.txt b/program/main_controller/CMakeLists.txt
index dba9b09..e92cd38 100644
--- a/program/main_controller/CMakeLists.txt
+++ b/program/main_controller/CMakeLists.txt
@@ -18,6 +18,7 @@ add_executable(main_controller
   src/main.c
   src/robot.c
   src/motors.c
+  src/servos.c
   src/motion_control.c
   src/i2c/i2c_master.c
   src/i2c/gyro.c
diff --git a/program/main_controller/src/headers/motion_control.h b/program/main_controller/src/headers/motion_control.h
index 05dafa6..614ba31 100644
--- a/program/main_controller/src/headers/motion_control.h
+++ b/program/main_controller/src/headers/motion_control.h
@@ -2,16 +2,23 @@
 #define MOTION_CONTROL_H
 
 #include <stdint.h>
+#include <stdbool.h>
 
 #define I2C_MOTION_CONTROLLER_ADDRESS 0x09
 
 typedef struct motion_control_data_t {
     int16_t target_speed;
     float target_angle;
+
+    int16_t motor1_speed;
+    int16_t motor2_speed;
+    bool led_state;
 } motion_control_data_t;
 
 void motion_control_init(void);
 
 void motion_control_update(void);
 
+void motion_control_send(void);
+
 #endif // MOTION_CONTROL_H
\ No newline at end of file
diff --git a/program/main_controller/src/headers/robot.h b/program/main_controller/src/headers/robot.h
index 9bb9c6c..bdba3a4 100644
--- a/program/main_controller/src/headers/robot.h
+++ b/program/main_controller/src/headers/robot.h
@@ -2,16 +2,20 @@
 #define ROBOT_H
 
 #include <stdbool.h>
-#include "i2c/headers/mcp23017.h"
 #include "i2c/headers/gyro.h"
-#include "motion_control.h"
+#include "i2c/headers/mcp23017.h"
+#include "headers/motion_control.h"
+#include "headers/servos.h"
 #include "wifi/headers/udp_client.h"
 
 typedef struct robot_t {
-    udp_client_t udp_client;
-    mcp23017_data_t mcp23017_data;
     gyro_data_t gyro_data;
+    mcp23017_data_t mcp23017_data;
+
     motion_control_data_t motion_control_data;
+    servos_data_t servos_data;
+
+    udp_client_t udp_client;
 
     bool is_running;
     float delta_time_ms;
@@ -22,7 +26,7 @@ extern robot_t robot;
 // Init all robot's components
 void robot_init(void);
 // Handle inputs and outputs
-void robot_handle_inputs_outputs(void);
+void robot_update(void);
 // Deinit all robot's components
 void robot_deinit(void);
 
diff --git a/program/main_controller/src/headers/servos.h b/program/main_controller/src/headers/servos.h
new file mode 100644
index 0000000..c1922db
--- /dev/null
+++ b/program/main_controller/src/headers/servos.h
@@ -0,0 +1,22 @@
+#ifndef SERVOS_H
+#define SERVOS_H
+
+#include <stdint.h>
+
+typedef enum servos_t {
+    SERVO1,
+    SERVO2,
+    SERVO3,
+
+    NB_SERVOS
+} servos_t;
+
+typedef struct servos_data_t {
+    uint16_t servos_pos[NB_SERVOS];
+} servos_data_t;
+
+void servos_init(void);
+
+void servo_set_angle(servos_t servo, uint16_t pos);
+
+#endif // SERVOS_H
\ No newline at end of file
diff --git a/program/main_controller/src/main.c b/program/main_controller/src/main.c
index 104dffc..05befcc 100644
--- a/program/main_controller/src/main.c
+++ b/program/main_controller/src/main.c
@@ -18,7 +18,7 @@ int main(void)
 
     while(robot.is_running)
     {
-        robot_handle_inputs_outputs();
+        robot_update();
     }
 
     robot_deinit();
diff --git a/program/main_controller/src/motion_control.c b/program/main_controller/src/motion_control.c
index fa4e26b..1435cf5 100644
--- a/program/main_controller/src/motion_control.c
+++ b/program/main_controller/src/motion_control.c
@@ -5,21 +5,29 @@
 #include <hardware/i2c.h>
 #include "i2c/headers/i2c_master.h"
 #include "headers/motors.h"
+#include "headers/servos.h"
 #include "headers/robot.h"
 
 #define ANGLE_PER_S 90.0f
 #define GAIN_KP 10
 
-#define MSG_LEN sizeof(uint8_t) * 2
-#define MSG_DELAY_MS 5.0f
+#define MSG_LEN sizeof(uint8_t)                + /*   0x00   */ \
+                sizeof(uint8_t)                + /*   LED    */ \
+                sizeof(uint8_t)  * NB_MOTORS   + /*  Motors  */ \
+                sizeof(uint16_t) * NB_SERVOS     /*  Servos  */
+
+#define MSG_DELAY_MS 2.0f
 
 void motion_control_init(void)
 {
+    robot.motion_control_data.led_state = false;
     robot.motion_control_data.target_speed = 0;
     robot.motion_control_data.target_angle = 0.0f;
+    robot.motion_control_data.motor1_speed = 0;
+    robot.motion_control_data.motor2_speed = 0;
 }
 
-void motion_control_update(void)
+static inline void motors_control_update(void)
 {
     // First we update motion control data from received data
     robot.motion_control_data.target_angle += (float)robot.udp_client.data.hard.inputs.joystick_y / 256.0f * robot.delta_time_ms / 1000.0f * ANGLE_PER_S;
@@ -46,8 +54,6 @@ void motion_control_update(void)
 
     //printf(">target_speed:%d\n", robot.motion_control_data.target_speed);
 
-    float target_angle_radian = robot.motion_control_data.target_angle / 180.0f * M_PI;
-
     float error = robot.motion_control_data.target_angle - robot.gyro_data.x_angle;
     while(error > 180) error -= 360;
     while(error < -180) error += 360;
@@ -65,19 +71,69 @@ void motion_control_update(void)
         correction = 150;
     }*/
 
-    float motor1_speed = (float)robot.motion_control_data.target_speed - correction;
-    float motor2_speed = (float)robot.motion_control_data.target_speed + correction;
+    robot.motion_control_data.motor1_speed = (int16_t)((float)robot.motion_control_data.target_speed - correction);
+    robot.motion_control_data.motor2_speed = (int16_t)((float)robot.motion_control_data.target_speed + correction);
 
-    if(motor1_speed > 255) motor1_speed = 255;
-    if(motor2_speed > 255) motor2_speed = 255;
+    if(robot.motion_control_data.motor1_speed > 255) robot.motion_control_data.motor1_speed = 255;
+    if(robot.motion_control_data.motor2_speed > 255) robot.motion_control_data.motor2_speed = 255;
 
-    if(motor1_speed < -255) motor1_speed = -255;
-    if(motor2_speed < -255) motor2_speed = -255;
+    if(robot.motion_control_data.motor1_speed < -255) robot.motion_control_data.motor1_speed = -255;
+    if(robot.motion_control_data.motor2_speed < -255) robot.motion_control_data.motor2_speed = -255;
 
     // Set dir pins
-    motor_set_dir(MOTOR1, (int16_t)motor1_speed);
-    motor_set_dir(MOTOR2, (int16_t)motor2_speed);
+    motor_set_dir(MOTOR1, robot.motion_control_data.motor1_speed);
+    motor_set_dir(MOTOR2, robot.motion_control_data.motor2_speed);
+}
 
+static inline void servos_control_update(void)
+{
+    const uint16_t SERVO1_POSITIONS[5] = {
+        500,
+        // MORE TO DO
+        1950,
+    };
+
+    const uint16_t SERVO2_POSITIONS[2] = {
+        600,
+        2850,
+    };
+
+    // XXX DOESN'T WORK ??
+    const uint16_t SERVO3_POSITIONS[2] = {
+        600,
+        2850,
+    };
+    /*
+    // toggle servo 1 pos if button green is pressed
+    static float servo1_toggle_elapsed_time = 0.0f;
+    servo1_toggle_elapsed_time += robot.delta_time_ms;
+
+    if(robot.udp_client.data.hard.inputs.buttons.button_green && servo1_toggle_elapsed_time >= 200.0f)
+    {
+        servo1_toggle_elapsed_time = 0.0f;
+
+        servo_set_angle(SERVO1, 90 - robot.servos_data.servos_pos[SERVO1]);
+    }
+    */
+    //servo_set_angle(SERVO3, 400);
+}
+
+void steppers_control_update(void)
+{
+    steppers_set_pos(48);
+}
+
+void motion_control_update(void)
+{
+    motors_control_update();
+
+    servos_control_update();
+
+    steppers_control_update();
+}
+
+void motion_control_send(void)
+{
     static float elapsed_time_ms = 0.0f;
     elapsed_time_ms += robot.delta_time_ms;
 
@@ -85,19 +141,22 @@ void motion_control_update(void)
     {
         elapsed_time_ms = 0;
         // Send motors speed via I2C
-        uint8_t data[5];
+        uint8_t data[MSG_LEN] = {
+            0x00,
+            (uint8_t)robot.motion_control_data.led_state,
+            (uint8_t)abs(robot.motion_control_data.motor1_speed),
+            (uint8_t)abs(robot.motion_control_data.motor2_speed),
+            (uint8_t)(robot.servos_data.servos_pos[SERVO1] >> 8) & 0xFF,
+            (uint8_t)(robot.servos_data.servos_pos[SERVO1])      & 0xFF,
+            (uint8_t)(robot.servos_data.servos_pos[SERVO2] >> 8) & 0xFF,
+            (uint8_t)(robot.servos_data.servos_pos[SERVO2])      & 0xFF,
+            (uint8_t)(robot.servos_data.servos_pos[SERVO3] >> 8) & 0xFF,
+            (uint8_t)(robot.servos_data.servos_pos[SERVO3])      & 0xFF
+        };
 
-        data[0] = 0x00; /// registre
-
-        data[1] = (uint8_t)abs((int)motor1_speed);
-        data[2] = (uint8_t)abs((int)motor2_speed);
-
-
-        uint8_t reg = 0x00;
-        
-        if(i2c_write_blocking(I2C_MASTER_INSTANCE, I2C_MOTION_CONTROLLER_ADDRESS, data, 3, false) == PICO_ERROR_GENERIC)
+        if(i2c_write_blocking(I2C_MASTER_INSTANCE, I2C_MOTION_CONTROLLER_ADDRESS, data, MSG_LEN, false) == PICO_ERROR_GENERIC)
         {
             puts("Motion controller not reachable");
         }
     }
-}
+}
\ No newline at end of file
diff --git a/program/main_controller/src/motors.c b/program/main_controller/src/motors.c
index 4e8853d..9d111d8 100644
--- a/program/main_controller/src/motors.c
+++ b/program/main_controller/src/motors.c
@@ -3,10 +3,10 @@
 #include <pico/types.h>
 #include "i2c/headers/mcp23017.h"
 
-#define MOTOR1_DIR1_MCP23017_GPIO 12
-#define MOTOR1_DIR2_MCP23017_GPIO 13
-#define MOTOR2_DIR1_MCP23017_GPIO 14
-#define MOTOR2_DIR2_MCP23017_GPIO 15
+#define MCP23017_MOTOR1_DIR1_GPIO 12
+#define MCP23017_MOTOR1_DIR2_GPIO 13
+#define MCP23017_MOTOR2_DIR1_GPIO 14
+#define MCP23017_MOTOR2_DIR2_GPIO 15
 
 void motor_set_dir(motors_t motor, int16_t value)
 {
@@ -16,16 +16,16 @@ void motor_set_dir(motors_t motor, int16_t value)
     switch(motor)
     {
       case MOTOR1:
-        gpio_dir1_pin = MOTOR1_DIR1_MCP23017_GPIO;
-        gpio_dir2_pin = MOTOR1_DIR2_MCP23017_GPIO;
+        gpio_dir1_pin = MCP23017_MOTOR1_DIR1_GPIO;
+        gpio_dir2_pin = MCP23017_MOTOR1_DIR2_GPIO;
         break;
 
       case MOTOR2:
-        gpio_dir1_pin = MOTOR2_DIR1_MCP23017_GPIO;
-        gpio_dir2_pin = MOTOR2_DIR2_MCP23017_GPIO;
+        gpio_dir1_pin = MCP23017_MOTOR2_DIR1_GPIO;
+        gpio_dir2_pin = MCP23017_MOTOR2_DIR2_GPIO;
         break;
 
-    default:
+      default:
         break;
     }
 
diff --git a/program/main_controller/src/robot.c b/program/main_controller/src/robot.c
index 249739d..6ee7e2a 100644
--- a/program/main_controller/src/robot.c
+++ b/program/main_controller/src/robot.c
@@ -20,6 +20,7 @@ void robot_init(void)
 
     cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, true);
 
+    // I2C and devices
     i2c_master_init();
 
     mcp23017_init();
@@ -31,31 +32,15 @@ void robot_init(void)
 
     motion_control_init();
 
-    if(wifi_operator_init()){
-        while(1){
-            printf("No Wifi\n");
-            robot.is_running = false;
-            cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, true);
-            sleep_ms(50);
-            cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, false);
-            sleep_ms(50);
-        }
-    }
-        
+    // WiFi
+    if(wifi_operator_init())
+        robot.is_running = false;
 
-    if(udp_client_init()){
-        while(1){
-            printf("No UDP\n");
-            robot.is_running = false;
-            cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, true);
-            sleep_ms(50);
-            cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, false);
-            sleep_ms(50);
-        }
-    }
-        
-    // Initialisation ended
-    for(uint8_t i = 0, led_state = true; i < 5; i++)
+    if(udp_client_init())
+        robot.is_running = false;
+
+    // Initialisation ended, loop forever if inititalisation failed
+    for(uint i = 0, led_state = false; robot.is_running ? (i < 5) : true; i++)
     {
         cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, led_state);
 
@@ -63,7 +48,6 @@ void robot_init(void)
 
         led_state = !led_state;
     }
-    printf(">robot_running:%d\n", robot.is_running);
 }
 
 static inline void update_time(void)
@@ -76,21 +60,18 @@ static inline void update_time(void)
     static float elapsed_time_ms = 0.0f;
     elapsed_time_ms += robot.delta_time_ms;
 
-    static bool led_state = false;
     if(elapsed_time_ms >= 1000.0f)
     {
-        cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, led_state);
+        cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, robot.motion_control_data.led_state);
 
         elapsed_time_ms = 0.0f;
-        led_state = !led_state;
+
+        robot.motion_control_data.led_state = !robot.motion_control_data.led_state;
     }
 }
 
-void robot_handle_inputs_outputs(void)
+void robot_update(void)
 {
-    static bool led_state=false;
-    static uint32_t temps_ms_old =0, timer_200_ms=0;
-    uint32_t temps_ms;
     update_time();
 
     cyw43_arch_poll();
@@ -99,17 +80,11 @@ void robot_handle_inputs_outputs(void)
 
     motion_control_update();
 
+    motion_control_send();
+
     mcp23017_update();
-    
-    temps_ms = to_ms_since_boot(get_absolute_time());
-    timer_200_ms += temps_ms - temps_ms_old;
-    temps_ms_old = temps_ms;
-    if(timer_200_ms > 200){
-        timer_200_ms = 0;
-        led_state= !led_state;
-        uint8_t data[] = {0x02, led_state};
-        int ret = i2c_write_blocking(I2C_MASTER_INSTANCE, 0x09, data, 2, false);
-    }
+
+    tight_loop_contents();
 }
 
 void robot_deinit(void)
diff --git a/program/main_controller/src/servos.c b/program/main_controller/src/servos.c
new file mode 100644
index 0000000..331036d
--- /dev/null
+++ b/program/main_controller/src/servos.c
@@ -0,0 +1,14 @@
+#include "headers/servos.h"
+
+#include "headers/robot.h"
+
+void servos_init(void)
+{
+    for(size_t servo = SERVO1; servo < NB_SERVOS; servo++)
+        robot.servos_data.servos_pos[servo] = 0;
+}
+
+inline void servo_set_angle(servos_t servo, uint16_t pos)
+{
+    robot.servos_data.servos_pos[servo] = pos;
+}
diff --git a/program/motion_controller/CMakeLists.txt b/program/motion_controller/CMakeLists.txt
index 79e5865..56e2020 100644
--- a/program/motion_controller/CMakeLists.txt
+++ b/program/motion_controller/CMakeLists.txt
@@ -13,6 +13,7 @@ add_executable(motion_controller
   src/robot.c
   src/i2c/i2c_slave.c
   src/motors.c
+  src/servos.c
 )
 
 target_include_directories(motion_controller PRIVATE
diff --git a/program/motion_controller/src/headers/motors.h b/program/motion_controller/src/headers/motors.h
index 550887a..bb3d54b 100644
--- a/program/motion_controller/src/headers/motors.h
+++ b/program/motion_controller/src/headers/motors.h
@@ -4,20 +4,18 @@
 #include <pico/stdlib.h>
 
 // Motors
-typedef enum motors_enum_t {
+typedef enum motors_t {
     MOTOR1,
     MOTOR2,
 
     NB_MOTORS
-} motors_enum_t;
+} motors_t;
 
-#define MOTOR1_PWM_PIN 9
-#define MOTOR2_PWM_PIN 8
+#define I2C_BUFFER_MOTOR1_SPEED 1
+#define I2C_BUFFER_MOTOR2_SPEED 2
 
 // Init all motors defined in the MOTORS_DEF array
 void motors_init(void);
-// Set [motor] in motor_enum_t at [value] between -127 and 128
-void motor_set_speed(motors_enum_t motor, uint8_t value);
 // Update motors from the data in the i2c buffer
 void motors_update(void);
 
diff --git a/program/motion_controller/src/headers/robot.h b/program/motion_controller/src/headers/robot.h
index 9e8f2fa..c773c6e 100644
--- a/program/motion_controller/src/headers/robot.h
+++ b/program/motion_controller/src/headers/robot.h
@@ -7,7 +7,7 @@ typedef struct robot_t {
     i2c_buffer_t i2c_buffer;
 
     bool is_running;
-    double delta_time_ms;
+    float delta_time_ms;
 } robot_t;
 
 extern robot_t robot;
@@ -15,7 +15,7 @@ extern robot_t robot;
 // Init all robot's components
 void robot_init(void);
 // Handle inputs and outputs
-void robot_handle_inputs_outputs(void);
+void robot_update(void);
 // Deinit all robot's components
 void robot_deinit(void);
 
diff --git a/program/motion_controller/src/headers/servos.h b/program/motion_controller/src/headers/servos.h
new file mode 100644
index 0000000..b8d02f5
--- /dev/null
+++ b/program/motion_controller/src/headers/servos.h
@@ -0,0 +1,23 @@
+#ifndef SERVOS_H
+#define SERVOS_H
+
+typedef enum servos_t {
+    SERVO1,
+    SERVO2,
+    SERVO3,
+
+    NB_SERVOS
+} servos_t;
+
+#define I2C_BUFFER_SERVO1_ANGLE_H 3
+#define I2C_BUFFER_SERVO1_ANGLE_L 4
+#define I2C_BUFFER_SERVO2_ANGLE_H 5
+#define I2C_BUFFER_SERVO2_ANGLE_L 6
+#define I2C_BUFFER_SERVO3_ANGLE_H 7
+#define I2C_BUFFER_SERVO3_ANGLE_L 8
+
+void servos_init(void);
+
+void servos_update(void);
+
+#endif // SERVOS_H
\ No newline at end of file
diff --git a/program/motion_controller/src/i2c/headers/i2c_slave.h b/program/motion_controller/src/i2c/headers/i2c_slave.h
index 7414382..017d4bd 100644
--- a/program/motion_controller/src/i2c/headers/i2c_slave.h
+++ b/program/motion_controller/src/i2c/headers/i2c_slave.h
@@ -25,7 +25,4 @@ void init_i2c_slave(void);
 // Deinit i2c slave
 void deinit_i2c_slave(void);
 
-uint8_t get_vitesse_moteur_1(void);
-uint8_t get_vitesse_moteur_2(void);
-
 #endif // I2C_SLAVE_H
\ No newline at end of file
diff --git a/program/motion_controller/src/i2c/i2c_slave.c b/program/motion_controller/src/i2c/i2c_slave.c
index 8b7673a..da20f7e 100644
--- a/program/motion_controller/src/i2c/i2c_slave.c
+++ b/program/motion_controller/src/i2c/i2c_slave.c
@@ -9,38 +9,35 @@
 #include <hardware/gpio.h>
 #include <hardware/i2c.h>
 #include "pico/i2c_slave.h"
-
-#include <stdio.h>
-
-extern i2c_buffer_t i2c_buffer;
+#include "headers/robot.h"
 
 void i2c_slave_buffer_handler(i2c_inst_t *i2c, i2c_slave_event_t event)
 {
     switch(event)
     {
       case I2C_SLAVE_RECEIVE: // master has written some data
-        if(!i2c_buffer.buffer_reg_written)
+        if(!robot.i2c_buffer.buffer_reg_written)
         {
             // writes always start with the memory address
-            i2c_buffer.buffer_reg = i2c_read_byte_raw(I2C_SLAVE_INSTANCE);
-            i2c_buffer.buffer_reg_written = true;
+            robot.i2c_buffer.buffer_reg = i2c_read_byte_raw(I2C_SLAVE_INSTANCE);
+            robot.i2c_buffer.buffer_reg_written = true;
         }
         else
         {
             // save into memory
-            i2c_buffer.buffer[i2c_buffer.buffer_reg] = i2c_read_byte_raw(I2C_SLAVE_INSTANCE);
-            i2c_buffer.buffer_reg++;
+            robot.i2c_buffer.buffer[robot.i2c_buffer.buffer_reg] = i2c_read_byte_raw(I2C_SLAVE_INSTANCE);
+            robot.i2c_buffer.buffer_reg++;
         }
         break;
 
       case I2C_SLAVE_REQUEST: // master is requesting data
         // load from memory
-        i2c_write_byte_raw(I2C_SLAVE_INSTANCE, i2c_buffer.buffer[i2c_buffer.buffer_reg]);
-        i2c_buffer.buffer_reg++;
+        i2c_write_byte_raw(I2C_SLAVE_INSTANCE, robot.i2c_buffer.buffer[robot.i2c_buffer.buffer_reg]);
+        robot.i2c_buffer.buffer_reg++;
         break;
 
       case I2C_SLAVE_FINISH: // master has signalled Stop / Restart
-        i2c_buffer.buffer_reg_written = false;
+        robot.i2c_buffer.buffer_reg_written = false;
         break;
 
       default:
@@ -70,11 +67,3 @@ void deinit_i2c_slave(void)
     // New SDK method to reset i2c slave
     i2c_slave_deinit(I2C_SLAVE_INSTANCE);
 }
-
-uint8_t get_vitesse_moteur_1(void){
-  return i2c_buffer.buffer[0];
-}
-
-uint8_t get_vitesse_moteur_2(void){
-  return i2c_buffer.buffer[1];
-}
diff --git a/program/motion_controller/src/main.c b/program/motion_controller/src/main.c
index e890bf2..ab0eea8 100644
--- a/program/motion_controller/src/main.c
+++ b/program/motion_controller/src/main.c
@@ -8,38 +8,20 @@
 *   Ce Pico est un esclave piloté par le Pico Principal.    *
 \* * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
 
-#include <pico/stdlib.h>
-#include <stdio.h>
-#include "i2c/headers/i2c_slave.h"
-#include "headers/motors.h"
+#include "headers/robot.h"
 
-
-
-i2c_buffer_t i2c_buffer;
+robot_t robot;
 
 int main(void)
 {
-    stdio_init_all();
+    robot_init();
 
-    sleep_ms(1000);
-
-    puts("STDIO INIT ALL DONE");
-
-    init_i2c_slave();
-
-    puts("SLAVE INIT DONE");
-
-    gpio_init(PICO_DEFAULT_LED_PIN);
-    gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT);
-
-    motors_init();
-
-    while(true)
+    while(robot.is_running)
     {
-        gpio_put(PICO_DEFAULT_LED_PIN, i2c_buffer.buffer[2]);
-        motors_update();
-        sleep_ms(10);
+        robot_update();
     }
 
+    robot_deinit();
+
     return 0;
 }
diff --git a/program/motion_controller/src/motors.c b/program/motion_controller/src/motors.c
index 3fc4958..fa59d45 100644
--- a/program/motion_controller/src/motors.c
+++ b/program/motion_controller/src/motors.c
@@ -1,14 +1,13 @@
 #include "headers/motors.h"
-#include "i2c/headers/i2c_slave.h"
 
 #include <pico/stdlib.h>
 #include <hardware/pwm.h>
-#include <headers/robot.h>
+#include "headers/robot.h"
 
+#define MOTOR1_PWM_PIN 9
+#define MOTOR2_PWM_PIN 8
 
-#include <stdio.h>
-
-static inline uint get_motor_pwm_pin(motors_enum_t motor)
+static inline uint get_motor_pwm_pin(motors_t motor)
 {
     switch(motor)
     {
@@ -23,10 +22,16 @@ static inline uint get_motor_pwm_pin(motors_enum_t motor)
     }
 }
 
+static inline void motor_set_speed(motors_t motor, uint8_t value)
+{
+    uint MOTOR_PWM_PIN = get_motor_pwm_pin(motor);
+    pwm_set_gpio_level(MOTOR_PWM_PIN, (uint16_t)value);
+}
+
 // Init all motors defined in the MOTORS_DEF array
 void motors_init(void)
 {
-    for(motors_enum_t motor = MOTOR1; motor < NB_MOTORS; motor++)
+    for(motors_t motor = MOTOR1; motor < NB_MOTORS; motor++)
     {
         // Init PWM
         const uint MOTOR_PWM_PIN = get_motor_pwm_pin(motor);
@@ -44,15 +49,8 @@ void motors_init(void)
     }
 }
 
-// Set [motor] in motor_enum_t at [value] between 0 and 255
-void motor_set_speed(motors_enum_t motor, uint8_t value)
-{
-    uint MOTOR_PWM_PIN = get_motor_pwm_pin(motor);
-    pwm_set_gpio_level(MOTOR_PWM_PIN, (uint16_t)value);
-}
-
 void motors_update(void)
 {
-    motor_set_speed(MOTOR1, get_vitesse_moteur_1());
-    motor_set_speed(MOTOR2, get_vitesse_moteur_2());
+    motor_set_speed(MOTOR1, robot.i2c_buffer.buffer[I2C_BUFFER_MOTOR1_SPEED]);
+    motor_set_speed(MOTOR2, robot.i2c_buffer.buffer[I2C_BUFFER_MOTOR2_SPEED]);
 }
diff --git a/program/motion_controller/src/robot.c b/program/motion_controller/src/robot.c
index aa6db4a..e06cf74 100644
--- a/program/motion_controller/src/robot.c
+++ b/program/motion_controller/src/robot.c
@@ -3,6 +3,7 @@
 #include <pico/stdlib.h>
 #include <time.h>
 #include "headers/motors.h"
+#include "headers/servos.h"
 #include "i2c/headers/i2c_slave.h"
 
 #include <stdio.h>
@@ -17,12 +18,14 @@ void robot_init(void)
     gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT);
     gpio_put(PICO_DEFAULT_LED_PIN, true);
 
-    //motors_init();
+    motors_init();
+
+    servos_init();
 
     init_i2c_slave();
 
-    // Initialisation ended
-    for(uint i = 0, led_state = false; i < 5; i++)
+    // Initialisation ended, loop forever if initialisation failed
+    for(uint i = 0, led_state = false; robot.is_running ? (i < 5) : true; i++)
     {
         gpio_put(PICO_DEFAULT_LED_PIN, led_state);
 
@@ -34,31 +37,23 @@ void robot_init(void)
 
 static inline void update_time(void)
 {
-    static bool led_state = false;
-    static double last_time = 0.0;
-    double start_time = (double)clock() * 1000.0 / (double)CLOCKS_PER_SEC;
+    static float last_time = 0.0f;
+    float start_time = (float)clock() * 1000.0f / (float)CLOCKS_PER_SEC;
     robot.delta_time_ms = start_time - last_time;
     last_time = start_time;
 
-    static double elapsed_time = 0.0;
-    elapsed_time += robot.delta_time_ms;
+    gpio_put(PICO_DEFAULT_LED_PIN, robot.i2c_buffer.buffer[0]);
 }
 
-void robot_handle_inputs_outputs(void)
+void robot_update(void)
 {
-    static bool led_state=false;
     update_time();
 
     motors_update();
 
-    printf(">motor1_speed:%d\n", get_vitesse_moteur_1());
-    printf(">motor2_speed:%d\n", get_vitesse_moteur_2());
-    sleep_ms(200);
-    led_state= !led_state;
-    uint8_t data[] = {0x02, led_state};
-    int ret = i2c_write_blocking(i2c0, 0x09, data, 2, false);
+    servos_update();
 
-    //tight_loop_contents();
+    tight_loop_contents();
 }
 
 void robot_deinit(void)
diff --git a/program/motion_controller/src/servos.c b/program/motion_controller/src/servos.c
new file mode 100644
index 0000000..e012e32
--- /dev/null
+++ b/program/motion_controller/src/servos.c
@@ -0,0 +1,58 @@
+#include "headers/servos.h"
+
+#include <hardware/pwm.h>
+#include <pico/stdlib.h>
+#include "headers/robot.h"
+
+#define SERVO1_PWM_PIN 7
+#define SERVO2_PWM_PIN 6
+#define SERVO3_PWM_PIN 0
+
+static inline uint get_servo_pwm_pin(servos_t servo)
+{
+    switch(servo)
+    {
+      case SERVO1:
+        return SERVO1_PWM_PIN;
+      case SERVO2:
+        return SERVO2_PWM_PIN;
+      case SERVO3:
+        return SERVO3_PWM_PIN;
+
+      default:
+        return -1;
+    }
+}
+
+static inline void servo_set_speed(servos_t servo, uint16_t pos)
+{
+    const uint SERVO_PWM_PIN = get_servo_pwm_pin(servo);
+    pwm_set_gpio_level(SERVO_PWM_PIN, pos);
+}
+
+void servos_init(void)
+{
+    for(servos_t servo = SERVO1; servo < NB_SERVOS; servo++)
+    {
+        // Init PWM
+        const uint SERVO_PWM_PIN = get_servo_pwm_pin(servo);
+
+        const uint SLICE_NUM = pwm_gpio_to_slice_num(SERVO_PWM_PIN);
+
+        gpio_set_function(SERVO_PWM_PIN, GPIO_FUNC_PWM);
+
+        pwm_set_clkdiv(SLICE_NUM, 100);
+        pwm_set_wrap(SLICE_NUM, 25000);
+
+        pwm_set_enabled(SLICE_NUM, true);
+
+        servo_set_speed(servo, 0);
+    }
+}
+
+void servos_update(void)
+{
+    servo_set_speed(SERVO1, robot.i2c_buffer.buffer[I2C_BUFFER_SERVO1_ANGLE_H] << 8 | robot.i2c_buffer.buffer[I2C_BUFFER_SERVO1_ANGLE_L]);
+    servo_set_speed(SERVO2, robot.i2c_buffer.buffer[I2C_BUFFER_SERVO2_ANGLE_H] << 8 | robot.i2c_buffer.buffer[I2C_BUFFER_SERVO2_ANGLE_L]);
+    servo_set_speed(SERVO3, robot.i2c_buffer.buffer[I2C_BUFFER_SERVO3_ANGLE_H] << 8 | robot.i2c_buffer.buffer[I2C_BUFFER_SERVO3_ANGLE_L]);
+}