Started with gobotrpc autogenerated parser and ctest (not working)

generation

gobotrpc/.vscode/tasks.json

@@ -0,0 +1,22 @@
+{
+    // See https://go.microsoft.com/fwlink/?LinkId=733558
+    // for the documentation about the tasks.json format
+    "version": "2.0.0",
+    "tasks": [
+        {
+            "taskName": "Run Main",
+            "command": "python3 generator/main.py",
+            "type": "shell",
+            "group": {
+                "kind": "build",
+                "isDefault": true
+            },
+            "presentation": {
+                "reveal": "always",
+                "panel": "dedicated",
+                "focus": true,
+                "clear": true
+            },
+        },
+    ]
+}

gobotrpc/generator/templates/CMakeLists.txt.j2

@@ -0,0 +1,13 @@
+add_libary(gobotrpc STATIC)
+
+target_sources(gobotrpc
+    PRIVATE
+        {%- for n in cpp_files %}
+        {{ n }}
+        {%- endfor %}
+)
+
+target_include_directories(gobotrpc
+    PUBLIC
+        include
+)

gobotrpc/generator/templates/enum_template.hpp.j2

@@ -0,0 +1,12 @@
+#pragma once
+/**
+ * This Header file was auto-generated by the GobotRPC-Protocol-Generator-Tool
+ */
+
+{% for enum in enum_list -%}
+enum class {{ enum.name }} {{ "{" }}
+{% for key in enum.map -%}
+{{ "\t" }}{{ key }} = {{ enum.map[key] }},
+{% endfor %}{{ "}" }};
+
+{% endfor %}

gobotrpc/generator/templates/package_struct_template.hpp.j2

@@ -0,0 +1,29 @@
+#pragma once
+/**
+ * This Header file was auto-generated by the GobotRPC-Protocol-Generator-Tool
+ */
+
+#include {{ "<stdint.h>" }}
+#include "{{ enum_header_file }}"
+
+enum {{ prefix }}RPCNames {{"{"}}
+{%- for n in rpcNames %}
+    {{ n }} = {{ rpcNames[n] }}, 
+{%- endfor %}
+{{"}"}};
+
+enum {{ prefix }}RPCTypes {{"{"}}
+{%- for n in rpcTypes %}
+    {{ n }} = {{ rpcTypes[n] }},
+{%- endfor %}
+{{"}"}};
+
+int getPackageSize(uint8_t data);
+
+{% for struct in package_list  %}
+struct {{ struct.name }} {{ "{" }}
+{% for field in struct.fields -%}
+{{ "\t" }}{{ field.type }} {{ field.name }} : {{ field.size_bits }};
+{% endfor -%}
+{{ "}" }};
+{% endfor %}

gobotrpc/generator/templates/package_util.cpp.j2

@@ -0,0 +1,20 @@
+#pragma once
+/**
+ * This Header file was auto-generated by the GobotRPC-Protocol-Generator-Tool
+ */
+
+#include {{ "<stdint.h>" }}
+{%- for h in headers %}
+#include {{ "\"" + h + "\"" }}
+{%- endfor %}
+
+int getPackageSize(uint8_t data) {
+    switch(data) {
+        {%- for n in rpcSizes %}
+        case {{ prefix }}RPCNames::{{ n }}:
+            return {{ rpcSizes[n] }};
+        {%- endfor %}
+        default:
+            return -1;
+    }
+};

gobotrpc/out/cpp/.vscode/tasks.json

@@ -0,0 +1,25 @@
+{
+	"version": "2.0.0",
+	"tasks": [
+		{
+			"type": "cmake",
+			"label": "CMake: build",
+			"command": "build",
+			"targets": [
+				"all"
+			],
+			"group": "build",
+			"problemMatcher": [],
+			"detail": "CMake template build task",
+			"presentation": {
+				"echo": true,
+				"reveal": "always",
+				"focus": false,
+				"panel": "dedicated",
+				"showReuseMessage": true,
+				"clear": true
+			}
+		},
+		
+	]
+}

gobotrpc/out/cpp/CMakeLists.txt

@@ -0,0 +1,20 @@
+cmake_minimum_required(VERSION 3.28.3)
+project(TestingGobotRPCParser)
+
+
+# GoogleTest requires at least C++23
+set(CMAKE_CXX_STANDARD 23)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+# Adding gtest
+include(FetchContent)
+FetchContent_Declare(
+  googletest
+  gcov
+  URL https://github.com/google/googletest/archive/03597a01ee50ed33e9dfd640b249b4be3799d395.zip
+)
+
+FetchContent_MakeAvailable(googletest)
+
+add_executable(TestingGobotRPCParser test/main.cpp)
+target_link_libraries(TestingGobotRPCParser gtest_main)

gobotrpc/out/cpp/gobotrpc.cmake

@@ -0,0 +1,11 @@
+add_libary(gobotrpc STATIC)
+
+target_sources(gobotrpc
+    PRIVATE
+        src/package_util.cpp
+)
+
+target_include_directories(gobotrpc
+    PUBLIC
+        include
+)

gobotrpc/out/cpp/include/enums.hpp

@@ -0,0 +1,33 @@
+#pragma once
+/**
+ * This Header file was auto-generated by the GobotRPC-Protocol-Generator-Tool
+ */
+
+enum class GOBOTRPC_ENUM_NodeType {
+	kHub = 0,
+	kHead = 1,
+	kCorexy = 2,
+	kVacun = 3,
+};
+
+enum class GOBOTRPC_ENUM_NodeStatus {
+	kReady = 0,
+	kWorking = 1,
+	kBusy = 2,
+	kError = 3,
+};
+
+enum class GOBOTRPC_ENUM_ErrorCode {
+};
+
+enum class GOBOTRPC_ENUM_HeadPos {
+	kUp = 0,
+	kDown = 1,
+};
+
+enum class GOBOTRPC_ENUM_StoneStatus {
+	kEmpty = 0,
+	kLow = 1,
+	kFill = 2,
+};
+

gobotrpc/out/cpp/include/package_structs.hpp

@@ -0,0 +1,127 @@
+#pragma once
+/**
+ * This Header file was auto-generated by the GobotRPC-Protocol-Generator-Tool
+ */
+
+#include <stdint.h>
+#include "enums.hpp"
+
+enum GOBOTRPC_RPCNames {
+    kInvalid = 255,
+    kGetInfo = 1,
+    kReset = 2,
+    kResetInfo = 3,
+    kStatusUpdate = 4,
+    kError = 5,
+    kHome = 7,
+    kGoto = 9,
+    kSetBoardCorner = 11,
+    kSetVacumOffset = 13,
+    kReleaseMotors = 15,
+    kDrop = 17,
+    kStoneStatus = 19,
+    kMoveZAxis = 21,
+    kSetVacum = 23,
+};
+
+enum GOBOTRPC_RPCTypes {
+    kReq = 0,
+    kRes = 1,
+};
+
+int getPackageSize(uint8_t data);
+
+
+struct GOBOTRPC_GetInfoReq {
+};
+
+struct GOBOTRPC_GetInfoRes {
+	int32_t addr : 32;
+	GOBOTRPC_ENUM_NodeType type : 8;
+	GOBOTRPC_ENUM_NodeStatus status : 8;
+};
+
+struct GOBOTRPC_ResetReq {
+};
+
+struct GOBOTRPC_ResetInfoRes {
+	GOBOTRPC_ENUM_NodeStatus status : 8;
+};
+
+struct GOBOTRPC_StatusUpdateRes {
+	GOBOTRPC_ENUM_NodeStatus status : 8;
+};
+
+struct GOBOTRPC_ErrorRes {
+	GOBOTRPC_ENUM_ErrorCode error : 8;
+};
+
+struct GOBOTRPC_HomeReq {
+};
+
+struct GOBOTRPC_HomeRes {
+	int32_t x : 32;
+	int32_t y : 32;
+};
+
+struct GOBOTRPC_GotoReq {
+	int8_t x : 8;
+	int8_t y : 8;
+	bool offset : 8;
+};
+
+struct GOBOTRPC_GotoRes {
+};
+
+struct GOBOTRPC_SetBoardCornerReq {
+	int32_t x1 : 32;
+	int32_t y1 : 32;
+	int32_t x2 : 32;
+	int32_t y2 : 32;
+};
+
+struct GOBOTRPC_SetBoardCornerRes {
+};
+
+struct GOBOTRPC_SetVacumOffsetReq {
+	int32_t x : 32;
+	int32_t y : 32;
+};
+
+struct GOBOTRPC_SetVacumOffsetRes {
+};
+
+struct GOBOTRPC_ReleaseMotorsReq {
+	bool enable : 8;
+};
+
+struct GOBOTRPC_ReleaseMotorsRes {
+};
+
+struct GOBOTRPC_DropReq {
+};
+
+struct GOBOTRPC_DropRes {
+	GOBOTRPC_ENUM_StoneStatus status : 8;
+};
+
+struct GOBOTRPC_StoneStatusReq {
+};
+
+struct GOBOTRPC_StoneStatusRes {
+	GOBOTRPC_ENUM_StoneStatus status : 8;
+};
+
+struct GOBOTRPC_MoveZAxisReq {
+	GOBOTRPC_ENUM_HeadPos pos : 8;
+};
+
+struct GOBOTRPC_MoveZAxisRes {
+};
+
+struct GOBOTRPC_SetVacumReq {
+	bool enable : 8;
+};
+
+struct GOBOTRPC_SetVacumRes {
+};

