Pimpest
a4691caeed
* 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>
44 lines
1.1 KiB
C
44 lines
1.1 KiB
C
#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);
|
|
}
|
|
|