From 2cab330b51907a6684e2dc12c0a31fdd8722abf5 Mon Sep 17 00:00:00 2001
From: jose5556 <josefilmeneses@gmail.com>
Date: Wed, 4 Feb 2026 14:19:08 +0000
Subject: [PATCH 1/9] Add heartbeat queue and improve CAN message logging

---
 ThreadX_Os/Core/Inc/app_threadx.h        | 1 +
 ThreadX_Os/Core/Src/threads/can/rx_can.c | 4 ++++
 ThreadX_Os/Core/Src/threads/can/tx_can.c | 2 +-
 3 files changed, 6 insertions(+), 1 deletion(-)

diff --git a/ThreadX_Os/Core/Inc/app_threadx.h b/ThreadX_Os/Core/Inc/app_threadx.h
index f89862b..e3680be 100644
--- a/ThreadX_Os/Core/Inc/app_threadx.h
+++ b/ThreadX_Os/Core/Inc/app_threadx.h
@@ -94,6 +94,7 @@ 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             heartbeat_queue;
 extern TX_MUTEX             i2c_mutex;
 extern t_threads            threads[THREAD_COUNT];
 /* USER CODE END EC */
diff --git a/ThreadX_Os/Core/Src/threads/can/rx_can.c b/ThreadX_Os/Core/Src/threads/can/rx_can.c
index dfa0b02..70dcfd8 100644
--- a/ThreadX_Os/Core/Src/threads/can/rx_can.c
+++ b/ThreadX_Os/Core/Src/threads/can/rx_can.c
@@ -38,11 +38,15 @@ VOID    thread_rx_can(ULONG thread_input)
                 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");
+                    uart_send("DC motor CAN msg received\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");
+                    uart_send("Servo CAN msg received\r\n");
                     break ;
+                case 0x103:
+                    
                 default:
                     uart_send("Received UNKNOWN CAN MSG\r\n");
                     break ;
diff --git a/ThreadX_Os/Core/Src/threads/can/tx_can.c b/ThreadX_Os/Core/Src/threads/can/tx_can.c
index 94c56bc..4dce355 100644
--- a/ThreadX_Os/Core/Src/threads/can/tx_can.c
+++ b/ThreadX_Os/Core/Src/threads/can/tx_can.c
@@ -28,7 +28,7 @@ VOID thread_tx_can(ULONG thread_input)
                     &hfdcan1,
                     &canFrames.tx_header_speed,
                     msg.data);
-                    uart_send("Speed CAN message sent\r\n");
+                    //uart_send("Speed CAN message sent\r\n");
                     break ;
 
                 case CAN_MSG_BATTERY:

From 656b3973a5f2e7345c8883f21c49261fc33e4b69 Mon Sep 17 00:00:00 2001
From: jose5556 <josefilmeneses@gmail.com>
Date: Thu, 5 Feb 2026 19:55:06 +0000
Subject: [PATCH 2/9] Add heartbeat thread and CAN message handling for
 heartbeat signals

---
 ThreadX_Os/CMakeLists.txt                     |  1 +
 ThreadX_Os/Core/Inc/app_threadx.h             | 11 +++--
 ThreadX_Os/Core/Inc/can_protocol.h            |  7 +--
 ThreadX_Os/Core/Src/app_threadx.c             |  9 ++++
 ThreadX_Os/Core/Src/init/init_can.c           | 15 +++++++
 ThreadX_Os/Core/Src/init/init_threads.c       | 43 +++++++++++++++----
 ThreadX_Os/Core/Src/threads/can/rx_can.c      |  8 +++-
 ThreadX_Os/Core/Src/threads/can/tx_can.c      | 19 +++++---
 ThreadX_Os/Core/Src/threads/heartbeat.c       | 22 ++++++++++
 .../Core/Src/threads/motors/emergency_brake.c |  3 +-
 10 files changed, 111 insertions(+), 27 deletions(-)
 create mode 100644 ThreadX_Os/Core/Src/threads/heartbeat.c

diff --git a/ThreadX_Os/CMakeLists.txt b/ThreadX_Os/CMakeLists.txt
index 7dbb070..ec83c4b 100644
--- a/ThreadX_Os/CMakeLists.txt
+++ b/ThreadX_Os/CMakeLists.txt
@@ -52,6 +52,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     Core/Src/threads/motors/emergency_brake.c
     Core/Src/threads/motors/servo.c
     Core/Src/threads/battery.c
+    Core/Src/threads/heartbeat.c
     Core/Src/threads/speed_sensor.c
     Core/Src/utils/debug_utils.c
     Core/Src/utils/i2c_utils.c
