Got GoRPC on Head working

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

@@ -3,6 +3,8 @@
 #include <strings.h>
 
 enum GobotRPCNumber {
+    DROP_STONE = 0x5,
+    MOVE_Z_AXIS = 0x7,
     VACUM = 0x8,
     RESET = 0xc,
     GET_INFO = 0xd

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

@@ -10,7 +10,8 @@
 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_VACUM = 0xa1,
+    NODE_TYPE_HEAD = 0xa2,
 };
 
 struct InfoData {

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

@@ -34,6 +34,9 @@ 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()
 
@@ -59,6 +62,7 @@ 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 * 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 */

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

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

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

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

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

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

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

@@ -7,6 +7,76 @@
 #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;
@@ -15,28 +85,7 @@ void vMainTask(void *pvParameters) {
 
     headSystem->setHeadUp(true);
 
-    unsigned int counter = 0;
+    vTaskSuspend(NULL);
-    
-    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()
@@ -44,9 +93,27 @@ int main()
     stdio_init_all();
 
     HeadSystem headSystem;
+    g_appData.headsystem = &headSystem;
     printf("HeadSystem initialized\n");
 
-    xTaskCreate(vMainTask, "Button Task", 1024, &headSystem, 1, NULL);
+    // 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);
+
     vTaskStartScheduler();
 
     while (1) {