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

0
firmware/COLCON_IGNORE Normal file
View File

6
firmware/README.md Normal file
View File

@ -0,0 +1,6 @@
# Magrob firmware
This directory contains the firmware used for our robot.
### Base
This code was meant to be used for the wheel base

50
firmware/base/CMakeLists.txt Normal file
View File

@ -0,0 +1,50 @@
cmake_minimum_required(VERSION 3.12)
set(PICO_BOARD pico CACHE STRING "Board type")
set(TOP ${PICO_TINYUSB_PATH})
include(pico_sdk_import.cmake)
set(CMAKE_C_STANDARD 11)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
project(wheelbase C CXX ASM)
pico_sdk_init()
add_executable(wheelbase
src/quadrature.c
src/main.c
src/stepper.c
tusb/tusb_descriptors.c
)
pico_set_program_name(wheelbase "wheelbase")
pico_set_program_version(wheelbase "0.1")
pico_enable_stdio_uart(wheelbase 0)
pico_enable_stdio_usb(wheelbase 1)
pico_generate_pio_header(wheelbase ${CMAKE_CURRENT_LIST_DIR}/pio/quadrature.pio)
pico_generate_pio_header(wheelbase ${CMAKE_CURRENT_LIST_DIR}/pio/blink.pio)
target_link_libraries(wheelbase
pico_stdlib
pico_stdio
pico_time
pico_multicore
hardware_pio
hardware_clocks
hardware_gpio
hardware_sync
tinyusb_board
tinyusb_device)
target_include_directories(wheelbase PRIVATE
${CMAKE_CURRENT_LIST_DIR}
src/
tusb/
)
pico_add_extra_outputs(wheelbase)

121
firmware/base/pico_sdk_import.cmake Normal file
View File

