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

25
firmware/base/src/config.h Normal file
View File

@ -0,0 +1,25 @@
#pragma once
//############## CONFIG STEPPERS ################
#define DIR_PIN_A 16
#define DIR_PIN_B 26
#define PULSE_PIN_A 17
#define PULSE_PIN_B 27
#define SM_A 0
#define SM_B 1
#define PULSE_PER_REV (1 / 3200.0)
//###############################################
//================ CONFIG ENCODERS =====================
#define ENCODER_RIGHT_PIN_A 12 // Lupio sam ove vrednosti
#define ENCODER_RIGHT_PIN_B 13
#define ENCODER_LEFT_PIN_A 6
#define ENCODER_LEFT_PIN_B 7
#define ENCODER_CPR 3840
#define WHEEL_RADIUS 0.0279
#define WHEEL_SEPARATION 0.31133
#define TIMER_CYCLE_US 1000
//======================================================

232
firmware/base/src/main.c Normal file
View File

@ -0,0 +1,232 @@
#include <stdio.h>
#include <math.h>
#include <string.h>
#include "bsp/board_api.h"
#include "tusb.h"
#ifdef PICO_STDIO_USB_RESET_INTERFACE_SUPPORT_MS_OS_20_DESCRIPTOR
#undef PICO_STDIO_USB_RESET_INTERFACE_SUPPORT_MS_OS_20_DESCRIPTOR
#endif
#include "pico/stdlib.h"
#include "pico/multicore.h"
#include "pico/time.h"
#include "quadrature.h"
#include "quadrature.pio.h"
#include "config.h"
#include "stepper.h"
static substep_state_t state_l;
static substep_state_t state_r;
static double base_x = 0;
static double base_y = 0;
static double base_theta = 0;
static double wheel_separation = WHEEL_SEPARATION;
static double wheel_ratio_l = 1;
static double wheel_ratio_r = 1;
static uint prev_time;
static int prev_position_l = 0;
static int prev_position_r = 0;
static unsigned char stepper_instr[2+sizeof(double)*2] = {};
typedef struct instr_buf_t{
char pos;
char data[2+sizeof(double) * 2];
} instr_buf_t;
instr_buf_t stepper_buf = {};
bool update_pos_cb() {
substep_update(&state_l);
substep_update(&state_r);
int position_l= state_l.position;
int position_r= state_r.position;
double vel_l = position_l - prev_position_l;
double vel_r = position_r - prev_position_r;
prev_position_l = state_l.position;
prev_position_r = state_r.position;
vel_l /=64 * ENCODER_CPR;
vel_r /=64 * ENCODER_CPR;
vel_l *= WHEEL_RADIUS * 2 * M_PI * wheel_ratio_l;
vel_r *= -WHEEL_RADIUS * 2 * M_PI * wheel_ratio_r;
const double linear = (vel_l + vel_r) / 2;
const double angular = (vel_r - vel_l) / wheel_separation;
const double r = linear / angular;
if(fabs(r) > 100) {
const double dir = base_theta + angular * 0.5;
base_x += linear * cos(dir);
base_y += linear * sin(dir);
base_theta += angular;
}
else {
const double base_theta_old = base_theta;
base_theta += angular;
base_x += r * (sin(base_theta) - sin(base_theta_old));
base_y += -r * (cos(base_theta) - cos(base_theta_old));
}
}
void zero() {
base_x = 0;
base_y = 0;
base_theta = 0;
}
void core2_entry()
{
pio_add_program(pio1, &quadrature_encoder_substep_program);
substep_init_state(pio1, 0, ENCODER_LEFT_PIN_A , &state_l);
substep_init_state(pio1, 1, ENCODER_RIGHT_PIN_A, &state_r);
// The sets the positions to initial values
substep_update(&state_l);
substep_update(&state_r);
prev_position_l = state_l.position;
prev_position_r = state_r.position;
prev_time = time_us_32();
// alarm_pool_t *ap = alarm_pool_create_with_unused_hardware_alarm(2);
// repeating_timer_t rt;
// alarm_pool_add_repeating_timer_us(ap, TIMER_CYCLE_US, update_pos_cb, NULL, &rt);
uint16_t cmd = 0;
uint64_t data = 0;
uint readNum = 0;
char type = 'w';
while (true) {
uint ch;
if(( ch = stdio_getchar_timeout_us(0)) != PICO_ERROR_TIMEOUT) {
cmd = (cmd << 8) | ch;
if(readNum > 0) {
data = (data >> 8ll) | ((uint64_t)ch<<56ll);
readNum--;
if(!readNum) {
if(type == 'w')
wheel_separation = *((double*)&data);
else
wheel_ratio_l = 1 + (*((double *)&data));
wheel_ratio_r = 1 - (*((double *)&data));
}
}
if(cmd == (((uint16_t)'g' << 8) | ';')) {
printf("%lf %lf %lf\n", base_x, base_y, base_theta);
cmd = 0;
} else if(cmd == (((uint16_t)'w' << 8) | ';')) {
readNum = 8;
type = (cmd >> 8) & 0xFF;
} else if(cmd == (((uint16_t)'r' << 8) | ';')){
readNum = 8;
type = (cmd >> 8) & 0xFF;
} else if(cmd == (((uint16_t)'z' << 8) | ';')) {
zero();
}
}
update_pos_cb();
sleep_ms(1);
}
}
double btod(unsigned char * input) {
uint64_t data = 0;
for (int i = 0; i<8; i++){
uint a = *(input + i);
data >>= 8;
data |= (uint64_t)a<<56;
}
const double ret = *((double*)&data);
return ret;
}
void stepper_fifo(char c, uint8_t itf) {
char instrh = stepper_buf.data[(stepper_buf.pos + 1) % sizeof(stepper_buf.data)];
char instrl = stepper_buf.data[(stepper_buf.pos + 2) % sizeof(stepper_buf.data)];
stepper_buf.data[stepper_buf.pos] = c;
stepper_buf.pos = (stepper_buf.pos + 1) % sizeof(stepper_buf.data);
if(instrh == 's' && instrl == ';') {
memcpy(stepper_instr,stepper_buf.data + stepper_buf.pos, sizeof(stepper_buf.data) - stepper_buf.pos);
memcpy(stepper_instr - stepper_buf.pos + sizeof(stepper_buf.data),stepper_buf.data, stepper_buf.pos);
memset(&stepper_buf, 0, sizeof(stepper_buf));
double vl = btod(stepper_instr + 2);
double vr = btod(stepper_instr + 10);
set_speeds(vl, vr);
// tud_cdc_n_write(itf, (uint8_t const *) stepper_instr, 18);
// tud_cdc_n_write_flush(itf);
}
}
void tud_cdc_rx_cb(uint8_t itf) {
// read the available data
// | IMPORTANT: also do this for CDC0 because otherwise
// | you won't be able to print anymore to CDC0
// | next time this function is called
if (itf == 1) {
int c;
while((c = tud_cdc_n_read_char(itf)) > -1) {
stepper_fifo(c, itf);
}
// tud_cdc_n_write(itf, (uint8_t const *) "OK\r\n", 4);
// tud_cdc_n_write_flush(itf);
}
}
void run_init()
{
board_init();
tusb_init();
tud_init(BOARD_TUD_RHPORT);
if (board_init_after_tusb) {
board_init_after_tusb();
}
if(!stdio_usb_init()) {
board_led_write(1);
}
stepper_init();
}
int main()
{
run_init();
multicore_launch_core1(core2_entry);
while (true) {
tud_task();
sleep_ms(1);
}
}

