diff --git a/firmware/COLCON_IGNORE b/firmware/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/firmware/README.md b/firmware/README.md new file mode 100644 index 0000000..d3cf1a9 --- /dev/null +++ b/firmware/README.md @@ -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 \ No newline at end of file diff --git a/firmware/base/CMakeLists.txt b/firmware/base/CMakeLists.txt new file mode 100644 index 0000000..3e3630d --- /dev/null +++ b/firmware/base/CMakeLists.txt @@ -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) \ No newline at end of file diff --git a/firmware/base/pico_sdk_import.cmake b/firmware/base/pico_sdk_import.cmake new file mode 100644 index 0000000..d493cc2 --- /dev/null +++ b/firmware/base/pico_sdk_import.cmake @@ -0,0 +1,121 @@ +# This is a copy of /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}) diff --git a/firmware/base/pio/blink.pio b/firmware/base/pio/blink.pio new file mode 100644 index 0000000..b10a1e4 --- /dev/null +++ b/firmware/base/pio/blink.pio @@ -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); +} +%} \ No newline at end of file diff --git a/firmware/base/pio/quadrature.pio b/firmware/base/pio/quadrature.pio new file mode 100644 index 0000000..f86b98d --- /dev/null +++ b/firmware/base/pio/quadrature.pio @@ -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); +} + +%} \ No newline at end of file diff --git a/firmware/base/src/config.h b/firmware/base/src/config.h new file mode 100644 index 0000000..5058e7c --- /dev/null +++ b/firmware/base/src/config.h @@ -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 +//====================================================== \ No newline at end of file diff --git a/firmware/base/src/main.c b/firmware/base/src/main.c new file mode 100644 index 0000000..9273865 --- /dev/null +++ b/firmware/base/src/main.c @@ -0,0 +1,232 @@ +#include +#include +#include + +#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); + } +} + diff --git a/firmware/base/src/quadrature.c b/firmware/base/src/quadrature.c new file mode 100644 index 0000000..b2c3965 --- /dev/null +++ b/firmware/base/src/quadrature.c @@ -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; +} \ No newline at end of file diff --git a/firmware/base/src/quadrature.h b/firmware/base/src/quadrature.h new file mode 100644 index 0000000..44e2c1b --- /dev/null +++ b/firmware/base/src/quadrature.h @@ -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); \ No newline at end of file diff --git a/firmware/base/src/stepper.c b/firmware/base/src/stepper.c new file mode 100644 index 0000000..54bbfc1 --- /dev/null +++ b/firmware/base/src/stepper.c @@ -0,0 +1,43 @@ +#include "blink.pio.h" +#include "hardware/pio.h" +#include "hardware/clocks.h" +#include "pico/time.h" + +#include + +#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); +} + diff --git a/firmware/base/src/stepper.h b/firmware/base/src/stepper.h new file mode 100644 index 0000000..fb14841 --- /dev/null +++ b/firmware/base/src/stepper.h @@ -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); \ No newline at end of file diff --git a/firmware/base/tusb/tusb_config.h b/firmware/base/tusb/tusb_config.h new file mode 100644 index 0000000..f2761b7 --- /dev/null +++ b/firmware/base/tusb/tusb_config.h @@ -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 diff --git a/firmware/base/tusb/tusb_descriptors.c b/firmware/base/tusb/tusb_descriptors.c new file mode 100644 index 0000000..63ca265 --- /dev/null +++ b/firmware/base/tusb/tusb_descriptors.c @@ -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; +} \ No newline at end of file diff --git a/mg_bringup/launch/launch.py b/mg_bringup/launch/launch.py index 9b15a96..67ba23d 100644 --- a/mg_bringup/launch/launch.py +++ b/mg_bringup/launch/launch.py @@ -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, diff --git a/mg_control/assets/mg_base_controllers.yaml b/mg_control/assets/mg_base_controllers.yaml index 0883a17..bc8e32c 100644 --- a/mg_control/assets/mg_base_controllers.yaml +++ b/mg_control/assets/mg_base_controllers.yaml @@ -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 diff --git a/mg_control/src/mg_control.cpp b/mg_control/src/mg_control.cpp index 625e96b..ad36632 100644 --- a/mg_control/src/mg_control.cpp +++ b/mg_control/src/mg_control.cpp @@ -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; diff --git a/mg_msgs/CMakeLists.txt b/mg_msgs/CMakeLists.txt index 9000f71..23c7e0d 100644 --- a/mg_msgs/CMakeLists.txt +++ b/mg_msgs/CMakeLists.txt @@ -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() \ No newline at end of file diff --git a/mg_msgs/srv/SendDouble.srv b/mg_msgs/srv/SendDouble.srv new file mode 100644 index 0000000..3368ac0 --- /dev/null +++ b/mg_msgs/srv/SendDouble.srv @@ -0,0 +1,2 @@ +float64 data +--- \ No newline at end of file