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

Added: Getting steps remaining on stop

Browse Source
This commit is contained in:
AlexanderHD27
2024-09-26 18:46:44 +02:00
parent 1ad6c75788
commit b2ffcb5d72
4 changed files with 76 additions and 85 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
motor-control/firmware/include/tmc2209/step.hpp
View File

@@ -35,6 +35,9 @@ private:
bool done_flag0_arm;
bool done_flag1_arm;
uint remaining_step_count0;
uint remaining_step_count1;
protected:
void init_pio();
void init_gpio();
@@ -65,6 +68,9 @@ public:
bool wait0(int timeout_ms);
bool wait1(int timeout_ms);
uint get_remaining_steps0();
uint get_remaining_steps1();
};
extern TMC2209_step_dual * g_step_driver_instance;

111
motor-control/firmware/src/main.cpp
View File

@@ -27,23 +27,6 @@
#define DIR1_PIN 9
int main() {
/*
gpio_init(ENABLE0_PIN);
gpio_set_dir(ENABLE0_PIN, GPIO_OUT);
gpio_put(ENABLE0_PIN, 0);
gpio_init(ENABLE1_PIN);
gpio_set_dir(ENABLE1_PIN, GPIO_OUT);
gpio_put(ENABLE1_PIN, 0);
//gpio_init(STEP0_PIN);
//gpio_set_dir(STEP0_PIN, GPIO_OUT);
//gpio_put(STEP0_PIN, 0);
gpio_init(STEP1_PIN);
gpio_set_dir(STEP1_PIN, GPIO_OUT);
gpio_put(STEP1_PIN, 0);
gpio_init(DIR0_PIN);
gpio_set_dir(DIR0_PIN, GPIO_OUT);
gpio_put(DIR0_PIN, 0);
@@ -52,70 +35,54 @@ int main() {
gpio_set_dir(DIR1_PIN, GPIO_OUT);
gpio_put(DIR1_PIN, 0);
for(int i=0; i<100; i++) {
gpio_put(DIR0_PIN, 1);
gpio_put(DIR1_PIN, 1);
sleep_ms(2);
gpio_put(DIR0_PIN, 0);
gpio_put(DIR1_PIN, 0);
sleep_ms(2);
}
PIO pio_core = pio0;
int SM_DIV0 = 0;
int SM_COUNT0 = 2;
uint pulser_offset = pio_add_program(pio_core, &pulser_program);
pio_sm_config c_div0 = pulser_program_get_default_config(pulser_offset);
pio_sm_config c_count0 = pulser_program_get_default_config(pulser_offset);
pio_gpio_init(pio_core, STEP0_PIN);
pio_gpio_init(pio_core, STEP1_PIN);
pio_sm_set_consecutive_pindirs(pio_core, SM_COUNT0, STEP0_PIN, 1, true);
sm_config_set_set_pins(&c_count0, STEP0_PIN, 1);
pio_sm_init(pio_core, SM_DIV0, pulser_offset, &c_div0);
pio_sm_init(pio_core, SM_COUNT0, pulser_offset + 6, &c_count0);
const double target_freq_khz = 500;
const double clock_div = ((double)(SYS_CLK_KHZ))/((double)(target_freq_khz));
pio_sm_set_clkdiv(pio_core, SM_DIV0, clock_div);
pio_sm_set_clkdiv(pio_core, SM_COUNT0, 1.0);
pio_sm_set_enabled(pio_core, SM_DIV0, true);
pio_sm_set_enabled(pio_core, SM_COUNT0, true);
pio_sm_put_blocking(pio_core, SM_DIV0, 0);
pio_sm_put_blocking(pio_core, SM_COUNT0, 0);
// freq = base / 6 + n cyles
// n = (base/freq) - 6
const double double_div_freq_khz = 0.5;
const uint32_t cycles = (target_freq_khz/double_div_freq_khz) - 6;
pio_sm_put_blocking(pio_core, SM_DIV0, cycles);
*/
TMC2209_step_dual step_driver = TMC2209_step_dual(STEP0_PIN, STEP1_PIN, DIR0_PIN, DIR1_PIN, ENABLE0_PIN, ENABLE1_PIN, pio0);
step_driver.set_conf0(10000, 1);
step_driver.set_conf1(10000, 0);
step_driver.pulse0(500);
step_driver.pulse1(50);
step_driver.wait1(0);
step_driver.pulse0(0);
step_driver.wait0(0);
step_driver.set_conf0(1000, 1);
step_driver.set_conf1(1000, 1);
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();
sleep_ms(5);
step_driver.set_conf0(10000, 0);
step_driver.set_conf1(10000, 0);
step_driver.pulse0(1000);
step_driver.pulse1(1000);
step_driver.wait0(0);
step_driver.wait1(0);
sleep_ms(200);
step_driver.set_conf0(10000, 1);
step_driver.set_conf1(10000, 1);
step_driver.pulse0(1000);
step_driver.pulse1(1000);
step_driver.wait0(0);
step_driver.wait1(0);
volatile uint t = 500 - step_driver.get_remaining_steps0();
volatile uint s = step_driver.get_remaining_steps1();
step_driver.set_conf0(1000, 1);
step_driver.set_conf1(1000, 1);
sleep_ms(100);
volatile uint32_t res1 = uart_driver1.read(0x00);
volatile uint32_t res0 = uart_driver0.read(0x00);
//TMC2209_Registers registers(uart_driver);
//registers.read_all_state();
gpio_init(LED_PIN);
gpio_set_dir(LED_PIN, GPIO_OUT);

7
motor-control/firmware/src/tmc2209/pulser.pio
View File

@@ -22,15 +22,20 @@ counter:
JMP X-- l1
l1:
PULL NOBLOCK
MOV Y, OSR
JMP !Y l1_end
IRQ WAIT 2 REL
SET PINS, 1
SET PINS, 1
IRQ WAIT 2 REL
SET PINS, 0
MOV X, OSR
JMP X-- l1
l1_end:
MOV ISR, X
PUSH NOBLOCK
IRQ SET 0 REL
; SM 2 + 0 -> IRQ 2
; SM 3 + 0 -> IRQ 3

37
motor-control/firmware/src/tmc2209/step.cpp
View File

@@ -47,18 +47,19 @@ TMC2209_step_dual::TMC2209_step_dual(
void TMC2209_step_dual::init_gpio() {
gpio_init(this->enable0_pin);
gpio_init(this->enable1_pin);
gpio_init(this->dir0_pin);
gpio_init(this->dir1_pin);
gpio_set_dir(this->enable0_pin, GPIO_OUT);
gpio_set_dir(this->enable1_pin, GPIO_OUT);
gpio_set_dir(this->dir0_pin, GPIO_OUT);
gpio_set_dir(this->dir1_pin, GPIO_OUT);
gpio_put(this->enable0_pin, 0);
gpio_put(this->enable1_pin, 0);
gpio_put(this->dir0_pin, 0);
gpio_put(this->dir1_pin, 0);
sleep_ms(1);
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);
sleep_ms(1);
};
void TMC2209_step_dual::init_pio() {
@@ -76,8 +77,6 @@ void TMC2209_step_dual::init_pio() {
pio_gpio_init(this->pio_core, this->step0_pin);
pio_gpio_init(this->pio_core, this->step1_pin);
pio_gpio_init(this->pio_core, this->dir0_pin);
pio_gpio_init(this->pio_core, this->dir1_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);
@@ -114,8 +113,8 @@ void TMC2209_step_dual::init_pio() {
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, clock_div*0.5);
pio_sm_set_clkdiv(this->pio_core, this->sm_num_counter1, clock_div*0.5);
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);
@@ -166,6 +165,7 @@ void TMC2209_step_dual::set_conf1(uint frequency, bool dir) {
};
void TMC2209_step_dual::pulse0(uint n) {
this->remaining_step_count0 = n;
this->done_flag0_arm = true;
pio_sm_put_blocking(
this->pio_core, this->sm_num_counter0, n
@@ -173,6 +173,7 @@ void TMC2209_step_dual::pulse0(uint n) {
};
void TMC2209_step_dual::pulse1(uint n) {
this->remaining_step_count1 = n;
this->done_flag1_arm = true;
pio_sm_put_blocking(
this->pio_core, this->sm_num_counter1, n
@@ -204,6 +205,8 @@ void TMC2209_step_dual::pulse1_int(int n) {
};
void TMC2209_step_dual::irq_handler0() {
this->remaining_step_count0 = pio_sm_get(this->pio_core, this->sm_num_counter0);
if(this->done_flag0_arm) {
done_flag0 = true;
} else {
@@ -213,6 +216,8 @@ void TMC2209_step_dual::irq_handler0() {
}
void TMC2209_step_dual::irq_handler1() {
this->remaining_step_count1 = pio_sm_get(this->pio_core, this->sm_num_counter1);
if(this->done_flag1_arm) {
done_flag1 = true;
} else {
@@ -268,3 +273,11 @@ bool TMC2209_step_dual::wait1(int timeout_ms) {
return false;
};
uint TMC2209_step_dual::get_remaining_steps0() {
return this->remaining_step_count0;
};
uint TMC2209_step_dual::get_remaining_steps1() {
return this->remaining_step_count1;
};