diff --git a/README.md b/README.md
index bc5703a..d6e51e3 100644
--- a/README.md
+++ b/README.md
@@ -48,12 +48,10 @@ This project implements a real-time speed sensor and CAN communication system on
  - I2C3 SDA  -> PC_1
 
 # How to Extend The Project
- - Implement a mechanism to process incoming data and perform the appropriate actions.
  - Create a Heartbeat mechanism.
  - Integrate Watchdog timer that resets if the system "breaks".
  - Integration tests for CAN.
  - Latency tests
- - Redefine diferent prioritys for threads.
 
 ### Creating new Threads
  - In app_threadx.h, increase THREAD_COUNT by one, and remember to update the comments above that definition.
diff --git a/ThreadX_Os/CMakeLists.txt b/ThreadX_Os/CMakeLists.txt
index 7dbb070..f3112c6 100644
--- a/ThreadX_Os/CMakeLists.txt
+++ b/ThreadX_Os/CMakeLists.txt
@@ -44,20 +44,26 @@ target_link_directories(${CMAKE_PROJECT_NAME} PRIVATE
 # Add sources to executable
 target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     # Add user sources here
+    #i2c_api
+    Core/Src/i2c_ina219.c
+    Core/Src/i2c_pca9685.c
+    #init
     Core/Src/init/init_can.c
     Core/Src/init/init_threads.c
+    #threads
+    #can
     Core/Src/threads/can/rx_can.c
     Core/Src/threads/can/tx_can.c
-    Core/Src/threads/motors/dc_motors.c
-    Core/Src/threads/motors/emergency_brake.c
-    Core/Src/threads/motors/servo.c
+    #motors
+    Core/Src/threads/motors/i2c_driving.c
+    #
     Core/Src/threads/battery.c
+    Core/Src/threads/emergency_brake.c
     Core/Src/threads/speed_sensor.c
+    #utils
     Core/Src/utils/debug_utils.c
     Core/Src/utils/i2c_utils.c
     Core/Src/utils/speed_rpm_utils.c
-    Core/Src/i2c_ina219.c
-    Core/Src/i2c_pca9685.c
 )
 
 # Add include paths