@ -0,0 +1,121 @@
# This is a copy of <PICO_SDK_PATH>/external/pico_sdk_import.cmake
# This can be dropped into an external project to help locate this SDK
# It should be include()ed prior to project()
# Copyright 2020 (c) 2020 Raspberry Pi (Trading) Ltd.
#
# Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
# following conditions are met:
#
# 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following
# disclaimer.
#
# 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following
# disclaimer in the documentation and/or other materials provided with the distribution.
#
# 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products
# derived from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
# INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
# WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
# THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
if (DEFINED ENV{PICO_SDK_PATH} AND (NOT PICO_SDK_PATH))
set(PICO_SDK_PATH $ENV{PICO_SDK_PATH})
message("Using PICO_SDK_PATH from environment ('${PICO_SDK_PATH}')")
endif ()
if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT} AND (NOT PICO_SDK_FETCH_FROM_GIT))
set(PICO_SDK_FETCH_FROM_GIT $ENV{PICO_SDK_FETCH_FROM_GIT})
message("Using PICO_SDK_FETCH_FROM_GIT from environment ('${PICO_SDK_FETCH_FROM_GIT}')")
endif ()
if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT_PATH} AND (NOT PICO_SDK_FETCH_FROM_GIT_PATH))
set(PICO_SDK_FETCH_FROM_GIT_PATH $ENV{PICO_SDK_FETCH_FROM_GIT_PATH})
message("Using PICO_SDK_FETCH_FROM_GIT_PATH from environment ('${PICO_SDK_FETCH_FROM_GIT_PATH}')")
endif ()
if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT_TAG} AND (NOT PICO_SDK_FETCH_FROM_GIT_TAG))
set(PICO_SDK_FETCH_FROM_GIT_TAG $ENV{PICO_SDK_FETCH_FROM_GIT_TAG})
message("Using PICO_SDK_FETCH_FROM_GIT_TAG from environment ('${PICO_SDK_FETCH_FROM_GIT_TAG}')")
endif ()
if (PICO_SDK_FETCH_FROM_GIT AND NOT PICO_SDK_FETCH_FROM_GIT_TAG)
set(PICO_SDK_FETCH_FROM_GIT_TAG "master")
message("Using master as default value for PICO_SDK_FETCH_FROM_GIT_TAG")
endif()
set(PICO_SDK_PATH "${PICO_SDK_PATH}" CACHE PATH "Path to the Raspberry Pi Pico SDK")
set(PICO_SDK_FETCH_FROM_GIT "${PICO_SDK_FETCH_FROM_GIT}" CACHE BOOL "Set to ON to fetch copy of SDK from git if not otherwise locatable")
set(PICO_SDK_FETCH_FROM_GIT_PATH "${PICO_SDK_FETCH_FROM_GIT_PATH}" CACHE FILEPATH "location to download SDK")
set(PICO_SDK_FETCH_FROM_GIT_TAG "${PICO_SDK_FETCH_FROM_GIT_TAG}" CACHE FILEPATH "release tag for SDK")
if (NOT PICO_SDK_PATH)
if (PICO_SDK_FETCH_FROM_GIT)
include(FetchContent)
set(FETCHCONTENT_BASE_DIR_SAVE ${FETCHCONTENT_BASE_DIR})
if (PICO_SDK_FETCH_FROM_GIT_PATH)
get_filename_component(FETCHCONTENT_BASE_DIR "${PICO_SDK_FETCH_FROM_GIT_PATH}" REALPATH BASE_DIR "${CMAKE_SOURCE_DIR}")
endif ()
FetchContent_Declare(
pico_sdk
GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk
GIT_TAG ${PICO_SDK_FETCH_FROM_GIT_TAG}
)
if (NOT pico_sdk)
message("Downloading Raspberry Pi Pico SDK")
# GIT_SUBMODULES_RECURSE was added in 3.17
if (${CMAKE_VERSION} VERSION_GREATER_EQUAL "3.17.0")
FetchContent_Populate(
pico_sdk
QUIET
GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk
GIT_TAG ${PICO_SDK_FETCH_FROM_GIT_TAG}
GIT_SUBMODULES_RECURSE FALSE
SOURCE_DIR ${FETCHCONTENT_BASE_DIR}/pico_sdk-src
BINARY_DIR ${FETCHCONTENT_BASE_DIR}/pico_sdk-build
SUBBUILD_DIR ${FETCHCONTENT_BASE_DIR}/pico_sdk-subbuild
)
else ()
FetchContent_Populate(
pico_sdk
QUIET
GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk
GIT_TAG ${PICO_SDK_FETCH_FROM_GIT_TAG}
SOURCE_DIR ${FETCHCONTENT_BASE_DIR}/pico_sdk-src
BINARY_DIR ${FETCHCONTENT_BASE_DIR}/pico_sdk-build
SUBBUILD_DIR ${FETCHCONTENT_BASE_DIR}/pico_sdk-subbuild
)
endif ()
set(PICO_SDK_PATH ${pico_sdk_SOURCE_DIR})
endif ()
set(FETCHCONTENT_BASE_DIR ${FETCHCONTENT_BASE_DIR_SAVE})
else ()
message(FATAL_ERROR
"SDK location was not specified. Please set PICO_SDK_PATH or set PICO_SDK_FETCH_FROM_GIT to on to fetch from git."
)
endif ()
endif ()
get_filename_component(PICO_SDK_PATH "${PICO_SDK_PATH}" REALPATH BASE_DIR "${CMAKE_BINARY_DIR}")
if (NOT EXISTS ${PICO_SDK_PATH})
message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' not found")
endif ()
set(PICO_SDK_INIT_CMAKE_FILE ${PICO_SDK_PATH}/pico_sdk_init.cmake)
if (NOT EXISTS ${PICO_SDK_INIT_CMAKE_FILE})
message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' does not appear to contain the Raspberry Pi Pico SDK")
endif ()
set(PICO_SDK_PATH ${PICO_SDK_PATH} CACHE PATH "Path to the Raspberry Pi Pico SDK" FORCE)
include(${PICO_SDK_INIT_CMAKE_FILE})

31
firmware/base/pio/blink.pio Normal file
View File

