Goto CoreXY to work

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

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 

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);
 
-
+extern absolute_time_t debounce_cooldown_ticks;
-void initLimitSwitches();
+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();

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;

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

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

motor-control/core-xy-firmware/src/corexy/limitSwitches.cpp

@@ -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;
+void CoreXYMaschine::debugPrintSwitch(uint32_t switchPin) {
-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) {
     printf("Switch: ");
 
     switch (switchPin) {
@@ -115,7 +106,7 @@ 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));

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

motor-control/core-xy-firmware/src/corexy/position.cpp

@@ -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;
+}

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

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;
+    machine.runTasks();
-    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);
 
 /*
     gpio_init(LIMIT0_PIN);

motor-control/core-xy-firmware/src/motors.cpp

@@ -128,8 +128,10 @@ void homeXY(TMC2209_step_dual * driver, HOME_AXIS_DIRECTION direction, unsigned
     driver->wait1(0);
 
     if(MOTOR_X) {
+        if(last_pos != NULL)
             last_pos->x = driver->get_remaining_steps1();
     } else {
+        if(last_pos != NULL)
             last_pos->x = driver->get_remaining_steps0();
     }
 
@@ -139,15 +141,9 @@ void homeXY(TMC2209_step_dual * driver, HOME_AXIS_DIRECTION direction, unsigned
         last_pos->y = driver->get_remaining_steps0();
     }
 
+    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;
-} 

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
     );
@@ -259,3 +258,11 @@ 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);
+};