diff --git a/ThreadX_Os/Core/Inc/app_threadx.h b/ThreadX_Os/Core/Inc/app_threadx.h
index f89862b..ec8fc61 100644
--- a/ThreadX_Os/Core/Inc/app_threadx.h
+++ b/ThreadX_Os/Core/Inc/app_threadx.h
@@ -34,7 +34,7 @@ extern "C" {
 #include <string.h>
 #include <stdio.h>
 #include <stdint.h>
-#include <utils.h>
+#include <utils_testing.h>
 
 /* USER CODE END Includes */
 
@@ -42,19 +42,19 @@ extern "C" {
 /* USER CODE BEGIN ET */
 
 // Thread with max priority
-#define MAX_PRIO   0
+#define MAX_PRIO   1
 
 // Thread with medium priority
-#define MEDIUM_PRIO   5
+#define MEDIUM_PRIO   3
 
 // Thread with low priority
-#define LOW_PRIO   10
+#define LOW_PRIO   5
 
 // Thread with no priority (lowest)
-#define NONE_PRIO   15
+#define NONE_PRIO   7
 
 //Queue size (number of messages)
-#define QUEUE_SIZE      8
+#define QUEUE_SIZE      6
 
 /*
 Number of threads
@@ -62,11 +62,10 @@ Number of threads
 2 -> CAN TX thread
 3 -> CAN RX thread
 4 -> dc_motors thread
-5 -> servo thread
-6 -> battery thread
-7 -> emergency brake thread
+5 -> battery thread
+6 -> emergency brake thread
 */
-#define THREAD_COUNT    7
+#define THREAD_COUNT    6
 
 // Thread structure
 typedef struct s_threads {
@@ -77,7 +76,6 @@ typedef struct s_threads {
 // CAN frames structure
 typedef struct s_canFrames {
   FDCAN_TxHeaderTypeDef tx_header_speed;
-  FDCAN_TxHeaderTypeDef tx_header_heart_beat;
   FDCAN_TxHeaderTypeDef tx_header_battery;
 } t_canFrames;
 
@@ -91,9 +89,8 @@ extern TIM_HandleTypeDef    htim1;
 extern I2C_HandleTypeDef    hi2c3;
 
 extern TX_QUEUE             can_tx_queue;
-extern TX_QUEUE             can_emergency_brake_queue;
-extern TX_QUEUE             i2c_dc_motors_queue;
-extern TX_QUEUE             i2c_servo_queue;
+extern TX_QUEUE             i2c_driving_queue;
+extern TX_QUEUE             emergency_brake_queue;
 extern TX_MUTEX             i2c_mutex;
 extern t_threads            threads[THREAD_COUNT];
 /* USER CODE END EC */
@@ -112,6 +109,9 @@ extern t_threads            threads[THREAD_COUNT];
 /* Exported macro ------------------------------------------------------------*/
 /* USER CODE BEGIN EM */
 
+// Macro to convert milliseconds to ThreadX ticks
+#define TX_MS_TO_TICKS(ms)    ((ms) * TX_TIMER_TICKS_PER_SECOND / 1000)
+
 /* USER CODE END EM */
 
 /* Exported functions prototypes ---------------------------------------------*/
@@ -124,8 +124,7 @@ void MX_ThreadX_Init(void);
 VOID  thread_SensorSpeed(ULONG thread_input);
 VOID  thread_tx_can(ULONG thread_input);
 VOID  thread_rx_can(ULONG thread_input);
-VOID  thread_dc_motors(ULONG thread_input);
-VOID  thread_servo(ULONG thread_input);
+VOID  thread_driving_command(ULONG thread_input);
 VOID  thread_battery(ULONG thread_input);
 VOID  thread_emergency_brake(ULONG thread_input);
 
@@ -134,13 +133,6 @@ void  initCanFrames(t_canFrames *canFrames);
 UINT  init_threads(VOID);
 UINT  init_queue(VOID);
 
-//utils
-VOID                uart_send(const char *msg);
-VOID                uart_send_int(int32_t value);
-VOID                rpm_debug_print(ULONG rpm, 
-                        ULONG cr1_reg, ULONG cnt_reg);
-HAL_StatusTypeDef   i2c_scan_bus(VOID);
-
 /* USER CODE END EFP */
 
 /* USER CODE BEGIN 1 */
diff --git a/ThreadX_Os/Core/Inc/can_protocol.h b/ThreadX_Os/Core/Inc/can_protocol.h
index 4c2f55f..b48fdc1 100644
--- a/ThreadX_Os/Core/Inc/can_protocol.h
+++ b/ThreadX_Os/Core/Inc/can_protocol.h
@@ -3,6 +3,8 @@
 
 #include "app_threadx.h"
 
+#define EMERGENCY_BRAKE 150
+
 // CAN message types
 typedef enum {
   CAN_MSG_SPEED,
@@ -22,10 +24,4 @@ typedef struct s_rx_can_message {
   uint8_t   len;
 } t_rx_can_msg;
 
-/* // Steering/throttle CAN message instructions
-typedef struct s_i2c_message {
-    int8_t steering;
-    int8_t throttle;
-} t_i2c_msg; */
-
 #endif
\ No newline at end of file
diff --git a/ThreadX_Os/Core/Inc/i2c_ina219.h b/ThreadX_Os/Core/Inc/i2c_ina219.h
index c13ebbe..9323897 100644
--- a/ThreadX_Os/Core/Inc/i2c_ina219.h
+++ b/ThreadX_Os/Core/Inc/i2c_ina219.h
@@ -1,7 +1,7 @@
 #ifndef I2C_INA219_H
 # define I2C_INA219_H
 
-#include "app_threadx.h"
+#include "utils.h"
 
 #define INA219_ADDR  0x41
 
diff --git a/ThreadX_Os/Core/Inc/i2c_pca9685.h b/ThreadX_Os/Core/Inc/i2c_pca9685.h
index 7c08ec0..11e3d70 100644
--- a/ThreadX_Os/Core/Inc/i2c_pca9685.h
+++ b/ThreadX_Os/Core/Inc/i2c_pca9685.h
@@ -1,7 +1,7 @@
 #ifndef I2C_PCA9685_H
 # define I2C_PCA9685_H
 
-#include "app_threadx.h"
+#include "utils.h"
 
 #define PCA9685_ADDR_SERVO  0x40
 #define PCA9685_ADDR_MOTOR  0x60
diff --git a/ThreadX_Os/Core/Inc/utils.h b/ThreadX_Os/Core/Inc/utils.h
index 0a7cb15..32e6c3e 100644
--- a/ThreadX_Os/Core/Inc/utils.h
+++ b/ThreadX_Os/Core/Inc/utils.h
@@ -1,34 +1,13 @@
 #ifndef UTILS_H_
  #define UTILS_H_
 
-#include <stdint.h>
-
-// Use ThreadX types if available, otherwise define our own
-#ifndef TX_API_H
-    typedef uint32_t    UINT;
-    typedef uint32_t    ULONG;
-    typedef void        VOID;
-#endif
-
-// Timer ticks per second definition
-#ifndef TX_TIMER_TICKS_PER_SECOND
-  #define TX_TIMER_TICKS_PER_SECOND    1000
-#endif
-
-//Maximum RPM value to prevent overflow
-#define MAX_RPM   10000
-
-//Pulses Per Revolution
-#define PPR       40
-
-// RPM calculation state, created for hardware abstraction and testability
-typedef struct s_rpm_state {
-    ULONG   last_time_ticks;
-    ULONG   last_count;
-    UINT    first_run;
-} t_rpm_state;
-
-UINT    convertValuesRPM(ULONG count, ULONG ticks, 
-    ULONG period, t_rpm_state *state);
+#include "app_threadx.h"
+
+HAL_StatusTypeDef   i2c_scan_bus(VOID);
+VOID                uart_send(const char *msg);
+VOID                uart_send_int(int32_t value);
+VOID                battery_debug_print(float voltage);
+VOID                rpm_debug_print(ULONG rpm, 
+                        ULONG cr1_reg, ULONG cnt_reg);
 
 #endif
\ No newline at end of file
diff --git a/ThreadX_Os/Core/Inc/utils_testing.h b/ThreadX_Os/Core/Inc/utils_testing.h
new file mode 100644
index 0000000..beb6197
--- /dev/null
+++ b/ThreadX_Os/Core/Inc/utils_testing.h
@@ -0,0 +1,34 @@
+#ifndef UTILS_TESTING_H_
+ #define UTILS_TESTING_H_
+
+#include <stdint.h>
+
+// Use ThreadX types if available, otherwise define our own
+#ifndef TX_API_H
+    typedef uint32_t    UINT;
+    typedef uint32_t    ULONG;
+    typedef void        VOID;
+#endif
+
+// Timer ticks per second definition
+#ifndef TX_TIMER_TICKS_PER_SECOND
+  #define TX_TIMER_TICKS_PER_SECOND    1000
+#endif
+
+//Maximum RPM value to prevent overflow
+#define MAX_RPM   10000
+
+//Pulses Per Revolution
+#define PPR       40
+
+// RPM calculation state, created for hardware abstraction and testability
+typedef struct s_rpm_state {
+    ULONG   last_time_ticks;
+    ULONG   last_count;
+    UINT    first_run;
+} t_rpm_state;
+
+UINT    convertValuesRPM(ULONG count, ULONG ticks, 
+    ULONG period, t_rpm_state *state);
+
+#endif
\ No newline at end of file
diff --git a/ThreadX_Os/Core/Src/app_threadx.c b/ThreadX_Os/Core/Src/app_threadx.c
index a9924e2..df187da 100644
--- a/ThreadX_Os/Core/Src/app_threadx.c
+++ b/ThreadX_Os/Core/Src/app_threadx.c
@@ -46,9 +46,8 @@
 /* Private variables ---------------------------------------------------------*/
 /* USER CODE BEGIN PV */
 TX_QUEUE    can_tx_queue;
-TX_QUEUE    can_emergency_brake_queue;
-TX_QUEUE    i2c_dc_motors_queue;
-TX_QUEUE    i2c_servo_queue;
+TX_QUEUE    i2c_driving_queue;
+TX_QUEUE    emergency_brake_queue;
 TX_MUTEX    i2c_mutex;
 t_threads   threads[THREAD_COUNT];
 
@@ -79,30 +78,21 @@ UINT App_ThreadX_Init(VOID *memory_ptr)
   if (ret != TX_SUCCESS)
     uart_send("ERROR! Failed TX queue creation.\r\n");
 
-  // Create Emergency Brake queue
-  UCHAR *can_emergency_brake_queue_memory = (UCHAR *)memory_ptr + QUEUE_SIZE * sizeof(t_tx_can_msg);
-  ret = tx_queue_create(&can_emergency_brake_queue, "CAN RX Queue", 
-                        sizeof(t_rx_can_msg) / sizeof(ULONG),
-                        can_emergency_brake_queue_memory, QUEUE_SIZE * sizeof(t_rx_can_msg));
-  if (ret != TX_SUCCESS)
-    uart_send("ERROR! Failed Emergency Brake queue creation.\r\n");
-
   // Create I2C DC Motors queue
-  UCHAR *i2c_motors_queue_memory = can_emergency_brake_queue_memory + QUEUE_SIZE * sizeof(t_rx_can_msg);
-  ret = tx_queue_create(&i2c_dc_motors_queue, "I2C DC Motors Queue", 
+  UCHAR *i2c_motors_queue_memory = (UCHAR *)memory_ptr + QUEUE_SIZE * sizeof(t_tx_can_msg);
+  ret = tx_queue_create(&i2c_driving_queue, "I2C Driving Command Queue", 
                         sizeof(t_rx_can_msg) / sizeof(ULONG),
                         i2c_motors_queue_memory, QUEUE_SIZE * sizeof(t_rx_can_msg));
   if (ret != TX_SUCCESS)
-    uart_send("ERROR! Failed I2C DC Motors queue creation.\r\n");
+    uart_send("ERROR! Failed I2C Driving Command queue creation.\r\n");
 
-  // Create I2C Servo queue
-  UCHAR *i2c_servo_queue_memory = i2c_motors_queue_memory + QUEUE_SIZE * sizeof(t_rx_can_msg);
-  ret = tx_queue_create(&i2c_servo_queue, "I2C Servo Queue", 
-                        sizeof(t_rx_can_msg) / sizeof(ULONG),
-                        i2c_servo_queue_memory, QUEUE_SIZE * sizeof(t_rx_can_msg));
+  // Create Emergency brake queue
+  UCHAR *emergency_brake_queue_memory = i2c_motors_queue_memory + QUEUE_SIZE * sizeof(t_rx_can_msg);
+  ret = tx_queue_create(&emergency_brake_queue, "emergency brake Queue", 
+                        sizeof(t_tx_can_msg) / sizeof(ULONG),
+                        emergency_brake_queue_memory, QUEUE_SIZE * sizeof(t_tx_can_msg));
   if (ret != TX_SUCCESS)
-    uart_send("ERROR! Failed I2C Servo queue creation.\r\n");
-
+    uart_send("ERROR! Failed Emergency brake queue creation.\r\n");
   if (init_threads() != TX_SUCCESS)
     exit(EXIT_FAILURE);
   else
diff --git a/ThreadX_Os/Core/Src/i2c_ina219.c b/ThreadX_Os/Core/Src/i2c_api/i2c_ina219.c
similarity index 93%
rename from ThreadX_Os/Core/Src/i2c_ina219.c
rename to ThreadX_Os/Core/Src/i2c_api/i2c_ina219.c
index ec97064..8769baf 100644
--- a/ThreadX_Os/Core/Src/i2c_ina219.c
+++ b/ThreadX_Os/Core/Src/i2c_api/i2c_ina219.c
@@ -2,8 +2,8 @@
 
 HAL_StatusTypeDef   ina219_init(UINT addr) {
 
-    HAL_StatusTypeDef status;
-    uint8_t data[2];
+    HAL_StatusTypeDef   status;
+    uint8_t             data[2];
 
     // Config:
     // Bus voltage range = 32V
@@ -45,8 +45,8 @@ HAL_StatusTypeDef   ina219_init(UINT addr) {
 
 HAL_StatusTypeDef ina219_read_voltage(float *voltage)
 {
-    uint8_t buf[2];
-    uint16_t raw;
+    uint8_t     buf[2];
+    uint16_t    raw;
 
     tx_mutex_get(&i2c_mutex, TX_WAIT_FOREVER);
 
@@ -93,8 +93,8 @@ HAL_StatusTypeDef ina219_read_current(float *current)
 
 HAL_StatusTypeDef ina219_read_power(float *power)
 {
-    uint8_t buf[2];
-    uint16_t raw;
+    uint8_t     buf[2];
+    uint16_t    raw;
 
     tx_mutex_get(&i2c_mutex, TX_WAIT_FOREVER);
 
diff --git a/ThreadX_Os/Core/Src/i2c_pca9685.c b/ThreadX_Os/Core/Src/i2c_api/i2c_pca9685.c
similarity index 100%
rename from ThreadX_Os/Core/Src/i2c_pca9685.c
rename to ThreadX_Os/Core/Src/i2c_api/i2c_pca9685.c
diff --git a/ThreadX_Os/Core/Src/init/init_threads.c b/ThreadX_Os/Core/Src/init/init_threads.c
index 4b831f8..0293ac0 100644
--- a/ThreadX_Os/Core/Src/init/init_threads.c
+++ b/ThreadX_Os/Core/Src/init/init_threads.c
@@ -8,80 +8,81 @@ UINT    init_threads(VOID)
     // Sensor speed thread
     ret = tx_thread_create(&threads[0].thread, "CANThread", thread_SensorSpeed, 0,
                                   threads[0].stack, 1024,
-                                  NONE_PRIO, NONE_PRIO,
+                                  LOW_PRIO, LOW_PRIO,
                                   TX_NO_TIME_SLICE,
                                   TX_AUTO_START);
-    if (ret != TX_SUCCESS)
+    if (ret != TX_SUCCESS) {
         uart_send("ERROR! Speed sensor thread creation failed!\r\n");
+        exit(EXIT_FAILURE);
+    }
     else
         uart_send("Speed Sensor Thread created successfully.\r\n");
 
     // CAN TX thread
     ret = tx_thread_create(&threads[1].thread, "TxCanThread", thread_tx_can, 0,
                                   threads[1].stack, 1024,
-                                  LOW_PRIO, LOW_PRIO,
+                                  MEDIUM_PRIO, MEDIUM_PRIO,
                                   TX_NO_TIME_SLICE,
                                   TX_AUTO_START);
-    if (ret != TX_SUCCESS)
+    if (ret != TX_SUCCESS) {
         uart_send("ERROR! CAN TX thread creation failed!\r\n");
+        exit(EXIT_FAILURE);
+    }
     else
         uart_send("CAN TX Thread created successfully.\r\n");
 
     // CAN RX thread
     ret = tx_thread_create(&threads[2].thread, "RxCanThread", thread_rx_can, 0,
                                   threads[2].stack, 1024,
-                                  MEDIUM_PRIO, MEDIUM_PRIO,
+                                  MAX_PRIO, MAX_PRIO,
                                   TX_NO_TIME_SLICE,
                                   TX_AUTO_START);
 
-    if (ret != TX_SUCCESS)
+    if (ret != TX_SUCCESS) {
         uart_send("ERROR! CAN RX thread creation failed!\r\n");
+        exit(EXIT_FAILURE);
+    }
     else
         uart_send("CAN RX Thread created successfully.\r\n");
 
-     // DC Motors thread
-    ret = tx_thread_create(&threads[3].thread, "DCMotorsThread", thread_dc_motors, 0,
+     // I2C Driving Command thread
+    ret = tx_thread_create(&threads[3].thread, "I2CDrivingThread", thread_driving_command, 0,
                                   threads[3].stack, 1024,
-                                  MAX_PRIO, MAX_PRIO,
-                                  TX_NO_TIME_SLICE,
-                                  TX_AUTO_START);
-    if (ret != TX_SUCCESS)
-        uart_send("ERROR! DC Motors thread creation failed!\r\n");
-    else
-        uart_send("DC Motors Thread created successfully.\r\n");
-
-    // Servo thread
-    ret = tx_thread_create(&threads[4].thread, "ServoThread", thread_servo, 0,
-                                  threads[4].stack, 1024,
                                   MEDIUM_PRIO, MEDIUM_PRIO,
                                   TX_NO_TIME_SLICE,
                                   TX_AUTO_START);
-    if (ret != TX_SUCCESS)
-        uart_send("ERROR! Servo thread creation failed!\r\n");
+    if (ret != TX_SUCCESS) {
+        uart_send("ERROR! I2C Driving Command thread creation failed!\r\n");
+        exit(EXIT_FAILURE);
+    }
     else
-        uart_send("Servo Thread created successfully.\r\n");
+        uart_send("I2C Driving Command Thread created successfully.\r\n");
 
-     // Emergency brake thread
-    ret = tx_thread_create(&threads[5].thread, "EmergencyBrakeThread", thread_emergency_brake, 0,
-                                  threads[5].stack, 1024,
+    // Emergency brake thread - MAX PRIORITY
+    ret = tx_thread_create(&threads[4].thread, "thread_emergency_brake", thread_emergency_brake, 0,
+                                  threads[4].stack, 1024,
                                   MAX_PRIO, MAX_PRIO,
                                   TX_NO_TIME_SLICE,
                                   TX_AUTO_START);
-    if (ret != TX_SUCCESS)
-        uart_send("ERROR! Emergency Brake thread creation failed!\r\n");
+    if (ret != TX_SUCCESS) {
+        uart_send("ERROR! Emergency brake thread creation failed!\r\n");
+        exit(EXIT_FAILURE);
+    }
     else
-        uart_send("Emergency Brake Thread created successfully.\r\n");
+        uart_send("Emergency brake Thread created successfully.\r\n");
 
-/*     // Battery thread
-    ret = tx_thread_create(&threads[6].thread, "BatteryThread", thread_battery, 0,
-                                  threads[6].stack, 1024,
+    // Battery thread
+    ret = tx_thread_create(&threads[5].thread, "BatteryThread", thread_battery, 0,
+                                  threads[5].stack, 1024,
                                   NONE_PRIO, NONE_PRIO,
                                   TX_NO_TIME_SLICE,
                                   TX_AUTO_START);
-    if (ret != TX_SUCCESS)
-        uart_send("ERROR! Battery thread creation failed!\r\n");
+    if (ret != TX_SUCCESS) {
+         uart_send("ERROR! Battery thread creation failed!\r\n");
+         exit(EXIT_FAILURE);
+    }
     else
-        uart_send("Battery Thread created successfully.\r\n"); */
+        uart_send("Battery Thread created successfully.\r\n");
 
     return (ret);
 }
diff --git a/ThreadX_Os/Core/Src/threads/battery.c b/ThreadX_Os/Core/Src/threads/battery.c
index d43603c..4ddf6bb 100644
--- a/ThreadX_Os/Core/Src/threads/battery.c
+++ b/ThreadX_Os/Core/Src/threads/battery.c
@@ -3,24 +3,32 @@
 
 VOID thread_battery(ULONG thread_input)
 {
-    float vbat;
+    float           vbat;
+    t_tx_can_msg    msg;
 
-    uart_send("Battery Thread started\r\n");
-
-    if (ina219_init(INA219_ADDR) != HAL_OK) {
+    memset(&msg, 0, sizeof(t_tx_can_msg));
+    if (ina219_init(INA219_ADDR) != HAL_OK) 
+    {
         uart_send("Battery Thread: INA219 init failed\r\n");
-        return;
-    }
+        return ;
+    } 
+    else 
+        uart_send("Battery Thread started\r\n");
+    msg.type = CAN_MSG_BATTERY;
 
-    while (1) {
-        if (ina219_read_voltage(&vbat) == HAL_OK) {
-            uart_send("Battery Voltage: ");
-            uart_send_int((int32_t)(vbat * 1000)); // in mV
-            uart_send(" mV\r\n");
-        } else {
+    while (1) 
+    {
+        if (ina219_read_voltage(&vbat) == HAL_OK) 
+        {
+            //battery_debug_print(vbat);
+            msg.data[0] = (uint8_t)vbat;
+            msg.data[1] = (uint8_t)((vbat - msg.data[0]) * 100);
+            msg.data[2] = (uint8_t)((vbat / 12.456f) * 100);
+            if (tx_queue_send(&can_tx_queue, &msg, TX_NO_WAIT) != TX_SUCCESS)
+                uart_send("Battery Thread: Failed to send CAN message\r\n");
+        } 
+        else
             uart_send("Battery Thread: INA219 read voltage failed\r\n");
-        }
-
-        tx_thread_sleep(100); // Sleep for 100 ticks
+        tx_thread_sleep(1500);
     }
 }
diff --git a/ThreadX_Os/Core/Src/threads/can/rx_can.c b/ThreadX_Os/Core/Src/threads/can/rx_can.c
index dfa0b02..5e33d01 100644
--- a/ThreadX_Os/Core/Src/threads/can/rx_can.c
+++ b/ThreadX_Os/Core/Src/threads/can/rx_can.c
@@ -32,22 +32,22 @@ VOID    thread_rx_can(ULONG thread_input)
             switch(msg.type) 
             {
                 case 0x100: // Emergency brake
-                    if (tx_queue_send(&can_emergency_brake_queue, &msg, TX_NO_WAIT) != TX_SUCCESS)
-                        uart_send("ERROR: Emergency brake queue FULL!\r\n");
+                    if (tx_queue_send(&emergency_brake_queue, &msg, TX_NO_WAIT) != TX_SUCCESS)
+                        uart_send("ERROR: emergency brake queue FULL!\r\n");
+                    uart_send("Emergency brake CAN msg received\r\n");
                     break ;
-                case 0x101: // throttle
-                    if (tx_queue_send(&i2c_dc_motors_queue, &msg, TX_NO_WAIT) != TX_SUCCESS)
-                        uart_send("ERROR: DC motors queue FULL!\r\n");
-                    break ;
-                case 0x102: // Steering
-                    if (tx_queue_send(&i2c_servo_queue, &msg, TX_NO_WAIT) != TX_SUCCESS)
-                        uart_send("ERROR: Servo queue FULL!\r\n");
+
+                case 0x101: // driving command
+                    if (tx_queue_send(&i2c_driving_queue, &msg, TX_NO_WAIT) != TX_SUCCESS)
+                        uart_send("ERROR: Driving command queue FULL!\r\n");
+                    uart_send("Driving command CAN msg received\r\n");
                     break ;
+
                 default:
                     uart_send("Received UNKNOWN CAN MSG\r\n");
                     break ;
             }
         }
-        tx_thread_sleep(1); // Sleep for 1 tick to avoid busy waiting
+        tx_thread_sleep(1);
     }
 }
diff --git a/ThreadX_Os/Core/Src/threads/can/tx_can.c b/ThreadX_Os/Core/Src/threads/can/tx_can.c
index 94c56bc..96e9d42 100644
--- a/ThreadX_Os/Core/Src/threads/can/tx_can.c
+++ b/ThreadX_Os/Core/Src/threads/can/tx_can.c
@@ -3,9 +3,7 @@
 // Function responsible to transmit CAN messages.
 VOID thread_tx_can(ULONG thread_input)
 {
-    //can message to send
     t_tx_can_msg    msg;
-    //can frames configuration
     t_canFrames     canFrames;
 
     memset(&msg, 0, sizeof(t_tx_can_msg));
@@ -14,7 +12,7 @@ VOID thread_tx_can(ULONG thread_input)
     if (!canFrames.tx_header_speed.Identifier ||
             !canFrames.tx_header_battery.Identifier) {
         uart_send("CAN frames not initialized!\r\n");
-        return ;
+        exit(EXIT_FAILURE);
     }
     while (1) 
     {
@@ -40,10 +38,9 @@ VOID thread_tx_can(ULONG thread_input)
                     break ;
 
                 default:
-                    uart_send("Unknown CAN message type\r\n");
-                    break;
+                    uart_send("Impossible to send this message, unknown ID...\r\n");
+                    break ;
             }
         }
-        tx_thread_sleep(1);
     }
 }
diff --git a/ThreadX_Os/Core/Src/threads/emergency_brake.c b/ThreadX_Os/Core/Src/threads/emergency_brake.c
new file mode 100644
index 0000000..3bb6298
--- /dev/null
+++ b/ThreadX_Os/Core/Src/threads/emergency_brake.c
@@ -0,0 +1,22 @@
+#include "app_threadx.h"
+#include "can_protocol.h"
+#include "i2c_pca9685.h"
+
+VOID  thread_emergency_brake(ULONG thread_input) 
+{
+    t_rx_can_msg    msg;
+    t_rx_can_msg    msg_to_send;
+    memset(&msg, 0, sizeof(t_rx_can_msg));
+    memset(&msg_to_send, 0, sizeof(t_rx_can_msg));
+
+    msg_to_send.data[0] = EMERGENCY_BRAKE;
+
+    while (1)
+    {
+        // Wait for emergency brake message from Raspberry Pi
+        if (tx_queue_receive(&emergency_brake_queue, &msg, TX_WAIT_FOREVER) == TX_SUCCESS) {
+            uart_send("Emergency message sent to i2c thread\r\n");
+            tx_queue_send(&i2c_driving_queue, &msg_to_send, TX_NO_WAIT);
+        }
+    }
+}
diff --git a/ThreadX_Os/Core/Src/threads/motors/dc_motors.c b/ThreadX_Os/Core/Src/threads/motors/dc_motors.c
deleted file mode 100644
index c029104..0000000
--- a/ThreadX_Os/Core/Src/threads/motors/dc_motors.c
+++ /dev/null
@@ -1,37 +0,0 @@
-#include "i2c_pca9685.h"
-#include "can_protocol.h"
-
-VOID thread_dc_motors(ULONG initial_input)
-{
-    t_rx_can_msg    msg;
-    t_rx_can_msg    last_msg;
-
-    if (pca9685_init(PCA9685_ADDR_MOTOR) != HAL_OK) 
-    {
-        uart_send("DC Motors Thread: PCA9685 initialization failed\r\n");
-        return ;
-    } else 
-        uart_send("DC Motors Thread: PCA9685 initialized successfully\r\n");
-
-    while (1)
-    {
-        // waits permanently for a new message in the queue
-        if (tx_queue_receive(&i2c_dc_motors_queue, &msg, TX_WAIT_FOREVER) == TX_SUCCESS)
-        {
-            // Empties the queue and always takes the last available message
-            last_msg = msg;
-            while (tx_queue_receive(&i2c_dc_motors_queue, &last_msg, TX_NO_WAIT) == TX_SUCCESS)
-                msg = last_msg;
-
-            if (msg.len >= 2)
-            {
-                int16_t throttle = (int16_t)(msg.data[0] | (msg.data[1] << 8));
-                if (motor_set(MOTOR_LEFT, -throttle, 0) != HAL_OK)
-                    uart_send("DC Motors Thread: Failed to set LEFT motor speed\r\n");
-
-                if (motor_set(MOTOR_RIGHT, throttle, 0) != HAL_OK)
-                    uart_send("DC Motors Thread: Failed to set RIGHT motor speed\r\n");
-            }
-        }
-    }
-}
diff --git a/ThreadX_Os/Core/Src/threads/motors/emergency_brake.c b/ThreadX_Os/Core/Src/threads/motors/emergency_brake.c
deleted file mode 100644
index 684ac6e..0000000
--- a/ThreadX_Os/Core/Src/threads/motors/emergency_brake.c
+++ /dev/null
@@ -1,19 +0,0 @@
-#include "i2c_pca9685.h"
-#include "can_protocol.h"
-
-VOID  thread_emergency_brake(ULONG thread_input) 
-{
-    t_rx_can_msg    msg;
-    //t_rx_can_msg    last_msg;
-
-    tx_thread_sleep(100); // Allow other threads to initialize
-    while (1) 
-    {
-        if (tx_queue_receive(&can_emergency_brake_queue, &msg, TX_WAIT_FOREVER) == TX_SUCCESS) 
-        {
-            motor_set(MOTOR_LEFT, 0, 1);
-            motor_set(MOTOR_RIGHT, 0, 1);
-            tx_thread_sleep(500);
-        }
-    }
-}
\ No newline at end of file
diff --git a/ThreadX_Os/Core/Src/threads/motors/i2c_driving.c b/ThreadX_Os/Core/Src/threads/motors/i2c_driving.c
new file mode 100644
index 0000000..11d5909
--- /dev/null
+++ b/ThreadX_Os/Core/Src/threads/motors/i2c_driving.c
@@ -0,0 +1,52 @@
+#include "i2c_pca9685.h"
+#include "can_protocol.h"
+
+VOID thread_driving_command(ULONG initial_input)
+{
+    t_rx_can_msg    msg;
+    t_rx_can_msg    last_msg;
+
+    if (pca9685_init(PCA9685_ADDR_MOTOR) != HAL_OK ||
+        pca9685_init(PCA9685_ADDR_SERVO) != HAL_OK)
+    {
+        uart_send("Driving Command Thread: PCA9685 initialization failed\r\n");
+        return ;
+    } else 
+        uart_send("Driving Command Thread: PCA9685 initialized successfully\r\n");
+
+    while (1)
+    {
+        // waits permanently for a new message in the queue
+        if (tx_queue_receive(&i2c_driving_queue, &msg, TX_WAIT_FOREVER) == TX_SUCCESS)
+        {
+            // Empties the queue and always takes the last available message
+            last_msg = msg;
+            while (tx_queue_receive(&i2c_driving_queue, &last_msg, TX_NO_WAIT) == TX_SUCCESS)
+                msg = last_msg;
+
+            if (msg.data[0] == EMERGENCY_BRAKE)
+            {
+                uart_send("EMERGENCY BREAK ACTIVATED!\r\n");
+                if (motor_set(MOTOR_LEFT, 0, 1) != HAL_OK)
+                    uart_send("Driving Command Thread: Failed to set emergency break in LEFT motor\r\n");
+                if (motor_set(MOTOR_RIGHT, 0, 1) != HAL_OK)
+                    uart_send("Driving Command Thread: Failed to set emergency break in RIGHT motor\r\n");
+            }
+            else if (msg.len >= 4)
+            {
+                int16_t throttle = (int16_t)(msg.data[0] | (msg.data[1] << 8));
+                int16_t steering = (int16_t)(msg.data[2] | (msg.data[3] << 8));
+
+                // Control motors
+                if (motor_set(MOTOR_LEFT, -throttle, 0) != HAL_OK)
+                    uart_send("Driving Command Thread: Failed to set LEFT motor speed\r\n");
+                if (motor_set(MOTOR_RIGHT, throttle, 0) != HAL_OK)
+                    uart_send("Driving Command Thread: Failed to set RIGHT motor speed\r\n");
+
+                // Control servo
+                if (pca9685_set_servo_angle(0, steering) != HAL_OK)
+                    uart_send("Driving Command Thread: Failed to set servo angle\r\n");
+            }
+        }
+    }
+}
diff --git a/ThreadX_Os/Core/Src/threads/motors/servo.c b/ThreadX_Os/Core/Src/threads/motors/servo.c
deleted file mode 100644
index f4cfc96..0000000
--- a/ThreadX_Os/Core/Src/threads/motors/servo.c
+++ /dev/null
@@ -1,25 +0,0 @@
-#include "i2c_pca9685.h"
-#include "can_protocol.h"
-
-VOID    thread_servo(ULONG initial_input)
-{
-    t_rx_can_msg    msg;
-    memset(&msg, 0, sizeof(t_rx_can_msg));
-
-    if (pca9685_init(PCA9685_ADDR_SERVO) != HAL_OK) {
-        uart_send("Servo Thread: PCA9685 initialization failed\r\n");
-        return ;
-    }
-
-    while (1)
-    {
-        if (tx_queue_receive(&i2c_servo_queue, &msg, TX_WAIT_FOREVER) == TX_SUCCESS) {
-            
-            if (msg.len >= 2) {
-                int16_t steering = (int16_t)(msg.data[0] | (msg.data[1] << 8));
-                if (pca9685_set_servo_angle(0, steering) != HAL_OK)
-                uart_send("Servo Thread: Failed to set servo angle\r\n");
-            }
-        }
-    }
-}
diff --git a/ThreadX_Os/Core/Src/threads/speed_sensor.c b/ThreadX_Os/Core/Src/threads/speed_sensor.c
index 0b854b3..865e0d3 100644
--- a/ThreadX_Os/Core/Src/threads/speed_sensor.c
+++ b/ThreadX_Os/Core/Src/threads/speed_sensor.c
@@ -39,10 +39,10 @@ VOID thread_SensorSpeed(ULONG thread_input)
         if (ret != TX_SUCCESS)
         {
             uart_send("CAN TX could not add message to queue!\r\n");
-            tx_thread_sleep(200);
+            tx_thread_sleep(TX_MS_TO_TICKS(200));
             continue ;
         }
-        tx_thread_sleep(50);
+        tx_thread_sleep(TX_MS_TO_TICKS(50));
     }
 }
 
diff --git a/ThreadX_Os/Core/Src/utils/debug_utils.c b/ThreadX_Os/Core/Src/utils/debug_utils.c
index 7a4de3d..3cb1157 100644
--- a/ThreadX_Os/Core/Src/utils/debug_utils.c
+++ b/ThreadX_Os/Core/Src/utils/debug_utils.c
@@ -1,4 +1,4 @@
-#include "app_threadx.h"
+#include "utils.h"
 
 // Function to send a string over UART
 VOID    uart_send(const char *msg) 
@@ -25,4 +25,15 @@ VOID    rpm_debug_print(ULONG rpm, ULONG cr1_reg, ULONG cnt_reg) {
 
     if (len > 0 && (size_t)len < sizeof(debug))
         HAL_UART_Transmit(&huart1, (uint8_t *)debug, len, 100);
+}
+
+VOID    battery_debug_print(float voltage)
+{
+    uart_send("Battery Voltage: ");
+    uart_send_int((int32_t)(voltage * 1000)); // in mV
+    uart_send(" mV\r\n");
+
+    uart_send("Battery Percentage: ");
+    uart_send_int((int32_t)((voltage / 12.444f) * 100)); // in percentage
+    uart_send(" %\r\n");
 }
\ No newline at end of file
diff --git a/ThreadX_Os/Core/Src/utils/speed_rpm_utils.c b/ThreadX_Os/Core/Src/utils/speed_rpm_utils.c
index a23cfc0..4597685 100644
--- a/ThreadX_Os/Core/Src/utils/speed_rpm_utils.c
+++ b/ThreadX_Os/Core/Src/utils/speed_rpm_utils.c
@@ -1,4 +1,4 @@
-#include "utils.h"
+#include "utils_testing.h"
 
 // RPM = pulse / PPR * (60 / dt_seconds)
 // RPM calculation from timer values
diff --git a/tests/unit/test/test_ut_rpm.c b/tests/unit/test/test_ut_rpm.c
index 480aef2..c8ac85c 100644
--- a/tests/unit/test/test_ut_rpm.c
+++ b/tests/unit/test/test_ut_rpm.c
@@ -1,5 +1,5 @@
 #include "unity.h"
-#include "utils.h"
+#include "utils_testing.h"
 
 // file being tested
 TEST_SOURCE_FILE("speed_rpm_utils.c");