diff --git a/ThreadX_Os/Core/Inc/app_threadx.h b/ThreadX_Os/Core/Inc/app_threadx.h
index e3680be..317146e 100644
--- a/ThreadX_Os/Core/Inc/app_threadx.h
+++ b/ThreadX_Os/Core/Inc/app_threadx.h
@@ -54,7 +54,7 @@ extern "C" {
 #define NONE_PRIO   15
 
 //Queue size (number of messages)
-#define QUEUE_SIZE      8
+#define QUEUE_SIZE      16
 
 /*
 Number of threads
@@ -65,8 +65,9 @@ Number of threads
 5 -> servo thread
 6 -> battery thread
 7 -> emergency brake thread
+8 -> heartbeat thread
 */
-#define THREAD_COUNT    7
+#define THREAD_COUNT    8
 
 // Thread structure
 typedef struct s_threads {
@@ -77,7 +78,7 @@ 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_heartbeat;
   FDCAN_TxHeaderTypeDef tx_header_battery;
 } t_canFrames;
 
@@ -113,6 +114,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 ---------------------------------------------*/
@@ -129,6 +133,7 @@ VOID  thread_dc_motors(ULONG thread_input);
 VOID  thread_servo(ULONG thread_input);
 VOID  thread_battery(ULONG thread_input);
 VOID  thread_emergency_brake(ULONG thread_input);
+VOID  thread_heartbeat(ULONG thread_input);
 
 //init
 void  initCanFrames(t_canFrames *canFrames);
diff --git a/ThreadX_Os/Core/Inc/can_protocol.h b/ThreadX_Os/Core/Inc/can_protocol.h
index 4c2f55f..94abb9a 100644
--- a/ThreadX_Os/Core/Inc/can_protocol.h
+++ b/ThreadX_Os/Core/Inc/can_protocol.h
@@ -7,6 +7,7 @@
 typedef enum {
   CAN_MSG_SPEED,
   CAN_MSG_BATTERY,
+  CAN_MSG_HEARTBEAT
 } e_can_msg_type;
 
 // TX CAN message structure (sending)
@@ -22,10 +23,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/Src/app_threadx.c b/ThreadX_Os/Core/Src/app_threadx.c
index a9924e2..57627e2 100644
--- a/ThreadX_Os/Core/Src/app_threadx.c
+++ b/ThreadX_Os/Core/Src/app_threadx.c
@@ -49,6 +49,7 @@ TX_QUEUE    can_tx_queue;
 TX_QUEUE    can_emergency_brake_queue;
 TX_QUEUE    i2c_dc_motors_queue;
 TX_QUEUE    i2c_servo_queue;
+TX_QUEUE    heartbeat_queue;
 TX_MUTEX    i2c_mutex;
 t_threads   threads[THREAD_COUNT];
 
@@ -103,6 +104,14 @@ UINT App_ThreadX_Init(VOID *memory_ptr)
   if (ret != TX_SUCCESS)
     uart_send("ERROR! Failed I2C Servo queue creation.\r\n");
 
+  // Create Heartbeat queue
+  UCHAR *heartbeat_queue_memory = i2c_servo_queue_memory + QUEUE_SIZE * sizeof(t_rx_can_msg);
+  ret = tx_queue_create(&heartbeat_queue, "Heartbeat Queue", 
+                        sizeof(t_tx_can_msg) / sizeof(ULONG),
+                        heartbeat_queue_memory, QUEUE_SIZE * sizeof(t_tx_can_msg));
+  if (ret != TX_SUCCESS)
+    uart_send("ERROR! Failed Heartbeat queue creation.\r\n");
+
   if (init_threads() != TX_SUCCESS)
     exit(EXIT_FAILURE);
   else
diff --git a/ThreadX_Os/Core/Src/init/init_can.c b/ThreadX_Os/Core/Src/init/init_can.c
index 7ab1165..ff5e16e 100644
--- a/ThreadX_Os/Core/Src/init/init_can.c
+++ b/ThreadX_Os/Core/Src/init/init_can.c
@@ -2,12 +2,14 @@
 
 static void TxSpeedConf(FDCAN_TxHeaderTypeDef *TxHeader);
 static void TxBatteryConf(FDCAN_TxHeaderTypeDef *TxHeader);
+static void TxHeartbeatConf(FDCAN_TxHeaderTypeDef *TxHeader);
 
 // Configuration of CAN frame for speed data
 void initCanFrames(t_canFrames *canFrames) 
 {
     TxSpeedConf(&canFrames->tx_header_speed);
     TxBatteryConf(&canFrames->tx_header_battery);
+    TxHeartbeatConf(&canFrames->tx_header_heartbeat);
 }
 
 static void    TxSpeedConf(FDCAN_TxHeaderTypeDef *TxHeader) 
@@ -35,3 +37,16 @@ static void    TxBatteryConf(FDCAN_TxHeaderTypeDef *TxHeader)
     TxHeader->TxEventFifoControl = FDCAN_NO_TX_EVENTS;
     TxHeader->MessageMarker = 0x0;
 }
+
+static void    TxHeartbeatConf(FDCAN_TxHeaderTypeDef *TxHeader) 
+{
+    TxHeader->Identifier = 0x202;
+    TxHeader->IdType = FDCAN_STANDARD_ID;
+    TxHeader->TxFrameType = FDCAN_DATA_FRAME;
+    TxHeader->DataLength = FDCAN_DLC_BYTES_8;
+    TxHeader->ErrorStateIndicator = FDCAN_ESI_ACTIVE;
+    TxHeader->BitRateSwitch = FDCAN_BRS_OFF;
+    TxHeader->FDFormat = FDCAN_CLASSIC_CAN;
+    TxHeader->TxEventFifoControl = FDCAN_NO_TX_EVENTS;
+    TxHeader->MessageMarker = 0x0;
+}
\ No newline at end of file
diff --git a/ThreadX_Os/Core/Src/init/init_threads.c b/ThreadX_Os/Core/Src/init/init_threads.c
index 4b831f8..8f43e96 100644
--- a/ThreadX_Os/Core/Src/init/init_threads.c
+++ b/ThreadX_Os/Core/Src/init/init_threads.c
@@ -11,8 +11,10 @@ UINT    init_threads(VOID)
                                   NONE_PRIO, NONE_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");
 
