diff --git a/motor-control/core-xy-firmware/.vscode/settings.json b/motor-control/core-xy-firmware/.vscode/settings.json index 6a94325..9577ef1 100644 --- a/motor-control/core-xy-firmware/.vscode/settings.json +++ b/motor-control/core-xy-firmware/.vscode/settings.json @@ -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" + } } diff --git a/motor-control/core-xy-firmware/CMakeLists.txt b/motor-control/core-xy-firmware/CMakeLists.txt index 0bac787..6cf78ee 100644 --- a/motor-control/core-xy-firmware/CMakeLists.txt +++ b/motor-control/core-xy-firmware/CMakeLists.txt @@ -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 diff --git a/motor-control/core-xy-firmware/include/CoreXY.hpp b/motor-control/core-xy-firmware/include/CoreXY.hpp index a1028aa..5c237fe 100644 --- a/motor-control/core-xy-firmware/include/CoreXY.hpp +++ b/motor-control/core-xy-firmware/include/CoreXY.hpp @@ -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(); \ No newline at end of file diff --git a/motor-control/core-xy-firmware/include/tmc2209/step.hpp b/motor-control/core-xy-firmware/include/tmc2209/step.hpp index 314415b..d78807c 100644 --- a/motor-control/core-xy-firmware/include/tmc2209/step.hpp +++ b/motor-control/core-xy-firmware/include/tmc2209/step.hpp @@ -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; diff --git a/motor-control/core-xy-firmware/src/corexy/init.cpp b/motor-control/core-xy-firmware/src/corexy/init.cpp new file mode 100644 index 0000000..93e6a9e --- /dev/null +++ b/motor-control/core-xy-firmware/src/corexy/init.cpp @@ -0,0 +1,18 @@ +#include "CoreXY.hpp" +#include "FreeRTOS.h" + +#include + +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); +} diff --git a/motor-control/core-xy-firmware/src/limitSwitches.cpp b/motor-control/core-xy-firmware/src/corexy/limitSwitches.cpp similarity index 89% rename from motor-control/core-xy-firmware/src/limitSwitches.cpp rename to motor-control/core-xy-firmware/src/corexy/limitSwitches.cpp index f79d1c1..d64d598 100644 --- a/motor-control/core-xy-firmware/src/limitSwitches.cpp +++ b/motor-control/core-xy-firmware/src/corexy/limitSwitches.cpp @@ -3,8 +3,19 @@ #include "pico/stdlib.h" #include +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)); -} \ No newline at end of file +} diff --git a/motor-control/core-xy-firmware/src/corexy/motorFunctions.cpp b/motor-control/core-xy-firmware/src/corexy/motorFunctions.cpp new file mode 100644 index 0000000..597fa0e --- /dev/null +++ b/motor-control/core-xy-firmware/src/corexy/motorFunctions.cpp @@ -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(¤t_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); +} \ No newline at end of file diff --git a/motor-control/core-xy-firmware/src/corexy/position.cpp b/motor-control/core-xy-firmware/src/corexy/position.cpp new file mode 100644 index 0000000..1242763 --- /dev/null +++ b/motor-control/core-xy-firmware/src/corexy/position.cpp @@ -0,0 +1,41 @@ +#include "CoreXY.hpp" + +#include + +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; +} \ No newline at end of file diff --git a/motor-control/core-xy-firmware/src/corexy/tasks.cpp b/motor-control/core-xy-firmware/src/corexy/tasks.cpp new file mode 100644 index 0000000..6a0b417 --- /dev/null +++ b/motor-control/core-xy-firmware/src/corexy/tasks.cpp @@ -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(); + } +} \ 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 7e44b6f..2020756 100755 --- a/motor-control/core-xy-firmware/src/main.cpp +++ b/motor-control/core-xy-firmware/src/main.cpp @@ -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); diff --git a/motor-control/core-xy-firmware/src/motors.cpp b/motor-control/core-xy-firmware/src/motors.cpp index b2d4051..ffb9bc3 100644 --- a/motor-control/core-xy-firmware/src/motors.cpp +++ b/motor-control/core-xy-firmware/src/motors.cpp @@ -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; -} \ 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 ef46074..4bbce51 100644 --- a/motor-control/core-xy-firmware/src/tmc2209/step.cpp +++ b/motor-control/core-xy-firmware/src/tmc2209/step.cpp @@ -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); }; \ No newline at end of file