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

Goto CoreXY to work

Browse Source
This commit is contained in:
AlexanderHD27
2025-01-01 19:44:15 +01:00
parent c136056ed2
commit ad61e7e9b7
12 changed files with 341 additions and 109 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

@@ -35,5 +35,8 @@
"raspberry-pi-pico.cmakeAutoConfigure": false,
"raspberry-pi-pico.useCmakeTools": true,
"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"
"raspberry-pi-pico.ninjaPath": "${HOME}/.pico-sdk/ninja/v1.12.1/ninja",
"files.associations": {
"cstdint": "cpp"
}
}

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

@@ -42,8 +42,12 @@ include(cmake/tmc2209.cmake)
add_executable(core-xy-firmware
src/main.cpp
src/limitSwitches.cpp
src/motors.cpp
src/corexy/init.cpp
src/corexy/tasks.cpp
src/corexy/motorFunctions.cpp
src/corexy/limitSwitches.cpp
src/corexy/position.cpp
)
target_include_directories(core-xy-firmware PUBLIC

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

@@ -31,6 +31,7 @@
#define HOME_SPEED_FAST 10000
#define HOME_SPEED_SLOW 2000
#define HOME_SPEED_VERY_SLOW 500
#define OPERATING_SPEED 9000
enum HOME_AXIS_DIRECTION {
TOP_LEFT = X_DIRECTION_TOP | Y_DIRECTION_LEFT,
@@ -44,14 +45,62 @@ struct MotorPosition {
int y;
};
struct BoardMessurements {
int x_padding;
int y_padding;
int x_num;
int y_num;
int cell_x;
int cell_y;
};
BoardMessurements calcBoardMessurements(MotorPosition p0, MotorPosition p1, int grid_x, int grid_y);
MotorPosition calcPosition(BoardMessurements * messurements, int x, int y);
void vTaskInitMotorHome(void *pvParameters);
#define HOME_COUNT 20
class CoreXYMaschine {
private:
TMC2209_UART * uart_driver0;
TMC2209_UART * uart_driver1;
TMC2209_step_dual * step_driver;
bool enableMotors = false;
bool posUnknow = true;
MotorPosition current_pos;
BoardMessurements boardMessurements;
SemaphoreHandle_t xMoveMutex;
enum HOME_AXIS_DIRECTION ORIGIN = TOP_LEFT;
int untilNextHome = HOME_COUNT;
public:
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);
void initLimitSW();
void debugPrintSwitch(uint32_t switchPin);
void debugAllSwitchStates();
void setMotorEnabled(bool enabled);
bool getMotorEnabled();
void setBoardMessurements(BoardMessurements messurements);
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);
MotorPosition homeSequence(TMC2209_step_dual * driver, MotorPosition * last_pos);
void initLimitSwitches();
extern absolute_time_t debounce_cooldown_ticks;
extern absolute_time_t limit_debounce_tick;
void isr_LimitSwitch(unsigned int gpio, uint32_t event_mask);
void waitForSwitch(uint32_t switchPin);
void debugPrintSwitch(uint32_t switchPin);
void printAllSwitchStates();

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

@@ -73,6 +73,9 @@ public:
uint get_remaining_steps0();
uint get_remaining_steps1();
void set_enable0(bool enable);
void set_enable1(bool enable);
};
extern TMC2209_step_dual * g_step_driver_instance;

18
motor-control/core-xy-firmware/src/corexy/init.cpp Normal file
View File

@@ -0,0 +1,18 @@
#include "CoreXY.hpp"
#include "FreeRTOS.h"
#include <math.h>
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;
this->uart_driver0->init();
this->uart_driver1->init();
this->xMoveMutex = xSemaphoreCreateMutex();
initLimitSW();
setMotorEnabled(true);
}

39
motor-control/core-xy-firmware/src/limitSwitches.cpp → motor-control/core-xy-firmware/src/corexy/limitSwitches.cpp
View File