184
firmware/base/src/quadrature.c Normal file
View File

@ -0,0 +1,184 @@
/**
* Copyright (c) 2023 Raspberry Pi (Trading) Ltd.
* Modified by Pimpest in 2024
*
* SPDX-License-Identifier: BSD-3-Clause
*
* This software has been modified from its original version
*/
#include "quadrature.pio.h"
#include "quadrature.h"
#include "memory.h"
static void read_pio_data(substep_state_t *state, uint *step, uint *step_us, uint *transition_us, int *forward)
{
int cycles;
// get the raw data from the PIO state machine
quadrature_encoder_substep_get_counts(state->pio, state->sm, step, &cycles, step_us);
// when the PIO program detects a transition, it sets cycles to either zero
// (when step is incrementing) or 2^31 (when step is decrementing) and keeps
// decrementing it on each 13 clock loop. We can use this information to get
// the time and direction of the last transition
if (cycles < 0) {
cycles = -cycles;
*forward = 1;
} else {
cycles = 0x80000000 - cycles;
*forward = 0;
}
*transition_us = *step_us - ((cycles * 13) / state->clocks_per_us);
}
// get the sub-step position of the start of a step
static uint get_step_start_transition_pos(substep_state_t *state, uint step)
{
return ((step << 6) & 0xFFFFFF00) | state->calibration_data[step & 3];
}
// compute speed in "sub-steps per 2^20 us" from a delta substep position and
// delta time in microseconds. This unit is cheaper to compute and use, so we
// only convert to "sub-steps per second" once per update, at most
static int substep_calc_speed(int delta_substep, int delta_us)
{
return ((int64_t) delta_substep << 20) / delta_us;
}
// main functions to be used by user code
// initialize the substep state structure and start PIO code
void substep_init_state(PIO pio, int sm, int pin_a, substep_state_t *state)
{
int forward;
// set all fields to zero by default
memset(state, 0, sizeof(substep_state_t));
// initialize the PIO program (and save the PIO reference)
state->pio = pio;
state->sm = sm;
quadrature_encoder_substep_program_init(pio, sm, pin_a);
// start with equal phase size calibration
state->calibration_data[0] = 0;
state->calibration_data[1] = 64;
state->calibration_data[2] = 128;
state->calibration_data[3] = 192;
state->idle_stop_samples = 3;
// start "stopped" so that we don't use stale data to compute speeds
state->stopped = 1;
// cache the PIO cycles per us
state->clocks_per_us = (clock_get_hz(clk_sys) + 500000) / 1000000;
// initialize the "previous state"
read_pio_data(state, &state->raw_step, &state->prev_step_us, &state->prev_trans_us, &forward);
state->position = get_step_start_transition_pos(state, state->raw_step) + 32;
}
// read the PIO data and update the speed / position estimate
void substep_update(substep_state_t *state)
{
uint step, step_us, transition_us, transition_pos, low, high;
int forward, speed_high, speed_low;
// read the current encoder state from the PIO
read_pio_data(state, &step, &step_us, &transition_us, &forward);
// from the current step we can get the low and high boundaries in substeps
// of the current position
low = get_step_start_transition_pos(state, step);
high = get_step_start_transition_pos(state, step + 1);
// if we were not stopped, but the last transition was more than
// "idle_stop_samples" ago, we are stopped now
if (step == state->raw_step)
state->idle_stop_sample_count++;
else
state->idle_stop_sample_count = 0;
if (!state->stopped && state->idle_stop_sample_count >= state->idle_stop_samples) {
state->speed = 0;
state->speed_2_20 = 0;
state->stopped = 1;
}
// when we are at a different step now, there is certainly a transition
if (state->raw_step != step) {
// the transition position depends on the direction of the move
transition_pos = forward ? low : high;
// if we are not stopped, that means there is valid previous transition
// we can use to estimate the current speed
if (!state->stopped)
state->speed_2_20 = substep_calc_speed(transition_pos - state->prev_trans_pos, transition_us - state->prev_trans_us);
// if we have a transition, we are not stopped now
state->stopped = 0;
// save the timestamp and position of this transition to use later to
// estimate speed
state->prev_trans_pos = transition_pos;
state->prev_trans_us = transition_us;
}
// if we are stopped, speed is zero and the position estimate remains
// constant. If we are not stopped, we have to update the position and speed
if (!state->stopped) {
// although the current step doesn't give us a precise position, it does
// give boundaries to the position, which together with the last
// transition gives us boundaries for the speed value. This can be very
// useful especially in two situations:
// - we have been stopped for a while and start moving quickly: although
// we only have one transition initially, the number of steps we moved
// can already give a non-zero speed estimate
// - we were moving but then stop: without any extra logic we would just
// keep the last speed for a while, but we know from the step
// boundaries that the speed must be decreasing
// if there is a transition between the last sample and now, and that
// transition is closer to now than the previous sample time, we should
// use the slopes from the last sample to the transition as these will
// have less numerical issues and produce a tighter boundary
if (state->prev_trans_us > state->prev_step_us &&
(int)(state->prev_trans_us - state->prev_step_us) > (int)(step_us - state->prev_trans_us)) {
speed_high = substep_calc_speed(state->prev_trans_pos - state->prev_low, state->prev_trans_us - state->prev_step_us);
speed_low = substep_calc_speed(state->prev_trans_pos - state->prev_high, state->prev_trans_us - state->prev_step_us);
} else {
// otherwise use the slopes from the last transition to now
speed_high = substep_calc_speed(high - state->prev_trans_pos, step_us - state->prev_trans_us);
speed_low = substep_calc_speed(low - state->prev_trans_pos, step_us - state->prev_trans_us);
}
// make sure the current speed estimate is between the maximum and
// minimum values obtained from the step slopes
if (state->speed_2_20 > speed_high)
state->speed_2_20 = speed_high;
if (state->speed_2_20 < speed_low)
state->speed_2_20 = speed_low;
// convert the speed units from "sub-steps per 2^20 us" to "sub-steps
// per second"
state->speed = (state->speed_2_20 * 62500LL) >> 16;
// estimate the current position by applying the speed estimate to the
// most recent transition
state->position = state->prev_trans_pos + (((int64_t)state->speed_2_20 * (step_us - transition_us)) >> 20);
// make sure the position estimate is between "low" and "high", as we
// can be sure the actual current position must be in this range
if ((int)(state->position - high) > 0)
state->position = high;
else if ((int)(state->position - low) < 0)
state->position = low;
}
// save the current values to use on the next sample
state->prev_low = low;
state->prev_high = high;
state->raw_step = step;
state->prev_step_us = step_us;
}

27
firmware/base/src/quadrature.h Normal file
View File

@ -0,0 +1,27 @@
#pragma once
#include "hardware/pio.h"
typedef struct substep_state_t {
uint calibration_data[4]; // relative phase sizes
uint clocks_per_us; // save the clk_sys frequency in clocks per us
uint idle_stop_samples; // after these samples without transitions, assume the encoder is stopped
PIO pio;
uint sm;
uint prev_trans_pos, prev_trans_us;
uint prev_step_us;
uint prev_low, prev_high;
uint idle_stop_sample_count;
int speed_2_20;
int stopped;
int speed;
uint position;
uint raw_step;
} substep_state_t;
void substep_init_state(PIO pio, int sm, int pin_a, substep_state_t *state);
void substep_update(substep_state_t *state);

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);
}

8
firmware/base/src/stepper.h Normal file
View File

@ -0,0 +1,8 @@
#pragma once
#include "hardware/pio.h"
void stepper_init();
void start_pulse(PIO pio, uint sm, uint offset, uint pin, uint freq);
void update_sm(PIO pio, uint sm, const uint pin, double v);
void set_speeds(double vl, double vr);