moved controll interface to seperate folder

# Conflicts:
#	.gitmodules
#	can-interface/.gitignore
#	can-interface/.vscode/settings.json
#	can-interface/main/CMakeLists.txt
#	can-interface/main/hello_world_main.cpp
#	common-libaries/mcp2521/include/mcp2521.hpp
#	common-libaries/mcp2521/include/mcp2521_command.hpp
#	common-libaries/mcp2521/include/mcp2521_toplevel.hpp
#	common-libaries/mcp2521/interface_commands.cpp
#	common-libaries/mcp2521/interface_interrupts.cpp
#	common-libaries/mcp2521_hardware_interface/CMakeLists.txt
#	common-libaries/mcp2521_hardware_interface/esp_implementation_cmd.cpp
#	common-libaries/mcp2521_hardware_interface/esp_implementation_init.cpp
#	common-libaries/mcp2521_hardware_interface/esp_implementation_int.cpp
#	common-libaries/mcp2521_hardware_interface/include/mcp2521_hardware_esp.hpp
#	common-libaries/mcp2521_hardware_interface/include/mcp2521_hardware_handle.hpp
#	motor-control/firmware/cmake/tmc2209.cmake

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

@@ -12,7 +12,7 @@
                 "echo": true,
                 "reveal": "always",
                 "focus": true,
-                "panel": "dedicated",
+                "panel": "shared",
                 "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,7 +63,6 @@ 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 * 84)
+#define configAPPLICATION_ALLOCATED_HEAP        (1024 * 64)
 
 /* Hook function related definitions. */
 #define configCHECK_FOR_STACK_OVERFLOW          0

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

@@ -14,10 +14,8 @@ 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