@@ -22,8 +24,10 @@ UINT    init_threads(VOID)
                                   LOW_PRIO, LOW_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");
 
@@ -34,8 +38,10 @@ UINT    init_threads(VOID)
                                   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");
 
@@ -45,8 +51,10 @@ UINT    init_threads(VOID)
                                   MAX_PRIO, MAX_PRIO,
                                   TX_NO_TIME_SLICE,
                                   TX_AUTO_START);
-    if (ret != TX_SUCCESS)
+    if (ret != TX_SUCCESS) {
         uart_send("ERROR! DC Motors thread creation failed!\r\n");
+        exit(EXIT_FAILURE);
+    }
     else
         uart_send("DC Motors Thread created successfully.\r\n");
 
@@ -56,8 +64,10 @@ UINT    init_threads(VOID)
                                   MEDIUM_PRIO, MEDIUM_PRIO,
                                   TX_NO_TIME_SLICE,
                                   TX_AUTO_START);
-    if (ret != TX_SUCCESS)
+    if (ret != TX_SUCCESS) {
         uart_send("ERROR! Servo thread creation failed!\r\n");
+        exit(EXIT_FAILURE);
+    }
     else
         uart_send("Servo Thread created successfully.\r\n");
 
@@ -67,8 +77,10 @@ UINT    init_threads(VOID)
                                   MAX_PRIO, MAX_PRIO,
                                   TX_NO_TIME_SLICE,
                                   TX_AUTO_START);
-    if (ret != TX_SUCCESS)
+    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");
 
@@ -78,10 +90,25 @@ UINT    init_threads(VOID)
                                   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"); */
 
+    // Heartbeat thread
+    ret = tx_thread_create(&threads[7].thread, "HeartbeatThread", thread_heartbeat, 0,
+                                  threads[7].stack, 1024,
+                                  LOW_PRIO, LOW_PRIO,
+                                  TX_NO_TIME_SLICE,
+                                  TX_AUTO_START);
+    if (ret != TX_SUCCESS) {
+        uart_send("ERROR! Heartbeat thread creation failed!\r\n");
+        exit(EXIT_FAILURE);
+    }
+    else
+        uart_send("Heartbeat Thread created successfully.\r\n");
+
     return (ret);
 }
diff --git a/ThreadX_Os/Core/Src/threads/can/rx_can.c b/ThreadX_Os/Core/Src/threads/can/rx_can.c
index 70dcfd8..8722a7a 100644
--- a/ThreadX_Os/Core/Src/threads/can/rx_can.c
+++ b/ThreadX_Os/Core/Src/threads/can/rx_can.c
@@ -45,8 +45,12 @@ VOID    thread_rx_can(ULONG thread_input)
                         uart_send("ERROR: Servo queue FULL!\r\n");
                     uart_send("Servo CAN msg received\r\n");
                     break ;
-                case 0x103:
-                    
+                case 0x103: // Heartbeat
+                    if (tx_queue_send(&heartbeat_queue, &msg, TX_NO_WAIT) != TX_SUCCESS)
+                        uart_send("ERROR: Heartbeat queue FULL!\r\n");
+                    uart_send("Heartbeat CAN msg received\r\n");
+                    break ;
+
                 default:
                     uart_send("Received UNKNOWN CAN MSG\r\n");
                     break ;