@@ -3,8 +3,19 @@
#include "pico/stdlib.h"
#include <stdio.h>
absolute_time_t debounce_cooldown_ticks = 5000;
absolute_time_t limit_debounce_tick = 0;
void initLimitSwitches() {
void isr_LimitSwitch(unsigned int gpio, uint32_t event_mask) {
absolute_time_t current_tick = get_absolute_time();
if((current_tick - limit_debounce_tick) < debounce_cooldown_ticks) {
return;
} else {
limit_debounce_tick = current_tick;
}
}
void CoreXYMaschine::initLimitSW() {
gpio_init(LIMIT_X0);
gpio_init(LIMIT_X1);
gpio_init(LIMIT_Y0);
@@ -54,27 +65,7 @@ void initLimitSwitches() {
);
}
absolute_time_t debounce_cooldown_ticks= 5000;
absolute_time_t limit_debounce_tick = 0;
void isr_LimitSwitch(unsigned int gpio, uint32_t event_mask) {
absolute_time_t current_tick = get_absolute_time();
if((current_tick - limit_debounce_tick) < debounce_cooldown_ticks) {
return;
} else {
limit_debounce_tick = current_tick;
}
debugPrintSwitch(gpio);
}
void waitForSwitch(uint32_t switchPin) {
while(!gpio_get(switchPin)) {
sleep_ms(1);
}
}
void debugPrintSwitch(uint32_t switchPin) {
void CoreXYMaschine::debugPrintSwitch(uint32_t switchPin) {
printf("Switch: ");
switch (switchPin) {
@@ -115,9 +106,9 @@ void debugPrintSwitch(uint32_t switchPin) {
printf(" (%d)\n", switchPin);
}
void printAllSwitchStates() {
void CoreXYMaschine::debugAllSwitchStates() {
printf("X0: %d\n", gpio_get(LIMIT_X0));
printf("X1: %d\n", gpio_get(LIMIT_X1));
printf("Y0: %d\n", gpio_get(LIMIT_Y0));
printf("Y1: %d\n", gpio_get(LIMIT_Y1));
}
}

105
motor-control/core-xy-firmware/src/corexy/motorFunctions.cpp Normal file
View File

@@ -0,0 +1,105 @@
#include "CoreXY.hpp"
#include "FreeRTOS.h"
void CoreXYMaschine::setMotorEnabled(bool enabled) {
this->step_driver->set_enable0(enabled);
this->step_driver->set_enable1(enabled);
this->enableMotors = enabled;
}
bool CoreXYMaschine::getMotorEnabled() {
return this->enableMotors;
}
void CoreXYMaschine::homeFast(MotorPosition * last_pos, bool ignoreMutex) {
if(!ignoreMutex)
xSemaphoreTake(xMoveMutex, portMAX_DELAY);
homeXY(this->step_driver, ORIGIN, HOME_SPEED_FAST, 0, last_pos);
posUnknow = false;
current_pos = {x:0, y:0};
untilNextHome = HOME_COUNT;
if(!ignoreMutex)
xSemaphoreGive(xMoveMutex);
}
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_SLOW, 500, &res);
posUnknow = false;
current_pos = {x:0, y:0};
untilNextHome = HOME_COUNT;
xSemaphoreGive(xMoveMutex);
}
void CoreXYMaschine::setBoardMessurements(BoardMessurements messurements) {
this->boardMessurements = messurements;
}
void CoreXYMaschine::gotoPosition(int x, int y) {
xSemaphoreTake(xMoveMutex, portMAX_DELAY);
MotorPosition pos = calcPosition(&this->boardMessurements, x, y);
if(posUnknow || untilNextHome == 0) {
homeFast(&current_pos, true);
} else {
untilNextHome--;
}
if(!enableMotors) {
setMotorEnabled(true);
}
int x_steps = pos.x - current_pos.x;
int y_steps = pos.y - current_pos.y;
current_pos.x = current_pos.x + x_steps;
current_pos.y = current_pos.y + y_steps;
// Direction Magic
uint8_t x_dir = ( ORIGIN & 0b01 ) ^ 1;
uint8_t y_dir = ((ORIGIN & 0b10) >> 1) ^ 1;
if(x_steps < 0) {
x_dir = x_dir ^ 1;
x_steps = x_steps * -1;
}
if(y_steps < 0) {
y_dir = y_dir ^ 1;
y_steps = y_steps * -1;
}
if(MOTOR_X) {
step_driver->set_conf1(OPERATING_SPEED, x_dir);
} else {
step_driver->set_conf0(OPERATING_SPEED, x_dir);
}
if(MOTOR_Y) {
step_driver->set_conf1(OPERATING_SPEED, y_dir);
} else {
step_driver->set_conf0(OPERATING_SPEED, y_dir);
}
if(MOTOR_X) {
step_driver->pulse1(x_steps);
} else {
step_driver->pulse0(x_steps);
}
if(MOTOR_Y) {
step_driver->pulse1(y_steps);
} else {
step_driver->pulse0(y_steps);
}
step_driver->wait0(0);
step_driver->wait1(0);
xSemaphoreGive(xMoveMutex);
}

41
motor-control/core-xy-firmware/src/corexy/position.cpp Normal file
View File

@@ -0,0 +1,41 @@
#include "CoreXY.hpp"
#include <math.h>
BoardMessurements calcBoardMessurements(MotorPosition p0, MotorPosition p1, int grid_x, int grid_y) {
BoardMessurements res;
MotorPosition * smaller;
MotorPosition * bigger;
if(p0.x < p1.x) {
smaller = &p0;
bigger = &p1;
} else {
smaller = &p1;
bigger = &p0;
}
res.x_num = grid_x;
res.y_num = grid_y;
int x_total = bigger->x - smaller->x;
int y_total = bigger->y - smaller->y;
res.cell_x = x_total / (grid_x-1);
res.cell_y = y_total / (grid_y-1);
res.x_padding = smaller->x;
res.y_padding = smaller->y;
return res;
}
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);
return res;
}