@@ -1,19 +0,0 @@
-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,5 +1,3 @@
-#include "crc16.hpp"
-
 #define POLY 0x8408
 /*
 //                                      16   12   5

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

@@ -1,28 +1,17 @@
 #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, QueueHandle_t ciInstructionReverseQueue) {
+GobotRPC_CI::GobotRPC_CI(I_GobotRPC_CI_Hardware *hardware, UBaseType_t core, QueueHandle_t ciInstructionQueue) {
     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);
 }
@@ -43,20 +32,6 @@ 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;
     }
@@ -67,11 +42,6 @@ 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,19 +23,11 @@ void GobotRPC_CI::txCIInstructionTask() {
                 send_ErrorTransmission(rx, addr);
                 break;
             }
-
             case CI_INSTRUCTION_SEND_TRANMISSION_SUCCESS: {
-                uint32_t addr = ciInstruction.data[0] \
-                    | (ciInstruction.data[1] << 8) \
-                    | (ciInstruction.data[2] << 16) \
-                    | (ciInstruction.data[3] << 24);
+                uint32_t addr = ciInstruction.data[1] | (ciInstruction.data[2] << 8) | (ciInstruction.data[3] << 16) | (ciInstruction.data[4] << 24);
                 send_SuccessTransmission(addr);
                 break;
             }
-
-            case CI_INSTRUCTION_SEND_INFO_RESET:
-                send_InfoReset();
-                break;
         }
     }
 }
@@ -66,25 +58,3 @@ 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,7 +116,6 @@ 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,14 +5,11 @@
 enum GobotRPC_CI_CMD {
     TX_CI_PACKET = 0x01,
     RX_CI_PACKET = 0x02,
-    SET_ADDR_PORT_MAP = 0x03,
-
+    PERFORM_SCAN_CI_PACKET = 0x03,
+    SCAN_RESULT_CI_PACKET = 0x04,
     SUCESS_TRANMISSION = 0xfc,
     ERROR_TRANMISSION = 0xfd,
-    HEARTBEAT = 0xff,
-
-    RESET_CI_PACKET = 0xcc,
-    RESET_INFO_CI_PACKET = 0xcd
+    HEARTBEAT = 0xff
 };
 
 #define CI_TX_PACKAGE_DATA_OFFSET 6
@@ -22,8 +19,6 @@ 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);
@@ -35,20 +30,14 @@ 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, QueueHandle_t ciInstructionReverseQueue);
+    GobotRPC_CI(I_GobotRPC_CI_Hardware *hardware, UBaseType_t core, QueueHandle_t ciInstructionQueue);
 
     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);
@@ -56,9 +45,7 @@ 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,6 +67,7 @@ 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,8 +1,6 @@
 enum CI_Instruction_Type {
     CI_INSTRUCTION_SEND_TRANMISSION_ERROR,
-    CI_INSTRUCTION_SEND_TRANMISSION_SUCCESS,
-    CI_INSTRUCTION_SEND_INFO_RESET,
-    CI_INSTRUCTION_REV_SEND_SET_ADDR_MAP,
+    CI_INSTRUCTION_SEND_TRANMISSION_SUCCESS
 };
 
 struct CI_Instruction_Transport {

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

@@ -9,7 +9,6 @@ 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/pinConfig.hpp

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

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

@@ -3,17 +3,7 @@
 #include <strings.h>
 
 enum GobotRPCNumber {
-    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
+    VACUM = 0x8
 };
 
 enum GobotRPCTypes {
@@ -22,10 +12,6 @@ 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)
 
@@ -38,8 +24,5 @@ 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/include/ti/transmission_interface.hpp

@@ -30,10 +30,6 @@ 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);
@@ -55,14 +51,8 @@ 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;
@@ -73,7 +63,7 @@ private:
     bool readI2C(GoRPCPackage_Transport * pkg, uint32_t addr);
 
 public:
-    GobotRPC_TI_Hardware_RP2040_I2C(UBaseType_t core, i2c_inst_t *i2c, QueueHandle_t revCIInstructionQueue);
+    GobotRPC_TI_Hardware_RP2040_I2C(UBaseType_t core, i2c_inst_t *i2c);
 
     void registerPullPackageCB(callback_pullPackage cb, void *args);
     void registerPushPackageCB(callback_pushPackage cb, void *args);
@@ -83,15 +73,11 @@ 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/node_interface/include/node_interface.hpp

@@ -1,45 +0,0 @@
-#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

@@ -1,78 +0,0 @@
-#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

@@ -1,73 +0,0 @@
-#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

@@ -1,127 +0,0 @@
-#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

@@ -1,99 +0,0 @@
-#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,7 +2,6 @@
 #include <stdint.h>
 
 #include "pico/stdlib.h"
-#include "hardware/watchdog.h"
 
 #include "crc16.hpp"
 
@@ -24,14 +23,3 @@ 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,29 +25,3 @@ 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,16 +1,13 @@
 #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, QueueHandle_t revCIInstructionQueue) {
+GobotRPC_TI_Hardware_RP2040_I2C::GobotRPC_TI_Hardware_RP2040_I2C(UBaseType_t core, i2c_inst_t *i2c) {
     this->core = core;
     this->i2c = i2c0;
-    this->revCIInstructionQueue = revCIInstructionQueue;
 
     g_GobotRPC_TI_Hardware_RP2040_I2C = this;
 
@@ -26,29 +23,19 @@ 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_irq_enabled_with_callback(GOBOTRPC_TI_INT_PIN_START + i, GPIO_IRQ_EDGE_RISE, true, intPin_ISR);
+        gpio_set_dir(GOBOTRPC_TI_INT_PIN_START< + i, GPIO_IN);
+        gpio_pull_down(GOBOTRPC_TI_INT_PIN_START + i);
     }
 
     initTasks();
 
-    //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_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);
 
 }
 
-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();
@@ -62,8 +49,6 @@ 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,19 +31,4 @@ 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,5 +1,4 @@
 #include "transmission_interface.hpp"
-#include "ci/base.hpp"
 
 void i2cRxTaskFn(void * args) {
     GobotRPC_TI_Hardware_RP2040_I2C * hw = (GobotRPC_TI_Hardware_RP2040_I2C *)args;
@@ -11,7 +10,6 @@ void GobotRPC_TI_Hardware_RP2040_I2C::i2cRxTask() {
         xSemaphoreTake(i2cRXSemaphore, portMAX_DELAY);
 
         uint32_t done_mask = 0xffffffff;
-
         uint32_t pinStates = readIntPins();
 
         do {
@@ -19,50 +17,42 @@ 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)
-                        continue;
-
-                    pkg.addr |= (i) << 8;
-
-                    if(pushPackageCB != NULL)
-                        pushPackageCB(&pkg, pushPackageCBArgs);
+                    if(read_res) {
+                        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);
     
-    i2c_read_blocking(i2c, addr, (uint8_t *)pkg->data + CI_RX_PACKAGE_DATA_OFFSET, 2, true);
+    uint res = i2c_read_burst_blocking(i2c, addr, (uint8_t *)pkg->data, 2);
     
+    if(res == PICO_ERROR_GENERIC)
+        return false; // I2C error
     
-    size_t len = pkg->data[CI_RX_PACKAGE_DATA_OFFSET + 1];
+    size_t len = pkg->data[1];
+    res = i2c_read_burst_blocking(i2c, addr, ((uint8_t *)pkg->data) + 2, len);
 
-    if(len > 64) {
-        return false;
-    }
+    if(res == PICO_ERROR_GENERIC)
+        return false; // I2C error
     
-    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_blocking(i2c, pkg.addr & 0x7f, (uint8_t * )(pkg.data), pkg.len, false);        
+        unsigned int res = i2c_write_burst_blocking(i2c, pkg.addr & 0x7f, (uint8_t *)(&pkg.data), pkg.len);
         xSemaphoreGive(i2cMutex);
 
         if(res == PICO_ERROR_GENERIC) {

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

@@ -32,7 +32,6 @@ 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;
@@ -42,43 +41,24 @@ 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(3, sizeof(CI_Instruction_Transport));
-    appData.ciInstructionReverseQueue = xQueueCreate(3, sizeof(CI_Instruction_Transport));
+    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, UART_CORE_MASK, appData.ciInstructionQueue, appData.ciInstructionReverseQueue);
-    gobotRPC_ci.registerCB_SetAddress(onSetAddrMap, &gobotRPC_ci);
+    GobotRPC_CI gobotRPC_ci(&gobotrpc_ci_hardware, 0b01, appData.ciInstructionQueue);
+    
     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, &appData, 1, UART_CORE_MASK, &taskHandleCore1);
-    xTaskCreateAffinitySet(onRXFromI2CTask, "RX From I2C Task", 2048, &gobotRPC_ci, 3, UART_CORE_MASK, &taskRXFromI2CTask);
+    xTaskCreateAffinitySet(main_core2, "Main Task Core 1", 2048, &gobotRPC_ci, 1, I2C_CORE_MASK, &taskHandleCore1);
 
     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 pushPackageCB(GoRPCPackage_Transport * src, void *args) {
+void pushPackage(GoRPCPackage_Transport * src, void *args) {
     QueueHandle_t queue = appData.rxQueue;
     xQueueSend(queue, src, portMAX_DELAY);
 }
@@ -26,17 +26,13 @@ void pushCIInstruction(CI_Instruction_Transport * src, void *args) {
 }
 
 void main_core2(void * pvParameters) {
-    AppData appData1 = *(AppData *)pvParameters;
-
-    GobotRPC_TI_Hardware_RP2040_I2C gobotrpc_ti_hardware(UART_CORE_MASK, i2c0, appData1.ciInstructionReverseQueue);
-    //gobotrpc_ti_hardware.setAddrMap(0x21, 0);
+    AppData appData = *((AppData *)pvParameters);
+    GobotRPC_TI_Hardware_RP2040_I2C gobotrpc_ti_hardware(I2C_CORE_MASK, i2c0);
 
     gobotrpc_ti_hardware.registerPullPackageCB(pullPackageCB, appData.txQueue);
-    gobotrpc_ti_hardware.registerPushPackageCB(pushPackageCB, appData.rxQueue);
+    gobotrpc_ti_hardware.registerPushPackageCB(pushPackage, 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

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

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

@@ -1 +0,0 @@
-{"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,9 +37,6 @@
     "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",
-        "array": "cpp",
-        "string": "cpp",
-        "string_view": "cpp"
+        "cstdint": "cpp"
     }
 }

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

@@ -34,9 +34,6 @@ 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()
 
@@ -47,6 +44,7 @@ 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
@@ -78,7 +76,6 @@ 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 * 128)
-#define configAPPLICATION_ALLOCATED_HEAP        (1024 * 64)
+#define configTOTAL_HEAP_SIZE                   (1024 * 64)
+#define configAPPLICATION_ALLOCATED_HEAP        (1024 * 32)
 
 /* 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 configNUMBER_OF_CORES                   2
+#define configUSE_PASSIVE_IDLE_HOOK             0
+#define configNUMBER_OF_CORES                   1
 #define configTICK_CORE                         0
 #define configRUN_MULTIPLE_PRIORITIES           1
-#define configUSE_CORE_AFFINITY                 1
-#define configUSE_PASSIVE_IDLE_HOOK             0
+//#define configUSE_CORE_AFFINITY                 1
 #endif
 
 /* RP2040 specific */

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

@@ -52,15 +52,12 @@ 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, MotorPosition offset);
-MotorPosition calcPosition(BoardMessurements * messurements, int x, int y, bool offset);
+BoardMessurements calcBoardMessurements(MotorPosition p0, MotorPosition p1, int grid_x, int grid_y);
+MotorPosition calcPosition(BoardMessurements * messurements, int x, int y);
 
 void vTaskInitMotorHome(void *pvParameters);
 
@@ -84,8 +81,9 @@ private:
     int untilNextHome = HOME_COUNT;
 
 public:
-    CoreXYMaschine(TMC2209_UART * uart_driver0, TMC2209_UART * uart_driver1, TMC2209_step_dual * step_driver, uint core);
+    CoreXYMaschine(TMC2209_UART * uart_driver0, TMC2209_UART * uart_driver1, TMC2209_step_dual * step_driver);
     
+    void runTasks();
     void homeFast(MotorPosition * last_pos, bool ignoreMutex);
     void home(MotorPosition * last_pos);
 
@@ -97,7 +95,7 @@ public:
     bool getMotorEnabled();
 
     void setBoardMessurements(BoardMessurements messurements);
-    void gotoPosition(int x, int y, bool offset);
+    void gotoPosition(int x, int y);
 };
 
 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

@@ -1,17 +0,0 @@
-#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