diff --git a/ThreadX_Os/Core/Src/threads/can/tx_can.c b/ThreadX_Os/Core/Src/threads/can/tx_can.c
index 4dce355..90b2279 100644
--- a/ThreadX_Os/Core/Src/threads/can/tx_can.c
+++ b/ThreadX_Os/Core/Src/threads/can/tx_can.c
@@ -3,18 +3,17 @@
 // 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));
     memset(&canFrames, 0, sizeof(t_canFrames));
     initCanFrames(&canFrames);
     if (!canFrames.tx_header_speed.Identifier ||
-            !canFrames.tx_header_battery.Identifier) {
+            !canFrames.tx_header_battery.Identifier
+            || !canFrames.tx_header_heartbeat.Identifier) {
         uart_send("CAN frames not initialized!\r\n");
-        return ;
+        exit(EXIT_FAILURE);
     }
     while (1) 
     {
@@ -39,9 +38,17 @@ VOID thread_tx_can(ULONG thread_input)
                     uart_send("Battery CAN message sent\r\n");
                     break ;
 
+                case CAN_MSG_HEARTBEAT:
+                    HAL_FDCAN_AddMessageToTxFifoQ(
+                        &hfdcan1,
+                        &canFrames.tx_header_heartbeat,
+                        msg.data);
+                    uart_send("Heartbeat CAN message sent\r\n");
+                    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/heartbeat.c b/ThreadX_Os/Core/Src/threads/heartbeat.c
new file mode 100644
index 0000000..64960c3
--- /dev/null
+++ b/ThreadX_Os/Core/Src/threads/heartbeat.c
@@ -0,0 +1,22 @@
+#include "app_threadx.h"
+#include "can_protocol.h"
+
+VOID  thread_heartbeat(ULONG thread_input) {
+    t_tx_can_msg    heartbeat_msg;
+    t_tx_can_msg    msg;
+    memset(&heartbeat_msg, 0, sizeof(t_tx_can_msg));
+    memset(&msg, 0, sizeof(t_tx_can_msg));
+
+    msg.data[0] = 0x01;
+    msg.type = CAN_MSG_HEARTBEAT;
+
+    while (1)
+    {
+        // Sends heartbeat message to CAN thread
+        if (tx_queue_receive(&heartbeat_queue, &heartbeat_msg, TX_MS_TO_TICKS(1000)) != TX_SUCCESS)
+            uart_send("Heartbeat timeout! No heartbeat received in the last second.\r\n");
+
+        if (tx_queue_send(&can_tx_queue, &msg, TX_NO_WAIT) != TX_SUCCESS)
+            uart_send("CAN TX could not add heartbeat message to queue!\r\n");
+    }
+}
diff --git a/ThreadX_Os/Core/Src/threads/motors/emergency_brake.c b/ThreadX_Os/Core/Src/threads/motors/emergency_brake.c
index 684ac6e..6f9fdce 100644
--- a/ThreadX_Os/Core/Src/threads/motors/emergency_brake.c
+++ b/ThreadX_Os/Core/Src/threads/motors/emergency_brake.c
@@ -4,7 +4,6 @@
 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) 
@@ -16,4 +15,4 @@ VOID  thread_emergency_brake(ULONG thread_input)
             tx_thread_sleep(500);
         }
     }
-}
\ No newline at end of file
+}

From 3f8d7de7e6e02ae8a53bccf5706fed013497ce57 Mon Sep 17 00:00:00 2001
From: jose5556 <josefilmeneses@gmail.com>
Date: Sat, 7 Feb 2026 18:47:46 +0000
Subject: [PATCH 3/9] Refactor CAN message handling and remove heartbeat
 thread; integrate emergency brake functionality (stable version)

---
 ThreadX_Os/CMakeLists.txt                     | 12 ++--
 ThreadX_Os/Core/Inc/app_threadx.h             | 29 ++++------
 ThreadX_Os/Core/Inc/can_protocol.h            |  3 +-
 ThreadX_Os/Core/Src/app_threadx.c             | 39 ++++---------
 ThreadX_Os/Core/Src/init/init_can.c           | 15 -----
 ThreadX_Os/Core/Src/init/init_threads.c       | 56 +++++--------------
 ThreadX_Os/Core/Src/threads/can/rx_can.c      | 26 +++------
 ThreadX_Os/Core/Src/threads/can/tx_can.c      | 14 +----
 ThreadX_Os/Core/Src/threads/emergency_brake.c | 23 ++++++++
 ThreadX_Os/Core/Src/threads/heartbeat.c       | 22 --------
 .../Core/Src/threads/motors/dc_motors.c       | 37 ------------
 .../Core/Src/threads/motors/emergency_brake.c | 18 ------
 .../Core/Src/threads/motors/i2c_driving.c     | 52 +++++++++++++++++
 ThreadX_Os/Core/Src/threads/motors/servo.c    | 25 ---------
 ThreadX_Os/Core/Src/threads/speed_sensor.c    |  4 +-
 15 files changed, 134 insertions(+), 241 deletions(-)
 create mode 100644 ThreadX_Os/Core/Src/threads/emergency_brake.c
 delete mode 100644 ThreadX_Os/Core/Src/threads/heartbeat.c
 delete mode 100644 ThreadX_Os/Core/Src/threads/motors/dc_motors.c
 delete mode 100644 ThreadX_Os/Core/Src/threads/motors/emergency_brake.c
 create mode 100644 ThreadX_Os/Core/Src/threads/motors/i2c_driving.c
 delete mode 100644 ThreadX_Os/Core/Src/threads/motors/servo.c

