diff --git a/main controller code/.vscode/settings.json b/main controller code/.vscode/settings.json
index 80f9ddc..15bc3e9 100644
--- a/main controller code/.vscode/settings.json	
+++ b/main controller code/.vscode/settings.json	
@@ -2,6 +2,7 @@
     "files.associations": {
         "binary_info.h": "c",
         "i2c.h": "c",
-        "stdlib.h": "c"
+        "stdlib.h": "c",
+        "stdint.h": "c"
     }
 }
\ No newline at end of file
diff --git a/main controller code/CMakeLists.txt b/main controller code/CMakeLists.txt
index 229c319..bcce13b 100644
--- a/main controller code/CMakeLists.txt	
+++ b/main controller code/CMakeLists.txt	
@@ -12,6 +12,13 @@ pico_sdk_init()
 
 add_executable(motion_controller
   src/main.c
+  src/robot.c
+  src/motors.c
+  src/gyro.c
+  src/motion_control.c
+  src/i2c_master.c
+  src/udp_client.c
+  src/udp_buffer.c
 )
 
 target_link_libraries(motion_controller
diff --git a/main controller code/src/gyro.c b/main controller code/src/gyro.c
new file mode 100644
index 0000000..277e70c
--- /dev/null
+++ b/main controller code/src/gyro.c	
@@ -0,0 +1 @@
+#include "include/gyro.h"
\ No newline at end of file
diff --git a/main controller code/src/i2c_master.c b/main controller code/src/i2c_master.c
new file mode 100644
index 0000000..4ce022f
--- /dev/null
+++ b/main controller code/src/i2c_master.c	
@@ -0,0 +1,17 @@
+#include "include/i2c_master.h"
+
+inline void i2c_master_send(uint8_t address, const uint8_t *src, size_t len)
+{
+    i2c_write_blocking_until(I2C_MASTER_INSTANCE, address, src, len, false, I2C_MASTER_MAX_TIMEOUT);
+}
+
+inline void i2c_master_receive(uint8_t address, uint8_t *dst, size_t len)
+{
+    i2c_read_blocking_until(I2C_MASTER_INSTANCE, address, dst, len, false, I2C_MASTER_MAX_TIMEOUT);
+}
+
+inline void i2c_master_send_receive(uint8_t address, uint8_t *data, size_t len)
+{
+    i2c_write_blocking_until(I2C_MASTER_INSTANCE, address, data, len, true, I2C_MASTER_MAX_TIMEOUT);
+    i2c_master_receive(address, data, len);
+}
\ No newline at end of file
diff --git a/main controller code/src/include/gyro.h b/main controller code/src/include/gyro.h
new file mode 100644
index 0000000..fce6496
--- /dev/null
+++ b/main controller code/src/include/gyro.h	
@@ -0,0 +1,6 @@
+#ifndef GYRO_H
+#define GYRO_H
+
+#define I2C_GYRO_ADDRESS 0x6b
+
+#endif // GYRO_H
\ No newline at end of file
diff --git a/main controller code/src/include/i2c_master.h b/main controller code/src/include/i2c_master.h
new file mode 100644
index 0000000..d30c5c9
--- /dev/null
+++ b/main controller code/src/include/i2c_master.h	
@@ -0,0 +1,18 @@
+#ifndef I2C_MASTER_H
+#define I2C_MASTER_H
+
+#include <hardware/i2c.h>
+
+#define I2C_MASTER_SDA_PIN 4
+#define I2C_MASTER_SCL_PIN 5
+#define I2C_MASTER_INSTANCE i2c0
+#define I2C_MASTER_MAX_TIMEOUT 10000
+
+// Send [src] of [len] to [address] and close communication
+void i2c_master_send(uint8_t address, const uint8_t *src, size_t len);
+// Receive [dst] of [len] from [address] and close communication
+void i2c_master_receive(uint8_t address, uint8_t *dst, size_t len);
+// Send [data] of [len] to [address], receive [data] of [len] from [address] and close communication
+void i2c_master_send_receive(uint8_t address, uint8_t *data, size_t len);
+
+#endif // I2C_MASTER_H
\ No newline at end of file
diff --git a/main controller code/src/include/motion_control.h b/main controller code/src/include/motion_control.h
new file mode 100644
index 0000000..e69de29
diff --git a/main controller code/src/include/motors.h b/main controller code/src/include/motors.h
index f7db597..ec9847b 100644
--- a/main controller code/src/include/motors.h	
+++ b/main controller code/src/include/motors.h	
@@ -1,6 +1,10 @@
 #ifndef MOTORS_H
 #define MOTORS_H
 
+#include <pico/types.h>
+
+#define I2C_MOTION_CONTROLLER_ADDRESS 0x09
+
 // Motors
 typedef enum motors_enum_t {
     MOTOR1,
@@ -39,17 +43,9 @@ typedef struct {
 
 extern const servo_motor_def_t SERVO_MOTORS_DEFS[];
 
-// Init all motors defined in the MOTORS_DEF array
-void init_motors(void);
-// Init all servo motors defined in the SERVO_MOTORS_DEF array
-void init_servo_motors(void);
-// Set [motor] to 0
-void motor_zero(motors_enum_t motor);
-// Set [motor] in motor_enum_t at [value] between -127 and 128 (for this config)
-void motor_set(motors_enum_t motor, int value);
-// Set servo motor to its open pos
-void servo_motor_zero(servo_motors_enum_t servo_motor);
-// Set servo to its close pos if [close] else open pos
-void servo_motor_set(servo_motors_enum_t servo_motor, bool close);
+// Send [motor] to [value] through i2c to motion controller
+void i2c_set_motor_speed(motors_enum_t motor, int8_t value);
+// Get [motor] speed from motion controller through i2c
+int8_t i2c_get_motor_speed(motors_enum_t motor);
 
 #endif // MOTORS_H
diff --git a/main controller code/src/include/robot.h b/main controller code/src/include/robot.h
index d23ef40..0774298 100644
--- a/main controller code/src/include/robot.h	
+++ b/main controller code/src/include/robot.h	
@@ -1,6 +1,8 @@
 #ifndef ROBOT_H
 #define ROBOT_H
 
+#include <stdbool.h>
+
 typedef struct robot_t {
     bool is_running;
     double delta_time_ms;
diff --git a/main controller code/src/include/udp_buffer.h b/main controller code/src/include/udp_buffer.h
new file mode 100644
index 0000000..3a8bd53
--- /dev/null
+++ b/main controller code/src/include/udp_buffer.h	
@@ -0,0 +1,4 @@
+#ifndef UDP_BUFFER_H
+#define UDP_BUFFER_H
+
+#endif // UDP_BUFFER_H
\ No newline at end of file
diff --git a/main controller code/src/include/udp_client.h b/main controller code/src/include/udp_client.h
new file mode 100644
index 0000000..3bc0f2d
--- /dev/null
+++ b/main controller code/src/include/udp_client.h	
@@ -0,0 +1,4 @@
+#ifndef UDP_CLIENT_H
+#define UDP_CLIENT_H
+
+#endif // UDP_CLIENT_H
\ No newline at end of file
diff --git a/main controller code/src/motion_control.c b/main controller code/src/motion_control.c
new file mode 100644
index 0000000..e69de29
diff --git a/main controller code/src/motors.c b/main controller code/src/motors.c
index 3969bb7..8013717 100644
--- a/main controller code/src/motors.c	
+++ b/main controller code/src/motors.c	
@@ -1,8 +1,8 @@
-#include <pico/stdlib.h>
-#include <hardware/pwm.h>
-
 #include "include/motors.h"
 
+#include <stdlib.h>
+#include "include/i2c_master.h"
+
 const motor_def_t MOTORS_DEFS[] = {
     {0,  4,  5, 0x00},
     {1,  6,  7, 0x01},
@@ -17,103 +17,24 @@ const servo_motor_def_t SERVO_MOTORS_DEFS[] = {
     {15, 0, 25000, 0x07},
 };
 
-// Init all motors defined in the MOTORS_DEF array
-void init_motors(void)
-{
-    for(motors_enum_t actual_motor = MOTOR1; actual_motor < NB_MOTORS; actual_motor++)
-    {
-        const motor_def_t *motor_def = &MOTORS_DEFS[actual_motor];
-
-        // Init PWM
-        uint slice_num = pwm_gpio_to_slice_num(motor_def->pwm_pin);
-
-        gpio_set_function(motor_def->pwm_pin, GPIO_FUNC_PWM);
-        pwm_set_wrap(slice_num, 128);
-        pwm_set_enabled(slice_num, true);
-
-        // Init dir pins
-        gpio_init(motor_def->dir1_pin);
-        gpio_set_dir(motor_def->dir1_pin, GPIO_OUT);
-
-        gpio_init(motor_def->dir2_pin);
-        gpio_set_dir(motor_def->dir2_pin, GPIO_OUT);
-
-        motor_zero(actual_motor);
-    }
-}
-
-// Init all servo motors defined in the SERVO_MOTORS_DEF array
-void init_servo_motors(void)
-{
-    for(servo_motors_enum_t actual_servo_motor = SERVO_MOTOR1; actual_servo_motor < NB_SERVO_MOTORS; actual_servo_motor++)
-    {
-        const servo_motor_def_t *servo_motor_def = &SERVO_MOTORS_DEFS[actual_servo_motor];
-
-        //  Init PWM   //
-        uint slice_num = pwm_gpio_to_slice_num(servo_motor_def->pwm_pin);
-
-        gpio_set_function(servo_motor_def->pwm_pin, GPIO_FUNC_PWM);
-        pwm_set_wrap(slice_num, 25000);
-        pwm_set_clkdiv(slice_num, 100);
-        pwm_set_enabled(slice_num, true);
-
-        servo_motor_zero(actual_servo_motor);
-    }
-}
-
-// Set [motor] to 0
-void motor_zero(motors_enum_t motor)
+void i2c_set_motor_speed(motors_enum_t motor, int8_t value)
 {
     const motor_def_t *motor_def = &MOTORS_DEFS[motor];
 
-    // Set PWM to zero
-    pwm_set_gpio_level(motor_def->pwm_pin, 0);
+    uint8_t x = *(uint8_t *)&value;
+    uint8_t data[2] = {motor_def->buffer_address, x};
 
-    // Set dir pins to false
-    gpio_put(motor_def->dir1_pin, false);
-    gpio_put(motor_def->dir2_pin, false);
+    i2c_master_send(I2C_MOTION_CONTROLLER_ADDRESS, data, 2);
 }
 
-// Set [motor] in motor_enum_t at [value] between -128 and 127 (for this config)
-void motor_set(motors_enum_t motor, int value)
+int8_t i2c_get_motor_speed(motors_enum_t motor)
 {
     const motor_def_t *motor_def = &MOTORS_DEFS[motor];
 
-    if(value < 0)
-    {
-        gpio_put(motor_def->dir1_pin, true);
-        gpio_put(motor_def->dir2_pin, false);
+    uint8_t data[1] = {motor_def->buffer_address};
 
-        value = -value;
-    }
-    else if(value > 0)
-    {
-        gpio_put(motor_def->dir1_pin, false);
-        gpio_put(motor_def->dir2_pin, true);
-    }
-    else
-    {
-        gpio_put(motor_def->dir1_pin, false);
-        gpio_put(motor_def->dir2_pin, false);
-    }
+    i2c_master_send_receive(I2C_MOTION_CONTROLLER_ADDRESS, data, 1);
 
-    pwm_set_gpio_level(motor_def->pwm_pin, (uint16_t)value);
-}
-
-// Set servo motor to its open pos
-void servo_motor_zero(servo_motors_enum_t servo_motor)
-{
-    const servo_motor_def_t *servo_motor_def = &SERVO_MOTORS_DEFS[servo_motor];
-
-    // Set PWM to zero //
-    pwm_set_gpio_level(servo_motor_def->pwm_pin, servo_motor_def->open_pos);
-}
-
-// Set servo to its close pos if [close] else open pos
-void servo_motor_set(servo_motors_enum_t servo_motor, bool close)
-{
-    const servo_motor_def_t *servo_motor_def = &SERVO_MOTORS_DEFS[servo_motor];
-
-    // Set PWM to zero //
-    pwm_set_gpio_level(servo_motor_def->pwm_pin, close ? servo_motor_def->close_pos : servo_motor_def->open_pos);
+    int8_t value = *(int8_t *)data;
+    return value;
 }
diff --git a/main controller code/src/robot.c b/main controller code/src/robot.c
index e1c3a85..31347c0 100644
--- a/main controller code/src/robot.c	
+++ b/main controller code/src/robot.c	
@@ -1,26 +1,18 @@
-#include <pico/stdlib.h>
-#include "include/motors.h"
-#include "include/i2c_slave.h"
-
 #include "include/robot.h"
 
+#include <pico/stdlib.h>
+
 void robot_init(void)
 {
     stdio_init_all();
 
-    init_motors();
-    init_servo_motors();
-    i2c_slave_init();
-
     robot.is_running = true;
 }
 
 void robot_handle_inputs_outputs(void)
 {
-    update_motors_from_buffer();
 }
 
 void robot_deinit(void)
 {
-    i2c_slave_deinit();
 }
\ No newline at end of file
diff --git a/main controller code/src/udp_buffer.c b/main controller code/src/udp_buffer.c
new file mode 100644
index 0000000..89efe85
--- /dev/null
+++ b/main controller code/src/udp_buffer.c	
@@ -0,0 +1 @@
+#include "include/udp_buffer.h"
\ No newline at end of file
diff --git a/main controller code/src/udp_client.c b/main controller code/src/udp_client.c
new file mode 100644
index 0000000..d818b29
--- /dev/null
+++ b/main controller code/src/udp_client.c	
@@ -0,0 +1 @@
+#include "include/udp_client.h"
\ No newline at end of file
diff --git a/motion controller code/.vscode/settings.json b/motion controller code/.vscode/settings.json
index d1d99fd..9fcb311 100644
--- a/motion controller code/.vscode/settings.json	
+++ b/motion controller code/.vscode/settings.json	
@@ -7,6 +7,7 @@
         "stdlib.h": "c",
         "robot.h": "c",
         "stdio.h": "c",
-        "i2c_buffer.h": "c"
+        "i2c_buffer.h": "c",
+        "motors.h": "c"
     }
 }
\ No newline at end of file
diff --git a/motion controller code/Readme.md b/motion controller code/Readme.md
index efc87d1..bb0c0ef 100644
--- a/motion controller code/Readme.md	
+++ b/motion controller code/Readme.md	
@@ -51,4 +51,29 @@ To control a servo motor you need to write data to its adress of the form :
 >uint8_t close;
 >```
 
-Value is 0 or 1 for the open pos or the close pos.
\ No newline at end of file
+Value is 0 or 1 for the open pos or the close pos.
+
+
+Pinout description
+-----------------------------------------------
+
+|Pin |Description                       |GPIO Type  |
+|----|----------------------------------|-----------|
+| 0  | Motor1 Speed                     | PWM       |
+| 1  | Motor2 Speed                     | PWM       |
+| 2  | Motor3 Speed                     | PWM       |
+| 3  | Motor4 Speed                     | PWM       |
+| 4  | Motor1 Dir1                      | OUTPUT    |
+| 5  | Motor1 Dir2                      | OUTPUT    |
+| 6  | Motor2 Dir1                      | OUTPUT    |
+| 7  | Motor2 Dir2                      | OUTPUT    |
+| 8  | Motor3 Dir1                      | OUTPUT    |
+| 9  | Motor3 Dir2                      | OUTPUT    |
+| 10 | Motor4 Dir1                      | OUTPUT    |
+| 11 | Motor5 Dir2                      | OUTPUT    |
+| 12 | Servo1 Angle                     | PWM       |
+| 13 | Servo2 Angle                     | PWM       |
+| 14 | Servo3 Angle                     | PWM       |
+| 15 | Servo4 Angle                     | PWM       |
+| 21 | I2C Bus SDA                      | I2C       |
+| 20 | I2C Bus SCL                      | I2C       |
diff --git a/motion controller code/src/i2c_buffer.c b/motion controller code/src/i2c_buffer.c
index 075b643..e7bf6f1 100644
--- a/motion controller code/src/i2c_buffer.c	
+++ b/motion controller code/src/i2c_buffer.c	
@@ -1,8 +1,8 @@
+#include "include/i2c_buffer.h"
+
 #include "include/robot.h"
 #include "include/motors.h"
 
-#include "include/i2c_buffer.h"
-
 void __not_in_flash_func(i2c_slave_buffer_handler)(i2c_slave_event_t event)
 {
     switch(event)
diff --git a/motion controller code/src/i2c_slave.c b/motion controller code/src/i2c_slave.c
index 59b23cd..d06c92c 100644
--- a/motion controller code/src/i2c_slave.c	
+++ b/motion controller code/src/i2c_slave.c	
@@ -4,12 +4,12 @@
  * SPDX-License-Identifier: MIT
  */
 
+#include "include/i2c_slave.h"
+
 #include <pico/stdlib.h>
 #include <hardware/irq.h>
 #include "include/i2c_buffer.h"
 
-#include "include/i2c_slave.h"
-
 static bool transfer_in_progress;
 
 static inline void finish_transfer(void)
diff --git a/motion controller code/src/include/motors.h b/motion controller code/src/include/motors.h
index f7db597..a5e231a 100644
--- a/motion controller code/src/include/motors.h	
+++ b/motion controller code/src/include/motors.h	
@@ -1,6 +1,8 @@
 #ifndef MOTORS_H
 #define MOTORS_H
 
+#include <pico/stdlib.h>
+
 // Motors
 typedef enum motors_enum_t {
     MOTOR1,
@@ -45,8 +47,8 @@ void init_motors(void);
 void init_servo_motors(void);
 // Set [motor] to 0
 void motor_zero(motors_enum_t motor);
-// Set [motor] in motor_enum_t at [value] between -127 and 128 (for this config)
-void motor_set(motors_enum_t motor, int value);
+// Set [motor] in motor_enum_t at [value] between -127 and 128
+void motor_set(motors_enum_t motor, int8_t value);
 // Set servo motor to its open pos
 void servo_motor_zero(servo_motors_enum_t servo_motor);
 // Set servo to its close pos if [close] else open pos
diff --git a/motion controller code/src/motors.c b/motion controller code/src/motors.c
index 3969bb7..15651bb 100644
--- a/motion controller code/src/motors.c	
+++ b/motion controller code/src/motors.c	
@@ -1,8 +1,8 @@
+#include "include/motors.h"
+
 #include <pico/stdlib.h>
 #include <hardware/pwm.h>
 
-#include "include/motors.h"
-
 const motor_def_t MOTORS_DEFS[] = {
     {0,  4,  5, 0x00},
     {1,  6,  7, 0x01},
@@ -75,7 +75,7 @@ void motor_zero(motors_enum_t motor)
 }
 
 // Set [motor] in motor_enum_t at [value] between -128 and 127 (for this config)
-void motor_set(motors_enum_t motor, int value)
+void motor_set(motors_enum_t motor, int8_t value)
 {
     const motor_def_t *motor_def = &MOTORS_DEFS[motor];
 
diff --git a/motion controller code/src/robot.c b/motion controller code/src/robot.c
index e1c3a85..80d6b28 100644
--- a/motion controller code/src/robot.c	
+++ b/motion controller code/src/robot.c	
@@ -1,9 +1,9 @@
+#include "include/robot.h"
+
 #include <pico/stdlib.h>
 #include "include/motors.h"
 #include "include/i2c_slave.h"
 
-#include "include/robot.h"
-
 void robot_init(void)
 {
     stdio_init_all();