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

Merged i2c-helper in Main

Browse Source
This commit is contained in:
AlexanderHD27
2025-01-12 00:16:00 +01:00
parent f4792de050
commit b5c7e5b4c1
396 changed files with 182143 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

307
motor-control/firmware/src/main.cpp Executable file
View File

@@ -0,0 +1,307 @@
/**
* @file main.c
* @author AlexanderHD27
* @brief Main file of the RPi Pico Firmware for the Oscilloscope-Renderer
* @version 0.1
* @date 2024-02-15
*
* @copyright Copyright (c) 2024
*
*/
#include "pico/stdio.h"
#include "pico/stdlib.h"
#include "pico/time.h"
#include "hardware/uart.h"
#include <stdio.h>
#include "pulser.pio.h"
#include "tmc2209/tmc2209.hpp"
#include "tmc2209/step.hpp"
#include "FreeRTOSConfig.h"
#include "FreeRTOS.h"
#include "task.h"
#define LED_PIN 25
#define RGB1_B_PIN 21
#define RGB1_G_PIN 22
#define RGB2_R_PIN 26
#define RGB2_G_PIN 27
#define RGB2_B_PIN 28
#define BOARD_LED1_PIN 14
#define BOARD_LED2_PIN 15
#define ENABLE0_PIN 2
#define ENABLE1_PIN 3
#define LIMIT0_PIN 10
#define LIMIT1_PIN 11
#define LIMIT2_PIN 12
#define LIMIT3_PIN 13
#define STEP0_PIN 6
#define STEP1_PIN 7
#define DIR0_PIN 8
#define DIR1_PIN 9
enum SYSTEM_STATE {
STARTUP,
HOMEING_FIRST_RUN,
HOMEING_BACKOFF,
HOMEING_SECOND_RUN,
IDLE
};
SYSTEM_STATE system_state = STARTUP;
absolute_time_t debounce_cooldown_ticks= 5000;
absolute_time_t limit_debounce_tick = 0;
void isr_LimitSwitch(uint 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;
}
switch (gpio) {
case LIMIT0_PIN:
break;
default:
break;
}
}
void homeMotors0(TMC2209_step_dual * driver) {
driver->set_conf0(10000, 1);
driver->pulse0(10000);
driver->wait0(0);
driver->set_conf0(10000, 0);
driver->pulse0(100000);
while(!gpio_get(LIMIT2_PIN)) {
sleep_ms(1);
}
driver->pulse0(0);
sleep_ms(500);
// Second Run
driver->set_conf0(5000, 1);
driver->pulse0(200);
driver->wait0(0);
driver->set_conf0(500, 0);
driver->pulse0(5000);
while(!gpio_get(LIMIT2_PIN)) {
sleep_ms(1);
}
driver->pulse0(0);
sleep_ms(1);
}
void homeMotors1(TMC2209_step_dual * driver) {
driver->set_conf1(10000, 1);
driver->pulse1(5000);
driver->wait1(0);
driver->set_conf1(10000, 0);
driver->pulse1(100000);
while(!gpio_get(LIMIT3_PIN)) {
sleep_ms(1);
}
driver->pulse1(0);
sleep_ms(500);
// Second Run
driver->set_conf1(5000, 1);
driver->pulse1(200);
driver->wait1(0);
driver->set_conf1(500, 0);
driver->pulse1(5000);
while(!gpio_get(LIMIT3_PIN)) {
sleep_ms(1);
}
driver->pulse1(0);
sleep_ms(1);
}
void task_home0(void *pvParameters) {
TMC2209_step_dual * driver = (TMC2209_step_dual *)pvParameters;
vTaskDelay(500);
homeMotors0(driver);
}
void task_home1(void *pvParameters) {
TMC2209_step_dual * driver = (TMC2209_step_dual *)pvParameters;
homeMotors1(driver);
}
int main() {
sleep_ms(100);
stdio_usb_init();
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);
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);
}
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_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);
gpio_set_dir(LIMIT0_PIN, GPIO_IN);
gpio_pull_up(LIMIT0_PIN);
gpio_set_input_hysteresis_enabled(LIMIT0_PIN, true);
gpio_init(LIMIT1_PIN);
gpio_set_dir(LIMIT1_PIN, GPIO_IN);
gpio_pull_up(LIMIT1_PIN);
gpio_set_input_hysteresis_enabled(LIMIT1_PIN, true);
gpio_init(LIMIT2_PIN);
gpio_set_dir(LIMIT2_PIN, GPIO_IN);
gpio_pull_up(LIMIT2_PIN);
gpio_set_input_hysteresis_enabled(LIMIT2_PIN, true);
gpio_init(LIMIT3_PIN);
gpio_set_dir(LIMIT3_PIN, GPIO_IN);
gpio_pull_up(LIMIT3_PIN);
gpio_set_input_hysteresis_enabled(LIMIT3_PIN, true);
/*
gpio_set_irq_enabled_with_callback(
LIMIT0_PIN,
GPIO_IRQ_EDGE_RISE,
true,
isr_LimitSwitch
);
gpio_set_irq_enabled_with_callback(
LIMIT1_PIN,
GPIO_IRQ_EDGE_RISE,
true,
isr_LimitSwitch
);
gpio_set_irq_enabled_with_callback(
LIMIT2_PIN,
GPIO_IRQ_EDGE_RISE,
true,
isr_LimitSwitch
);
gpio_set_irq_enabled_with_callback(
LIMIT3_PIN,
GPIO_IRQ_EDGE_RISE,
true,
isr_LimitSwitch
);*/
gpio_init(RGB2_B_PIN);
gpio_init(RGB2_G_PIN);
gpio_init(RGB2_R_PIN);
gpio_set_dir(RGB2_B_PIN, GPIO_OUT);
gpio_set_dir(RGB2_G_PIN, GPIO_OUT);
gpio_set_dir(RGB2_R_PIN, GPIO_OUT);
gpio_init(RGB1_B_PIN);
gpio_init(RGB1_G_PIN);
gpio_set_dir(RGB1_B_PIN, GPIO_OUT);
gpio_set_dir(RGB1_G_PIN, GPIO_OUT);
gpio_put(RGB1_B_PIN, 1);
gpio_put(RGB1_G_PIN, 1);
gpio_put(RGB2_B_PIN, 1);
gpio_put(RGB2_G_PIN, 1);
gpio_put(RGB2_R_PIN, 1);
/*
gpio_init(LED_PIN);
gpio_set_dir(LED_PIN, GPIO_OUT);
//step_driver.set_conf0(10000, 0);
//step_driver.pulse0(1000000);
*/
homeMotors0(&step_driver);
printf("Home0 done\n");
homeMotors1(&step_driver);
printf("Home1 done\n");
//xTaskCreate(task_home0, "Home0", 1024, &step_driver, 5, NULL);
//xTaskCreate(task_home1, "Home1", 1024, &step_driver, 5, NULL);
vTaskStartScheduler();
/*
while (true) {
sleep_ms(50);
gpio_put(LED_PIN, 0);
sleep_ms(50);
gpio_put(LED_PIN, 1);
if(gpio_get(LIMIT1_PIN)) {
gpio_put(RGB1_B_PIN, 0);
} else {
gpio_put(RGB1_B_PIN, 1);
}
if(gpio_get(LIMIT3_PIN)) {
gpio_put(RGB1_G_PIN, 0);
} else {
gpio_put(RGB1_G_PIN, 1);
}
if(gpio_get(LIMIT0_PIN)) {
gpio_put(RGB2_B_PIN, 0);
} else {
gpio_put(RGB2_B_PIN, 1);
}
if(gpio_get(LIMIT2_PIN)) {
gpio_put(RGB2_G_PIN, 0);
} else {
gpio_put(RGB2_G_PIN, 1);
}
}
*/
return 0;
}

