diff --git a/main controller code/CMakeLists.txt b/main controller code/CMakeLists.txt
index 9da1f4d..e3b07dc 100644
--- a/main controller code/CMakeLists.txt	
+++ b/main controller code/CMakeLists.txt	
@@ -23,9 +23,9 @@ add_executable(main_controller
   src/gyro.c
   src/motion_control.c
   src/i2c_master.c
+  src/wifi_operator.c
   src/udp_client.c
   src/udp_payload.c
-  src/wifi_operator.c
 )
 
 target_include_directories(main_controller PRIVATE
diff --git a/main controller code/src/include/i2c_master.h b/main controller code/src/include/i2c_master.h
index 9d59aa0..1a0ec0f 100644
--- a/main controller code/src/include/i2c_master.h	
+++ b/main controller code/src/include/i2c_master.h	
@@ -6,7 +6,7 @@
 #define I2C_MASTER_SDA_PIN 4
 #define I2C_MASTER_SCL_PIN 5
 #define I2C_MASTER_INSTANCE i2c0
-#define I2C_MASTER_BAUD_RATE 100 * 1000
+#define I2C_MASTER_BAUD_RATE 300 * 1000
 
 // Init master i2c
 void i2c_master_init(void);
diff --git a/main controller code/src/include/motion_control.h b/main controller code/src/include/motion_control.h
index 0b190cb..0363efb 100644
--- a/main controller code/src/include/motion_control.h	
+++ b/main controller code/src/include/motion_control.h	
@@ -2,17 +2,20 @@
 #define MOTION_CONTROL_H
 
 #include <stdint.h>
+#include "motors.h"
 
 typedef struct motion_control_data_t {
     int16_t angle;
     int8_t x_axis_speed;
     int8_t y_axis_speed;
+    uint8_t servo_motors_pos[NB_SERVO_MOTORS];
 } motion_control_data_t;
 
 // Init values for motion control
 void init_motion_control(void);
-
-// Update motion control buffer from udp buffer and gyro data
+// Update motion control buffer from motion control data and gyro data
 void i2c_update_motion_control(void);
+// Update servo motors from motion control data
+void i2c_update_servo_motors(void);
 
 #endif // MOTION_CONTROL_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
index 59bdcea..0d00ccd 100644
--- a/main controller code/src/motion_control.c	
+++ b/main controller code/src/motion_control.c	
@@ -56,3 +56,9 @@ void i2c_update_motion_control(void)
     i2c_set_motor(MOTOR3, motor3_speed);
     i2c_set_motor(MOTOR4, motor4_speed);
 }
+
+void i2c_update_servo_motors(void)
+{
+    for(servo_motors_enum_t actual_servo_motor = SERVO_MOTOR1; actual_servo_motor < NB_SERVO_MOTORS; actual_servo_motor++)
+        i2c_set_servo_motor(actual_servo_motor, robot.motion_control_data.servo_motors_pos[actual_servo_motor]);
+}