diff --git a/ThreadX_Os/CMakeLists.txt b/ThreadX_Os/CMakeLists.txt
index ec83c4b..5b2021f 100644
--- a/ThreadX_Os/CMakeLists.txt
+++ b/ThreadX_Os/CMakeLists.txt
@@ -44,16 +44,20 @@ target_link_directories(${CMAKE_PROJECT_NAME} PRIVATE
 # Add sources to executable
 target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     # Add user sources here
+    #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/heartbeat.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
diff --git a/ThreadX_Os/Core/Inc/app_threadx.h b/ThreadX_Os/Core/Inc/app_threadx.h
index 317146e..c062244 100644
--- a/ThreadX_Os/Core/Inc/app_threadx.h
+++ b/ThreadX_Os/Core/Inc/app_threadx.h
@@ -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      16
+#define QUEUE_SIZE      9
 
 /*
 Number of threads
@@ -62,12 +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
-8 -> heartbeat thread
+5 -> battery thread
+6 -> emergency brake thread
 */
-#define THREAD_COUNT    8
+#define THREAD_COUNT    6
 
 // Thread structure
 typedef struct s_threads {
@@ -78,7 +76,6 @@ typedef struct s_threads {
 // CAN frames structure
 typedef struct s_canFrames {
   FDCAN_TxHeaderTypeDef tx_header_speed;
-  FDCAN_TxHeaderTypeDef tx_header_heartbeat;
   FDCAN_TxHeaderTypeDef tx_header_battery;
 } t_canFrames;
 
@@ -92,10 +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             heartbeat_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 */
@@ -129,11 +124,9 @@ 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);
-VOID  thread_heartbeat(ULONG thread_input);
 
 //init
 void  initCanFrames(t_canFrames *canFrames);
diff --git a/ThreadX_Os/Core/Inc/can_protocol.h b/ThreadX_Os/Core/Inc/can_protocol.h
index 94abb9a..b48fdc1 100644
--- a/ThreadX_Os/Core/Inc/can_protocol.h
+++ b/ThreadX_Os/Core/Inc/can_protocol.h
@@ -3,11 +3,12 @@
 
 #include "app_threadx.h"
 
+#define EMERGENCY_BRAKE 150
+
 // CAN message types
 typedef enum {
   CAN_MSG_SPEED,
   CAN_MSG_BATTERY,
-  CAN_MSG_HEARTBEAT
 } e_can_msg_type;
 
 // TX CAN message structure (sending)
diff --git a/ThreadX_Os/Core/Src/app_threadx.c b/ThreadX_Os/Core/Src/app_threadx.c
index 57627e2..df187da 100644
--- a/ThreadX_Os/Core/Src/app_threadx.c
+++ b/ThreadX_Os/Core/Src/app_threadx.c
@@ -46,10 +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    heartbeat_queue;
+TX_QUEUE    i2c_driving_queue;
+TX_QUEUE    emergency_brake_queue;
 TX_MUTEX    i2c_mutex;
 t_threads   threads[THREAD_COUNT];
 
@@ -80,38 +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));
-  if (ret != TX_SUCCESS)
-    uart_send("ERROR! Failed I2C Servo queue creation.\r\n");
-
-  // Create Heartbeat queue
-  UCHAR *heartbeat_queue_memory = i2c_servo_queue_memory + QUEUE_SIZE * sizeof(t_rx_can_msg);
-  ret = tx_queue_create(&heartbeat_queue, "Heartbeat Queue", 
+  // 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),
-                        heartbeat_queue_memory, QUEUE_SIZE * sizeof(t_tx_can_msg));
+                        emergency_brake_queue_memory, QUEUE_SIZE * sizeof(t_tx_can_msg));
   if (ret != TX_SUCCESS)
-    uart_send("ERROR! Failed Heartbeat 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/init/init_can.c b/ThreadX_Os/Core/Src/init/init_can.c
index ff5e16e..7ab1165 100644
--- a/ThreadX_Os/Core/Src/init/init_can.c
+++ b/ThreadX_Os/Core/Src/init/init_can.c
@@ -2,14 +2,12 @@
 
 static void TxSpeedConf(FDCAN_TxHeaderTypeDef *TxHeader);
 static void TxBatteryConf(FDCAN_TxHeaderTypeDef *TxHeader);
-static void TxHeartbeatConf(FDCAN_TxHeaderTypeDef *TxHeader);
 
 // Configuration of CAN frame for speed data
 void initCanFrames(t_canFrames *canFrames) 
 {
     TxSpeedConf(&canFrames->tx_header_speed);
     TxBatteryConf(&canFrames->tx_header_battery);
-    TxHeartbeatConf(&canFrames->tx_header_heartbeat);
 }
 
 static void    TxSpeedConf(FDCAN_TxHeaderTypeDef *TxHeader) 
@@ -37,16 +35,3 @@ static void    TxBatteryConf(FDCAN_TxHeaderTypeDef *TxHeader)
     TxHeader->TxEventFifoControl = FDCAN_NO_TX_EVENTS;
     TxHeader->MessageMarker = 0x0;
 }
-
-static void    TxHeartbeatConf(FDCAN_TxHeaderTypeDef *TxHeader) 
-{
-    TxHeader->Identifier = 0x202;
-    TxHeader->IdType = FDCAN_STANDARD_ID;
-    TxHeader->TxFrameType = FDCAN_DATA_FRAME;
-    TxHeader->DataLength = FDCAN_DLC_BYTES_8;
-    TxHeader->ErrorStateIndicator = FDCAN_ESI_ACTIVE;
-    TxHeader->BitRateSwitch = FDCAN_BRS_OFF;
-    TxHeader->FDFormat = FDCAN_CLASSIC_CAN;
-    TxHeader->TxEventFifoControl = FDCAN_NO_TX_EVENTS;
-    TxHeader->MessageMarker = 0x0;
-}
\ No newline at end of file
diff --git a/ThreadX_Os/Core/Src/init/init_threads.c b/ThreadX_Os/Core/Src/init/init_threads.c
index 8f43e96..f363d30 100644
--- a/ThreadX_Os/Core/Src/init/init_threads.c
+++ b/ThreadX_Os/Core/Src/init/init_threads.c
@@ -8,7 +8,7 @@ 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) {
@@ -21,7 +21,7 @@ UINT    init_threads(VOID)
     // 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) {
@@ -45,50 +45,37 @@ UINT    init_threads(VOID)
     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");
-        exit(EXIT_FAILURE);
-    }
-    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");
+        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");
+        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,
+                                  10, // Time-slice
                                   TX_AUTO_START);
     if (ret != TX_SUCCESS) {
          uart_send("ERROR! Battery thread creation failed!\r\n");
@@ -97,18 +84,5 @@ UINT    init_threads(VOID)
     else
         uart_send("Battery Thread created successfully.\r\n"); */
 
-    // Heartbeat thread
-    ret = tx_thread_create(&threads[7].thread, "HeartbeatThread", thread_heartbeat, 0,
-                                  threads[7].stack, 1024,
-                                  LOW_PRIO, LOW_PRIO,
-                                  TX_NO_TIME_SLICE,
-                                  TX_AUTO_START);
-    if (ret != TX_SUCCESS) {
-        uart_send("ERROR! Heartbeat thread creation failed!\r\n");
-        exit(EXIT_FAILURE);
-    }
-    else
-        uart_send("Heartbeat Thread created successfully.\r\n");
-
     return (ret);
 }