1
motor-control/firmware/src/mcp2521 Symbolic link
View File

@@ -0,0 +1 @@
/home/alexander/Projects/gobot/common-libaries/mcp2521

1
motor-control/firmware/src/mcp2521_hardware_interface Symbolic link
View File

@@ -0,0 +1 @@
/home/alexander/Projects/gobot/common-libaries/mcp2521_hardware_interface

43
motor-control/firmware/src/tmc2209/pulser.pio Normal file
View File

@@ -0,0 +1,43 @@
.program pulser
clock_div:
PULL NOBLOCK
MOV X, OSR
MOV Y, X
l0:
JMP Y-- l0
IRQ CLEAR 0 REL
; SM 0 + 0 -> IRQ 0
; SM 1 + 0 -> IRQ 1
JMP clock_div
counter:
; SM 2 + 2 -> IRQ 0
; SM 3 + 2 -> IRQ 1
PULL
MOV X, OSR
JMP !X l1_end
JMP X-- l1
l1:
PULL NOBLOCK
MOV Y, OSR
MOV X, OSR
JMP !Y l1_end
IRQ WAIT 2 REL
SET PINS, 1
IRQ WAIT 2 REL
SET PINS, 0
JMP X-- l1
l1_end:
MOV ISR, X
PUSH NOBLOCK
IRQ SET 0 REL
; SM 2 + 0 -> IRQ 2
; SM 3 + 0 -> IRQ 3
JMP counter

290
motor-control/firmware/src/tmc2209/step.cpp Normal file
View File

