diff --git a/main controller code/src/robot.c b/main controller code/src/robot.c
index 5675500..f6eab3c 100644
--- a/main controller code/src/robot.c	
+++ b/main controller code/src/robot.c	
@@ -3,12 +3,15 @@
 #include <pico/stdlib.h>
 #include <pico/cyw43_arch.h>
 #include <time.h>
+#include <pico/mutex.h>
 #include "include/i2c_master.h"
 #include "include/udp_client.h"
 #include "include/wifi_operator.h"
 
 //#include "include/udp_fake_client.h"
 
+auto_init_mutex(wifi_mutex);
+
 int robot_init(void)
 {
     stdio_init_all();
@@ -20,8 +23,8 @@ int robot_init(void)
 
     cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, 1);
 
-    udp_client_init();
     wifi_operator_init();
+    udp_client_init();
 
     i2c_master_init();
     if(init_gyro()) return -1;
@@ -49,7 +52,9 @@ static inline void update_time(void)
     {
         elapsed_time = 0;
 
+        mutex_enter_blocking(&wifi_mutex);
         cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, led_state);
+        mutex_exit(&wifi_mutex);
 
         led_state = !led_state;
     }
@@ -57,7 +62,9 @@ static inline void update_time(void)
 
 void robot_handle_inputs_outputs(void)
 {
+    mutex_enter_blocking(&wifi_mutex);
     cyw43_arch_poll();
+    mutex_exit(&wifi_mutex);
 
     update_time();
 
@@ -65,6 +72,8 @@ void robot_handle_inputs_outputs(void)
 
     i2c_update_motion_control();
 
+    i2c_update_servo_motors();
+
     tight_loop_contents();
 }
 
diff --git a/main controller code/src/udp_client.c b/main controller code/src/udp_client.c
index 299cad0..56fea9f 100644
--- a/main controller code/src/udp_client.c	
+++ b/main controller code/src/udp_client.c	
@@ -32,7 +32,8 @@ static void default_message_callback(uint8_t *payload, uint16_t len, const ip_ad
 
 void udp_client_init(void)
 {
-    udp_client.message_callback = false ? udp_client_message_handler : default_message_callback;
+    //udp_client.message_callback = udp_client_message_handler;
+    udp_client.message_callback = default_message_callback;
 
     udp_client.pcb = udp_new();
     if(udp_client.pcb == NULL)
diff --git a/main controller code/src/udp_payload.c b/main controller code/src/udp_payload.c
index a7b5153..b4e0ad8 100644
--- a/main controller code/src/udp_payload.c	
+++ b/main controller code/src/udp_payload.c	
@@ -5,6 +5,8 @@
 
 void __not_in_flash_func(udp_client_message_handler)(uint8_t *payload, uint16_t len, const ip_addr_t *addr, u16_t port)
 {
+    if(len != 16) return;
+
     robot.motion_control_data.angle = ((payload[UDP_PAYLOAD_ANGLE_H_BYTE] << 8) | payload[UDP_PAYLOAD_ANGLE_L_BYTE]);
 
     robot.motion_control_data.x_axis_speed = payload[UDP_PAYLOAD_X_AXIS_SPEED_BYTE];
@@ -14,6 +16,6 @@ void __not_in_flash_func(udp_client_message_handler)(uint8_t *payload, uint16_t
     {
         const servo_motor_def_t *SERVO_MOTOR_DEF = &SERVO_MOTORS_DEFS[actual_servo_motor];
 
-        i2c_set_servo_motor(actual_servo_motor, payload[SERVO_MOTOR_DEF->buffer_reg_and_payload_byte]);
+        robot.motion_control_data.servo_motors_pos[actual_servo_motor] = payload[SERVO_MOTOR_DEF->buffer_reg_and_payload_byte];
     }
 }