@ -0,0 +1,31 @@
.program oscillate
pause:
pull block
out x,32
.wrap_target
pull noblock
out x, 32
mov y, x
set pins, 1 ; Pin ON
jmp y-- lp1 ; Delay for (x + 1) cycles, x is a 32 bit number
jmp pause
lp1:
jmp y-- lp1 ; Delay for (x + 1) cycles, x is a 32 bit number
mov y, x
set pins, 0 [2] ; Pin OFF
lp2:
jmp y-- lp2 ; Delay for the same number of cycles again
.wrap ; Blink forever!
% c-sdk {
// this is a raw helper function for use by the user which sets up the GPIO output, and configures the SM to output on a particular pin
void oscillate_program_init(PIO pio, uint sm, uint offset, uint pin) {
pio_gpio_init(pio, pin);
pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, true);
pio_sm_config c = oscillate_program_get_default_config(offset);
sm_config_set_set_pins(&c, pin, 1);
pio_sm_init(pio, sm, offset, &c);
}
%}

208
firmware/base/pio/quadrature.pio Normal file
View File

@ -0,0 +1,208 @@
;
; Copyright (c) 2023 Raspberry Pi (Trading) Ltd.
;
; SPDX-License-Identifier: BSD-3-Clause
;
; quadrature_encoder_substep: reads a quadrature encoder with no CPU
; intervention and provides the current position on request.
;
; the "substep" version uses not only the step counts, but also the timing of
; the steps to compute the current speed. See README.md for details
.program quadrature_encoder_substep
.origin 0
; the PIO code counts steps like the standard quadrature encoder code, but also
; keeps track of the time passed since the last transition. That allows the C
; code to build a good estimate of a fractional step position based on the
; latest speed and time passed
;
; since it needs to push two values, it only pushes new data when the FIFO has
; enough space to hold both values. Otherwise it could either stall or go out
; of sync
;
; because we need to count the time passed, all loops must take the same number
; of cycles and there are delays added to the fastest branches to make sure it
; always takes 13 cycles per loop (e.g., sysclk 133MHz, max step rate = ~10.2
; Msteps/sec)
; push the step count and transition clock count to the RX FIFO (using
; auto push). This is reached by the "MOV PC, ~STATUS" instruction when
; status is all 1 (meaning fifo has space for this push). It also may
; execute once at program start, but that has little effect
IN X, 32
IN Y, 32
update_state:
; build the state by using 2 bits from the negated previous state of the
; pins and the new 2 bit state of the pins
OUT ISR, 2
IN PINS, 2
MOV OSR, ~ISR
; use the jump table to update the step count accordingly
MOV PC, OSR
decrement:
; decrement the step count
JMP Y--, decrement_cont
decrement_cont:
; when decrementing, X is set to 2^31, when incrementing it is set to
; zero. That way the C code can infer in which direction the last
; transition was taken and how long ago
SET X, 1
MOV X, ::X
; after incrementing or decrementing, continue to "check_fifo"
check_fifo:
.wrap_target
; on each iteration we decrement X to count the number of loops since
; the last transition
JMP X--, check_fifo_cont
check_fifo_cont:
; push data or continue, depending on the state of the fifo
MOV PC, ~STATUS
increment:
; the PIO does not have a increment instruction, so to do that we do a
; negate, decrement, negate sequence
MOV Y, ~Y
JMP Y--, increment_cont
increment_cont:
MOV Y, ~Y
; reset X to zero when incrementing
SET X, 0
; wrap above to check the fifo state
.wrap
invalid:
; this is just a placeholder to document what the code does on invalid
; transitions, where the two phases change at the same time. We don't do
; anything special, but the encoder should note generate these invalid
; transitions anyway
JMP update_state
; this jump table starts at address 16 and is accessed by the
; "MOV PC, OSR" instruction above, that loads the PC with the state on
; the lower 4 bits and the 5th bit on. The delays here extend the faster
; branches to take the same time as the slower branches
JMP invalid
JMP increment [0]
JMP decrement [1]
JMP check_fifo [4]
JMP decrement [1]
JMP invalid
JMP check_fifo [4]
JMP increment [0]
JMP increment [0]
JMP check_fifo [4]
JMP invalid
JMP decrement [1]
JMP check_fifo [4]
JMP decrement [1]
JMP increment [0]
; this instruction should be usually reached by the "MOV PC, ~STATUS"
; instruction above when the status is zero, which means that the fifo
; has data and we don't want to push more data. This can also be reached
; on an invalid state transition, which should not happen. Even if it
; happens, it should be a transient state and the only side effect is
; that we'll call update_state twice in a row
JMP update_state [1]
% c-sdk {
#include "hardware/clocks.h"
#include "hardware/timer.h"
#include "hardware/gpio.h"
#include "hardware/sync.h"
// "substep" version low-level interface
//
// note: user code should use the high level functions in quadrature_encoder.c
// and not call these directly
// initialize the PIO state and the substep_state_t structure that keeps track
// of the encoder state
static inline void quadrature_encoder_substep_program_init(PIO pio, uint sm, uint pin_A)
{
uint pin_state, position, ints;
pio_gpio_init(pio, pin_A);
pio_gpio_init(pio, pin_A + 1);
pio_sm_set_consecutive_pindirs(pio, sm, pin_A, 2, false);
gpio_pull_up(pin_A);
gpio_pull_up(pin_A + 1);
pio_sm_config c = quadrature_encoder_substep_program_get_default_config(0);
sm_config_set_in_pins(&c, pin_A); // for WAIT, IN
// shift to left, auto-push at 32 bits
sm_config_set_in_shift(&c, false, true, 32);
sm_config_set_out_shift(&c, true, false, 32);
// don't join FIFO's
sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_NONE);
// always run at sysclk, to have the maximum possible time resolution
sm_config_set_clkdiv(&c, 1.0);
pio_sm_init(pio, sm, 0, &c);
// set up status to be rx_fifo < 1
pio->sm[sm].execctrl = ((pio->sm[sm].execctrl & 0xFFFFFF80) | 0x12);
// init the state machine according to the current phase. Since we are
// setting the state running PIO instructions from C state, the encoder may
// step during this initialization. This should not be a problem though,
// because as long as it is just one step, the state machine will update
// correctly when it starts. We disable interrupts anyway, to be safe
ints = save_and_disable_interrupts();
pin_state = (gpio_get_all() >> pin_A) & 3;
// to setup the state machine, we need to set the lower 2 bits of OSR to be
// the negated pin state
pio_sm_exec(pio, sm, pio_encode_set(pio_y, ~pin_state));
pio_sm_exec(pio, sm, pio_encode_mov(pio_osr, pio_y));
// also set the Y (current step) so that the lower 2 bits of Y have a 1:1
// mapping to the current phase (input pin state). That simplifies the code
// to compensate for differences in encoder phase sizes:
switch (pin_state) {
case 0: position = 0; break;
case 1: position = 3; break;
case 2: position = 1; break;
case 3: position = 2; break;
}
pio_sm_exec(pio, sm, pio_encode_set(pio_y, position));
pio_sm_set_enabled(pio, sm, true);
restore_interrupts(ints);
}
static inline void quadrature_encoder_substep_get_counts(PIO pio, uint sm, uint *step, int *cycles, uint *us)
{
int i, pairs;
uint ints;
pairs = pio_sm_get_rx_fifo_level(pio, sm) >> 1;
// read all data with interrupts disabled, so that there can not be a
// big time gap between reading the PIO data and the current us
ints = save_and_disable_interrupts();
for (i = 0; i < pairs + 1; i++) {
*cycles = pio_sm_get_blocking(pio, sm);
*step = pio_sm_get_blocking(pio, sm);
}
*us = time_us_32();
restore_interrupts(ints);
}
%}

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

