diff --git a/i2c-hub/firmware/i2c-hub-firmware/include/pinConfig.hpp b/i2c-hub/firmware/i2c-hub-firmware/include/pinConfig.hpp
index 531ae5f..2b4eb20 100644
--- a/i2c-hub/firmware/i2c-hub-firmware/include/pinConfig.hpp
+++ b/i2c-hub/firmware/i2c-hub-firmware/include/pinConfig.hpp
@@ -20,4 +20,4 @@
 #define GOBOTRPC_HEARTBEAT_INTERVAL 3000
 
 #define UART_CORE_MASK 0b01
-#define I2C_CORE_MASK  0b10
\ No newline at end of file
+#define I2C_CORE_MASK  0b01
\ No newline at end of file
diff --git a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/node_interface/rp2040/i2c/rx_tx.cpp b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/node_interface/rp2040/i2c/rx_tx.cpp
index 91c1c8d..d202c9d 100644
--- a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/node_interface/rp2040/i2c/rx_tx.cpp
+++ b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/node_interface/rp2040/i2c/rx_tx.cpp
@@ -41,13 +41,13 @@ void GobotRPC_NI_Hardware_RP2040_I2C::onI2CIRQ(I2C_SLAVE_EVENT event, BaseType_t
     }
 
     case I2C_SLAVE_REQUEST: { // I2C Read from Master
-        if(xQueueIsQueueEmptyFromISR(isrTXQueue) == pdFALSE) {
-            readState = I2C_READ_STAGE_INVALID;
-        }
+        //if(xQueueIsQueueEmptyFromISR(isrTXQueue) == pdTRUE) {
+        //    readState = I2C_READ_STAGE_INVALID;
+        //}
 
         switch (readState) {
         case I2C_READ_STAGE_INVALID:
-            i2c_write_byte_raw(i2c_inst, 0x00);
+            i2c_write_byte_raw(i2c_inst, 0x01);
             break;
         
         case I2C_READ_STAGE_FIRST:
@@ -93,6 +93,7 @@ void GobotRPC_NI_Hardware_RP2040_I2C::preTxTask() {
     while (1) {
         xQueueReceive(isrPreTxQueue, &pkg, portMAX_DELAY);
         xQueueSend(isrTXQueue, &pkg, portMAX_DELAY);
+        vTaskDelay(5 / portTICK_PERIOD_MS);
         gpio_put(int_PIN, 1);
     }
 }
\ No newline at end of file
diff --git a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/transmission_interface/rp2040_i2c_init.cpp b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/transmission_interface/rp2040_i2c_init.cpp
index c0ab419..fb30593 100644
--- a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/transmission_interface/rp2040_i2c_init.cpp
+++ b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/transmission_interface/rp2040_i2c_init.cpp
@@ -23,8 +23,9 @@ GobotRPC_TI_Hardware_RP2040_I2C::GobotRPC_TI_Hardware_RP2040_I2C(UBaseType_t cor
     for(int i=0; i<GOBOTRPC_TI_INT_NUM; i++) {
         intAddressMap[i] = 0xffffffff;
         gpio_init(GOBOTRPC_TI_INT_PIN_START + i);
-        gpio_set_dir(GOBOTRPC_TI_INT_PIN_START< + i, GPIO_IN);
-        gpio_pull_down(GOBOTRPC_TI_INT_PIN_START + i);
+        gpio_set_dir(GOBOTRPC_TI_INT_PIN_START + i, GPIO_IN);
+        //gpio_pull_down(GOBOTRPC_TI_INT_PIN_START + i);
+        gpio_set_irq_enabled_with_callback(GOBOTRPC_TI_INT_PIN_START + i, GPIO_IRQ_EDGE_RISE, true, intPin_ISR);
     }
 
     initTasks();
@@ -32,7 +33,6 @@ GobotRPC_TI_Hardware_RP2040_I2C::GobotRPC_TI_Hardware_RP2040_I2C(UBaseType_t cor
     //gpio_init(GOBOTRPC_TI_COMBINED_INT_PIN);
     //gpio_set_dir(GOBOTRPC_TI_COMBINED_INT_PIN, GPIO_IN);
     //gpio_pull_down(GOBOTRPC_TI_COMBINED_INT_PIN);
-    //gpio_set_irq_enabled_with_callback(GOBOTRPC_TI_COMBINED_INT_PIN, GPIO_IRQ_EDGE_RISE , true, &intPin_ISR);
 
 }
 
diff --git a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/transmission_interface/rp2040_i2c_rx.cpp b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/transmission_interface/rp2040_i2c_rx.cpp
index 1b18d1e..1545f07 100644
--- a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/transmission_interface/rp2040_i2c_rx.cpp
+++ b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/transmission_interface/rp2040_i2c_rx.cpp
@@ -48,7 +48,10 @@ bool GobotRPC_TI_Hardware_RP2040_I2C::readI2C(GoRPCPackage_Transport * pkg, uint
         return false; // I2C error
     
     size_t len = pkg->data[1];
-    res = i2c_read_burst_blocking(i2c, addr, ((uint8_t *)pkg->data) + 2, len);
+
+    size_t remaining = len >= 2 ? len - 2 : 0;
+
+    res = i2c_read_burst_blocking(i2c, addr, ((uint8_t *)pkg->data) + 2, len - 2);
 
     if(res == PICO_ERROR_GENERIC)
         return false; // I2C error
diff --git a/i2c-hub/firmware/i2c-hub-firmware/src/main_core0.cpp b/i2c-hub/firmware/i2c-hub-firmware/src/main_core0.cpp
index 0d9564d..c755c4f 100644
--- a/i2c-hub/firmware/i2c-hub-firmware/src/main_core0.cpp
+++ b/i2c-hub/firmware/i2c-hub-firmware/src/main_core0.cpp
@@ -50,15 +50,15 @@ int main() {
     appData.ciInstructionQueue = xQueueCreate(5, sizeof(CI_Instruction_Transport));
 
     GobotRPC_CI_Hardware_RP2040_UART gobotrpc_ci_hardware(uart0, 115200, UART_CORE_MASK);
-
-    GobotRPC_CI gobotRPC_ci(&gobotrpc_ci_hardware, 0b01, appData.ciInstructionQueue);
+    
+    GobotRPC_CI gobotRPC_ci(&gobotrpc_ci_hardware, UART_CORE_MASK, appData.ciInstructionQueue);
     
     gobotRPC_ci.registerCB_TxPacket(onTxPacket, &gobotRPC_ci);
 
     TaskHandle_t taskHandleCore0;
     TaskHandle_t taskHandleCore1;
     xTaskCreateAffinitySet(vTaskMain, "Main Task Core 0", 2048, &gobotRPC_ci, 1, UART_CORE_MASK, &taskHandleCore0);
-    xTaskCreateAffinitySet(main_core2, "Main Task Core 1", 2048, &gobotRPC_ci, 1, I2C_CORE_MASK, &taskHandleCore1);
+    xTaskCreateAffinitySet(main_core2, "Main Task Core 1", 2048, &gobotRPC_ci, 1, UART_CORE_MASK, &taskHandleCore1);
 
     vTaskStartScheduler();
 
diff --git a/i2c-hub/firmware/i2c-hub-firmware/src/main_core1.cpp b/i2c-hub/firmware/i2c-hub-firmware/src/main_core1.cpp
index c03f268..af0fffd 100644
--- a/i2c-hub/firmware/i2c-hub-firmware/src/main_core1.cpp
+++ b/i2c-hub/firmware/i2c-hub-firmware/src/main_core1.cpp
@@ -27,7 +27,8 @@ void pushCIInstruction(CI_Instruction_Transport * src, void *args) {
 
 void main_core2(void * pvParameters) {
     AppData appData = *((AppData *)pvParameters);
-    GobotRPC_TI_Hardware_RP2040_I2C gobotrpc_ti_hardware(I2C_CORE_MASK, i2c0);
+    GobotRPC_TI_Hardware_RP2040_I2C gobotrpc_ti_hardware(UART_CORE_MASK, i2c0);
+    gobotrpc_ti_hardware.setAddrMap(0x21, 2);
 
     gobotrpc_ti_hardware.registerPullPackageCB(pullPackageCB, appData.txQueue);
     gobotrpc_ti_hardware.registerPushPackageCB(pushPackage, appData.rxQueue);