268 lines
7.8 KiB
C++
268 lines
7.8 KiB
C++
#include "step.hpp"
|
|
#include "pico/stdio.h"
|
|
#include "hardware/pio.h"
|
|
#include "hardware/irq.h"
|
|
#include <pico/time.h>
|
|
|
|
#include <math.h>
|
|
|
|
#include "FreeRTOSConfig.h"
|
|
#include "FreeRTOS.h"
|
|
#include "task.h"
|
|
#include "semphr.h"
|
|
|
|
TMC2209_step_dual * g_step_driver_instance;
|
|
|
|
void step_irq0_handler() {
|
|
g_step_driver_instance->irq_handler0();
|
|
};
|
|
|
|
void step_irq1_handler() {
|
|
g_step_driver_instance->irq_handler1();
|
|
};
|
|
|
|
TMC2209_step_dual::TMC2209_step_dual(
|
|
uint step0_pin,
|
|
uint step1_pin,
|
|
uint dir0_pin,
|
|
uint dir1_pin,
|
|
uint enable0_pin,
|
|
uint enable1_pin,
|
|
PIO pio_core
|
|
) {
|
|
this->step0_pin = step0_pin;
|
|
this->step1_pin = step1_pin;
|
|
this->dir0_pin = dir0_pin;
|
|
this->dir1_pin = dir1_pin;
|
|
this->enable0_pin = enable0_pin;
|
|
this->enable1_pin = enable1_pin;
|
|
this->pio_core = pio_core;
|
|
|
|
g_step_driver_instance = this;
|
|
|
|
this->xDoneSemaphore0 = xSemaphoreCreateBinary();
|
|
this->xDoneSemaphore1 = xSemaphoreCreateBinary();
|
|
|
|
init_gpio();
|
|
init_pio();
|
|
};
|
|
|
|
void TMC2209_step_dual::init_gpio() {
|
|
gpio_init(this->enable0_pin);
|
|
gpio_init(this->enable1_pin);
|
|
gpio_set_dir(this->enable0_pin, GPIO_OUT);
|
|
gpio_set_dir(this->enable1_pin, GPIO_OUT);
|
|
gpio_put(this->enable0_pin, 0);
|
|
gpio_put(this->enable1_pin, 0);
|
|
vTaskDelay(1 / portTICK_PERIOD_MS);
|
|
|
|
gpio_init(this->dir0_pin);
|
|
gpio_init(this->dir1_pin);
|
|
gpio_set_dir(this->dir0_pin, GPIO_OUT);
|
|
gpio_set_dir(this->dir1_pin, GPIO_OUT);
|
|
gpio_put(this->dir0_pin, 1);
|
|
gpio_put(this->dir1_pin, 1);
|
|
vTaskDelay(1 / portTICK_PERIOD_MS);
|
|
};
|
|
|
|
void TMC2209_step_dual::init_pio() {
|
|
pio_sm_set_enabled(this->pio_core, this->sm_num_divider0, false);
|
|
pio_sm_set_enabled(this->pio_core, this->sm_num_divider1, false);
|
|
pio_sm_set_enabled(this->pio_core, this->sm_num_counter0, false);
|
|
pio_sm_set_enabled(this->pio_core, this->sm_num_counter1, false);
|
|
|
|
this->pulser_program_offset = pio_add_program(this->pio_core, &pulser_program);
|
|
|
|
this->sm_config_divider0 = pulser_program_get_default_config(this->pulser_program_offset);
|
|
this->sm_config_divider1 = pulser_program_get_default_config(this->pulser_program_offset);
|
|
this->sm_config_counter0 = pulser_program_get_default_config(this->pulser_program_offset);
|
|
this->sm_config_counter1 = pulser_program_get_default_config(this->pulser_program_offset);
|
|
|
|
pio_gpio_init(this->pio_core, this->step0_pin);
|
|
pio_gpio_init(this->pio_core, this->step1_pin);
|
|
|
|
pio_sm_set_consecutive_pindirs(this->pio_core, this->sm_num_counter0, this->step0_pin, 1, true);
|
|
pio_sm_set_consecutive_pindirs(this->pio_core, this->sm_num_counter1, this->step1_pin, 1, true);
|
|
|
|
sm_config_set_set_pins(&this->sm_config_counter0, this->step0_pin, 1);
|
|
sm_config_set_set_pins(&this->sm_config_counter1, this->step1_pin, 1);
|
|
|
|
pio_sm_init(
|
|
this->pio_core,
|
|
this->sm_num_divider0,
|
|
this->pulser_program_offset + this->subprogram_offset_divider,
|
|
&this->sm_config_divider0
|
|
);
|
|
pio_sm_init(
|
|
this->pio_core,
|
|
this->sm_num_divider1,
|
|
this->pulser_program_offset + this->subprogram_offset_divider,
|
|
&this->sm_config_divider1
|
|
);
|
|
pio_sm_init(
|
|
this->pio_core,
|
|
this->sm_num_counter0,
|
|
this->pulser_program_offset + this->subprogram_offset_counter,
|
|
&this->sm_config_counter0
|
|
);
|
|
pio_sm_init(
|
|
this->pio_core,
|
|
this->sm_num_counter1,
|
|
this->pulser_program_offset + this->subprogram_offset_counter,
|
|
&this->sm_config_counter1
|
|
);
|
|
|
|
float clock_div = ((float)(SYS_CLK_KHZ))/(base_clock_freq_khz);
|
|
|
|
pio_sm_set_clkdiv(this->pio_core, this->sm_num_divider0, clock_div);
|
|
pio_sm_set_clkdiv(this->pio_core, this->sm_num_divider1, clock_div);
|
|
pio_sm_set_clkdiv(this->pio_core, this->sm_num_counter0, 1.0);
|
|
pio_sm_set_clkdiv(this->pio_core, this->sm_num_counter1, 1.0);
|
|
|
|
pio_sm_set_enabled(this->pio_core, this->sm_num_divider0, true);
|
|
pio_sm_set_enabled(this->pio_core, this->sm_num_divider1, true);
|
|
pio_sm_set_enabled(this->pio_core, this->sm_num_counter0, true);
|
|
pio_sm_set_enabled(this->pio_core, this->sm_num_counter1, true);
|
|
|
|
pio_set_irq0_source_enabled(this->pio_core, pis_interrupt2, true);
|
|
pio_set_irq1_source_enabled(this->pio_core, pis_interrupt3, true);
|
|
|
|
if(this->pio_core == pio0) {
|
|
irq_set_exclusive_handler(PIO0_IRQ_0, step_irq0_handler);
|
|
irq_set_exclusive_handler(PIO0_IRQ_1, step_irq1_handler);
|
|
|
|
irq_set_enabled(PIO0_IRQ_0, true);
|
|
irq_set_enabled(PIO0_IRQ_1, true);
|
|
} else if(this->pio_core == pio1) {
|
|
irq_set_exclusive_handler(PIO1_IRQ_0, step_irq0_handler);
|
|
irq_set_exclusive_handler(PIO1_IRQ_1, step_irq1_handler);
|
|
|
|
irq_set_enabled(PIO1_IRQ_0, true);
|
|
irq_set_enabled(PIO1_IRQ_1, true);
|
|
}
|
|
|
|
set_conf0(1000, 0);
|
|
set_conf1(1000, 0);
|
|
};
|
|
|
|
void TMC2209_step_dual::set_conf0(uint frequency, bool dir) {
|
|
uint32_t cycles = (uint32_t)(base_clock_freq_khz / (((float)(frequency))/1000.0)) - 6;
|
|
|
|
gpio_put(this->dir0_pin, dir);
|
|
pio_sm_put_blocking(
|
|
this->pio_core,
|
|
this->sm_num_divider0,
|
|
cycles
|
|
);
|
|
};
|
|
|
|
void TMC2209_step_dual::set_conf1(uint frequency, bool dir) {
|
|
uint32_t cycles = (uint32_t)(base_clock_freq_khz / (((float)(frequency))/1000.0)) - 6;
|
|
|
|
gpio_put(this->dir1_pin, dir);
|
|
pio_sm_put_blocking(
|
|
this->pio_core,
|
|
this->sm_num_divider1,
|
|
cycles
|
|
);
|
|
};
|
|
|
|
void TMC2209_step_dual::pulse0(uint n) {
|
|
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(uint n) {
|
|
if(n > 0)
|
|
this->remaining_step_count1 = n;
|
|
|
|
pio_sm_put_blocking(
|
|
this->pio_core, this->sm_num_counter1, n
|
|
);
|
|
};
|
|
|
|
void TMC2209_step_dual::pulse0_int(int 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) {
|
|
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
|
|
);
|
|
};
|
|
|
|
void TMC2209_step_dual::irq_handler0() {
|
|
uint n = pio_sm_get(this->pio_core, this->sm_num_counter0);
|
|
if(n > 0)
|
|
this->remaining_step_count0 = n;
|
|
|
|
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
|
|
xSemaphoreGiveFromISR(this->xDoneSemaphore0, &xHigherPriorityTaskWoken);
|
|
|
|
pio_interrupt_clear(this->pio_core, 2);
|
|
|
|
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
|
|
}
|
|
|
|
void TMC2209_step_dual::irq_handler1() {
|
|
uint n = pio_sm_get(this->pio_core, this->sm_num_counter1);
|
|
if(n > 0)
|
|
this->remaining_step_count1 = n;
|
|
|
|
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
|
|
xSemaphoreGiveFromISR(this->xDoneSemaphore1, &xHigherPriorityTaskWoken);
|
|
|
|
pio_interrupt_clear(this->pio_core, 3);
|
|
|
|
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
|
|
}
|
|
|
|
void TMC2209_step_dual::wait0(int timeout_ms) {
|
|
int t = timeout_ms <= 0 ? portMAX_DELAY : timeout_ms / portTICK_PERIOD_MS;
|
|
|
|
xSemaphoreTake(this->xDoneSemaphore0, t);
|
|
};
|
|
|
|
void TMC2209_step_dual::wait1(int timeout_ms) {
|
|
int t = timeout_ms <= 0 ? portMAX_DELAY : timeout_ms / portTICK_PERIOD_MS;
|
|
|
|
xSemaphoreTake(this->xDoneSemaphore1, t);
|
|
};
|
|
|
|
uint TMC2209_step_dual::get_remaining_steps0() {
|
|
return this->remaining_step_count0;
|
|
};
|
|
|
|
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);
|
|
}; |