alexander/gobot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Implemented RPCs for CoreXY

Browse Source
This commit is contained in:
AlexanderHD27
2025-01-08 21:19:03 +01:00
parent 0b620a76ca
commit b040662551
23 changed files with 335 additions and 303 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
motor-control/core-xy-firmware/.vscode/settings.json vendored
View File

@@ -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"
}
}

5
motor-control/core-xy-firmware/CMakeLists.txt
View File

@@ -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

10
motor-control/core-xy-firmware/FreeRTOSConfig.h
View File

@@ -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 */

3
motor-control/core-xy-firmware/include/CoreXY.hpp
View File

@@ -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);

17
motor-control/core-xy-firmware/include/pinConfigNode.hpp Normal file
View File

@@ -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

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

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

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

@@ -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);

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

@@ -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();
}
}

313
motor-control/core-xy-firmware/src/main.cpp
View File

@@ -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();
}
}

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

@@ -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() {