gobotrpc/out/cpp/src/package_util.cpp

@@ -0,0 +1,63 @@
+#pragma once
+/**
+ * This Header file was auto-generated by the GobotRPC-Protocol-Generator-Tool
+ */
+
+#include <stdint.h>
+#include "package_structs.hpp"
+#include "enums.hpp"
+
+int getPackageSize(uint8_t data) {
+    switch(data) {
+        case RPCNames::GOBOTRPC_GetInfoReq:
+            return 0;
+        case RPCNames::GOBOTRPC_GetInfoRes:
+            return 6;
+        case RPCNames::GOBOTRPC_ResetReq:
+            return 0;
+        case RPCNames::GOBOTRPC_ResetInfoRes:
+            return 1;
+        case RPCNames::GOBOTRPC_StatusUpdateRes:
+            return 1;
+        case RPCNames::GOBOTRPC_ErrorRes:
+            return 1;
+        case RPCNames::GOBOTRPC_HomeReq:
+            return 0;
+        case RPCNames::GOBOTRPC_HomeRes:
+            return 8;
+        case RPCNames::GOBOTRPC_GotoReq:
+            return 3;
+        case RPCNames::GOBOTRPC_GotoRes:
+            return 0;
+        case RPCNames::GOBOTRPC_SetBoardCornerReq:
+            return 16;
+        case RPCNames::GOBOTRPC_SetBoardCornerRes:
+            return 0;
+        case RPCNames::GOBOTRPC_SetVacumOffsetReq:
+            return 8;
+        case RPCNames::GOBOTRPC_SetVacumOffsetRes:
+            return 0;
+        case RPCNames::GOBOTRPC_ReleaseMotorsReq:
+            return 1;
+        case RPCNames::GOBOTRPC_ReleaseMotorsRes:
+            return 0;
+        case RPCNames::GOBOTRPC_DropReq:
+            return 0;
+        case RPCNames::GOBOTRPC_DropRes:
+            return 1;
+        case RPCNames::GOBOTRPC_StoneStatusReq:
+            return 0;
+        case RPCNames::GOBOTRPC_StoneStatusRes:
+            return 1;
+        case RPCNames::GOBOTRPC_MoveZAxisReq:
+            return 1;
+        case RPCNames::GOBOTRPC_MoveZAxisRes:
+            return 0;
+        case RPCNames::GOBOTRPC_SetVacumReq:
+            return 1;
+        case RPCNames::GOBOTRPC_SetVacumRes:
+            return 0;
+        default:
+            return -1;
+    }
+};

gobotrpc/out/cpp/test/main.cpp

@@ -0,0 +1,14 @@
+#include <gtest/gtest.h>
+
+TEST(DummyTest, BasicAssertions) {
+// Expect two strings not to be equal.
+EXPECT_STRNE("hello", "world");
+// Expect equality.
+EXPECT_EQ(7 * 6, 42);
+}
+
+
+int main(int argc, char * argv[]) {
+    testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}

gobotrpc/requirements.txt

@@ -0,0 +1,9 @@
+defusedxml==0.7.1
+ezodf==0.3.2
+Jinja2==3.1.5
+lxml==5.3.0
+MarkupSafe==3.0.2
+mkl-service==2.4.0
+odfpy==1.4.1
+setuptools==75.1.0
+wheel==0.44.0

i2c-hub/firmware/i2c-hub-firmware/.vscode/tasks.json

@@ -12,7 +12,7 @@
                 "echo": true,
                 "reveal": "always",
                 "focus": true,
-                "panel": "shared",
+                "panel": "dedicated",
                 "showReuseMessage": true,
                 "clear": true
             },

i2c-hub/firmware/i2c-hub-firmware/CMakeLists.txt

@@ -1,5 +1,5 @@
 # Generated Cmake Pico project file
-
+set(CMAKE_VERBOSE_MAKEFILE ON CACHE BOOL "ON")
 cmake_minimum_required(VERSION 3.13)
 
 set(CMAKE_C_STANDARD 11)