diff --git a/ThreadX_Os/Core/Src/threads/can/rx_can.c b/ThreadX_Os/Core/Src/threads/can/rx_can.c
index 8722a7a..5e33d01 100644
--- a/ThreadX_Os/Core/Src/threads/can/rx_can.c
+++ b/ThreadX_Os/Core/Src/threads/can/rx_can.c
@@ -32,23 +32,15 @@ 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");
-                    uart_send("DC motor CAN msg received\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");
-                    uart_send("Servo CAN msg received\r\n");
-                    break ;
-                case 0x103: // Heartbeat
-                    if (tx_queue_send(&heartbeat_queue, &msg, TX_NO_WAIT) != TX_SUCCESS)
-                        uart_send("ERROR: Heartbeat queue FULL!\r\n");
-                    uart_send("Heartbeat CAN msg received\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:
@@ -56,6 +48,6 @@ VOID    thread_rx_can(ULONG thread_input)
                     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 90b2279..96e9d42 100644
--- a/ThreadX_Os/Core/Src/threads/can/tx_can.c
+++ b/ThreadX_Os/Core/Src/threads/can/tx_can.c
@@ -10,8 +10,7 @@ VOID thread_tx_can(ULONG thread_input)
     memset(&canFrames, 0, sizeof(t_canFrames));
     initCanFrames(&canFrames);
     if (!canFrames.tx_header_speed.Identifier ||
-            !canFrames.tx_header_battery.Identifier
-            || !canFrames.tx_header_heartbeat.Identifier) {
+            !canFrames.tx_header_battery.Identifier) {
         uart_send("CAN frames not initialized!\r\n");
         exit(EXIT_FAILURE);
     }
@@ -27,7 +26,7 @@ VOID thread_tx_can(ULONG thread_input)
                     &hfdcan1,
                     &canFrames.tx_header_speed,
                     msg.data);
-                    //uart_send("Speed CAN message sent\r\n");
+                    uart_send("Speed CAN message sent\r\n");
                     break ;
 
                 case CAN_MSG_BATTERY:
@@ -38,19 +37,10 @@ VOID thread_tx_can(ULONG thread_input)
                     uart_send("Battery CAN message sent\r\n");
                     break ;
 
-                case CAN_MSG_HEARTBEAT:
-                    HAL_FDCAN_AddMessageToTxFifoQ(
-                        &hfdcan1,
-                        &canFrames.tx_header_heartbeat,
-                        msg.data);
-                    uart_send("Heartbeat CAN message sent\r\n");
-                    break ;
-
                 default:
                     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..81913b7
--- /dev/null
+++ b/ThreadX_Os/Core/Src/threads/emergency_brake.c
@@ -0,0 +1,23 @@
+#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);
+            tx_queue_send(&i2c_driving_queue, &msg_to_send, TX_NO_WAIT);
+        }
+    }
+}
diff --git a/ThreadX_Os/Core/Src/threads/heartbeat.c b/ThreadX_Os/Core/Src/threads/heartbeat.c
deleted file mode 100644
index 64960c3..0000000
--- a/ThreadX_Os/Core/Src/threads/heartbeat.c
+++ /dev/null
@@ -1,22 +0,0 @@
-#include "app_threadx.h"
-#include "can_protocol.h"
-
-VOID  thread_heartbeat(ULONG thread_input) {
-    t_tx_can_msg    heartbeat_msg;
-    t_tx_can_msg    msg;
-    memset(&heartbeat_msg, 0, sizeof(t_tx_can_msg));
-    memset(&msg, 0, sizeof(t_tx_can_msg));
-
-    msg.data[0] = 0x01;
-    msg.type = CAN_MSG_HEARTBEAT;
-
-    while (1)
-    {
-        // Sends heartbeat message to CAN thread
-        if (tx_queue_receive(&heartbeat_queue, &heartbeat_msg, TX_MS_TO_TICKS(1000)) != TX_SUCCESS)
-            uart_send("Heartbeat timeout! No heartbeat received in the last second.\r\n");
-
-        if (tx_queue_send(&can_tx_queue, &msg, TX_NO_WAIT) != TX_SUCCESS)
-            uart_send("CAN TX could not add heartbeat message to queue!\r\n");
-    }
-}
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 6f9fdce..0000000
--- a/ThreadX_Os/Core/Src/threads/motors/emergency_brake.c
+++ /dev/null
@@ -1,18 +0,0 @@
-#include "i2c_pca9685.h"
-#include "can_protocol.h"
-
-VOID  thread_emergency_brake(ULONG thread_input) 
-{
-    t_rx_can_msg    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);
-        }
-    }
-}
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));
     }
 }
 