47
firmware/base/tusb/tusb_config.h Normal file
View File

@ -0,0 +1,47 @@
#pragma once
#ifdef PICO_STDIO_USB_RESET_INTERFACE_SUPPORT_MS_OS_20_DESCRIPTOR
#undef PICO_STDIO_USB_RESET_INTERFACE_SUPPORT_MS_OS_20_DESCRIPTOR
#endif
#define CFG_TUSB_RHPORT0_MODE (OPT_MODE_DEVICE | OPT_MODE_FULL_SPEED)
#ifndef BOARD_TUD_RHPORT
#define BOARD_TUD_RHPORT 0
#endif
#ifndef BOARD_TUD_MAX_SPEED
#define BOARD_TUD_MAX_SPEED OPT_MODE_DEFAULT_SPEED
#endif
#ifndef CFG_TUSB_MCU
#error CFG_TUSB_MCU must be defined
#endif
#ifndef CFG_TUSB_OS
#define CFG_TUSB_OS OPT_OS_NONE
#endif
#ifndef CFG_TUD_ENABLED
#define CFG_TUD_ENABLED 1
#endif
#ifndef CFG_TUD_MAX_SPEED
#define CFG_TUD_MAX_SPEED BOARD_TUD_MAX_SPEED
#endif
#ifndef CFG_TUD_ENDPOINT0_SIZE
#define CFG_TUD_ENDPOINT0_SIZE 64
#endif
#define CFG_TUD_CDC 2
#define CFG_TUD_MSC 0
#define CFG_TUD_HID 0
#define CFG_TUD_MIDI 0
#define CFG_TUD_VENDOR 0
#define CFG_TUD_CDC_RX_BUFSIZE (TUD_OPT_HIGH_SPEED ? 512 : 64)
#define CFG_TUD_CDC_TX_BUFSIZE (TUD_OPT_HIGH_SPEED ? 512 : 64)
#define CFG_TUD_CDC_EP_BUFSIZE (TUD_OPT_HIGH_SPEED ? 512 : 64)
#define CFG_TUD_MSC_EP_BUFSIZE 512