67
motor-control/core-xy-firmware/src/corexy/tasks.cpp Normal file
View File

@@ -0,0 +1,67 @@
#include "CoreXY.hpp"
#include "FreeRTOS.h"
#include "task.h"
#include "stdio.h"
void waitForUserInput() {
getchar();
}
void performGridCall(CoreXYMaschine * machine) {
MotorPosition p0;
MotorPosition p1;
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);
machine->gotoPosition(15, 15);
vTaskDelay(100 / portTICK_PERIOD_MS);
//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();
}
}

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

@@ -124,82 +124,30 @@ void task_home1(void *pvParameters) {
}
*/
void vTaskInitMotorHome(void *pvParameters) {
TMC2209_step_dual * driver = (TMC2209_step_dual *)pvParameters;
vTaskDelay(500 / portTICK_PERIOD_MS);
// Drive Forward 1000 Steps
driver->set_conf0(5000, 0);
driver->set_conf1(5000, 0);
driver->pulse0(3000);
driver->pulse1(2000);
driver->wait0(0);
driver->wait1(0);
MotorPosition a;
homeSequence(driver, &a);
printf("Home done: %d, %d\n", a.x, a.y);
vTaskDelete(NULL);
}
void vTaskPrintSwitchStates(void *pvParameters) {
while(true) {
//printAllSwitchStates();
vTaskDelay(500 / portTICK_PERIOD_MS);
}
}
int main() {
stdio_init_all();
printf("Hello, World!\n");
initLimitSwitches();
gpio_init(DIR0_PIN);
gpio_set_dir(DIR0_PIN, GPIO_OUT);
gpio_put(DIR0_PIN, 0);
gpio_init(DIR1_PIN);
gpio_set_dir(DIR1_PIN, GPIO_OUT);
gpio_put(DIR1_PIN, 0);
TMC2209_step_dual step_driver = TMC2209_step_dual(
STEP0_PIN, STEP1_PIN,
DIR0_PIN, DIR1_PIN,
ENABLE0_PIN, ENABLE1_PIN,
pio0
);
TMC2209_UART uart_driver0 = TMC2209_UART(uart0, 0, 19200, 0, 1);
TMC2209_UART uart_driver1 = TMC2209_UART(uart1, 0, 19200, 4, 5);
uart_driver0.init();
uart_driver1.init();
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);
TaskHandle_t initMotorTaskHandle;
TaskHandle_t printSwitchStatesTaskHandle;
xTaskCreate(vTaskInitMotorHome, "Initial Motor Home", 1024, &step_driver, 5, &initMotorTaskHandle);
xTaskCreate(vTaskPrintSwitchStates, "Print Switch States", 1024, NULL, 2, &printSwitchStatesTaskHandle);
//vTaskCoreAffinitySet(initMotorTaskHandle, 0);
//vTaskCoreAffinitySet(printSwitchStatesTaskHandle, 0);
vTaskStartScheduler();
while(1) {
tight_loop_contents();
}
//step_driver.set_conf1(10000, 0);
//step_driver.set_conf0(10000, 0);
//step_driver.pulse1(1000);
//step_driver.pulse0(1000);
//step_driver.wait1(0);
//step_driver.wait0(0);
machine.runTasks();
/*
gpio_init(LIMIT0_PIN);

22
motor-control/core-xy-firmware/src/motors.cpp
View File

@@ -128,9 +128,11 @@ void homeXY(TMC2209_step_dual * driver, HOME_AXIS_DIRECTION direction, unsigned
driver->wait1(0);
if(MOTOR_X) {
last_pos->x = driver->get_remaining_steps1();
if(last_pos != NULL)
last_pos->x = driver->get_remaining_steps1();
} else {
last_pos->x = driver->get_remaining_steps0();
if(last_pos != NULL)
last_pos->x = driver->get_remaining_steps0();
}
if(MOTOR_Y) {
@@ -139,15 +141,9 @@ void homeXY(TMC2209_step_dual * driver, HOME_AXIS_DIRECTION direction, unsigned
last_pos->y = driver->get_remaining_steps0();
}
last_pos->x = total_steps - last_pos->x;
last_pos->y = total_steps - last_pos->y;
if(last_pos != NULL)
last_pos->x = total_steps - last_pos->x;
if(last_pos != NULL)
last_pos->y = total_steps - last_pos->y;
}
MotorPosition homeSequence(TMC2209_step_dual * driver, MotorPosition * last_pos) {
MotorPosition res = {0, 0};
homeXY(driver, TOP_LEFT, HOME_SPEED_FAST, 0, last_pos);
homeXY(driver, TOP_LEFT, HOME_SPEED_SLOW, 500, &res);
return res;
}

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

@@ -186,29 +186,28 @@ void TMC2209_step_dual::pulse1(uint n) {
};
void TMC2209_step_dual::pulse0_int(int n) {
if(n == 0)
this->remaining_step_count0 = n;
if(n < 0) {
n = abs(n);
gpio_put(this->dir1_pin, gpio_get(this->dir1_pin) ^ 1);
}
if(n != 0)
this->remaining_step_count0 = n;
pio_sm_put_blocking(
this->pio_core, this->sm_num_counter0, n
);
};
void TMC2209_step_dual::pulse1_int(int n) {
if(n == 0)
this->remaining_step_count1 = n;
if(n < 0) {
n = abs(n);
gpio_put(this->dir1_pin, gpio_get(this->dir1_pin) ^ 1);
}
if(n != 0)
this->remaining_step_count0 = n;
pio_sm_put_blocking(
this->pio_core, this->sm_num_counter1, n
);
@@ -258,4 +257,12 @@ uint TMC2209_step_dual::get_remaining_steps0() {
uint TMC2209_step_dual::get_remaining_steps1() {
return this->remaining_step_count1;
};
void TMC2209_step_dual::set_enable0(bool enable) {
gpio_put(this->enable0_pin, !enable);
};
void TMC2209_step_dual::set_enable1(bool enable) {
gpio_put(this->enable1_pin, !enable);
};