@@ -63,6 +63,7 @@ target_link_libraries(i2c-hub-firmware
 # Add the standard include files to the build
 target_include_directories(i2c-hub-firmware PRIVATE
   ${CMAKE_CURRENT_LIST_DIR}
+  ${CMAKE_CURRENT_LIST_DIR}/include
 )
 
 # Add any user requested libraries

i2c-hub/firmware/i2c-hub-firmware/FreeRTOSConfig.h

@@ -73,7 +73,7 @@
 #define configSUPPORT_STATIC_ALLOCATION         0
 #define configSUPPORT_DYNAMIC_ALLOCATION        1
 #define configTOTAL_HEAP_SIZE                   (1024 * 128)
-#define configAPPLICATION_ALLOCATED_HEAP        (1024 * 64)
+#define configAPPLICATION_ALLOCATED_HEAP        (1024 * 84)
 
 /* Hook function related definitions. */
 #define configCHECK_FOR_STACK_OVERFLOW          0

i2c-hub/firmware/i2c-hub-firmware/include/pinConfig.hpp

@@ -15,9 +15,9 @@
 #define GOBOTRPC_TI_INT_NUM 4
 #define GOBOTRPC_TI_COMBINED_INT_PIN 18
 
-#define GOBOTRPC_TI_EXTERNAL_PULLUP 0
+#define GOBOTRPC_TI_EXTERNAL_PULLUP 1
 
 #define GOBOTRPC_HEARTBEAT_INTERVAL 3000
 
 #define UART_CORE_MASK 0b01
-#define I2C_CORE_MASK  0b10
+#define I2C_CORE_MASK  0b01

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/cmake/CMakeLists.txt

@@ -14,8 +14,10 @@ add_library(GobotRPC STATIC
 
 target_include_directories(GobotRPC PUBLIC 
     ${CMAKE_CURRENT_LIST_DIR}/../include
+    ${CMAKE_CURRENT_LIST_DIR}/../include/util
     ${CMAKE_CURRENT_LIST_DIR}/../include/ti
     ${CMAKE_CURRENT_LIST_DIR}/../../..
+    ${CMAKE_CURRENT_LIST_DIR}/../../../include
 )
 
 target_link_libraries(GobotRPC

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/cmake/gobotRPC_Node.cmake

@@ -0,0 +1,19 @@
+add_library(GobotRPC_Node_RP2040_I2C STATIC
+    ${CMAKE_CURRENT_LIST_DIR}/../node_interface/rp2040/i2c/init.cpp  
+    ${CMAKE_CURRENT_LIST_DIR}/../node_interface/rp2040/i2c/rx_tx.cpp  
+    ${CMAKE_CURRENT_LIST_DIR}/../crc16.cpp   
+    ${CMAKE_CURRENT_LIST_DIR}/../protocol_base.cpp
+    ${CMAKE_CURRENT_LIST_DIR}/../node_interface/init.cpp
+)
+target_include_directories(GobotRPC_Node_RP2040_I2C PUBLIC 
+    ${CMAKE_CURRENT_LIST_DIR}/../node_interface/include
+    ${CMAKE_CURRENT_LIST_DIR}/../include/util
+    ${CMAKE_CURRENT_LIST_DIR}/../../..
+)
+
+target_link_libraries(GobotRPC_Node_RP2040_I2C
+    FreeRTOS-Kernel-Heap4
+    pico_stdlib
+    hardware_i2c
+    hardware_irq
+)

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/crc16.cpp

@@ -1,3 +1,5 @@
+#include "crc16.hpp"
+
 #define POLY 0x8408
 /*
 //                                      16   12   5

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/ctrl_interface/base.cpp

@@ -1,17 +1,28 @@
 #include "ci/base.hpp"
 #include "pinConfig.hpp"
 
+#include "protocol.hpp"
+
+#include "pico/stdlib.h"
+#include "hardware/watchdog.h"
+
 #include "FreeRTOSConfig.h"
 #include "FreeRTOS.h"
 #include "task.h"
 
 
-GobotRPC_CI::GobotRPC_CI(I_GobotRPC_CI_Hardware *hardware, UBaseType_t core, QueueHandle_t ciInstructionQueue) {
+GobotRPC_CI::GobotRPC_CI(I_GobotRPC_CI_Hardware *hardware, UBaseType_t core, QueueHandle_t ciInstructionQueue, QueueHandle_t ciInstructionReverseQueue) {
     this->ciInstructionQueue = ciInstructionQueue;
+    this->ciInstructionReverseQueue = ciInstructionReverseQueue;
     
     this->hardware = hardware;
     this->hardware->registerCB_RxData(GobotRPC_CI_rxData_cb, this);
 
+    this->cb_TxPacket = NULL;
+    this->cb_TxPacket_args = NULL;
+    this->cb_SetAddressMap = NULL;
+    this->cb_SetAddressMap_args = NULL;
+
     xTaskCreateAffinitySet(GobotRPC_CI_heartBeatTaskFn, "Heartbeat Task", 2048, this, 2, core, &this->heartBeatTaskHandle);
     xTaskCreateAffinitySet(GobotRPC_CI_txCIInstructionTaskFn, "Tx CI Instruction Task", 2048, this, 3, core, &this->txCIInstructionTaskHandle);
 }
@@ -32,6 +43,20 @@ void GobotRPC_CI::onRxData(char *data, size_t len) {
                 this->cb_TxPacket(this->cb_TxPacket_args, data+6, len-6, addr);
             }
             break;
+
+        case RESET_CI_PACKET:
+            //softwareReset();
+            break;
+
+        case SET_ADDR_PORT_MAP: {
+            if(this->cb_SetAddressMap != NULL) {
+                uint32_t addr = (data[2] << 24) | (data[3] << 16) | (data[4] << 8) | data[5];
+                uint8_t port = data[6];
+                this->cb_SetAddressMap(cb_SetAddressMap_args, addr, port);
+            }
+            break;
+        }
+
         default:
             break;
     }
@@ -42,6 +67,11 @@ void GobotRPC_CI::registerCB_TxPacket(callback_TxPacket cb, void *args) {
     this->cb_TxPacket_args = args;
 }
 
+void GobotRPC_CI::registerCB_SetAddress(callback_SetAddress cb, void *args) {
+    this->cb_SetAddressMap = cb;
+    this->cb_SetAddressMap_args = args;
+}
+
 void GobotRPC_CI::send_RxPacket(char *data, size_t len, uint32_t addr) {
     data[0] = RX_CI_PACKET;
     data[1] = len + CI_RX_PACKAGE_DATA_OFFSET;

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/ctrl_interface/ci_instruction.cpp

@@ -23,11 +23,19 @@ void GobotRPC_CI::txCIInstructionTask() {
                 send_ErrorTransmission(rx, addr);
                 break;
             }
+
             case CI_INSTRUCTION_SEND_TRANMISSION_SUCCESS: {
-                uint32_t addr = ciInstruction.data[1] | (ciInstruction.data[2] << 8) | (ciInstruction.data[3] << 16) | (ciInstruction.data[4] << 24);
+                uint32_t addr = ciInstruction.data[0] \
+                    | (ciInstruction.data[1] << 8) \
+                    | (ciInstruction.data[2] << 16) \
+                    | (ciInstruction.data[3] << 24);
                 send_SuccessTransmission(addr);
                 break;
             }
+
+            case CI_INSTRUCTION_SEND_INFO_RESET:
+                send_InfoReset();
+                break;
         }
     }
 }
@@ -58,3 +66,25 @@ void GobotRPC_CI::send_SuccessTransmission(uint64_t addr) {
 
     this->hardware->send(successPacket, 6);
 }
+
+void GobotRPC_CI::send_InfoReset() {
+    char resetPacket[2];
+
+    resetPacket[0] = RESET_INFO_CI_PACKET;
+    resetPacket[1] = 2;
+
+    this->hardware->send(resetPacket, 2);
+}
+
+// CI Reverse Instructions (CI -> TI)
+void GobotRPC_CI::send_rev_SetAddrMap(uint32_t addr, uint8_t port) {
+    CI_Instruction_Transport ciInstruction;
+    ciInstruction.type = CI_INSTRUCTION_REV_SEND_SET_ADDR_MAP;
+    ciInstruction.data[0] = addr & 0xff;
+    ciInstruction.data[1] = (addr >> 8) & 0xff;
+    ciInstruction.data[2] = (addr >> 16) & 0xff;
+    ciInstruction.data[3] = (addr >> 24) & 0xff;
+    ciInstruction.data[4] = port;
+
+    xQueueSend(ciInstructionReverseQueue, &ciInstruction, portMAX_DELAY);
+}

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/ctrl_interface/uart_rp2040/rx.cpp

@@ -116,6 +116,7 @@ void GobotRPC_CI_Hardware_RP2040_UART::rxProcessingTaskFn() {
             this->cb_rxData(this->cb_rxData_args, inputBuffer->data, inputBuffer->len);
         }
 
+        xQueueSend(emptyInputBuffersQueue, &inputBuffer, portMAX_DELAY);
         vTaskDelay(1000);
     }
 }  

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/include/ci/base.hpp

@@ -5,11 +5,14 @@
 enum GobotRPC_CI_CMD {
     TX_CI_PACKET = 0x01,
     RX_CI_PACKET = 0x02,
-    PERFORM_SCAN_CI_PACKET = 0x03,
+    SET_ADDR_PORT_MAP = 0x03,
-    SCAN_RESULT_CI_PACKET = 0x04,
+
     SUCESS_TRANMISSION = 0xfc,
     ERROR_TRANMISSION = 0xfd,
-    HEARTBEAT = 0xff
+    HEARTBEAT = 0xff,
+
+    RESET_CI_PACKET = 0xcc,
+    RESET_INFO_CI_PACKET = 0xcd
 };
 
 #define CI_TX_PACKAGE_DATA_OFFSET 6
@@ -19,6 +22,8 @@ enum GobotRPC_CI_CMD {
 #define CI_RX_PACKAGE_SIZE(data_len) (data_len + CI_RX_PACKAGE_DATA_OFFSET)
 
 typedef void (*callback_TxPacket)(void * args, char *data, size_t len, uint32_t addr);
+typedef void (*callback_SetAddress)(void * args, uint32_t addr, uint8_t port);
+
 void GobotRPC_CI_rxData_cb(void * args, char *data, size_t len);
 void GobotRPC_CI_heartBeatTaskFn(void *args);
 void GobotRPC_CI_txCIInstructionTaskFn(void *args);
@@ -30,14 +35,20 @@ private:
     callback_TxPacket cb_TxPacket;
     void * cb_TxPacket_args;
 
+    callback_SetAddress cb_SetAddressMap;
+    void * cb_SetAddressMap_args;
+
     TaskHandle_t heartBeatTaskHandle;
     TaskHandle_t txCIInstructionTaskHandle;
 
     QueueHandle_t ciInstructionQueue;
+    QueueHandle_t ciInstructionReverseQueue;
 public:
-    GobotRPC_CI(I_GobotRPC_CI_Hardware *hardware, UBaseType_t core, QueueHandle_t ciInstructionQueue);
+    GobotRPC_CI(I_GobotRPC_CI_Hardware *hardware, UBaseType_t core, QueueHandle_t ciInstructionQueue, QueueHandle_t ciInstructionReverseQueue);
 
     void registerCB_TxPacket(callback_TxPacket cb, void *args);
+    void registerCB_SetAddress(callback_SetAddress cb, void *args);
+
     void send_RxPacket(char *data, size_t len, uint32_t addr);
 
     void onRxData(char *data, size_t len);
@@ -45,7 +56,9 @@ public:
     void heartBeartTaskFn();
 
     // CI Instruction Stuff
+    void send_rev_SetAddrMap(uint32_t addr, uint8_t port);
     void txCIInstructionTask();
     void send_ErrorTransmission(bool rx, uint64_t addr);
     void send_SuccessTransmission(uint64_t addr);
+    void send_InfoReset();
 };

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/include/ci/hardware.hpp

@@ -67,7 +67,6 @@ public:
     void onRx_ISR();
     void rxBufferingTaskFn();
     void rxProcessingTaskFn();
-
 };
 
 extern GobotRPC_CI_Hardware_RP2040_UART * g_GobotRPC_CI_Hardware_RP2040_UART;

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/include/ci/instructions.hpp

@@ -1,6 +1,8 @@
 enum CI_Instruction_Type {
     CI_INSTRUCTION_SEND_TRANMISSION_ERROR,
-    CI_INSTRUCTION_SEND_TRANMISSION_SUCCESS
+    CI_INSTRUCTION_SEND_TRANMISSION_SUCCESS,
+    CI_INSTRUCTION_SEND_INFO_RESET,
+    CI_INSTRUCTION_REV_SEND_SET_ADDR_MAP,
 };
 
 struct CI_Instruction_Transport {

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/include/main.hpp

@@ -9,6 +9,7 @@ struct AppData {
     QueueHandle_t txQueue;
     QueueHandle_t rxQueue;
     QueueHandle_t ciInstructionQueue;
+    QueueHandle_t ciInstructionReverseQueue;
 };
 
 void main_core2(void * pvParameters);

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/include/ti/transmission_interface.hpp

@@ -30,6 +30,10 @@ public:
     virtual void registerPushPackageCB(callback_pushPackage cb, void *args) = 0;
     virtual void registerPushCIInstructionCB(callback_pushCIInstruction cb, void *args) = 0;
     virtual void setAddrMap(uint32_t addr, int intNum) = 0;
+
+    void raiseTranmissionError(bool rx, uint32_t addr);
+    void raiseTransmissionSuceess(uint32_t addr);
+    void raiseInfoReset();
 };
 
 void i2cRxTaskFn(void * args);
@@ -51,8 +55,14 @@ private:
     SemaphoreHandle_t i2cMutex;
     SemaphoreHandle_t i2cRXSemaphore;
 
+
     TaskHandle_t i2cRxTaskHandle;
     TaskHandle_t i2cTxTaskHandle;
+    TaskHandle_t i2cIntTaskHandle;
+    TaskHandle_t revCIInstructionTaskHandle;
+
+    QueueHandle_t revCIInstructionQueue;
+
     UBaseType_t core;
 
     i2c_inst_t * i2c;
@@ -63,7 +73,7 @@ private:
     bool readI2C(GoRPCPackage_Transport * pkg, uint32_t addr);
 
 public:
-    GobotRPC_TI_Hardware_RP2040_I2C(UBaseType_t core, i2c_inst_t *i2c);
+    GobotRPC_TI_Hardware_RP2040_I2C(UBaseType_t core, i2c_inst_t *i2c, QueueHandle_t revCIInstructionQueue);
 
     void registerPullPackageCB(callback_pullPackage cb, void *args);
     void registerPushPackageCB(callback_pushPackage cb, void *args);
@@ -73,11 +83,15 @@ public:
 
     void i2cRxTask();
     void i2cTxTask();
+    void manualTriggerIntTask();
     void intPinISR(BaseType_t * xHigherPriorityTaskWoken);
     uint32_t readIntPins();
 
     void raiseTranmissionError(bool rx, uint32_t addr);
     void raiseTransmissionSuceess(uint32_t addr);
+    void raiseInfoReset();
+
+    void revCIInstructionTask();
 };
 
 

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/include/util/protocol.hpp

@@ -3,7 +3,17 @@
 #include <strings.h>
 
 enum GobotRPCNumber {
-    VACUM = 0x8
+    HOME_XY = 0x1,
+    SET_PADDING = 0x2,
+    GOTO = 0x3,
+    RELEASE_MOTORS = 0x4,
+
+    DROP_STONE = 0x5,
+    MOVE_Z_AXIS = 0x7,
+
+    VACUM = 0x8,
+    RESET = 0xc,
+    GET_INFO = 0xd
 };
 
 enum GobotRPCTypes {
@@ -12,6 +22,10 @@ enum GobotRPCTypes {
     ERROR    = 0b10
 };
 
+struct GobotRPCPackage_Req_Vacum {
+    uint8_t enable : 8;
+};
+
 #define GobotRPC_Package_DATA_OFFSET 2
 #define CALC_SIZE_GobotRPC_PACKAGE(data_len) (data_len + GobotRPC_Package_DATA_OFFSET + 2)
 
@@ -24,5 +38,8 @@ public:
 
 void assembleGobotRPCHeader(char * buffer, GobotRPCNumber number, GobotRPCTypes data_size, size_t);
 void assembleCRC(char * buffer, size_t data_len);
+bool checkCRC(char * buffer, size_t data_len);
 
 GobotRPCHeaderInfo extractGobotRPCHeader(char * buffer);
+
+void softwareReset();

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/node_interface/include/node_interface.hpp

@@ -0,0 +1,45 @@
+#pragma once
+#include "protocol.hpp"
+#include "node_interface_hardware.hpp"
+
+#include "FreeRTOSConfig.h"
+#include "FreeRTOS.h"
+#include "task.h"
+#include "queue.h"
+
+typedef void (*onPackageRxCallback)(void * args, char *data, uint16_t len, GobotRPCTypes type, GobotRPCNumber number);
+
+enum NODE_TYPE: uint8_t {
+    NODE_TYPE_VACUM = 0xa1,
+    NODE_TYPE_HEAD = 0xa2,
+    NODE_TYPE_COREXY = 0xa3,
+};
+
+struct InfoData {
+    uint32_t addr;
+    NODE_TYPE type;
+};
+
+class GobotRPC_NI {
+private:
+    onPackageRxCallback onPackageRx;
+    void *onPackageRxArgs;
+
+    QueueHandle_t txQueue;
+    QueueHandle_t rxQueue;
+    TaskHandle_t rxTaskHandle;
+    
+    InfoData info;
+
+    unsigned int core;
+
+public:
+    GobotRPC_NI(unsigned int core, QueueHandle_t txQueue, QueueHandle_t rxQueue, InfoData info);
+    
+    void registerOnPackageRxCallback(onPackageRxCallback callback, void *context);
+    void sendPackage(char *data, size_t len, GobotRPCTypes type, GobotRPCNumber number);
+    
+    InfoData getInfo();
+
+    void rxTask();
+};

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/node_interface/include/node_interface_hardware.hpp

@@ -0,0 +1,78 @@
+#pragma once
+
+#include "FreeRTOSConfig.h"
+#include "FreeRTOS.h"
+#include "queue.h"
+
+#include "pico/stdlib.h"
+#include "hardware/i2c.h"
+#include "hardware/irq.h"
+
+struct GobotRPC_NI_Package_Transport {
+    char data[256];
+    uint length;
+    uint index;
+};
+
+enum I2C_WRITE_STATE {
+    I2C_WRITE_STATE_FIRST,
+    I2C_WRITE_STATE_LENGTH,
+    I2C_WRITE_STATE_DATA,
+};
+
+enum I2C_READ_STATE {
+    I2C_READ_STAGE_FIRST,
+    I2C_READ_STAGE_DONE,
+    I2C_READ_STAGE_WIP,
+    I2C_READ_STAGE_INVALID
+};
+
+enum I2C_SLAVE_EVENT {
+    I2C_SLAVE_RECEIVE,
+    I2C_SLAVE_REQUEST,
+    I2C_SLAVE_FINISH
+};
+
+class GobotRPC_NI_Hardware {
+
+};
+
+struct GobotRPC_NI_Hardware_RP2040_I2C_PreTxQueues {
+    QueueHandle_t isrPreTxQueue;
+    QueueHandle_t isrTXQueue;
+};
+
+class GobotRPC_NI_Hardware_RP2040_I2C : public GobotRPC_NI_Hardware {
+    uint core;
+
+    QueueHandle_t isrPreTxQueue;
+    QueueHandle_t isrTXQueue;
+    QueueHandle_t isrRXQueue;
+    TaskHandle_t preTxTaskHandle;
+
+    uint i2cSDA_PIN;
+    uint i2cSCL_PIN;
+    uint int_PIN;
+    uint8_t i2cAddress;
+
+    I2C_WRITE_STATE writeState = I2C_WRITE_STATE_FIRST;
+    I2C_READ_STATE readState = I2C_READ_STAGE_INVALID;
+
+    GobotRPC_NI_Package_Transport rxPackage;
+    GobotRPC_NI_Package_Transport txPackage;
+    
+public:
+    i2c_inst_t * i2c_inst;
+
+    GobotRPC_NI_Hardware_RP2040_I2C(
+        xQueueHandle TXQueue, xQueueHandle RXQueue, uint core,
+        i2c_inst_t * i2c_inst,
+        uint8_t i2cAddress,
+        uint i2cSDA_PIN, uint i2cSCL_PIN, uint int_PIN
+    );
+
+    void onI2CIRQ(I2C_SLAVE_EVENT event, BaseType_t * xHigherPriorityTaskWoken);
+    void preTxTask();
+};
+
+extern GobotRPC_NI_Hardware_RP2040_I2C * g_gobotrpc_ni_hardware_rp2040_i2c;

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/node_interface/init.cpp

@@ -0,0 +1,73 @@
+#include "node_interface.hpp"
+
+#include "FreeRTOSConfig.h"
+#include "FreeRTOS.h"
+#include "task.h"
+#include <string.h>
+
+static void rxTaskFn(void *pvParameters) {
+    GobotRPC_NI *ni = (GobotRPC_NI *)pvParameters;
+    ni->rxTask();
+}
+
+GobotRPC_NI::GobotRPC_NI(unsigned int core, QueueHandle_t txQueue, QueueHandle_t rxQueue, InfoData info) {
+    this->core = core;
+    this->info = info;
+
+    this->txQueue = txQueue;
+    this->rxQueue = rxQueue;
+
+    this->onPackageRx = NULL;
+    this->onPackageRxArgs = NULL;
+
+    xTaskCreateAffinitySet(
+        rxTaskFn,
+        "GobotRPC_NI RX Task",
+        2048,
+        this,
+        4,
+        this->core,
+        &this->rxTaskHandle
+    );
+}
+
+void GobotRPC_NI::registerOnPackageRxCallback(onPackageRxCallback callback, void *context) {
+    this->onPackageRx = callback;
+    this->onPackageRxArgs = context;
+}
+
+void GobotRPC_NI::rxTask() {
+    while (true) {
+        GobotRPC_NI_Package_Transport package;
+        
+        if (xQueueReceive(this->rxQueue, &package, portMAX_DELAY) == pdTRUE) {    
+            GobotRPCHeaderInfo header = extractGobotRPCHeader(package.data);
+
+            if(this->onPackageRx != NULL) {
+                if (!checkCRC(package.data, header.len)) {
+                    volatile int a = 0;
+                    continue;
+                }
+
+                this->onPackageRx(
+                    onPackageRxArgs, 
+                    package.data + GobotRPC_Package_DATA_OFFSET, 
+                    header.len, header.type, header.number
+                );
+            }
+        }
+    }
+}
+
+void GobotRPC_NI::sendPackage(char *data, size_t len, GobotRPCTypes type, GobotRPCNumber number) {
+    GobotRPC_NI_Package_Transport package;
+    package.length = len + GobotRPC_Package_DATA_OFFSET + 2;
+    memcpy(package.data + GobotRPC_Package_DATA_OFFSET, data, len);
+    assembleGobotRPCHeader(package.data, number, type, len);
+    assembleCRC(package.data, len);
+    xQueueSend(this->txQueue, &package, portMAX_DELAY);
+}
+
+InfoData GobotRPC_NI::getInfo() {
+    return info;
+}

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/node_interface/rp2040/i2c/init.cpp

@@ -0,0 +1,127 @@
+#include "node_interface_hardware.hpp"
+#include "pico/stdlib.h"
+#include "hardware/i2c.h"
+#include <string.h>
+
+GobotRPC_NI_Hardware_RP2040_I2C * g_gobotrpc_ni_hardware_rp2040_i2c;
+i2c_inst_t * g_i2c_inst;
+bool g_transfer_in_progress = false;
+
+static void finish_transfer(BaseType_t * xHigherPriorityTaskWoken) {
+    if(g_transfer_in_progress) {
+        GobotRPC_NI_Hardware_RP2040_I2C * gobotrpc_ni = g_gobotrpc_ni_hardware_rp2040_i2c;
+        g_transfer_in_progress = false;
+        gobotrpc_ni->onI2CIRQ(I2C_SLAVE_FINISH, xHigherPriorityTaskWoken);
+    }
+}
+
+static void i2c_isr() {
+    GobotRPC_NI_Hardware_RP2040_I2C * gobotrpc_ni = g_gobotrpc_ni_hardware_rp2040_i2c;
+    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
+
+    i2c_hw_t * hw = i2c_get_hw(g_i2c_inst);
+    uint32_t intr_stat = hw->intr_stat;
+
+    if (intr_stat == 0) {
+        return;
+    }
+
+    if (intr_stat & I2C_IC_INTR_STAT_R_TX_ABRT_BITS) {
+        hw->clr_tx_abrt;
+        finish_transfer(&xHigherPriorityTaskWoken);
+    }
+
+    if (intr_stat & I2C_IC_INTR_STAT_R_START_DET_BITS) {
+        hw->clr_start_det;
+        finish_transfer(&xHigherPriorityTaskWoken);
+    }
+
+    if (intr_stat & I2C_IC_INTR_STAT_R_STOP_DET_BITS) {
+        hw->clr_stop_det;
+        finish_transfer(&xHigherPriorityTaskWoken);
+    }
+
+    if (intr_stat & I2C_IC_INTR_STAT_R_RX_FULL_BITS) {
+        g_transfer_in_progress = true;
+        gobotrpc_ni->onI2CIRQ(I2C_SLAVE_RECEIVE, &xHigherPriorityTaskWoken);
+    }
+
+    if (intr_stat & I2C_IC_INTR_STAT_R_RD_REQ_BITS) {
+        hw->clr_rd_req;
+        g_transfer_in_progress = true;
+        gobotrpc_ni->onI2CIRQ(I2C_SLAVE_REQUEST, &xHigherPriorityTaskWoken);
+    }
+
+    portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
+}
+
+static void preTxTaskFn(void *pvParameters) {
+    GobotRPC_NI_Hardware_RP2040_I2C * hw = (GobotRPC_NI_Hardware_RP2040_I2C *)(pvParameters);
+    hw->preTxTask();
+};
+
+GobotRPC_NI_Hardware_RP2040_I2C::GobotRPC_NI_Hardware_RP2040_I2C(
+    xQueueHandle TXQueue, xQueueHandle RXQueue, uint core,
+    i2c_inst_t * i2c_inst,
+    uint8_t i2cAddress,
+    uint i2cSDA_PIN, uint i2cSCL_PIN, uint int_PIN
+) {
+    g_gobotrpc_ni_hardware_rp2040_i2c = this;
+    g_i2c_inst = i2c_inst;
+
+    this->core = core;
+
+    this->readState = I2C_READ_STAGE_FIRST;
+    this->writeState = I2C_WRITE_STATE_FIRST;
+
+    this->isrRXQueue = RXQueue;
+    this->isrPreTxQueue = TXQueue;
+    this->isrTXQueue = xQueueCreate(3, sizeof(GobotRPC_NI_Package_Transport));
+
+    xTaskCreateAffinitySet(
+        preTxTaskFn,
+        "GobotRPC_NI Hardware RP2040 I2C PreTx Task",
+        2048,
+        this,
+        4,
+        this->core,
+        &this->preTxTaskHandle
+    );
+
+    this->i2cSDA_PIN = i2cSDA_PIN;
+    this->i2cSCL_PIN = i2cSCL_PIN;
+    this->int_PIN = int_PIN;
+
+    this->i2cAddress = i2cAddress;
+    this->i2c_inst = i2c_inst;
+
+    memset(&rxPackage.data, 0, sizeof(GobotRPC_NI_Package_Transport::data));
+    memset(&txPackage.data, 0, sizeof(GobotRPC_NI_Package_Transport::data));
+
+    gpio_init(i2cSDA_PIN);
+    gpio_init(i2cSCL_PIN);
+    gpio_set_function(i2cSDA_PIN, GPIO_FUNC_I2C);
+    gpio_set_function(i2cSCL_PIN, GPIO_FUNC_I2C);
+
+    gpio_init(int_PIN);
+    gpio_set_dir(int_PIN, GPIO_OUT);
+    gpio_put(int_PIN, 0);
+
+    // Note: The I2C slave does clock stretching implicitly after a RD_REQ, while the Tx FIFO is empty.
+    // There is also an option to enable clock stretching while the Rx FIFO is full, but we leave it
+    // disabled since the Rx FIFO should never fill up (unless slave->handler() is way too slow).
+    i2c_set_slave_mode(i2c_inst, true, i2cAddress);
+
+    i2c_hw_t *hw = i2c_get_hw(i2c_inst);
+    // unmask necessary interrupts
+    hw->intr_mask = I2C_IC_INTR_MASK_M_RX_FULL_BITS |\
+        I2C_IC_INTR_MASK_M_RD_REQ_BITS |\
+        I2C_IC_RAW_INTR_STAT_TX_ABRT_BITS |\
+        I2C_IC_INTR_MASK_M_STOP_DET_BITS |\ 
+        I2C_IC_INTR_MASK_M_START_DET_BITS;
+
+    // enable interrupt for current core
+    unsigned int intNum = i2c_inst == i2c0 ? I2C0_IRQ : I2C1_IRQ;
+    irq_set_exclusive_handler(intNum, i2c_isr);
+    irq_set_enabled(intNum, true);
+}

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/node_interface/rp2040/i2c/rx_tx.cpp

@@ -0,0 +1,99 @@
+#include "node_interface_hardware.hpp"
+#include "hardware/irq.h"
+
+void GobotRPC_NI_Hardware_RP2040_I2C::onI2CIRQ(I2C_SLAVE_EVENT event, BaseType_t * xHigherPriorityTaskWoken) {
+
+    switch (event) {
+    case I2C_SLAVE_RECEIVE: { // I2C Write from Master
+        char data = i2c_read_byte_raw(i2c_inst);
+
+        switch (writeState) {
+        case I2C_WRITE_STATE_FIRST:
+            writeState = I2C_WRITE_STATE_LENGTH;
+            rxPackage.index = 0;
+            rxPackage.length = 0;
+            rxPackage.data[rxPackage.index++] = data;
+            break;
+
+        case I2C_WRITE_STATE_LENGTH:
+            rxPackage.data[rxPackage.index++] = data;
+            rxPackage.length = data;
+            writeState = I2C_WRITE_STATE_DATA;
+            break;
+        
+        case I2C_WRITE_STATE_DATA:
+            rxPackage.data[rxPackage.index++] = data;
+            
+            if(rxPackage.index == rxPackage.length) {
+                xQueueSendFromISR(isrRXQueue, &rxPackage, xHigherPriorityTaskWoken);
+                writeState = I2C_WRITE_STATE_FIRST;
+            } else {
+                writeState = I2C_WRITE_STATE_DATA;
+            }
+
+            break;
+
+        default:
+            break;
+        }
+
+        break;
+    }
+
+    case I2C_SLAVE_REQUEST: { 
+        // I2C Read from Master
+        //if(xQueueIsQueueEmptyFromISR(isrTXQueue) == pdTRUE) {
+        //    readState = I2C_READ_STAGE_INVALID;
+        //}
+
+        switch (readState) {
+        case I2C_READ_STAGE_INVALID:
+            i2c_write_byte_raw(i2c_inst, 0x01);
+            break;
+        
+        case I2C_READ_STAGE_FIRST:
+            xQueueReceiveFromISR(isrTXQueue, &txPackage, xHigherPriorityTaskWoken);
+            txPackage.index = 0;
+            i2c_write_byte_raw(i2c_inst, txPackage.data[txPackage.index++]);
+            readState = I2C_READ_STAGE_WIP;
+            break;
+
+        case I2C_READ_STAGE_WIP:
+            i2c_write_byte_raw(i2c_inst, txPackage.data[txPackage.index++]);
+            
+            if(txPackage.index == txPackage.length) {
+                readState = I2C_READ_STAGE_FIRST;
+                gpio_put(int_PIN, 0);
+            }
+
+            break;
+
+        default:
+            break;
+        }
+
+        break;
+    }
+
+    case I2C_SLAVE_FINISH:
+        writeState = I2C_WRITE_STATE_FIRST;
+        
+        rxPackage.index = 0;
+        txPackage.index = 0;
+        break;
+
+    default:
+        break;
+    }
+}
+
+void GobotRPC_NI_Hardware_RP2040_I2C::preTxTask() {
+    GobotRPC_NI_Package_Transport pkg;
+
+    while (1) {
+        xQueueReceive(isrPreTxQueue, &pkg, portMAX_DELAY);
+        xQueueSend(isrTXQueue, &pkg, portMAX_DELAY);
+        vTaskDelay(5 / portTICK_PERIOD_MS);
+        gpio_put(int_PIN, 1);
+    }
+}

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/protocol_base.cpp

@@ -2,6 +2,7 @@
 #include <stdint.h>
 
 #include "pico/stdlib.h"
+#include "hardware/watchdog.h"
 
 #include "crc16.hpp"
 
@@ -23,3 +24,14 @@ void assembleCRC(char * buffer, size_t data_len) {
     buffer[GobotRPC_Package_DATA_OFFSET + data_len + 1] = crc & 0xff;
     buffer[GobotRPC_Package_DATA_OFFSET + data_len + 0] = (crc >> 8) & 0xff;
 }
+
+bool checkCRC(char * buffer, size_t data_len) {
+    unsigned short crc = crc16(buffer, data_len - 2);
+    unsigned short crc_received = buffer[data_len - GobotRPC_Package_DATA_OFFSET + 1] | (buffer[data_len - GobotRPC_Package_DATA_OFFSET + 0] << 8);
+    return crc == crc_received;
+}
+
+void softwareReset() {
+    watchdog_enable(1, 1);
+    while(1);
+}

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/transmission_interface/rp2040_i2c_ci_instructions.cpp

@@ -25,3 +25,29 @@ void GobotRPC_TI_Hardware_RP2040_I2C::raiseTransmissionSuceess(uint32_t addr) {
     
     this->pushCIInstructionCB(&ciInstruction, pushCIInstructionCBArgs);
 }
+
+void GobotRPC_TI_Hardware_RP2040_I2C::raiseInfoReset() {
+    CI_Instruction_Transport ciInstruction;
+    ciInstruction.type = CI_INSTRUCTION_SEND_INFO_RESET;
+    
+    this->pushCIInstructionCB(&ciInstruction, pushCIInstructionCBArgs);
+}
+
+void GobotRPC_TI_Hardware_RP2040_I2C::revCIInstructionTask() {
+    CI_Instruction_Transport ciInstruction;
+    while(1) {
+        xQueueReceive(revCIInstructionQueue, &ciInstruction, portMAX_DELAY);
+        
+        switch (ciInstruction.type) {
+            case CI_INSTRUCTION_REV_SEND_SET_ADDR_MAP: {
+                uint32_t addr = ciInstruction.data[0] |\
+                    (ciInstruction.data[1] << 8) |\ 
+                    (ciInstruction.data[2] << 16) |\
+                    (ciInstruction.data[3] << 24);
+                uint8_t port = ciInstruction.data[4];
+                this->setAddrMap(addr, port);
+                break;
+            }
+        }
+    }
+}

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/transmission_interface/rp2040_i2c_init.cpp

@@ -1,13 +1,16 @@
 #include "transmission_interface.hpp"
 #include "hardware/i2c.h"
 
+#include <string.h>
+
 #include "pinConfig.hpp"
 
 GobotRPC_TI_Hardware_RP2040_I2C * g_GobotRPC_TI_Hardware_RP2040_I2C;
 
-GobotRPC_TI_Hardware_RP2040_I2C::GobotRPC_TI_Hardware_RP2040_I2C(UBaseType_t core, i2c_inst_t *i2c) {
+GobotRPC_TI_Hardware_RP2040_I2C::GobotRPC_TI_Hardware_RP2040_I2C(UBaseType_t core, i2c_inst_t *i2c, QueueHandle_t revCIInstructionQueue) {
     this->core = core;
     this->i2c = i2c0;
+    this->revCIInstructionQueue = revCIInstructionQueue;
 
     g_GobotRPC_TI_Hardware_RP2040_I2C = this;
 
@@ -23,19 +26,29 @@ 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_set_dir(GOBOTRPC_TI_INT_PIN_START + i, GPIO_IN);
-        gpio_pull_down(GOBOTRPC_TI_INT_PIN_START + i);
+        //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();
 
-    gpio_init(GOBOTRPC_TI_COMBINED_INT_PIN);
+    //gpio_init(GOBOTRPC_TI_COMBINED_INT_PIN);
-    gpio_set_dir(GOBOTRPC_TI_COMBINED_INT_PIN, GPIO_IN);
+    //gpio_set_dir(GOBOTRPC_TI_COMBINED_INT_PIN, GPIO_IN);
-    gpio_pull_down(GOBOTRPC_TI_COMBINED_INT_PIN);
+    //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);
 
 }
 
+static void i2cIntTaskFn(void * pvParameters) {
+    GobotRPC_TI_Hardware_RP2040_I2C * hw = (GobotRPC_TI_Hardware_RP2040_I2C *)pvParameters;
+    hw->manualTriggerIntTask();
+}
+
+static void revCIInstructionTaskFn(void * pvParameters) {
+    GobotRPC_TI_Hardware_RP2040_I2C * hw = (GobotRPC_TI_Hardware_RP2040_I2C *)pvParameters;
+    hw->revCIInstructionTask();
+}
+
 void GobotRPC_TI_Hardware_RP2040_I2C::initTasks() {
     i2cMutex = xSemaphoreCreateMutex();
     i2cRXSemaphore = xSemaphoreCreateBinary();
@@ -49,6 +62,8 @@ void GobotRPC_TI_Hardware_RP2040_I2C::initTasks() {
 
     xTaskCreateAffinitySet(i2cRxTaskFn, "i2c Rx Task", 4096, this, 3, core, &i2cRxTaskHandle);
     xTaskCreateAffinitySet(i2cTxTaskFn, "i2c Tx Task", 4096, this, 3, core, &i2cTxTaskHandle);
+    xTaskCreateAffinitySet(i2cIntTaskFn, "i2c Int Task", 4096, this, 3, core, &i2cIntTaskHandle);
+    xTaskCreateAffinitySet(revCIInstructionTaskFn, "Rev CI Instruction Task", 4096, this, 3, core, &revCIInstructionTaskHandle);
 }
 
 void GobotRPC_TI_Hardware_RP2040_I2C::registerPullPackageCB(callback_pullPackage cb, void *args) {

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/transmission_interface/rp2040_i2c_int.cpp

@@ -31,4 +31,19 @@ void GobotRPC_TI_Hardware_RP2040_I2C::intPinISR(BaseType_t * xHigherPriorityTask
     xSemaphoreGiveFromISR(i2cRXSemaphore, xHigherPriorityTaskWoken);
 }
 
+void GobotRPC_TI_Hardware_RP2040_I2C::manualTriggerIntTask() {
+    while (1) {
+        uint32_t intPinsStates = readIntPins();
 
+        if(intPinsStates == 0) {
+            vTaskDelay(500 / portTICK_PERIOD_MS);
+            continue;
+        }
+
+        vTaskDelay(500 / portTICK_PERIOD_MS);
+
+        xSemaphoreGive(i2cRXSemaphore);
+    
+    }
+    
+}

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/transmission_interface/rp2040_i2c_rx.cpp

@@ -1,4 +1,5 @@
 #include "transmission_interface.hpp"
+#include "ci/base.hpp"
 
 void i2cRxTaskFn(void * args) {
     GobotRPC_TI_Hardware_RP2040_I2C * hw = (GobotRPC_TI_Hardware_RP2040_I2C *)args;
@@ -10,6 +11,7 @@ void GobotRPC_TI_Hardware_RP2040_I2C::i2cRxTask() {
         xSemaphoreTake(i2cRXSemaphore, portMAX_DELAY);
 
         uint32_t done_mask = 0xffffffff;
+
         uint32_t pinStates = readIntPins();
 
         do {
@@ -17,42 +19,50 @@ void GobotRPC_TI_Hardware_RP2040_I2C::i2cRxTask() {
                 if((pinStates & done_mask) & (1 << i)) {
                     done_mask &= ~(1 << i);
             
+
                     uint32_t addr = intAddressMap[i];
 
+                    if(addr > 0xff)
+                        continue;
+
                     GoRPCPackage_Transport pkg;
 
                     xSemaphoreTake(i2cMutex, portMAX_DELAY);
                     bool read_res = readI2C(&pkg, addr);
                     xSemaphoreGive(i2cMutex);
 
-                    if(read_res) {
+                    if(!read_res)
-                        if(pushPackageCB != NULL)
+                        continue;
-                            pushPackageCB(&pkg, pushPackageCBArgs);
+
-                    }
+                    pkg.addr |= (i) << 8;
+
+                    if(pushPackageCB != NULL)
+                        pushPackageCB(&pkg, pushPackageCBArgs);
             
                     break;
                 }
             }
             
-
             pinStates = readIntPins();
         } while(pinStates & done_mask);
     }
 }
 
 bool GobotRPC_TI_Hardware_RP2040_I2C::readI2C(GoRPCPackage_Transport * pkg, uint32_t addr) {
+    unsigned int res;
+    //uint res = i2c_read_blocking(i2c, addr, (uint8_t *)pkg->data, 2, true);
 
-    uint res = i2c_read_burst_blocking(i2c, addr, (uint8_t *)pkg->data, 2);
+    i2c_read_blocking(i2c, addr, (uint8_t *)pkg->data + CI_RX_PACKAGE_DATA_OFFSET, 2, true);
 
-    if(res == PICO_ERROR_GENERIC)
-        return false; // I2C error
 
-    size_t len = pkg->data[1];
+    size_t len = pkg->data[CI_RX_PACKAGE_DATA_OFFSET + 1];
-    res = i2c_read_burst_blocking(i2c, addr, ((uint8_t *)pkg->data) + 2, len);
 
-    if(res == PICO_ERROR_GENERIC)
+    if(len > 64) {
-        return false; // I2C error
+        return false;
+    }
 
+    res = i2c_read_blocking(i2c, addr, ((uint8_t *)pkg->data) + CI_RX_PACKAGE_DATA_OFFSET, len, false);
+    pkg->len = len;
     pkg->addr = addr;
 
     return true;

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/transmission_interface/rp2040_i2c_tx.cpp

@@ -17,7 +17,7 @@ void GobotRPC_TI_Hardware_RP2040_I2C::i2cTxTask() {
         pullPackageCB(&pkg, pullPackageCBArgs);
 
         xSemaphoreTake(i2cMutex, portMAX_DELAY);
-        unsigned int res = i2c_write_burst_blocking(i2c, pkg.addr & 0x7f, (uint8_t *)(&pkg.data), pkg.len);
+        unsigned int res = i2c_write_blocking(i2c, pkg.addr & 0x7f, (uint8_t * )(pkg.data), pkg.len, false);        
         xSemaphoreGive(i2cMutex);
 
         if(res == PICO_ERROR_GENERIC) {

i2c-hub/firmware/i2c-hub-firmware/src/main_core0.cpp

@@ -32,6 +32,7 @@ void onTxPacket(void * args, char *data, size_t len, uint32_t addr) {
     GobotRPC_CI * gobotRPC_ci = (GobotRPC_CI *)args;
 
     GoRPCPackage_Transport pkg;
+
     memcpy(pkg.data, data, len);
     pkg.len = len;
     pkg.addr = addr;
@@ -41,24 +42,43 @@ void onTxPacket(void * args, char *data, size_t len, uint32_t addr) {
     //gobotRPC_ci->send_RxPacket(pkg.data, CALC_SIZE_GobotRPC_PACKAGE(0), addr);
 }
 
+void onRXFromI2CTask(void * pvParameters) {
+    GobotRPC_CI * gobotRPC_ci = (GobotRPC_CI *)pvParameters;
+
+    GoRPCPackage_Transport pkg;
+    while (true) {
+        xQueueReceive(appData.rxQueue, &pkg, portMAX_DELAY);
+
+        gobotRPC_ci->send_RxPacket(pkg.data, pkg.len, pkg.addr);
+    }
+}
+
+static void onSetAddrMap(void * args, uint32_t addr, uint8_t port) {
+    GobotRPC_CI * gobotrpc_ci = (GobotRPC_CI *)args;
+    gobotrpc_ci->send_rev_SetAddrMap(addr, port);
+}
+
 int main() {
     gpio_init(LED_PIN);
     gpio_set_dir(LED_PIN, true);
 
     appData.txQueue = xQueueCreate(5, sizeof(GoRPCPackage_Transport));
     appData.rxQueue = xQueueCreate(5, sizeof(GoRPCPackage_Transport));
-    appData.ciInstructionQueue = xQueueCreate(5, sizeof(CI_Instruction_Transport));
+    appData.ciInstructionQueue = xQueueCreate(3, sizeof(CI_Instruction_Transport));
+    appData.ciInstructionReverseQueue = xQueueCreate(3, 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, appData.ciInstructionReverseQueue);
-    
+    gobotRPC_ci.registerCB_SetAddress(onSetAddrMap, &gobotRPC_ci);
     gobotRPC_ci.registerCB_TxPacket(onTxPacket, &gobotRPC_ci);
 
     TaskHandle_t taskHandleCore0;
     TaskHandle_t taskHandleCore1;
+    TaskHandle_t taskRXFromI2CTask;
     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, &appData, 1, UART_CORE_MASK, &taskHandleCore1);
+    xTaskCreateAffinitySet(onRXFromI2CTask, "RX From I2C Task", 2048, &gobotRPC_ci, 3, UART_CORE_MASK, &taskRXFromI2CTask);
 
     vTaskStartScheduler();
 

i2c-hub/firmware/i2c-hub-firmware/src/main_core1.cpp

@@ -15,7 +15,7 @@ void pullPackageCB(GoRPCPackage_Transport * dest, void *args) {
     xQueueReceive(queue, dest, portMAX_DELAY);
 }
 
-void pushPackage(GoRPCPackage_Transport * src, void *args) {
+void pushPackageCB(GoRPCPackage_Transport * src, void *args) {
     QueueHandle_t queue = appData.rxQueue;
     xQueueSend(queue, src, portMAX_DELAY);
 }
@@ -26,13 +26,17 @@ void pushCIInstruction(CI_Instruction_Transport * src, void *args) {
 }
 
 void main_core2(void * pvParameters) {
-    AppData appData = *((AppData *)pvParameters);
+    AppData appData1 = *(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, appData1.ciInstructionReverseQueue);
+    //gobotrpc_ti_hardware.setAddrMap(0x21, 0);
 
     gobotrpc_ti_hardware.registerPullPackageCB(pullPackageCB, appData.txQueue);
-    gobotrpc_ti_hardware.registerPushPackageCB(pushPackage, appData.rxQueue);
+    gobotrpc_ti_hardware.registerPushPackageCB(pushPackageCB, appData.rxQueue);
     gobotrpc_ti_hardware.registerPushCIInstructionCB(pushCIInstruction, appData.ciInstructionQueue);
 
+    gobotrpc_ti_hardware.raiseInfoReset();
+
     while(1) {
         vTaskDelay(pdMS_TO_TICKS(10000));
     }

i2c-hub/uart-adapter/cache/boardpos_1736386482.json

@@ -0,0 +1 @@
+{"pos1": {"x": 1026, "y": 2096}, "pos2": {"x": 16651, "y": 18856}, "pos3": {"x": 1901, "y": 131}}

i2c-hub/uart-adapter/cache/scan_result.json

@@ -0,0 +1 @@
+{"corexy_available": true, "head_available": true, "vacuum_available": true, "corexy_port": 0, "head_port": 2, "vacuum_port": 1}

motor-control/core-xy-firmware/.vscode/settings.json

@@ -37,6 +37,9 @@
     "raspberry-pi-pico.cmakePath": "${HOME}/.pico-sdk/cmake/v3.29.9/bin/cmake",
     "raspberry-pi-pico.ninjaPath": "${HOME}/.pico-sdk/ninja/v1.12.1/ninja",
     "files.associations": {
-        "cstdint": "cpp"
+        "cstdint": "cpp",
+        "array": "cpp",
+        "string": "cpp",
+        "string_view": "cpp"
     }
 }

motor-control/core-xy-firmware/CMakeLists.txt

@@ -34,6 +34,9 @@ project(core-xy-firmware C CXX ASM)
 # Add FreeRTOS
 include(cmake/FreeRTOS_Kernel_import.cmake)
 
+# Adding GobotRPC Node Library
+include(lib/gobotrpc/cmake/gobotRPC_Node.cmake)
+
 # Initialise the Raspberry Pi Pico SDK
 pico_sdk_init()
 
@@ -44,7 +47,6 @@ add_executable(core-xy-firmware
     src/main.cpp
     src/motors.cpp
     src/corexy/init.cpp
-    src/corexy/tasks.cpp
     src/corexy/motorFunctions.cpp
     src/corexy/limitSwitches.cpp
     src/corexy/position.cpp
@@ -76,6 +78,7 @@ target_link_libraries(core-xy-firmware
 
     tmc2209_driver
     FreeRTOS-Kernel-Heap4
+    GobotRPC_Node_RP2040_I2C
 )
 
 # Add the standard include files to the build

motor-control/core-xy-firmware/FreeRTOSConfig.h

@@ -72,8 +72,8 @@
 /* Memory allocation related definitions. */
 #define configSUPPORT_STATIC_ALLOCATION         0
 #define configSUPPORT_DYNAMIC_ALLOCATION        1
-#define configTOTAL_HEAP_SIZE                   (1024 * 64)
+#define configTOTAL_HEAP_SIZE                   (1024 * 128)
-#define configAPPLICATION_ALLOCATED_HEAP        (1024 * 32)
+#define configAPPLICATION_ALLOCATED_HEAP        (1024 * 64)
 
 /* Hook function related definitions. */
 #define configCHECK_FOR_STACK_OVERFLOW          0
@@ -104,11 +104,11 @@
 
 #if FREE_RTOS_KERNEL_SMP // set by the RP2040 SMP port of FreeRTOS
 /* SMP port only */
-#define configUSE_PASSIVE_IDLE_HOOK             0
+#define configNUMBER_OF_CORES                   2
-#define configNUMBER_OF_CORES                   1
 #define configTICK_CORE                         0
 #define configRUN_MULTIPLE_PRIORITIES           1
-//#define configUSE_CORE_AFFINITY                 1
+#define configUSE_CORE_AFFINITY                 1
+#define configUSE_PASSIVE_IDLE_HOOK             0
 #endif
 
 /* RP2040 specific */

motor-control/core-xy-firmware/include/CoreXY.hpp

@@ -52,12 +52,15 @@ struct BoardMessurements {
     int x_num;
     int y_num;
     
+    int x_offset;
+    int y_offset;
+
     int cell_x;
     int cell_y;
 };
 
-BoardMessurements calcBoardMessurements(MotorPosition p0, MotorPosition p1, int grid_x, int grid_y);
+BoardMessurements calcBoardMessurements(MotorPosition p0, MotorPosition p1, int grid_x, int grid_y, MotorPosition offset);
-MotorPosition calcPosition(BoardMessurements * messurements, int x, int y);
+MotorPosition calcPosition(BoardMessurements * messurements, int x, int y, bool offset);
 
 void vTaskInitMotorHome(void *pvParameters);
 
@@ -81,9 +84,8 @@ private:
     int untilNextHome = HOME_COUNT;
 
 public:
-    CoreXYMaschine(TMC2209_UART * uart_driver0, TMC2209_UART * uart_driver1, TMC2209_step_dual * step_driver);
+    CoreXYMaschine(TMC2209_UART * uart_driver0, TMC2209_UART * uart_driver1, TMC2209_step_dual * step_driver, uint core);
     
-    void runTasks();
     void homeFast(MotorPosition * last_pos, bool ignoreMutex);
     void home(MotorPosition * last_pos);
 
@@ -95,7 +97,7 @@ public:
     bool getMotorEnabled();
 
     void setBoardMessurements(BoardMessurements messurements);
-    void gotoPosition(int x, int y);
+    void gotoPosition(int x, int y, bool offset);
 };
 
 void homeXY(TMC2209_step_dual * driver, HOME_AXIS_DIRECTION direction, unsigned int speed, unsigned int backupSpace, MotorPosition * last_pos);

motor-control/core-xy-firmware/include/pinConfigNode.hpp

@@ -0,0 +1,17 @@
+#pragma once
+#define CORE_MASK_GOBOTRPC 0b01
+#define CORE_MASK_MOTOR    0b01
+
+#define LED1_PIN 25
+#define LED2_PIN 16
+
+#define RELAY_PIN 15
+
+#define GORPC_INT_PIN 17
+#define GORPC_SDA_PIN 18
+#define GORPC_SCL_PIN 19
+
+#define INTERRUPTION_DURATION ((int)(2.5 * 1000))
+#define INTERRUPTION_DELAY 5 * 1000
+
+#define I2C_ADDR 0x23

motor-control/core-xy-firmware/lib/gobotrpc

@@ -0,0 +1 @@
+/home/alexander/Projects/gobot/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc

motor-control/core-xy-firmware/src/corexy/init.cpp

@@ -3,7 +3,7 @@
 
 #include <math.h>
 
-CoreXYMaschine::CoreXYMaschine(TMC2209_UART * uart_driver0, TMC2209_UART * uart_driver1, TMC2209_step_dual * step_driver) {
+CoreXYMaschine::CoreXYMaschine(TMC2209_UART * uart_driver0, TMC2209_UART * uart_driver1, TMC2209_step_dual * step_driver, uint core) {
     this->uart_driver0 = uart_driver0;
     this->uart_driver1 = uart_driver1;
     this->step_driver = step_driver;