143
firmware/base/tusb/tusb_descriptors.c Normal file
View File

@ -0,0 +1,143 @@
#include "bsp/board_api.h"
#include "tusb.h"
#define _PID_MAP(itf,n) ( (CFG_TUD_##itf) << (n) )
#define USB_PID (0x4d47 | _PID_MAP(CDC, 0) | _PID_MAP(MSC, 1)| _PID_MAP(HID, 2) | \
_PID_MAP(MIDI, 3) | _PID_MAP(VENDOR, 4) )
#define USB_VID 0x1209
#define USB_BCD 0x0200
tusb_desc_device_t const desc_device = {
.bLength = sizeof(tusb_desc_device_t),
.bDescriptorType = TUSB_DESC_DEVICE,
.bcdUSB = USB_BCD,
.bDeviceClass = TUSB_CLASS_MISC,
.bDeviceSubClass = MISC_SUBCLASS_COMMON,
.bDeviceProtocol = MISC_PROTOCOL_IAD,
.bMaxPacketSize0 = CFG_TUD_ENDPOINT0_SIZE,
.idVendor = USB_VID,
.idProduct = USB_PID,
.bcdDevice = 0x0100,
.iManufacturer = 0x01,
.iProduct = 0x02,
.iSerialNumber = 0x03,
.bNumConfigurations = 0x01
};
uint8_t const * tud_descriptor_device_cb(void) {
return (uint8_t const *) &desc_device;
}
enum
{
ITF_NUM_CDC_0 = 0,
ITF_NUM_CDC_0_DATA,
ITF_NUM_CDC_1,
ITF_NUM_CDC_1_DATA,
ITF_NUM_TOTAL,
};
#define CONFIG_TOTAL_LEN (TUD_CONFIG_DESC_LEN + CFG_TUD_CDC * TUD_CDC_DESC_LEN)
#define EPNUM_CDC_NOTIF 0x81
#define EPNUM_CDC_OUT 0x02
#define EPNUM_CDC_IN 0x82
#define EPNUM_CDC_1_NOTIF 0x84
#define EPNUM_CDC_1_OUT 0x05
#define EPNUM_CDC_1_IN 0x85
uint8_t const desc_fs_configuration[] =
{
TUD_CONFIG_DESCRIPTOR(1, ITF_NUM_TOTAL, 0, CONFIG_TOTAL_LEN, 0x00, 250),
TUD_CDC_DESCRIPTOR(ITF_NUM_CDC_0, 4, EPNUM_CDC_NOTIF, 8, EPNUM_CDC_OUT, EPNUM_CDC_IN, 64),
TUD_CDC_DESCRIPTOR(ITF_NUM_CDC_1, 4, EPNUM_CDC_1_NOTIF, 8, EPNUM_CDC_1_OUT, EPNUM_CDC_1_IN, 64)
};
tusb_desc_device_qualifier_t const desc_device_qualifier =
{
.bLength = sizeof(tusb_desc_device_qualifier_t),
.bDescriptorType = TUSB_DESC_DEVICE,
.bcdUSB = USB_BCD,
.bDeviceClass = TUSB_CLASS_MISC,
.bDeviceSubClass = MISC_SUBCLASS_COMMON,
.bDeviceProtocol = MISC_PROTOCOL_IAD,
.bMaxPacketSize0 = CFG_TUD_ENDPOINT0_SIZE,
.bNumConfigurations = 0x01
};
uint8_t const* tud_descriptor_device_qualifier_cb(void) {
return (uint8_t const*) &desc_device_qualifier;
}
uint8_t const* tud_descriptor_configuration_cb(uint8_t index) {
(void) index;
return desc_fs_configuration;
}
enum {
STRID_LANGID = 0,
STRID_MANUFACTURER,
STRID_PRODUCT,
STRID_SERIAL,
STRID_CDC_0,
STRID_CDC_1
};
char const *string_desc_arr[] = {
(const char[]) { 0x09, 0x04},
"Mg Robotics",
"Magrob Odometry MCU",
NULL,
"Odometry CDC",
"Stepper CDC"
};
static uint16_t _desc_str[32+1];
uint16_t const *tud_descriptor_string_cb(uint8_t index, uint16_t langid) {
(void) langid;
size_t chr_count;
switch (index) {
case STRID_LANGID:
memcpy(&_desc_str[1], string_desc_arr[0], 2);
chr_count = 1;
break;
case STRID_SERIAL:
chr_count = board_usb_get_serial(_desc_str + 1, 32);
break;
default:
if( !(index < sizeof(string_desc_arr) / sizeof(string_desc_arr[0])) ) return NULL;
const char *str = string_desc_arr[index];
chr_count = strlen(str);
size_t const max_count = sizeof(_desc_str) / sizeof(_desc_str[0]) - 1;
if ( chr_count > max_count ) chr_count = max_count;
for(size_t i = 0; i < chr_count; i++) {
_desc_str[i + 1] = str[i];
}
break;
}
_desc_str[0] = (uint16_t) ((TUSB_DESC_STRING << 8) | (2 * chr_count + 2));
return _desc_str;
}

