Magnezijum-Robotics/magrob
2
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

Add firmware code to repo (#8)

Browse Source 
* Added primary stepper motor driver code to repo
* Added odometery encoder code to repo
* Added the ability to update encoder wheel ratio via ros service
Co-authored-by: Pimpest <82343504+Pimpest@users.noreply.github.com>
Co-committed-by: Pimpest <82343504+Pimpest@users.noreply.github.com>
This commit is contained in:
Pimpest
2025-04-09 14:49:19 +00:00
committed by Pimpest
parent c1b5a3ea1c
commit a4691caeed
19 changed files with 1131 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

43
firmware/base/src/stepper.c Normal file
View File

@ -0,0 +1,43 @@
#include "blink.pio.h"
#include "hardware/pio.h"
#include "hardware/clocks.h"
#include "pico/time.h"
#include <math.h>
#include "config.h"
#include "stepper.h"
void stepper_init() {
PIO pio = pio0;
uint offset = pio_add_program(pio, &oscillate_program);
start_pulse(pio, SM_A, offset, PULSE_PIN_A, 1);
start_pulse(pio, SM_B, offset, PULSE_PIN_B, 1);
gpio_init(DIR_PIN_A);
gpio_init(DIR_PIN_B);
gpio_set_dir(DIR_PIN_A, GPIO_OUT);
gpio_set_dir(DIR_PIN_B, GPIO_OUT);
}
void set_speeds(double a, double b) {
update_sm(pio0, SM_A, DIR_PIN_A, a);
update_sm(pio0, SM_B, DIR_PIN_B, b);
}
void start_pulse(PIO pio, uint sm, uint offset, uint pin, uint freq) {
oscillate_program_init(pio, sm, offset, pin);
pio->txf[sm] = 0;
pio_sm_set_enabled(pio, sm, true);
}
void update_sm(PIO pio, uint sm, const uint pin ,double v) {
double u_v = fabs(v);
if(u_v > 0.0005)
pio->txf[sm] = (int)((double)clock_get_hz(clk_sys) * PULSE_PER_REV / u_v) / 2 - 5;
else
pio->txf[sm] = 0;
gpio_put(pin, v < 0);
}