@@ -0,0 +1,290 @@
#include "step.hpp"
#include "pico/stdio.h"
#include "hardware/pio.h"
#include "hardware/irq.h"
#include <pico/time.h>
#include <math.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->done_flag0 = false;
this->done_flag1 = false;
this->done_flag0_arm = false;
this->done_flag1_arm = false;
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);
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() {
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) {
this->remaining_step_count0 = n;
this->done_flag0_arm = true;
this->done_flag0 = false;
pio_sm_put_blocking(
this->pio_core, this->sm_num_counter0, n
);
};
void TMC2209_step_dual::pulse1(uint n) {
this->remaining_step_count1 = n;
this->done_flag1_arm = true;
this->done_flag1 = false;
pio_sm_put_blocking(
this->pio_core, this->sm_num_counter1, n
);
};
void TMC2209_step_dual::pulse0_int(int n) {
this->remaining_step_count0 = n;
this->done_flag0_arm = true;
if(n < 0) {
n = abs(n);
gpio_put(this->dir1_pin, gpio_get(this->dir1_pin) ^ 1);
}
pio_sm_put_blocking(
this->pio_core, this->sm_num_counter0, n
);
};
void TMC2209_step_dual::pulse1_int(int n) {
this->remaining_step_count1 = n;
this->done_flag1_arm = true;
if(n < 0) {
n = abs(n);
gpio_put(this->dir1_pin, gpio_get(this->dir1_pin) ^ 1);
}
pio_sm_put_blocking(
this->pio_core, this->sm_num_counter1, 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 {
done_flag0 = false;
}
pio_interrupt_clear(this->pio_core, 2);
}
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 {
done_flag1 = false;
}
pio_interrupt_clear(this->pio_core, 3);
}
bool TMC2209_step_dual::wait0(int timeout_ms) {
int t = timeout_ms*1000;
bool continues = timeout_ms <= 0;
if(!this->done_flag0_arm)
return true;
while(t > 0 || continues) {
if(done_flag0) {
this->done_flag0_arm = false;
done_flag0 = false;
return true;
}
if(!continues)
t -= sleep_time_us;
sleep_us(sleep_time_us);
}
return false;
};
bool TMC2209_step_dual::wait1(int timeout_ms) {
int t = timeout_ms*1000;
bool continues = timeout_ms <= 0;
if(!this->done_flag1_arm)
return true;
while(t > 0 || continues) {
if(done_flag1) {
this->done_flag1_arm = false;
done_flag1 = false;
return true;
}
if(!continues)
t -= sleep_time_us;
sleep_us(sleep_time_us);
}
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;
};

80
motor-control/firmware/src/tmc2209/uart_interface.cpp Normal file
View File

@@ -0,0 +1,80 @@
#include "pico/stdlib.h"
#include "pico.h"
#include "hardware/uart.h"
#include "tmc2209.hpp"
TMC2209_UART::TMC2209_UART(
uart_inst_t *uart_inst,
uint8_t node_address,
uint baudrate,
uint tx_pin,
uint rx_pin
) {
this->uart_inst = uart_inst;
this->node_address = node_address & 0b11;
this->baudrate = baudrate;
this->tx_pin = tx_pin;
this->rx_pin = rx_pin;
}
void TMC2209_UART::init() {
gpio_set_function(this->tx_pin, UART_FUNCSEL_NUM(this->uart_inst, this->tx_pin));
gpio_set_function(this->rx_pin, UART_FUNCSEL_NUM(this->uart_inst, this->rx_pin));
// UART format: 8 data bits, 1 stop bit, no parity
// -> https://www.analog.com/media/en/technical-documentation/data-sheets/TMC2209_datasheet_rev1.09.pdf
uart_set_format(this->uart_inst, 8, 1, UART_PARITY_NONE);
// Clear to Send (CTS) and Request to Send (RTS) are not used
// (This is for automaticlly telling the other side that it is ready to receive/send data)
uart_set_hw_flow(this->uart_inst, false, false);
uart_set_fifo_enabled(this->uart_inst, true);
uart_init(this->uart_inst, this->baudrate);
}
uint8_t TMC2209_UART::calc_crc8_atm(uint8_t *data, uint8_t size) {
int i,j;
uint8_t crc = 0; // CRC located in last byte of message
uint8_t currentByte;
for (i=0; i<size; i++) { // Execute for all bytes of a message
currentByte = data[i]; // Retrieve a byte to be sent from Array
for (j=0; j<8; j++) {
if ((crc >> 7) ^ (currentByte & 0x01)) { // update CRC based result of XOR operation
crc = (crc << 1) ^ 0x07;
} else {
crc = (crc << 1);
}
currentByte = currentByte >> 1;
} // for CRC bit
} // for message byte
return crc;
}
uint32_t TMC2209_UART::read(uint8_t address) {
uint8_t data[4] = {
0b01010101, // Sync byte
this->node_address, // Node address
(address & (uint8_t)(0x7F)), // Register address, last bit is 0 for read
0
};
data[3] = this->calc_crc8_atm(data, 3);
uart_write_blocking(this->uart_inst, data, 4);
uint8_t response[12];
uart_read_blocking(this->uart_inst, response, 12);
if(response[11] != calc_crc8_atm(&response[4], 7)) {
this->crc_error_flag = true;
return 0;
} else {
return *((uint32_t*) &response[7]);
}
}

1
motor-control/firmware/src/tmc2209/uart_registers.cpp Normal file
View File

@@ -0,0 +1 @@