2
mg_bringup/launch/launch.py
View File

@ -33,7 +33,7 @@ def generate_launch_description():
condition=UnlessCondition(LaunchConfiguration('local_test')),
parameters=[{
'odom': "odom",
'serial_path': "/dev/ttyACM1",
'serial_path': "/dev/ttyACM0",
}],
emulate_tty=True,

2
mg_control/assets/mg_base_controllers.yaml
View File

@ -12,7 +12,7 @@ diffdrive_controller:
left_wheel_names: ["left_motor"]
right_wheel_names: ["right_motor"]
enable_odom_tf: true
enable_odom_tf: false
odom_frame_id: odom_excpected
base_frame_id: base-link

2
mg_control/src/mg_control.cpp
View File

@ -11,7 +11,7 @@ CallbackReturn mg::MgStepperInterface::on_init(const hardware_interface::Hardwar
if (info_.hardware_parameters.find("device_path") != info.hardware_parameters.end()) {
serial_port_name = info_.hardware_parameters["device_path"];
} else {
serial_port_name = "/dev/ttyACM0";
serial_port_name = "/dev/ttyACM1";
}
left_wheel_pos_state = 0;

2
mg_msgs/CMakeLists.txt
View File

@ -15,7 +15,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"action/MovePoint.action"
"action/LookAt.action"
"action/Rotate.action"
"srv/SetWidth.srv"
"srv/SendDouble.srv"
)
ament_package()

2
mg_msgs/srv/SendDouble.srv Normal file
View File

@ -0,0 +1,2 @@
float64 data
---