From 8aec8649bb0e765294f436f612087aaa1fa9858f Mon Sep 17 00:00:00 2001
From: jose5556 <josefilmeneses@gmail.com>
Date: Sat, 7 Feb 2026 18:57:24 +0000
Subject: [PATCH 4/9] Add delay after emergency brake activation in driving
 command thread

---
 ThreadX_Os/Core/Src/threads/motors/i2c_driving.c | 1 +
 1 file changed, 1 insertion(+)

diff --git a/ThreadX_Os/Core/Src/threads/motors/i2c_driving.c b/ThreadX_Os/Core/Src/threads/motors/i2c_driving.c
index 11d5909..8107e41 100644
--- a/ThreadX_Os/Core/Src/threads/motors/i2c_driving.c
+++ b/ThreadX_Os/Core/Src/threads/motors/i2c_driving.c
@@ -31,6 +31,7 @@ VOID thread_driving_command(ULONG initial_input)
                     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");
+                tx_thread_sleep(TX_MS_TO_TICKS(100));
             }
             else if (msg.len >= 4)
             {

From 4c03329b1ceec15cfabf22ec39c310ea4d4d880c Mon Sep 17 00:00:00 2001
From: jose5556 <josefilmeneses@gmail.com>
Date: Mon, 9 Feb 2026 14:49:25 +0000
Subject: [PATCH 5/9] Refactor includes and update utility headers; replace
 app_threadx.h with utils.h in I2C files, adjust queue size, and enhance
 battery thread functionality

---
 ThreadX_Os/Core/Inc/app_threadx.h           | 11 ++----
 ThreadX_Os/Core/Inc/i2c_ina219.h            |  2 +-
 ThreadX_Os/Core/Inc/i2c_pca9685.h           |  2 +-
 ThreadX_Os/Core/Inc/utils.h                 | 37 +++++---------------
 ThreadX_Os/Core/Inc/utils_testing.h         | 34 ++++++++++++++++++
 ThreadX_Os/Core/Src/i2c_ina219.c            | 12 +++----
 ThreadX_Os/Core/Src/init/init_threads.c     |  8 ++---
 ThreadX_Os/Core/Src/threads/battery.c       | 38 +++++++++++++--------
 ThreadX_Os/Core/Src/utils/debug_utils.c     | 13 ++++++-
 ThreadX_Os/Core/Src/utils/speed_rpm_utils.c |  2 +-
 tests/unit/test/test_ut_rpm.c               |  2 +-
 11 files changed, 93 insertions(+), 68 deletions(-)
 create mode 100644 ThreadX_Os/Core/Inc/utils_testing.h

diff --git a/ThreadX_Os/Core/Inc/app_threadx.h b/ThreadX_Os/Core/Inc/app_threadx.h
index c062244..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 */
 
@@ -54,7 +54,7 @@ extern "C" {
 #define NONE_PRIO   7
 
 //Queue size (number of messages)
-#define QUEUE_SIZE      9
+#define QUEUE_SIZE      6
 
 /*
 Number of threads
@@ -133,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/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/i2c_ina219.c b/ThreadX_Os/Core/Src/i2c_ina219.c
index ec97064..8769baf 100644
--- a/ThreadX_Os/Core/Src/i2c_ina219.c
+++ b/ThreadX_Os/Core/Src/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/init/init_threads.c b/ThreadX_Os/Core/Src/init/init_threads.c
index f363d30..0293ac0 100644
--- a/ThreadX_Os/Core/Src/init/init_threads.c
+++ b/ThreadX_Os/Core/Src/init/init_threads.c
@@ -34,7 +34,7 @@ UINT    init_threads(VOID)
     // 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);
 
@@ -71,18 +71,18 @@ UINT    init_threads(VOID)
     else
         uart_send("Emergency brake Thread created successfully.\r\n");
 
-    /*     // Battery thread
+    // Battery thread
     ret = tx_thread_create(&threads[5].thread, "BatteryThread", thread_battery, 0,
                                   threads[5].stack, 1024,
                                   NONE_PRIO, NONE_PRIO,
-                                  10, // Time-slice
+                                  TX_NO_TIME_SLICE,
                                   TX_AUTO_START);
     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/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");

From 1c3ffc3dcab141123601c67c60dcf65063f01486 Mon Sep 17 00:00:00 2001
From: jose5556 <josefilmeneses@gmail.com>
Date: Wed, 11 Feb 2026 12:46:09 +0000
Subject: [PATCH 6/9] Remove duplicate message sending in emergency brake
 thread and eliminate unnecessary sleep in driving command thread

---
 ThreadX_Os/Core/Src/threads/emergency_brake.c    | 1 -
 ThreadX_Os/Core/Src/threads/motors/i2c_driving.c | 1 -
 2 files changed, 2 deletions(-)

diff --git a/ThreadX_Os/Core/Src/threads/emergency_brake.c b/ThreadX_Os/Core/Src/threads/emergency_brake.c
index 81913b7..3bb6298 100644
--- a/ThreadX_Os/Core/Src/threads/emergency_brake.c
+++ b/ThreadX_Os/Core/Src/threads/emergency_brake.c
@@ -17,7 +17,6 @@ VOID  thread_emergency_brake(ULONG thread_input)
         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);
-            tx_queue_send(&i2c_driving_queue, &msg_to_send, TX_NO_WAIT);
         }
     }
 }
diff --git a/ThreadX_Os/Core/Src/threads/motors/i2c_driving.c b/ThreadX_Os/Core/Src/threads/motors/i2c_driving.c
index 8107e41..11d5909 100644
--- a/ThreadX_Os/Core/Src/threads/motors/i2c_driving.c
+++ b/ThreadX_Os/Core/Src/threads/motors/i2c_driving.c
@@ -31,7 +31,6 @@ VOID thread_driving_command(ULONG initial_input)
                     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");
-                tx_thread_sleep(TX_MS_TO_TICKS(100));
             }
             else if (msg.len >= 4)
             {

From 3ff6d0635162e31a4c47cd9ddf7ba8a64fdd7404 Mon Sep 17 00:00:00 2001
From: jose5556 <josefilmeneses@gmail.com>
Date: Wed, 11 Feb 2026 13:03:18 +0000
Subject: [PATCH 7/9] Add I2C driver for INA219 and PCA9685, implementing
 initialization and read functions

---
 ThreadX_Os/Core/Src/{ => i2c}/i2c_ina219.c  | 0
 ThreadX_Os/Core/Src/{ => i2c}/i2c_pca9685.c | 0
 2 files changed, 0 insertions(+), 0 deletions(-)
 rename ThreadX_Os/Core/Src/{ => i2c}/i2c_ina219.c (100%)
 rename ThreadX_Os/Core/Src/{ => i2c}/i2c_pca9685.c (100%)

diff --git a/ThreadX_Os/Core/Src/i2c_ina219.c b/ThreadX_Os/Core/Src/i2c/i2c_ina219.c
similarity index 100%
rename from ThreadX_Os/Core/Src/i2c_ina219.c
rename to ThreadX_Os/Core/Src/i2c/i2c_ina219.c
diff --git a/ThreadX_Os/Core/Src/i2c_pca9685.c b/ThreadX_Os/Core/Src/i2c/i2c_pca9685.c
similarity index 100%
rename from ThreadX_Os/Core/Src/i2c_pca9685.c
rename to ThreadX_Os/Core/Src/i2c/i2c_pca9685.c

From 2051ec4c7f5b4781016228a59ccba5da3002d2c8 Mon Sep 17 00:00:00 2001
From: jose5556 <josefilmeneses@gmail.com>
Date: Wed, 11 Feb 2026 13:59:22 +0000
Subject: [PATCH 8/9] Add I2C drivers for INA219 and PCA9685, implementing
 initialization and control functions

---
 ThreadX_Os/CMakeLists.txt                          | 5 +++--
 ThreadX_Os/Core/Src/{i2c => i2c_api}/i2c_ina219.c  | 0
 ThreadX_Os/Core/Src/{i2c => i2c_api}/i2c_pca9685.c | 0
 3 files changed, 3 insertions(+), 2 deletions(-)
 rename ThreadX_Os/Core/Src/{i2c => i2c_api}/i2c_ina219.c (100%)
 rename ThreadX_Os/Core/Src/{i2c => i2c_api}/i2c_pca9685.c (100%)

diff --git a/ThreadX_Os/CMakeLists.txt b/ThreadX_Os/CMakeLists.txt
index 5b2021f..f3112c6 100644
--- a/ThreadX_Os/CMakeLists.txt
+++ b/ThreadX_Os/CMakeLists.txt
@@ -44,6 +44,9 @@ 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
@@ -61,8 +64,6 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     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/Src/i2c/i2c_ina219.c b/ThreadX_Os/Core/Src/i2c_api/i2c_ina219.c
similarity index 100%
rename from ThreadX_Os/Core/Src/i2c/i2c_ina219.c
rename to ThreadX_Os/Core/Src/i2c_api/i2c_ina219.c
diff --git a/ThreadX_Os/Core/Src/i2c/i2c_pca9685.c b/ThreadX_Os/Core/Src/i2c_api/i2c_pca9685.c
similarity index 100%
rename from ThreadX_Os/Core/Src/i2c/i2c_pca9685.c
rename to ThreadX_Os/Core/Src/i2c_api/i2c_pca9685.c

From e414d1b338956afa3716812fb3947d4b12cb1448 Mon Sep 17 00:00:00 2001
From: jose5556 <josefilmeneses@gmail.com>
Date: Wed, 11 Feb 2026 16:07:37 +0000
Subject: [PATCH 9/9] Update README.md to refine project extension guidelines
 and remove redundant instructions

---
 README.md | 2 --
 1 file changed, 2 deletions(-)

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.