@@ -1 +0,0 @@
-/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, uint core) {
+CoreXYMaschine::CoreXYMaschine(TMC2209_UART * uart_driver0, TMC2209_UART * uart_driver1, TMC2209_step_dual * step_driver) {
     this->uart_driver0 = uart_driver0;
     this->uart_driver1 = uart_driver1;
     this->step_driver = step_driver;

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

@@ -28,10 +28,9 @@ void CoreXYMaschine::home(MotorPosition * last_pos) {
     xSemaphoreTake(xMoveMutex, portMAX_DELAY);
     MotorPosition res = {0, 0};
 
-    homeXY(step_driver, ORIGIN, HOME_SPEED_FAST, 0, last_pos);
+    homeXY(step_driver, ORIGIN, HOME_SPEED_FAST, 0, &res);
     homeXY(step_driver, ORIGIN, HOME_SPEED_SLOW, 500, &res);
     posUnknow = false;
-
     current_pos = {x:0, y:0};
     untilNextHome = HOME_COUNT;
     xSemaphoreGive(xMoveMutex);
@@ -41,9 +40,9 @@ void CoreXYMaschine::setBoardMessurements(BoardMessurements messurements) {
     this->boardMessurements = messurements;
 }
 
-void CoreXYMaschine::gotoPosition(int x, int y, bool offset) {
+void CoreXYMaschine::gotoPosition(int x, int y) {
     xSemaphoreTake(xMoveMutex, portMAX_DELAY);
-    MotorPosition pos = calcPosition(&this->boardMessurements, x, y, offset);
+    MotorPosition pos = calcPosition(&this->boardMessurements, x, y);
 
     if(posUnknow || untilNextHome == 0) {
         homeFast(&current_pos, true);

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

@@ -2,7 +2,7 @@
 
 #include <math.h>
 
-BoardMessurements calcBoardMessurements(MotorPosition p0, MotorPosition p1, int grid_x, int grid_y, MotorPosition offset) {
+BoardMessurements calcBoardMessurements(MotorPosition p0, MotorPosition p1, int grid_x, int grid_y) {
     BoardMessurements res;
 
     MotorPosition * smaller;
@@ -16,9 +16,6 @@ BoardMessurements calcBoardMessurements(MotorPosition p0, MotorPosition p1, int
         bigger = &p0;
     }
 
-    res.x_offset = smaller->x - offset.x;
-    res.y_offset = smaller->y - offset.y;
-
     res.x_num = grid_x;
     res.y_num = grid_y;
 
@@ -34,16 +31,11 @@ BoardMessurements calcBoardMessurements(MotorPosition p0, MotorPosition p1, int
     return res;
 }
 
-MotorPosition calcPosition(BoardMessurements * messurements, int x, int y, bool offset) {
+MotorPosition calcPosition(BoardMessurements * messurements, int x, int y) {
     MotorPosition res;
 
     res.x = messurements->x_padding + (x * messurements->cell_x);
     res.y = messurements->y_padding + (y * messurements->cell_y);
 
-    if(offset) {
-        res.x -= messurements->x_offset;
-        res.y -= messurements->y_offset;
-    }
-
     return res;
 }

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

@@ -0,0 +1,83 @@
+#include "CoreXY.hpp"
+
+#include "FreeRTOS.h"
+#include "task.h"
+#include "stdio.h"
+
+void waitForUserInput() {
+    getchar();
+}
+
+void performGridCall(CoreXYMaschine * machine) {
+
+    while (true) {
+    
+        MotorPosition p0 = {.x=1296, .y=1802};
+        MotorPosition p1 = {.x=16696, .y=18722};
+
+//        machine->homeFast(&p0, false);
+//        printf("First Position\n");
+//        machine->setMotorEnabled(false);
+//        waitForUserInput();
+//        machine->setMotorEnabled(true);
+//        machine->homeFast(&p0, false);
+//
+//        printf("Second Position\n");
+//        machine->setMotorEnabled(false);
+//        waitForUserInput();
+//        machine->setMotorEnabled(true);
+//        machine->homeFast(&p1, false);
+//
+//        printf("First Position: %d, %d\n", p0.x, p0.y);
+//        printf("Second Position: %d, %d\n", p1.x, p1.y);
+//
+        BoardMessurements messurements = calcBoardMessurements(p0, p1, 19, 19);
+        machine->setBoardMessurements(messurements);
+        printf("Hi!\n");
+        machine->home(NULL);
+        waitForUserInput();
+        machine->gotoPosition(10, 9);
+        waitForUserInput();
+        machine->gotoPosition(9, 10);
+        waitForUserInput();
+        machine->gotoPosition(8, 9);
+        waitForUserInput();
+        machine->gotoPosition(9, 8);
+        waitForUserInput();
+        machine->gotoPosition(10, 7);
+        waitForUserInput();
+        machine->home(NULL);
+    }
+
+    //machine->gotoPosition(7, 6);
+
+    //for(int i = 0; i < 19; i++) {
+    //    for(int j = 0; j < 19; j++) {
+    //        machine->gotoPosition(i, j);
+    //        vTaskDelay(100 / portTICK_PERIOD_MS);
+    //    }
+    //}
+
+    //machine->home(NULL);
+}
+
+void vTaskInitMotorHome(void *pvParameters) {
+    CoreXYMaschine * machine = (CoreXYMaschine *)pvParameters;
+    vTaskDelay(500 / portTICK_PERIOD_MS);    
+
+    MotorPosition res;
+    machine->home(&res);
+
+    performGridCall(machine);
+
+    vTaskDelete(NULL);
+}
+
+void CoreXYMaschine::runTasks() {
+    xTaskCreate(vTaskInitMotorHome, "Initial Motor Home", 1024, this, 5, NULL);
+
+    vTaskStartScheduler();
+    while(1) {
+        tight_loop_contents();
+    }
+}

motor-control/core-xy-firmware/src/main.cpp

@@ -24,12 +24,6 @@
 #include "FreeRTOSConfig.h"
 #include "FreeRTOS.h"
 #include "task.h"
-#include "queue.h"
-
-#include "pinConfigNode.hpp"
-#include "node_interface_hardware.hpp"
-#include "node_interface.hpp"
-#include "protocol.hpp"
 
 #define LED_PIN 25
 
@@ -61,20 +55,83 @@ enum SYSTEM_STATE {
 
 SYSTEM_STATE system_state = STARTUP;
 
-struct AppData {
-    xQueueHandle txQueue;
-    xQueueHandle rxQueue;
-    GobotRPC_NI * gobotrpc_ni;
-    CoreXYMaschine * machine;
-};
+/*
+void homeMotors0(TMC2209_step_dual * driver) {
 
-AppData g_appData;
+    driver->set_conf0(10000, 1);
+    driver->pulse0(10000);
+    driver->wait0(0);
+
+    driver->set_conf0(10000, 0);
+    driver->pulse0(100000);
+    while(!gpio_get(LIMIT2_PIN)) {
+        sleep_ms(1);
+    }
+    driver->pulse0(0);
+    sleep_ms(500);
+
+    // Second Run
+    driver->set_conf0(5000, 1);
+    driver->pulse0(200);
+    driver->wait0(0);
+
+    driver->set_conf0(500, 0);
+    driver->pulse0(5000);
+    while(!gpio_get(LIMIT2_PIN)) {
+        sleep_ms(1);
+    }
+    driver->pulse0(0);
+    sleep_ms(1);
+}
+
+void homeMotors1(TMC2209_step_dual * driver) {
+
+    driver->set_conf1(10000, 1);
+    driver->pulse1(5000);
+    driver->wait1(0);
+
+    driver->set_conf1(10000, 0);
+    driver->pulse1(100000);
+    while(!gpio_get(LIMIT3_PIN)) {
+        sleep_ms(1);
+    }
+    driver->pulse1(0);
+    sleep_ms(500);
+
+    // Second Run
+    driver->set_conf1(5000, 1);
+    driver->pulse1(200);
+    driver->wait1(0);
+
+    driver->set_conf1(500, 0);
+    driver->pulse1(5000);
+    while(!gpio_get(LIMIT3_PIN)) {
+        sleep_ms(1);
+    }
+    driver->pulse1(0);
+    sleep_ms(1);
+}
+
+void task_home0(void *pvParameters) {
+    TMC2209_step_dual * driver = (TMC2209_step_dual *)pvParameters;
+    vTaskDelay(500);
+    homeMotors0(driver);
+}
+
+void task_home1(void *pvParameters) {
+    TMC2209_step_dual * driver = (TMC2209_step_dual *)pvParameters;
+    homeMotors1(driver);
+}
+*/
 
 
-void MainTaskMotors(void * pvParameters) {
-    AppData * appData = (AppData *) pvParameters;
 
-    // Initing Motors
+
+int main() {
+    stdio_init_all();
+
+    printf("Hello, World!\n");
+
     TMC2209_step_dual step_driver = TMC2209_step_dual(
         STEP0_PIN, STEP1_PIN, 
         DIR0_PIN, DIR1_PIN, 
@@ -85,139 +142,123 @@ void MainTaskMotors(void * pvParameters) {
     TMC2209_UART uart_driver0 = TMC2209_UART(uart0, 0, 19200, 0, 1);
     TMC2209_UART uart_driver1 = TMC2209_UART(uart1, 0, 19200, 4, 5);
 
-    CoreXYMaschine machine = CoreXYMaschine(&uart_driver0, &uart_driver1, &step_driver, CORE_MASK_MOTOR);
-    appData->machine = &machine;
+    CoreXYMaschine machine = CoreXYMaschine(&uart_driver0, &uart_driver1, &step_driver);
     
     volatile uint8_t a = uart_driver0.read(REG_ADDR_CHOPPER_CHOPCONF);
     volatile uint8_t b = uart_driver1.read(REG_ADDR_CHOPPER_CHOPCONF);
 
-    appData->machine->home(NULL);
-    vTaskSuspend(NULL);
-}
+    machine.runTasks();
 
-void onRxPackage(void * args, char *data, uint16_t len, GobotRPCTypes type, GobotRPCNumber number) {
-    AppData * appData = (AppData *) args;
-    char txBuffer[32];
+/*
+    gpio_init(LIMIT0_PIN);
+    gpio_set_dir(LIMIT0_PIN, GPIO_IN);
+    gpio_pull_up(LIMIT0_PIN);
+    gpio_set_input_hysteresis_enabled(LIMIT0_PIN, true);
 
-    switch (number) {
-        case GobotRPCNumber::GET_INFO: {
-            InfoData info = appData->gobotrpc_ni->getInfo();
-            txBuffer[0] = (info.addr >> 24) & 0xFF;
-            txBuffer[1] = (info.addr >> 16) & 0xFF;
-            txBuffer[2] = (info.addr >> 8) & 0xFF;
-            txBuffer[3] = info.addr & 0xFF;
-            txBuffer[4] = info.type;
+    gpio_init(LIMIT1_PIN);
+    gpio_set_dir(LIMIT1_PIN, GPIO_IN);
+    gpio_pull_up(LIMIT1_PIN);
+    gpio_set_input_hysteresis_enabled(LIMIT1_PIN, true);
 
-            g_appData.gobotrpc_ni->sendPackage(txBuffer, 5, GobotRPCTypes::RESPONSE, GobotRPCNumber::GET_INFO);
-            break;
-        }
+    gpio_init(LIMIT2_PIN);
+    gpio_set_dir(LIMIT2_PIN, GPIO_IN);
+    gpio_pull_up(LIMIT2_PIN);
+    gpio_set_input_hysteresis_enabled(LIMIT2_PIN, true);
 
-        case GobotRPCNumber::RESET: {
-            softwareReset();
-            break;
-        }
+    gpio_init(LIMIT3_PIN);
+    gpio_set_dir(LIMIT3_PIN, GPIO_IN);
+    gpio_pull_up(LIMIT3_PIN);
+    gpio_set_input_hysteresis_enabled(LIMIT3_PIN, true);
 
-        case GobotRPCNumber::HOME_XY: {
-            MotorPosition pos;
-            appData->machine->home(&pos);
-            txBuffer[0] = (pos.x >> 24) & 0xFF;
-            txBuffer[1] = (pos.x >> 16) & 0xFF;
-            txBuffer[2] = (pos.x >> 8) & 0xFF;
-            txBuffer[3] = pos.x & 0xFF;
-            
-            txBuffer[4] = (pos.y >> 24) & 0xFF;
-            txBuffer[5] = (pos.y >> 16) & 0xFF;
-            txBuffer[6] = (pos.y >> 8) & 0xFF;
-            txBuffer[7] = pos.y & 0xFF;
-
-            g_appData.gobotrpc_ni->sendPackage(txBuffer, 8, GobotRPCTypes::RESPONSE, GobotRPCNumber::HOME_XY);
-            break;
-        }
-
-        case GobotRPCNumber::RELEASE_MOTORS: {
-            bool enable = data[0];
-            appData->machine->setMotorEnabled(enable);
-            g_appData.gobotrpc_ni->sendPackage(txBuffer, 0, GobotRPCTypes::RESPONSE, GobotRPCNumber::RELEASE_MOTORS);
-            break;
-        }
-
-        case GobotRPCNumber::SET_PADDING: {
-            MotorPosition p1;
-            MotorPosition p2;
-            MotorPosition p3;
-
-            p1.x = (data[0] << 24) | (data[1] << 16) | (data[2] << 8) | data[3];
-            p1.y = (data[4] << 24) | (data[5] << 16) | (data[6] << 8) | data[7];
-            p2.x = (data[8] << 24) | (data[9] << 16) | (data[10] << 8) | data[11];
-            p2.y = (data[12] << 24) | (data[13] << 16) | (data[14] << 8) | data[15];
-            p3.x = (data[16] << 24) | (data[17] << 16) | (data[18] << 8) | data[19];
-            p3.y = (data[20] << 24) | (data[21] << 16) | (data[22] << 8) | data[23];
-            uint8_t cellX = data[24];
-            uint8_t cellY = data[25];
-
-            BoardMessurements messurements = calcBoardMessurements(p1, p2, cellX, cellY, p3);
-            appData->machine->setBoardMessurements(messurements);
-            g_appData.gobotrpc_ni->sendPackage(txBuffer, 0, GobotRPCTypes::RESPONSE, GobotRPCNumber::SET_PADDING);
-            break;
-        }
-
-        case GobotRPCNumber::GOTO: {
-            int x = (data[0] << 24) | (data[1] << 16) | (data[2] << 8) | data[3];
-            int y = (data[4] << 24) | (data[5] << 16) | (data[6] << 8) | data[7];
-            bool offset = data[8] > 0;
-            appData->machine->gotoPosition(x, y, offset);
-            g_appData.gobotrpc_ni->sendPackage(txBuffer, 0, GobotRPCTypes::RESPONSE, GobotRPCNumber::GOTO);
-            break;
-        }
-        
-        default: {
-            break;
-        }
-    }
-}
-
-void MainTaskGobotRPC(void * pvParameters) {
-    AppData * appData = (AppData *) pvParameters;
-
-    GobotRPC_NI_Hardware_RP2040_I2C gobotrpc_ni_hardware(
-        appData->txQueue, appData->rxQueue, CORE_MASK_GOBOTRPC,
-        i2c1, I2C_ADDR, GORPC_SDA_PIN, GORPC_SCL_PIN, GORPC_INT_PIN
+    gpio_set_irq_enabled_with_callback(
+        LIMIT0_PIN, 
+        GPIO_IRQ_EDGE_RISE, 
+        true, 
+        isr_LimitSwitch
+    );
+    gpio_set_irq_enabled_with_callback(
+        LIMIT1_PIN, 
+        GPIO_IRQ_EDGE_RISE, 
+        true, 
+        isr_LimitSwitch
+    );
+    gpio_set_irq_enabled_with_callback(
+        LIMIT2_PIN, 
+        GPIO_IRQ_EDGE_RISE, 
+        true, 
+        isr_LimitSwitch
+    );
+    gpio_set_irq_enabled_with_callback(
+        LIMIT3_PIN, 
+        GPIO_IRQ_EDGE_RISE, 
+        true, 
+        isr_LimitSwitch
     );
 
-    while (true) {
-        gpio_put(LED1_PIN, 0);
-        vTaskDelay(500 / portTICK_PERIOD_MS);
-        gpio_put(LED1_PIN, 1);
-        vTaskDelay(500 / portTICK_PERIOD_MS);
-    }
-}
+    gpio_init(RGB2_B_PIN);
+    gpio_init(RGB2_G_PIN);
+    gpio_init(RGB2_R_PIN);
+    gpio_set_dir(RGB2_B_PIN, GPIO_OUT);
+    gpio_set_dir(RGB2_G_PIN, GPIO_OUT);
+    gpio_set_dir(RGB2_R_PIN, GPIO_OUT);
 
-int main() {
-    stdio_init_all();
+    gpio_init(RGB1_B_PIN);
+    gpio_init(RGB1_G_PIN);
+    gpio_set_dir(RGB1_B_PIN, GPIO_OUT);
+    gpio_set_dir(RGB1_G_PIN, GPIO_OUT);
+
+    gpio_put(RGB1_B_PIN, 1);
+    gpio_put(RGB1_G_PIN, 1);
+    gpio_put(RGB2_B_PIN, 1);
+    gpio_put(RGB2_G_PIN, 1);
+    gpio_put(RGB2_R_PIN, 1);
 
-    printf("Hello, World!\n");
 
     gpio_init(LED_PIN);
-    gpio_set_dir(LED1_PIN, GPIO_OUT);
-    gpio_put(LED_PIN, 1);
+    gpio_set_dir(LED_PIN, GPIO_OUT);
 
-    // Creating Queues
-    g_appData.txQueue = xQueueCreate(16, sizeof(GobotRPC_NI_Package_Transport));
-    g_appData.rxQueue = xQueueCreate(16, sizeof(GobotRPC_NI_Package_Transport));
+    //step_driver.set_conf0(10000, 0);
+    //step_driver.pulse0(1000000);
+    homeMotors0(&step_driver);
+    printf("Home0 done\n");
+    homeMotors1(&step_driver);
+    printf("Home1 done\n");
+*/
+    //xTaskCreate(task_home0, "Home0", 1024, &step_driver, 5, NULL);
+    //xTaskCreate(task_home1, "Home1", 1024, &step_driver, 5, NULL);
 
-    // Setting up GobotRPC
-    InfoData info = {.addr=I2C_ADDR, .type=NODE_TYPE_COREXY};
+    /*
+    while (true) {
+        sleep_ms(50);
+        gpio_put(LED_PIN, 0);
+        sleep_ms(50);
+        gpio_put(LED_PIN, 1);
 
-    GobotRPC_NI gobotrpc_ni(CORE_MASK_MOTOR, g_appData.txQueue, g_appData.rxQueue, info);
-    gobotrpc_ni.registerOnPackageRxCallback(onRxPackage, &g_appData);
-    g_appData.gobotrpc_ni = &gobotrpc_ni;
+        if(gpio_get(LIMIT1_PIN)) {
+            gpio_put(RGB1_B_PIN, 0);
+        } else {
+            gpio_put(RGB1_B_PIN, 1);
+        }
 
-    // Starting Tasks
-    xTaskCreateAffinitySet(MainTaskMotors, "Main Task Motors", 1024, &g_appData, 5, CORE_MASK_MOTOR, NULL);
-    xTaskCreateAffinitySet(MainTaskGobotRPC, "Main Task GobotRPC", 1024, &g_appData, 5, CORE_MASK_GOBOTRPC, NULL);
+        if(gpio_get(LIMIT3_PIN)) {
+            gpio_put(RGB1_G_PIN, 0);
+        } else {
+            gpio_put(RGB1_G_PIN, 1);
+        }
 
-    vTaskStartScheduler();
-    while(1) {
-        tight_loop_contents();
+        if(gpio_get(LIMIT0_PIN)) {
+            gpio_put(RGB2_B_PIN, 0);
+        } else {
+            gpio_put(RGB2_B_PIN, 1);
+        }
+
+        if(gpio_get(LIMIT2_PIN)) {
+            gpio_put(RGB2_G_PIN, 0);
+        } else {
+            gpio_put(RGB2_G_PIN, 1);
+        }
     }
+    */
+
+    return 0;
 }

motor-control/core-xy-firmware/src/tmc2209/step.cpp

@@ -54,7 +54,7 @@ void TMC2209_step_dual::init_gpio() {
     gpio_set_dir(this->enable1_pin, GPIO_OUT);
     gpio_put(this->enable0_pin, 0);
     gpio_put(this->enable1_pin, 0);
-    vTaskDelay(1 / portTICK_PERIOD_MS);
+    sleep_ms(1);
 
     gpio_init(this->dir0_pin);
     gpio_init(this->dir1_pin);
@@ -62,7 +62,7 @@ void TMC2209_step_dual::init_gpio() {
     gpio_set_dir(this->dir1_pin, GPIO_OUT);    
     gpio_put(this->dir0_pin, 1);
     gpio_put(this->dir1_pin, 1);
-    vTaskDelay(1 / portTICK_PERIOD_MS);
+    sleep_ms(1);
 };
 
 void TMC2209_step_dual::init_pio() {

stone-dispencer-control/firmware-stone-dispencer/CMakeLists.txt

@@ -34,9 +34,6 @@ project(firmware-stone-dispencer C CXX ASM)
 set(FREERTOS_KERNEL_PATH ${CMAKE_CURRENT_LIST_DIR}/lib/FreeRTOS-Kernel)
 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()
 
@@ -62,7 +59,6 @@ target_link_libraries(firmware-stone-dispencer
     pico_stdlib
     hardware_pwm
     FreeRTOS-Kernel-Heap4
-    GobotRPC_Node_RP2040_I2C
 )
 
 target_include_directories(firmware-stone-dispencer PUBLIC

stone-dispencer-control/firmware-stone-dispencer/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 * 128)
-#define configAPPLICATION_ALLOCATED_HEAP        (1024 * 64)
+#define configTOTAL_HEAP_SIZE                   (1024 * 64)
+#define configAPPLICATION_ALLOCATED_HEAP        (1024 * 32)
 
 /* 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 configNUMBER_OF_CORES                   2
+#define configUSE_PASSIVE_IDLE_HOOK             0
+#define configNUMBER_OF_CORES                   1
 #define configTICK_CORE                         0
 #define configRUN_MULTIPLE_PRIORITIES           1
-#define configUSE_CORE_AFFINITY                 1
-#define configUSE_PASSIVE_IDLE_HOOK             0
+//#define configUSE_CORE_AFFINITY                 1
 #endif
 
 /* RP2040 specific */

stone-dispencer-control/firmware-stone-dispencer/include/headSystem.hpp

@@ -1,5 +1,5 @@
 #pragma once
-#include "pinConfigNode.hpp"
+#include "pinConfig.hpp"
 #include "FreeRTOS.h"
 #include "queue.h"
 #include "semphr.h"
@@ -37,4 +37,7 @@ public:
     STONE_STATE dropSequence();
 
     void vHeadTask();
+
+    
 };
+

stone-dispencer-control/firmware-stone-dispencer/include/pinConfig.hpp

@@ -12,15 +12,10 @@
 #define LED2_PIN 16
 #define BUTTON_PIN 20
 
-// GoRPC Config
-#define GORPC_INT_PIN 2
-#define GORPC_SDA_PIN 4
-#define GORPC_SCL_PIN 5
+#define GOBOTRPC_SCL_PIN 5
+#define GOBOTRPC_SDA_PIN 4
+#define GOBOTRPC_INT_PIN 2
 
-#define I2C_ADDR 0x22
-
-#define CORE_MASK_GOBOTRPC 0b01
-#define CORE_MASK_HEAD     0b01
 
 // Pwm Config
 
@@ -48,7 +43,7 @@
 
 // Chopper States
 #define CHOPPER1_OPEN  MOTOR_MS18_MID - 400
-#define CHOPPER1_CLOSE MOTOR_MS18_MID + 20
+#define CHOPPER1_CLOSE MOTOR_MS18_MID + 40
 #define CHOPPER2_OPEN  MOTOR_MS18_MID + 375
 #define CHOPPER2_CLOSE MOTOR_MS18_MID + 30
 

stone-dispencer-control/firmware-stone-dispencer/lib/gobotrpc

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

stone-dispencer-control/firmware-stone-dispencer/src/main.cpp

@@ -7,76 +7,6 @@
 #include "hardware/pwm.h"
 
 #include "headSystem.hpp"
-#include "node_interface_hardware.hpp"
-#include "node_interface.hpp"
-#include "protocol.hpp"
-
-struct AppData {
-    xQueueHandle txQueue;
-    xQueueHandle rxQueue;
-    HeadSystem * headsystem;
-    GobotRPC_NI * gobotrpc_ni;
-};
-
-AppData g_appData;
-
-void onRxPackage(void * args, char *data, uint16_t len, GobotRPCTypes type, GobotRPCNumber number) {
-    AppData * appData = (AppData *) args;
-    char txBuffer[32];
-
-    switch (number) {
-        case GobotRPCNumber::GET_INFO: {
-            InfoData info = appData->gobotrpc_ni->getInfo();
-            txBuffer[0] = (info.addr >> 24) & 0xFF;
-            txBuffer[1] = (info.addr >> 16) & 0xFF;
-            txBuffer[2] = (info.addr >> 8) & 0xFF;
-            txBuffer[3] = info.addr & 0xFF;
-            txBuffer[4] = info.type;
-
-            g_appData.gobotrpc_ni->sendPackage(txBuffer, 5, GobotRPCTypes::RESPONSE, GobotRPCNumber::GET_INFO);
-            break;
-        }
-
-        case GobotRPCNumber::DROP_STONE: {
-            STONE_STATE state = appData->headsystem->dropStone();
-            txBuffer[0] = state;
-            g_appData.gobotrpc_ni->sendPackage(txBuffer, 1, GobotRPCTypes::RESPONSE, GobotRPCNumber::DROP_STONE);
-            break;
-        }
-
-        case GobotRPCNumber::MOVE_Z_AXIS: {
-            bool up = data[0] > 0;
-            appData->headsystem->setHeadUp(up);
-            g_appData.gobotrpc_ni->sendPackage(txBuffer, 0, GobotRPCTypes::RESPONSE, GobotRPCNumber::MOVE_Z_AXIS);
-
-            break;
-        }
-
-        case GobotRPCNumber::RESET: {
-            softwareReset();
-            break;
-        }
-        
-        default: {
-            break;
-        }
-    }
-}
-
-void vMainRPCHardwareTask(void *pvParameters) {
-    AppData * appData = (AppData *) pvParameters;
-    GobotRPC_NI_Hardware_RP2040_I2C gobotrpc_ni_hardware(
-        appData->txQueue, appData->rxQueue, CORE_MASK_GOBOTRPC,
-        i2c0, I2C_ADDR, GORPC_SDA_PIN, GORPC_SCL_PIN, GORPC_INT_PIN
-    );
-
-    while (1) {
-        vTaskDelay(500 / portTICK_PERIOD_MS);
-        gpio_put(LED1_PIN, 1);
-        vTaskDelay(500 / portTICK_PERIOD_MS);
-        gpio_put(LED1_PIN, 0);
-    }
-}
 
 void vMainTask(void *pvParameters) {
     HeadSystem * headSystem = (HeadSystem *) pvParameters;
@@ -85,7 +15,28 @@ void vMainTask(void *pvParameters) {
 
     headSystem->setHeadUp(true);
 
-    vTaskSuspend(NULL);
+    unsigned int counter = 0;
+    
+    while (1) {
+        buttonState = !gpio_get(BUTTON_PIN);
+
+        if(buttonState && counter == 4) {
+            headSystem->setHeadUp(false);
+        } else if(buttonState && counter == 5) {
+            headSystem->setHeadUp(true);
+        } else if(buttonState) {
+            STONE_STATE s = headSystem->dropSequence();
+            printf("Stone dropped: %d\n", s);
+        }
+
+        if(buttonState) {
+            printf("Counter: %d\n", counter);
+            counter = (counter + 1) % 6;
+        }
+
+        buttonLastState = buttonState;
+        vTaskDelay(pdMS_TO_TICKS(100));
+    }
 }
 
 int main()
@@ -93,27 +44,9 @@ int main()
     stdio_init_all();
 
     HeadSystem headSystem;
-    g_appData.headsystem = &headSystem;
     printf("HeadSystem initialized\n");
 
-    // Creating Queues
-    g_appData.txQueue = xQueueCreate(16, sizeof(GobotRPC_NI_Package_Transport));
-    g_appData.rxQueue = xQueueCreate(16, sizeof(GobotRPC_NI_Package_Transport));
-
-    // Setting GobotRPC Node Interface
-    InfoData info = {.addr=I2C_ADDR, .type=NODE_TYPE_HEAD};
-    GobotRPC_NI gobotrpc_ni(CORE_MASK_HEAD, g_appData.txQueue, g_appData.rxQueue, info);
-    gobotrpc_ni.registerOnPackageRxCallback(onRxPackage, &g_appData);
-    g_appData.gobotrpc_ni = &gobotrpc_ni;
-
-    // Sending Reset Notification
-    char txBuffer[8];
-    g_appData.gobotrpc_ni->sendPackage(txBuffer, 0, GobotRPCTypes::RESPONSE, GobotRPCNumber::RESET);
-
-    // Creating Tasks
-    xTaskCreate(vMainTask, "Main Head Task", 1024, &headSystem, 1, NULL);
-    xTaskCreate(vMainRPCHardwareTask, "Main GoboRPC Task", 2048, &g_appData, 1, NULL);
-
+    xTaskCreate(vMainTask, "Button Task", 1024, &headSystem, 1, NULL);
     vTaskStartScheduler();
 
     while (1) {

vacum-control/vacum-control-firmware/.vscode/c_cpp_properties.json

@@ -7,8 +7,8 @@
                 "${userHome}/.pico-sdk/sdk/2.1.0/**"
             ],
             "forcedInclude": [
-                "${workspaceFolder}/build/generated/pico_base/pico/config_autogen.h",
-                "${userHome}/.pico-sdk/sdk/2.1.0/src/common/pico_base_headers/include/pico.h"
+                "${userHome}/.pico-sdk/sdk/2.1.0/src/common/pico_base_headers/include/pico.h",
+                "${workspaceFolder}/build/generated/pico_base/pico/config_autogen.h"
             ],
             "defines": [],
             "compilerPath": "${userHome}/.pico-sdk/toolchain/13_3_Rel1/bin/arm-none-eabi-gcc",

vacum-control/vacum-control-firmware/.vscode/tasks.json

@@ -9,12 +9,8 @@
             "args": ["-C", "${workspaceFolder}/build"],
             "group": "build",
             "presentation": {
-                "echo": true,
                 "reveal": "always",
-                "focus": true,
-                "panel": "dedicated",
-                "showReuseMessage": true,
-                "clear": true
+                "panel": "dedicated"
             },
             "problemMatcher": "$gcc",
             "windows": {
@@ -31,12 +27,8 @@
                 "-fx"
             ],
             "presentation": {
-                "echo": true,
                 "reveal": "always",
-                "focus": true,
-                "panel": "shared",
-                "showReuseMessage": true,
-                "clear": true
+                "panel": "dedicated"
             },
             "problemMatcher": [],
             "windows": {
@@ -47,51 +39,16 @@
             "label": "Flash",
             "type": "process",
             "command": "${userHome}/.pico-sdk/openocd/0.12.0+dev/openocd.exe",
-            "args": [
-                "-s",
-                "${userHome}/.pico-sdk/openocd/0.12.0+dev/scripts",
-                "-f", "interface/cmsis-dap.cfg",
-                "-f", "target/${command:raspberry-pi-pico.getTarget}.cfg",
-                "-c", "adapter speed 5000; program \"${command:raspberry-pi-pico.launchTargetPath}\" reset exit",
-
-            ],
-            "presentation": {
-                "echo": true,
-                "reveal": "always",
-                "focus": true,
-                "panel": "shared",
-                "showReuseMessage": true,
-                "clear": true
-            },
-            "dependsOn": ["Compile Project"],
-            "problemMatcher": [],
-            "windows": {
-                "command": "${env:USERPROFILE}/.pico-sdk/openocd/0.12.0+dev/openocd.exe",
-            }
-        },
-        {
-            "label": "Reset",
-            "type": "process",
-            "command": "${userHome}/.pico-sdk/openocd/0.12.0+dev/openocd.exe",
             "args": [
                 "-s",
                 "${userHome}/.pico-sdk/openocd/0.12.0+dev/scripts",
                 "-f",
                 "interface/cmsis-dap.cfg",
-                "-f", "target/${command:raspberry-pi-pico.getTarget}.cfg",
-                "-c", "adapter speed 5000; init; reset halt;",
-                "-c", "rp2040.core1 arp_reset assert 0",
-                "-c", "rp2040.core0 arp_reset assert 0",
-                "-c", "exit"
+                "-f",
+                "target/${command:raspberry-pi-pico.getTarget}.cfg",
+                "-c",
+                "adapter speed 5000; program \"${command:raspberry-pi-pico.launchTargetPath}\" verify reset exit"
             ],
-            "presentation": {
-                "echo": true,
-                "reveal": "always",
-                "focus": true,
-                "panel": "shared",
-                "showReuseMessage": true,
-                "clear": true
-            },
             "problemMatcher": [],
             "windows": {
                 "command": "${env:USERPROFILE}/.pico-sdk/openocd/0.12.0+dev/openocd.exe",

vacum-control/vacum-control-firmware/CMakeLists.txt

@@ -34,9 +34,6 @@ project(vacum-control-firmware C CXX ASM)
 set(FREERTOS_KERNEL_PATH ${CMAKE_CURRENT_LIST_DIR}/lib/FreeRTOS-Kernel)
 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()
 
@@ -57,7 +54,6 @@ pico_enable_stdio_usb(vacum-control-firmware 0)
 # Add the standard library to the build
 target_link_libraries(vacum-control-firmware
     pico_stdlib
-    GobotRPC_Node_RP2040_I2C
     FreeRTOS-Kernel-Heap4
 )
 

vacum-control/vacum-control-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 * 128)
-#define configAPPLICATION_ALLOCATED_HEAP        (1024 * 64)
+#define configTOTAL_HEAP_SIZE                   (1024 * 64)
+#define configAPPLICATION_ALLOCATED_HEAP        (1024 * 32)
 
 /* 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 configNUMBER_OF_CORES                   2
+#define configUSE_PASSIVE_IDLE_HOOK             0
+#define configNUMBER_OF_CORES                   1
 #define configTICK_CORE                         0
 #define configRUN_MULTIPLE_PRIORITIES           1
-#define configUSE_CORE_AFFINITY                 1
-#define configUSE_PASSIVE_IDLE_HOOK             0
+//#define configUSE_CORE_AFFINITY                 1
 #endif
 
 /* RP2040 specific */

vacum-control/vacum-control-firmware/include/pinConfig.hpp

@@ -1,13 +1,10 @@
 #pragma once
-#define CORE_MASK_GOBOTRPC 0b10
-#define CORE_MASK_VACUM    0b01
-
 #define LED1_PIN 16
 #define LED2_PIN 25
 
 #define RELAY_PIN 15
 
-#define GORPC_INT_PIN 2
+#define GORPC_INT_PIN 3
 #define GORPC_SDA_PIN 4
 #define GORPC_SCL_PIN 5
 
@@ -15,5 +12,3 @@
 
 #define INTERRUPTION_DURATION ((int)(2.5 * 1000))
 #define INTERRUPTION_DELAY 5 * 1000
-
-#define I2C_ADDR 0x21

vacum-control/vacum-control-firmware/include/vacum.hpp

@@ -1,5 +1,5 @@
 #pragma once
-#include "pinConfigNode.hpp"
+#include "pinConfig.hpp"
 #include "FreeRTOS.h"
 #include "task.h"
 #include "semphr.h"
@@ -9,7 +9,6 @@ void vButtonTaskFn(void *pvParameters);
 
 class VacumControl {
 private:
-    uint coreMask;
     bool state = false;
     xSemaphoreHandle vacumMutex;
     xSemaphoreHandle interruptionSemaphore;
@@ -17,7 +16,7 @@ private:
     xTaskHandle interruptionTask;
     xTaskHandle buttonTask;
 public:
-    VacumControl(uint coreMask);
+    VacumControl();
 
     void initVacumPins();
     void setState(bool on);

vacum-control/vacum-control-firmware/lib/gobotrpc

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

vacum-control/vacum-control-firmware/src/base.cpp

@@ -1,5 +1,5 @@
 #include "vacum.hpp"
-#include "pinConfigNode.hpp"
+#include "pinConfig.hpp"
 
 #include "FreeRTOS.h"
 #include "task.h"
@@ -17,14 +17,13 @@ void vButtonTaskFn(void *pvParameters) {
     vacumControl->buttonTaskFn();
 }
 
-VacumControl::VacumControl(uint coreMask) {
-    this->coreMask = coreMask;
+VacumControl::VacumControl() {
     initVacumPins();
     state = false;
     vacumMutex = xSemaphoreCreateMutex();
     interruptionSemaphore = xSemaphoreCreateBinary();
-    xTaskCreateAffinitySet(vInterruptionTaskFn, "Interruption Task", 1024, this, 3, coreMask, &interruptionTask);
-    xTaskCreateAffinitySet(vButtonTaskFn, "Button Task", 1024, this, 2, coreMask, &buttonTask);
+    xTaskCreate(vInterruptionTaskFn, "Interruption Task", 1024, this, 3, &interruptionTask);
+    xTaskCreate(vButtonTaskFn, "Button Task", 1024, this, 2, &buttonTask);
 }
 
 void VacumControl::initVacumPins() {

vacum-control/vacum-control-firmware/src/main.cpp

@@ -1,5 +1,4 @@
 #include <stdio.h>
-#include <string.h>
 #include "pico/stdlib.h"
 
 #include "vacum.hpp"
@@ -7,110 +6,22 @@
 #include "FreeRTOSConfig.h"
 #include "FreeRTOS.h"
 #include "task.h"
-#include "queue.h"
-
-#include "hardware/i2c.h"
-
-#include "pinConfigNode.hpp"
-#include "node_interface_hardware.hpp"
-#include "node_interface.hpp"
-#include "protocol.hpp"
-
-struct AppData {
-    xQueueHandle txQueue;
-    xQueueHandle rxQueue;
-    VacumControl * vacumControl;
-    GobotRPC_NI * gobotrpc_ni;
-};
-
-AppData g_appData;
-
-void onRxPackage(void * args, char *data, uint16_t len, GobotRPCTypes type, GobotRPCNumber number) {
-    AppData * appData = (AppData *) args;
-    char txBuffer[32];
-
-    switch (number) {
-        case GobotRPCNumber::VACUM: {
-            GobotRPCPackage_Req_Vacum * pkg = (GobotRPCPackage_Req_Vacum *) data;
-            appData->vacumControl->setState(pkg->enable);
-            g_appData.gobotrpc_ni->sendPackage(txBuffer, 0, GobotRPCTypes::RESPONSE, GobotRPCNumber::VACUM);
-            break;
-        }
-
-        case GobotRPCNumber::GET_INFO: {
-            InfoData info = appData->gobotrpc_ni->getInfo();
-            txBuffer[0] = (info.addr >> 24) & 0xFF;
-            txBuffer[1] = (info.addr >> 16) & 0xFF;
-            txBuffer[2] = (info.addr >> 8) & 0xFF;
-            txBuffer[3] = info.addr & 0xFF;
-            txBuffer[4] = info.type;
-
-            g_appData.gobotrpc_ni->sendPackage(txBuffer, 5, GobotRPCTypes::RESPONSE, GobotRPCNumber::GET_INFO);
-            break;
-        }
-
-        case GobotRPCNumber::RESET: {
-            softwareReset();
-            break;
-        }
-        
-        default: {
-            break;
-        }
-    }
-}
-
-void core1_main(void *pvParameters) {
-    AppData * appData = (AppData *) pvParameters;
-    GobotRPC_NI_Hardware_RP2040_I2C gobotrpc_ni_hardware(
-        appData->txQueue, appData->rxQueue, CORE_MASK_VACUM,
-        i2c0, I2C_ADDR, GORPC_SDA_PIN, GORPC_SCL_PIN, GORPC_INT_PIN
-    );
 
+void vTaskFunction(void *pvParameters) {
     while (true) {
-        gpio_put(LED2_PIN, 0);
-        vTaskDelay(500 / portTICK_PERIOD_MS);
-        gpio_put(LED2_PIN, 1);
-        vTaskDelay(500 / portTICK_PERIOD_MS);
+        printf("Button: %d\n", gpio_get(BUTTON_PIN));
+        vTaskDelay(pdMS_TO_TICKS(1000));
     }
 }
 
-TaskHandle_t xCore0TaskHandle = NULL;
-TaskHandle_t xCore1TaskHandle = NULL;
-
 int main() {
     stdio_init_all();
     printf("Hello, world!\n");
 
-    gpio_init(LED1_PIN);
-    gpio_set_dir(LED1_PIN, GPIO_OUT);
-    gpio_put(LED1_PIN, 1);
-
-    gpio_init(LED2_PIN);
-    gpio_set_dir(LED2_PIN, GPIO_OUT);
-    gpio_put(LED2_PIN, 1);
-
-    g_appData.txQueue = xQueueCreate(16, sizeof(GobotRPC_NI_Package_Transport));
-    g_appData.rxQueue = xQueueCreate(16, sizeof(GobotRPC_NI_Package_Transport));
-
-    VacumControl vacumControl = VacumControl(CORE_MASK_VACUM);
+    VacumControl vacumControl;
     vacumControl.setState(false);
-    g_appData.vacumControl = &vacumControl;
-
-    InfoData info = {.addr=I2C_ADDR, .type=NODE_TYPE_VACUM};
-
-    GobotRPC_NI gobotrpc_ni(CORE_MASK_VACUM, g_appData.txQueue, g_appData.rxQueue, info);
-    gobotrpc_ni.registerOnPackageRxCallback(onRxPackage, &g_appData);
-
-    g_appData.gobotrpc_ni = &gobotrpc_ni;
-
-    xTaskCreateAffinitySet(
-        core1_main, "Core 1 Main", 2048, &g_appData, 1, CORE_MASK_VACUM, &xCore1TaskHandle
-    );
-
-    char txBuffer[32];
-    g_appData.gobotrpc_ni->sendPackage(txBuffer, 0, GobotRPCTypes::RESPONSE, GobotRPCNumber::RESET);
     
+    xTaskCreate(vTaskFunction, "Main Task", 1024, NULL, tskIDLE_PRIORITY, NULL);
     vTaskStartScheduler();
 
     while (true) {