diff --git a/controller/fw/embed/include/process_can.h b/controller/fw/embed/include/process_can.h
index e9f378a..e56ea6e 100644
--- a/controller/fw/embed/include/process_can.h
+++ b/controller/fw/embed/include/process_can.h
@@ -33,4 +33,4 @@ void setup_angle(float target_angle);
 void setup_velocity(float target_velocity);
 void process_can_messages();
 
-void listen_can(const CAN_message_t &msg);
+int listen_can(const CAN_message_t &msg);
diff --git a/controller/fw/embed/src/main.cpp b/controller/fw/embed/src/main.cpp
index 3eb5abe..b7656d9 100644
--- a/controller/fw/embed/src/main.cpp
+++ b/controller/fw/embed/src/main.cpp
@@ -36,7 +36,6 @@ uint32_t timeout;
 volatile bool CAN_GET = false;
 
 volatile float kt = 0.1;  // Torque calculation constant
-
 FLASH_RECORD* flash_rec;
 
 
@@ -62,7 +61,7 @@ MotorControlInputs motor_control_inputs;
  volatile uint8_t crc_l;
 
 volatile bool rx_can = false;
-CAN_message_t msg;
+CAN_message_t msg_g;
 
   void setup(){
   SCB->VTOR = (volatile uint32_t)0x08008004;
@@ -104,16 +103,33 @@ void wait_for_can_tx_complete() {
     CAN2->TSR |= CAN_TSR_ABRQ0 | CAN_TSR_ABRQ1 | CAN_TSR_ABRQ2;
 }
 
+bool can_mailbox_free(uint8_t mailbox) {
+  return (CAN2->TSR & (CAN_TSR_TME0 << mailbox)) != 0;
+}
+
+void clear_all_mailboxes() {
+    CAN2->TSR |= CAN_TSR_ABRQ0 | CAN_TSR_ABRQ1 | CAN_TSR_ABRQ2;
+    while ((CAN2->TSR & (CAN_TSR_ABRQ0 | CAN_TSR_ABRQ1 | CAN_TSR_ABRQ2)) != 0);
+}
+int can_err = 0;
 void loop() {
-  send_velocity();
-  HAL_Delay(1);
-  send_angle();
-  HAL_Delay(1);
-  if(rx_can){
-    Can.read(msg);
-    listen_can(msg);
+  // Process incoming CAN messages
+   if(rx_can){
+    CAN_WAIT();
+    clear_all_mailboxes();
+    can_err = listen_can(msg_g);
+    if(can_err == -1)
+      HAL_Delay(1000);
     rx_can = false;
-    HAL_Delay(1);
-  }
+    CAN_STR();
+}
+  if(can_mailbox_free(0))
+    send_velocity();
+  while(!is_can_busy);
+  if(can_mailbox_free(1))
+    send_angle();
+  while(!is_can_busy);
+  __enable_irq();
   foc_step(&motor);
 }
+
diff --git a/controller/fw/embed/src/process_can.cpp b/controller/fw/embed/src/process_can.cpp
index 31afdb0..d1a6866 100644
--- a/controller/fw/embed/src/process_can.cpp
+++ b/controller/fw/embed/src/process_can.cpp
@@ -53,6 +53,7 @@ void send_angle() {
   }
   uint8_t id = flash_rec[addr_id].value;
   send_can_with_id_crc(id,'A',&current_angle);
+  delayMicroseconds(1000);
 }
 
 
@@ -189,7 +190,7 @@ void process_can_messages() {
  * Write with data or send data
  * @param msg 
  */
-void listen_can(const CAN_message_t &msg) {
+int listen_can(const CAN_message_t &msg) {
   msg_id = msg.id;
   msg_ch = msg_id & 0xF;        // Extract message channel
   uint16_t id_x = (msg_id >> 4) & 0x7FF; // Extract device address 
@@ -207,7 +208,7 @@ void listen_can(const CAN_message_t &msg) {
   
   // Verify CRC match
   if (calculated_crc != received_crc) {
-    return; // Ignore message on CRC mismatch
+    return -1; // Ignore message on CRC mismatch
   }
   flash_rec = load_params();
 
@@ -215,7 +216,7 @@ void listen_can(const CAN_message_t &msg) {
      69 - Device address
      1 - Action code */
   if(id_x != flash_rec[addr_id].value){
-      return;
+      return -1;
   }
   if(msg_ch == REG_WRITE){
     switch(msg.buf[0]) {
@@ -328,6 +329,6 @@ void listen_can(const CAN_message_t &msg) {
           
     }
   }
-  HAL_Delay(10);
+  return 1;
 }