From b040662551f3a126b8f54a98e97a390402791bd3 Mon Sep 17 00:00:00 2001 From: AlexanderHD27 Date: Wed, 8 Jan 2025 21:19:03 +0100 Subject: [PATCH] Implemented RPCs for CoreXY --- .../docs/.$NetworkStack.drawio.bkp | 76 ++++- .../i2c-hub-firmware/docs/NetworkStack.drawio | 81 ++++- .../src/gobotrpc/include/util/protocol.hpp | 6 + .../node_interface/include/node_interface.hpp | 1 + .../include/node_interface_hardware.hpp | 2 +- i2c-hub/uart-adapter/src/gobotrpc/mapping.py | 4 +- .../gobotrpc/packages/corexy/package_home.py | 3 + .../packages/corexy/package_release_motors.py | 3 + .../packages/corexy/package_set_padding.py | 3 + .../gobotrpc/packages/corexy/packages_goto.py | 3 + .../src/gobotrpc/rpc_packages_corexy.py | 3 + i2c-hub/uart-adapter/src/gobotrpc/util.py | 4 +- i2c-hub/uart-adapter/src/main.py | 4 +- .../core-xy-firmware/.vscode/settings.json | 5 +- motor-control/core-xy-firmware/CMakeLists.txt | 5 +- .../core-xy-firmware/FreeRTOSConfig.h | 10 +- .../core-xy-firmware/include/CoreXY.hpp | 3 +- .../include/pinConfigNode.hpp | 17 + .../core-xy-firmware/src/corexy/init.cpp | 2 +- .../src/corexy/motorFunctions.cpp | 3 +- .../core-xy-firmware/src/corexy/tasks.cpp | 83 ----- motor-control/core-xy-firmware/src/main.cpp | 313 ++++++++---------- .../core-xy-firmware/src/tmc2209/step.cpp | 4 +- 23 files changed, 335 insertions(+), 303 deletions(-) create mode 100644 i2c-hub/uart-adapter/src/gobotrpc/packages/corexy/package_home.py create mode 100644 i2c-hub/uart-adapter/src/gobotrpc/packages/corexy/package_release_motors.py create mode 100644 i2c-hub/uart-adapter/src/gobotrpc/packages/corexy/package_set_padding.py create mode 100644 i2c-hub/uart-adapter/src/gobotrpc/packages/corexy/packages_goto.py create mode 100644 i2c-hub/uart-adapter/src/gobotrpc/rpc_packages_corexy.py create mode 100644 motor-control/core-xy-firmware/include/pinConfigNode.hpp delete mode 100644 motor-control/core-xy-firmware/src/corexy/tasks.cpp diff --git a/i2c-hub/firmware/i2c-hub-firmware/docs/.$NetworkStack.drawio.bkp b/i2c-hub/firmware/i2c-hub-firmware/docs/.$NetworkStack.drawio.bkp index ba25082..59a231e 100644 --- a/i2c-hub/firmware/i2c-hub-firmware/docs/.$NetworkStack.drawio.bkp +++ b/i2c-hub/firmware/i2c-hub-firmware/docs/.$NetworkStack.drawio.bkp @@ -1,10 +1,10 @@ - + - + @@ -205,10 +205,10 @@ - + - + @@ -407,7 +407,7 @@ - + @@ -1545,7 +1545,7 @@ - + @@ -1612,7 +1612,7 @@ - + @@ -1630,18 +1630,72 @@ - + - + - + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/i2c-hub/firmware/i2c-hub-firmware/docs/NetworkStack.drawio b/i2c-hub/firmware/i2c-hub-firmware/docs/NetworkStack.drawio index a6d9ce7..ab075f9 100644 --- a/i2c-hub/firmware/i2c-hub-firmware/docs/NetworkStack.drawio +++ b/i2c-hub/firmware/i2c-hub-firmware/docs/NetworkStack.drawio @@ -1,10 +1,10 @@ - + - + @@ -205,10 +205,10 @@ - + - + @@ -407,7 +407,7 @@ - + @@ -1545,7 +1545,7 @@ - + @@ -1612,7 +1612,7 @@ - + @@ -1630,21 +1630,78 @@ - + - + - + - + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/include/util/protocol.hpp b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/include/util/protocol.hpp index 8b5859f..4194187 100644 --- a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/include/util/protocol.hpp +++ b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/include/util/protocol.hpp @@ -3,8 +3,14 @@ #include 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 diff --git a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/node_interface/include/node_interface.hpp b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/node_interface/include/node_interface.hpp index 3439b96..58e3554 100644 --- a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/node_interface/include/node_interface.hpp +++ b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/node_interface/include/node_interface.hpp @@ -12,6 +12,7 @@ typedef void (*onPackageRxCallback)(void * args, char *data, uint16_t len, Gobot enum NODE_TYPE: uint8_t { NODE_TYPE_VACUM = 0xa1, NODE_TYPE_HEAD = 0xa2, + NODE_TYPE_COREXY = 0xa3, }; struct InfoData { diff --git a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/node_interface/include/node_interface_hardware.hpp b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/node_interface/include/node_interface_hardware.hpp index 3dbae54..3b29ca5 100644 --- a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/node_interface/include/node_interface_hardware.hpp +++ b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/node_interface/include/node_interface_hardware.hpp @@ -9,7 +9,7 @@ #include "hardware/irq.h" struct GobotRPC_NI_Package_Transport { - char data[32]; + char data[256]; uint length; uint index; }; diff --git a/i2c-hub/uart-adapter/src/gobotrpc/mapping.py b/i2c-hub/uart-adapter/src/gobotrpc/mapping.py index dd300da..49a11ff 100644 --- a/i2c-hub/uart-adapter/src/gobotrpc/mapping.py +++ b/i2c-hub/uart-adapter/src/gobotrpc/mapping.py @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:ed072b7a08fe3f09bc6b018745c02d96e1194c619e437d83990b11715949d0d3 -size 1912 +oid sha256:321be867cf18345e70bf45c233050043a46c9a577929724ea92729e1d6fb17e2 +size 2305 diff --git a/i2c-hub/uart-adapter/src/gobotrpc/packages/corexy/package_home.py b/i2c-hub/uart-adapter/src/gobotrpc/packages/corexy/package_home.py new file mode 100644 index 0000000..60a386d --- /dev/null +++ b/i2c-hub/uart-adapter/src/gobotrpc/packages/corexy/package_home.py @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2bebb5ee59c241d37c0349a17411bd4c3bcfd7b211b83c21cd68a885a766e9d4 +size 3240 diff --git a/i2c-hub/uart-adapter/src/gobotrpc/packages/corexy/package_release_motors.py b/i2c-hub/uart-adapter/src/gobotrpc/packages/corexy/package_release_motors.py new file mode 100644 index 0000000..b81b29a --- /dev/null +++ b/i2c-hub/uart-adapter/src/gobotrpc/packages/corexy/package_release_motors.py @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0e2d4aa74aba3e044329648c236f2f85b5d74e31258dda0f5ffbf04fb74d824d +size 3177 diff --git a/i2c-hub/uart-adapter/src/gobotrpc/packages/corexy/package_set_padding.py b/i2c-hub/uart-adapter/src/gobotrpc/packages/corexy/package_set_padding.py new file mode 100644 index 0000000..8c08cc9 --- /dev/null +++ b/i2c-hub/uart-adapter/src/gobotrpc/packages/corexy/package_set_padding.py @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4a76ecf9c47b7f1167f562e3ae7276c94d4e90cd2f16f7a192d67cbdb89f6a13 +size 5063 diff --git a/i2c-hub/uart-adapter/src/gobotrpc/packages/corexy/packages_goto.py b/i2c-hub/uart-adapter/src/gobotrpc/packages/corexy/packages_goto.py new file mode 100644 index 0000000..eb2b0f4 --- /dev/null +++ b/i2c-hub/uart-adapter/src/gobotrpc/packages/corexy/packages_goto.py @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:153b52bb66cd7c7ae875dfa860030f8686d4a3d1931376d1e371a8524b98ea1f +size 3602 diff --git a/i2c-hub/uart-adapter/src/gobotrpc/rpc_packages_corexy.py b/i2c-hub/uart-adapter/src/gobotrpc/rpc_packages_corexy.py new file mode 100644 index 0000000..2bf6b71 --- /dev/null +++ b/i2c-hub/uart-adapter/src/gobotrpc/rpc_packages_corexy.py @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5a3da22f228f58517bfd3d98e558510e97b794fa7e8dba35cbbfa7cb23106054 +size 415 diff --git a/i2c-hub/uart-adapter/src/gobotrpc/util.py b/i2c-hub/uart-adapter/src/gobotrpc/util.py index 7d4b669..9fd82f5 100644 --- a/i2c-hub/uart-adapter/src/gobotrpc/util.py +++ b/i2c-hub/uart-adapter/src/gobotrpc/util.py @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:e813723d0250f6833f876487846af355f78e4036e9d606cb97a362ead70943cd -size 1984 +oid sha256:26efff00c4b3c42db65424fa0b3a87f5d90b495e31479983943ab909cef002bf +size 2303 diff --git a/i2c-hub/uart-adapter/src/main.py b/i2c-hub/uart-adapter/src/main.py index 39ae81d..4146658 100644 --- a/i2c-hub/uart-adapter/src/main.py +++ b/i2c-hub/uart-adapter/src/main.py @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:a6ff76bff1f1ded03a3742c6b45d43aee21c468f297a3c63f4c8725d3c3833b0 -size 1635 +oid sha256:78c351a82c131a1e75d346a030011d83d8ed17d267bcb0153f7aa87815c8491d +size 2205 diff --git a/motor-control/core-xy-firmware/.vscode/settings.json b/motor-control/core-xy-firmware/.vscode/settings.json index 9577ef1..62a76ac 100644 --- a/motor-control/core-xy-firmware/.vscode/settings.json +++ b/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" } } diff --git a/motor-control/core-xy-firmware/CMakeLists.txt b/motor-control/core-xy-firmware/CMakeLists.txt index 6cf78ee..65c7694 100644 --- a/motor-control/core-xy-firmware/CMakeLists.txt +++ b/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 diff --git a/motor-control/core-xy-firmware/FreeRTOSConfig.h b/motor-control/core-xy-firmware/FreeRTOSConfig.h index 601b4db..b332416 100644 --- a/motor-control/core-xy-firmware/FreeRTOSConfig.h +++ b/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 configAPPLICATION_ALLOCATED_HEAP (1024 * 32) +#define configTOTAL_HEAP_SIZE (1024 * 128) +#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 1 +#define configNUMBER_OF_CORES 2 #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 */ diff --git a/motor-control/core-xy-firmware/include/CoreXY.hpp b/motor-control/core-xy-firmware/include/CoreXY.hpp index 5c237fe..10395a4 100644 --- a/motor-control/core-xy-firmware/include/CoreXY.hpp +++ b/motor-control/core-xy-firmware/include/CoreXY.hpp @@ -81,9 +81,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); diff --git a/motor-control/core-xy-firmware/include/pinConfigNode.hpp b/motor-control/core-xy-firmware/include/pinConfigNode.hpp new file mode 100644 index 0000000..ce1b437 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/motor-control/core-xy-firmware/src/corexy/init.cpp b/motor-control/core-xy-firmware/src/corexy/init.cpp index 93e6a9e..b969db2 100644 --- a/motor-control/core-xy-firmware/src/corexy/init.cpp +++ b/motor-control/core-xy-firmware/src/corexy/init.cpp @@ -3,7 +3,7 @@ #include -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; diff --git a/motor-control/core-xy-firmware/src/corexy/motorFunctions.cpp b/motor-control/core-xy-firmware/src/corexy/motorFunctions.cpp index 597fa0e..43a5f13 100644 --- a/motor-control/core-xy-firmware/src/corexy/motorFunctions.cpp +++ b/motor-control/core-xy-firmware/src/corexy/motorFunctions.cpp @@ -28,9 +28,10 @@ void CoreXYMaschine::home(MotorPosition * last_pos) { xSemaphoreTake(xMoveMutex, portMAX_DELAY); MotorPosition res = {0, 0}; - homeXY(step_driver, ORIGIN, HOME_SPEED_FAST, 0, &res); + homeXY(step_driver, ORIGIN, HOME_SPEED_FAST, 0, last_pos); homeXY(step_driver, ORIGIN, HOME_SPEED_SLOW, 500, &res); posUnknow = false; + current_pos = {x:0, y:0}; untilNextHome = HOME_COUNT; xSemaphoreGive(xMoveMutex); diff --git a/motor-control/core-xy-firmware/src/corexy/tasks.cpp b/motor-control/core-xy-firmware/src/corexy/tasks.cpp deleted file mode 100644 index a2b4d6a..0000000 --- a/motor-control/core-xy-firmware/src/corexy/tasks.cpp +++ /dev/null @@ -1,83 +0,0 @@ -#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(); - } -} \ No newline at end of file diff --git a/motor-control/core-xy-firmware/src/main.cpp b/motor-control/core-xy-firmware/src/main.cpp index 2020756..9303983 100755 --- a/motor-control/core-xy-firmware/src/main.cpp +++ b/motor-control/core-xy-firmware/src/main.cpp @@ -24,6 +24,12 @@ #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 @@ -55,83 +61,20 @@ enum SYSTEM_STATE { SYSTEM_STATE system_state = STARTUP; -/* -void homeMotors0(TMC2209_step_dual * driver) { +struct AppData { + xQueueHandle txQueue; + xQueueHandle rxQueue; + GobotRPC_NI * gobotrpc_ni; + CoreXYMaschine * machine; +}; - 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); -} -*/ +AppData g_appData; +void MainTaskMotors(void * pvParameters) { + AppData * appData = (AppData *) pvParameters; - -int main() { - stdio_init_all(); - - printf("Hello, World!\n"); - + // Initing Motors TMC2209_step_dual step_driver = TMC2209_step_dual( STEP0_PIN, STEP1_PIN, DIR0_PIN, DIR1_PIN, @@ -142,123 +85,139 @@ int main() { 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); - + CoreXYMaschine machine = CoreXYMaschine(&uart_driver0, &uart_driver1, &step_driver, CORE_MASK_MOTOR); + appData->machine = &machine; + volatile uint8_t a = uart_driver0.read(REG_ADDR_CHOPPER_CHOPCONF); volatile uint8_t b = uart_driver1.read(REG_ADDR_CHOPPER_CHOPCONF); - machine.runTasks(); + appData->machine->home(NULL); + vTaskSuspend(NULL); +} -/* - gpio_init(LIMIT0_PIN); - gpio_set_dir(LIMIT0_PIN, GPIO_IN); - gpio_pull_up(LIMIT0_PIN); - gpio_set_input_hysteresis_enabled(LIMIT0_PIN, true); +void onRxPackage(void * args, char *data, uint16_t len, GobotRPCTypes type, GobotRPCNumber number) { + AppData * appData = (AppData *) args; + char txBuffer[32]; - gpio_init(LIMIT1_PIN); - gpio_set_dir(LIMIT1_PIN, GPIO_IN); - gpio_pull_up(LIMIT1_PIN); - gpio_set_input_hysteresis_enabled(LIMIT1_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(LIMIT2_PIN); - gpio_set_dir(LIMIT2_PIN, GPIO_IN); - gpio_pull_up(LIMIT2_PIN); - gpio_set_input_hysteresis_enabled(LIMIT2_PIN, true); - - gpio_init(LIMIT3_PIN); - gpio_set_dir(LIMIT3_PIN, GPIO_IN); - gpio_pull_up(LIMIT3_PIN); - gpio_set_input_hysteresis_enabled(LIMIT3_PIN, true); - - 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 - ); - - 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); - - 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); - - - gpio_init(LED_PIN); - gpio_set_dir(LED_PIN, GPIO_OUT); - - //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); - - /* - while (true) { - sleep_ms(50); - gpio_put(LED_PIN, 0); - sleep_ms(50); - gpio_put(LED_PIN, 1); - - if(gpio_get(LIMIT1_PIN)) { - gpio_put(RGB1_B_PIN, 0); - } else { - gpio_put(RGB1_B_PIN, 1); + g_appData.gobotrpc_ni->sendPackage(txBuffer, 5, GobotRPCTypes::RESPONSE, GobotRPCNumber::GET_INFO); + break; } - if(gpio_get(LIMIT3_PIN)) { - gpio_put(RGB1_G_PIN, 0); - } else { - gpio_put(RGB1_G_PIN, 1); + case GobotRPCNumber::RESET: { + softwareReset(); + break; } - if(gpio_get(LIMIT0_PIN)) { - gpio_put(RGB2_B_PIN, 0); - } else { - gpio_put(RGB2_B_PIN, 1); + 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; } - if(gpio_get(LIMIT2_PIN)) { - gpio_put(RGB2_G_PIN, 0); - } else { - gpio_put(RGB2_G_PIN, 1); + 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); + 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); + g_appData.gobotrpc_ni->sendPackage(txBuffer, 0, GobotRPCTypes::RESPONSE, GobotRPCNumber::GOTO); + break; + } + + default: { + break; } } - */ +} - return 0; +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 + ); + + while (true) { + gpio_put(LED1_PIN, 0); + vTaskDelay(500 / portTICK_PERIOD_MS); + gpio_put(LED1_PIN, 1); + vTaskDelay(500 / portTICK_PERIOD_MS); + } +} + +int main() { + stdio_init_all(); + + printf("Hello, World!\n"); + + gpio_init(LED_PIN); + gpio_set_dir(LED1_PIN, GPIO_OUT); + gpio_put(LED_PIN, 1); + + // Creating Queues + g_appData.txQueue = xQueueCreate(16, sizeof(GobotRPC_NI_Package_Transport)); + g_appData.rxQueue = xQueueCreate(16, sizeof(GobotRPC_NI_Package_Transport)); + + // Setting up GobotRPC + InfoData info = {.addr=I2C_ADDR, .type=NODE_TYPE_COREXY}; + + 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; + + // 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); + + vTaskStartScheduler(); + while(1) { + tight_loop_contents(); + } } \ No newline at end of file diff --git a/motor-control/core-xy-firmware/src/tmc2209/step.cpp b/motor-control/core-xy-firmware/src/tmc2209/step.cpp index 4bbce51..5e66e4f 100644 --- a/motor-control/core-xy-firmware/src/tmc2209/step.cpp +++ b/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); - sleep_ms(1); + vTaskDelay(1 / portTICK_PERIOD_MS); 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); - sleep_ms(1); + vTaskDelay(1 / portTICK_PERIOD_MS); }; void TMC2209_step_dual::init_pio() {