Pimpest/ros2-encoder
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
Pimpest
2024-12-18 12:29:19 +01:00
commit fff7576e18
25 changed files with 1563 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
.gitignore vendored Normal file
View File

@ -0,0 +1,4 @@
build
install
firmware/usb-test/build
firmware/usb-test/install

21
.vscode/c_cpp_properties.json vendored Normal file
View File

@ -0,0 +1,21 @@
{
"configurations": [
{
"browse": {
"databaseFilename": "${default}",
"limitSymbolsToIncludedHeaders": false
},
// "includePath": [
// "/opt/ros/jazzy/include/**",
// "/usr/include/libserial"
// ],
"name": "ROS",
"intelliSenseMode": "gcc-x64",
"compilerPath": "/usr/bin/gcc",
"cStandard": "gnu11",
"cppStandard": "c++14",
"compileCommands": "${workspaceFolder}/build/compile_commands.json"
}
],
"version": 4
}

0
firmware/COLCON_IGNORE Normal file
View File

0
firmware/usb-test/.COLCON_IGNORE Normal file
View File

1
firmware/usb-test/.gitignore vendored Normal file
View File

@ -0,0 +1 @@
build

22
firmware/usb-test/.vscode/c_cpp_properties.json vendored Normal file
View File

@ -0,0 +1,22 @@
{
"configurations": [
{
"name": "Pico",
"includePath": [
"${workspaceFolder}/**",
"${userHome}/.pico-sdk/sdk/2.1.0/**"
],
"forcedInclude": [
"${userHome}/.pico-sdk/sdk/2.1.0/src/common/pico_base_headers/include/pico.h",
"${workspaceFolder}/build/generated/pico_base/pico/config_autogen.h"
],
"defines": [],
"compilerPath": "${userHome}/.pico-sdk/toolchain/13_3_Rel1/bin/arm-none-eabi-gcc",
"compileCommands": "${workspaceFolder}/build/compile_commands.json",
"cStandard": "c17",
"cppStandard": "c++14",
"intelliSenseMode": "linux-gcc-arm"
}
],
"version": 4
}

15
firmware/usb-test/.vscode/cmake-kits.json vendored Normal file
View File

@ -0,0 +1,15 @@
[
{
"name": "Pico",
"compilers": {
"C": "${command:raspberry-pi-pico.getCompilerPath}",
"CXX": "${command:raspberry-pi-pico.getCxxCompilerPath}"
},
"environmentVariables": {
"PATH": "${command:raspberry-pi-pico.getEnvPath};${env:PATH}"
},
"cmakeSettings": {
"Python3_EXECUTABLE": "${command:raspberry-pi-pico.getPythonPath}"
}
}
]

9
firmware/usb-test/.vscode/extensions.json vendored Normal file
View File

@ -0,0 +1,9 @@
{
"recommendations": [
"ms-vscode.cpptools",
"marus25.cortex-debug",
"raspberry-pi.raspberry-pi-pico",
"ms-vscode.vscode-serial-monitor",
"ms-vscode.cpptools-extension-pack"
]
}

70
firmware/usb-test/.vscode/launch.json vendored Normal file
View File

@ -0,0 +1,70 @@
{
"version": "0.2.0",
"configurations": [
{
"name": "Pico Debug (Cortex-Debug)",
"cwd": "${userHome}/.pico-sdk/openocd/0.12.0+dev/scripts",
"executable": "${command:raspberry-pi-pico.launchTargetPath}",
"request": "launch",
"type": "cortex-debug",
"servertype": "openocd",
"serverpath": "${userHome}/.pico-sdk/openocd/0.12.0+dev/openocd.exe",
"gdbPath": "${command:raspberry-pi-pico.getGDBPath}",
"device": "${command:raspberry-pi-pico.getChipUppercase}",
"configFiles": [
"interface/cmsis-dap.cfg",
"target/${command:raspberry-pi-pico.getTarget}.cfg"
],
"svdFile": "${userHome}/.pico-sdk/sdk/2.1.0/src/${command:raspberry-pi-pico.getChip}/hardware_regs/${command:raspberry-pi-pico.getChipUppercase}.svd",
"runToEntryPoint": "main",
// Fix for no_flash binaries, where monitor reset halt doesn't do what is expected
// Also works fine for flash binaries
"overrideLaunchCommands": [
"monitor reset init",
"load \"${command:raspberry-pi-pico.launchTargetPath}\""
],
"openOCDLaunchCommands": [
"adapter speed 5000"
]
},
{
"name": "Pico Debug (Cortex-Debug with external OpenOCD)",
"cwd": "${workspaceRoot}",
"executable": "${command:raspberry-pi-pico.launchTargetPath}",
"request": "launch",
"type": "cortex-debug",
"servertype": "external",
"gdbTarget": "localhost:3333",
"gdbPath": "${command:raspberry-pi-pico.getGDBPath}",
"device": "${command:raspberry-pi-pico.getChipUppercase}",
"svdFile": "${userHome}/.pico-sdk/sdk/2.1.0/src/${command:raspberry-pi-pico.getChip}/hardware_regs/${command:raspberry-pi-pico.getChipUppercase}.svd",
"runToEntryPoint": "main",
// Fix for no_flash binaries, where monitor reset halt doesn't do what is expected
// Also works fine for flash binaries
"overrideLaunchCommands": [
"monitor reset init",
"load \"${command:raspberry-pi-pico.launchTargetPath}\""
]
},
{
"name": "Pico Debug (C++ Debugger)",
"type": "cppdbg",
"request": "launch",
"cwd": "${workspaceRoot}",
"program": "${command:raspberry-pi-pico.launchTargetPath}",
"MIMode": "gdb",
"miDebuggerPath": "${command:raspberry-pi-pico.getGDBPath}",
"miDebuggerServerAddress": "localhost:3333",
"debugServerPath": "${userHome}/.pico-sdk/openocd/0.12.0+dev/openocd.exe",
"debugServerArgs": "-f interface/cmsis-dap.cfg -f target/${command:raspberry-pi-pico.getTarget}.cfg -c \"adapter speed 5000\"",
"serverStarted": "Listening on port .* for gdb connections",
"filterStderr": true,
"hardwareBreakpoints": {
"require": true,
"limit": 4
},
"preLaunchTask": "Flash",
"svdPath": "${userHome}/.pico-sdk/sdk/2.1.0/src/${command:raspberry-pi-pico.getChip}/hardware_regs/${command:raspberry-pi-pico.getChipUppercase}.svd"
},
]
}

39
firmware/usb-test/.vscode/settings.json vendored Normal file
View File

@ -0,0 +1,39 @@
{
"cmake.options.statusBarVisibility": "hidden",
"cmake.options.advanced": {
"build": {
"statusBarVisibility": "hidden"
},
"launch": {
"statusBarVisibility": "hidden"
},
"debug": {
"statusBarVisibility": "hidden"
}
},
"cmake.configureOnEdit": false,
"cmake.automaticReconfigure": false,
"cmake.configureOnOpen": false,
"cmake.generator": "Ninja",
"cmake.cmakePath": "${userHome}/.pico-sdk/cmake/v3.29.9/bin/cmake",
"C_Cpp.debugShortcut": false,
"terminal.integrated.env.windows": {
"PICO_SDK_PATH": "${env:USERPROFILE}/.pico-sdk/sdk/2.1.0",
"PICO_TOOLCHAIN_PATH": "${env:USERPROFILE}/.pico-sdk/toolchain/13_3_Rel1",
"Path": "${env:USERPROFILE}/.pico-sdk/toolchain/13_3_Rel1/bin;${env:USERPROFILE}/.pico-sdk/picotool/2.1.0/picotool;${env:USERPROFILE}/.pico-sdk/cmake/v3.29.9/bin;${env:USERPROFILE}/.pico-sdk/ninja/v1.12.1;${env:PATH}"
},
"terminal.integrated.env.osx": {
"PICO_SDK_PATH": "${env:HOME}/.pico-sdk/sdk/2.1.0",
"PICO_TOOLCHAIN_PATH": "${env:HOME}/.pico-sdk/toolchain/13_3_Rel1",
"PATH": "${env:HOME}/.pico-sdk/toolchain/13_3_Rel1/bin:${env:HOME}/.pico-sdk/picotool/2.1.0/picotool:${env:HOME}/.pico-sdk/cmake/v3.29.9/bin:${env:HOME}/.pico-sdk/ninja/v1.12.1:${env:PATH}"
},
"terminal.integrated.env.linux": {
"PICO_SDK_PATH": "${env:HOME}/.pico-sdk/sdk/2.1.0",
"PICO_TOOLCHAIN_PATH": "${env:HOME}/.pico-sdk/toolchain/13_3_Rel1",
"PATH": "${env:HOME}/.pico-sdk/toolchain/13_3_Rel1/bin:${env:HOME}/.pico-sdk/picotool/2.1.0/picotool:${env:HOME}/.pico-sdk/cmake/v3.29.9/bin:${env:HOME}/.pico-sdk/ninja/v1.12.1:${env:PATH}"
},
"raspberry-pi-pico.cmakeAutoConfigure": true,
"raspberry-pi-pico.useCmakeTools": false,
"raspberry-pi-pico.cmakePath": "${HOME}/.pico-sdk/cmake/v3.29.9/bin/cmake",
"raspberry-pi-pico.ninjaPath": "${HOME}/.pico-sdk/ninja/v1.12.1/ninja"
}

58
firmware/usb-test/.vscode/tasks.json vendored Normal file
View File

@ -0,0 +1,58 @@
{
"version": "2.0.0",
"tasks": [
{
"label": "Compile Project",
"type": "process",
"isBuildCommand": true,
"command": "${userHome}/.pico-sdk/ninja/v1.12.1/ninja",
"args": ["-C", "${workspaceFolder}/build"],
"group": "build",
"presentation": {
"reveal": "always",
"panel": "dedicated"
},
"problemMatcher": "$gcc",
"windows": {
"command": "${env:USERPROFILE}/.pico-sdk/ninja/v1.12.1/ninja.exe"
}
},
{
"label": "Run Project",
"type": "process",
"command": "${env:HOME}/.pico-sdk/picotool/2.1.0/picotool/picotool",
"args": [
"load",
"${command:raspberry-pi-pico.launchTargetPath}",
"-fx"
],
"presentation": {
"reveal": "always",
"panel": "dedicated"
},
"problemMatcher": [],
"windows": {
"command": "${env:USERPROFILE}/.pico-sdk/picotool/2.1.0/picotool/picotool.exe"
}
},
{
"label": "Flash",
"type": "process",
"command": "${userHome}/.pico-sdk/openocd/0.12.0+dev/openocd.exe",
"args": [
"-s",
"${userHome}/.pico-sdk/openocd/0.12.0+dev/scripts",
"-f",
"interface/cmsis-dap.cfg",
"-f",
"target/${command:raspberry-pi-pico.getTarget}.cfg",
"-c",
"adapter speed 5000; program \"${command:raspberry-pi-pico.launchTargetPath}\" verify reset exit"
],
"problemMatcher": [],
"windows": {
"command": "${env:USERPROFILE}/.pico-sdk/openocd/0.12.0+dev/openocd.exe",
}
}
]
}

89
firmware/usb-test/CMakeLists.txt Normal file
View File

@ -0,0 +1,89 @@
# == DO NOT EDIT THE FOLLOWING LINES for the Raspberry Pi Pico VS Code Extension to work ==
if(WIN32)
set(USERHOME $ENV{USERPROFILE})
else()
set(USERHOME $ENV{HOME})
endif()
set(sdkVersion 2.1.0)
set(toolchainVersion 13_3_Rel1)
set(picotoolVersion 2.1.0)
set(picoVscode ${USERHOME}/.pico-sdk/cmake/pico-vscode.cmake)
if (EXISTS ${picoVscode})
include(${picoVscode})
endif()
# ====================================================================================
# Generated Cmake Pico project file
cmake_minimum_required(VERSION 3.13)
set(CMAKE_C_STANDARD 11)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
# Initialise pico_sdk from installed location
# (note this can come from environment, CMake cache etc)
# == DO NOT EDIT THE FOLLOWING LINES for the Raspberry Pi Pico VS Code Extension to work ==
if(WIN32)
set(USERHOME $ENV{USERPROFILE})
else()
set(USERHOME $ENV{HOME})
endif()
set(sdkVersion 2.1.0)
set(toolchainVersion 13_3_Rel1)
set(picotoolVersion 2.1.0)
set(picoVscode ${USERHOME}/.pico-sdk/cmake/pico-vscode.cmake)
if (EXISTS ${picoVscode})
include(${picoVscode})
endif()
# ====================================================================================
set(PICO_BOARD pico CACHE STRING "Board type")
set(TOP ${PICO_TINYUSB_PATH})
# Pull in Raspberry Pi Pico SDK (must be before project)
include(${PICO_SDK_PATH}/external/pico_sdk_import.cmake)
project(usb-test C CXX ASM)
# Initialise the Raspberry Pi Pico SDK
pico_sdk_init()
# Add executable. Default name is the project name, version 0.1
add_executable(usb-test
src/quadrature.c
src/usb-test.c
src/tusb_descriptors.c
)
pico_set_program_name(usb-test "usb-test")
pico_set_program_version(usb-test "0.1")
# Modify the below lines to enable/disable output over UART/USB
pico_enable_stdio_uart(usb-test 0)
pico_enable_stdio_usb(usb-test 1)
pico_generate_pio_header(usb-test ${CMAKE_CURRENT_LIST_DIR}/src/quadrature.pio)
# Add the standard library to the build
target_link_libraries(usb-test
pico_stdlib
pico_multicore
pico_time
hardware_pio
hardware_clocks
hardware_gpio
hardware_sync
tinyusb_board
tinyusb_device)
# Add the standard include files to the build
target_include_directories(usb-test PRIVATE
${CMAKE_CURRENT_LIST_DIR}
src/
)
pico_add_extra_outputs(usb-test)

84
firmware/usb-test/pico_sdk_import.cmake Normal file
View File

@ -0,0 +1,84 @@
# 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()
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 ()
# GIT_SUBMODULES_RECURSE was added in 3.17
if (${CMAKE_VERSION} VERSION_GREATER_EQUAL "3.17.0")
FetchContent_Declare(
pico_sdk
GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk
GIT_TAG ${PICO_SDK_FETCH_FROM_GIT_TAG}
GIT_SUBMODULES_RECURSE FALSE
)
else ()
FetchContent_Declare(
pico_sdk
GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk
GIT_TAG ${PICO_SDK_FETCH_FROM_GIT_TAG}
)
endif ()
if (NOT pico_sdk)
message("Downloading Raspberry Pi Pico SDK")
FetchContent_Populate(pico_sdk)
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})

84
firmware/usb-test/src/pico_sdk_import.cmake Normal file
View File

@ -0,0 +1,84 @@
# 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()
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 ()
# GIT_SUBMODULES_RECURSE was added in 3.17
if (${CMAKE_VERSION} VERSION_GREATER_EQUAL "3.17.0")
FetchContent_Declare(
pico_sdk
GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk
GIT_TAG ${PICO_SDK_FETCH_FROM_GIT_TAG}
GIT_SUBMODULES_RECURSE FALSE
)
else ()
FetchContent_Declare(
pico_sdk
GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk
GIT_TAG ${PICO_SDK_FETCH_FROM_GIT_TAG}
)
endif ()
if (NOT pico_sdk)
message("Downloading Raspberry Pi Pico SDK")
FetchContent_Populate(pico_sdk)
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})

184
firmware/usb-test/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/usb-test/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);

208
firmware/usb-test/src/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);
}
%}

144
firmware/usb-test/src/quadrature.pio.h Normal file
View File

@ -0,0 +1,144 @@
// -------------------------------------------------- //
// This file is autogenerated by pioasm; do not edit! //
// -------------------------------------------------- //
#pragma once
#if !PICO_NO_HARDWARE
#include "hardware/pio.h"
#endif
// -------------------------- //
// quadrature_encoder_substep //
// -------------------------- //
#define quadrature_encoder_substep_wrap_target 9
#define quadrature_encoder_substep_wrap 14
#define quadrature_encoder_substep_pio_version 0
static const uint16_t quadrature_encoder_substep_program_instructions[] = {
0x4020, // 0: in x, 32
0x4040, // 1: in y, 32
0x60c2, // 2: out isr, 2
0x4002, // 3: in pins, 2
0xa0ee, // 4: mov osr, !isr
0xa0a7, // 5: mov pc, osr
0x0087, // 6: jmp y--, 7
0xe021, // 7: set x, 1
0xa031, // 8: mov x, ::x
// .wrap_target
0x004a, // 9: jmp x--, 10
0xa0ad, // 10: mov pc, !status
0xa04a, // 11: mov y, !y
0x008d, // 12: jmp y--, 13
0xa04a, // 13: mov y, !y
0xe020, // 14: set x, 0
// .wrap
0x0002, // 15: jmp 2
0x000f, // 16: jmp 15
0x000b, // 17: jmp 11
0x0106, // 18: jmp 6 [1]
0x0409, // 19: jmp 9 [4]
0x0106, // 20: jmp 6 [1]
0x000f, // 21: jmp 15
0x0409, // 22: jmp 9 [4]
0x000b, // 23: jmp 11
0x000b, // 24: jmp 11
0x0409, // 25: jmp 9 [4]
0x000f, // 26: jmp 15
0x0106, // 27: jmp 6 [1]
0x0409, // 28: jmp 9 [4]
0x0106, // 29: jmp 6 [1]
0x000b, // 30: jmp 11
0x0102, // 31: jmp 2 [1]
};
#if !PICO_NO_HARDWARE
static const struct pio_program quadrature_encoder_substep_program = {
.instructions = quadrature_encoder_substep_program_instructions,
.length = 32,
.origin = 0,
.pio_version = quadrature_encoder_substep_pio_version,
#if PICO_PIO_VERSION > 0
.used_gpio_ranges = 0x0
#endif
};
static inline pio_sm_config quadrature_encoder_substep_program_get_default_config(uint offset) {
pio_sm_config c = pio_get_default_sm_config();
sm_config_set_wrap(&c, offset + quadrature_encoder_substep_wrap_target, offset + quadrature_encoder_substep_wrap);
return c;
}
#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);
}
#endif

42
firmware/usb-test/src/tusb_config.h Normal file
View File

@ -0,0 +1,42 @@
#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
#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_ENDPOINT0_SIZE
#define CFG_TUD_ENDPOINT0_SIZE 64
#endif
#define CFG_TUD_CDC 1
#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

132
firmware/usb-test/src/tusb_descriptors.c Normal file
View File

@ -0,0 +1,132 @@
#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_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
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)
};
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
};
char const *string_desc_arr[] = {
(const char[]) { 0x09, 0x04},
"Mg Robotics",
"Magrob Odometry MCU",
NULL,
"Odometry 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;
}

158
firmware/usb-test/src/usb-test.c Normal file
View File

@ -0,0 +1,158 @@
#include <stdio.h>
#include <math.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"
//===================== CONFIG =========================
#define ENCODER_LEFT_PIN_A 14 // Lupio sam ove vrednosti
#define ENCODER_LEFT_PIN_B 15
#define ENCODER_RIGHT_PIN_A 18
#define ENCODER_RIGHT_PIN_B 19
#define ENCODER_CPR 3840
#define WHEEL_RADIUS 0.035
#define WHEEL_SEPRATATION 0.04
#define TIMER_CYCLE_US 1000
//======================================================
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 uint prev_time;
static int prev_position_l = 0;
static int prev_position_r = 0;
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;
vel_r *= -WHEEL_RADIUS * 2 * M_PI;
const double linear = (vel_l + vel_r) / 2;
const double angular = (vel_r - vel_l) / WHEEL_SEPRATATION;
if(fabs(angular) < 1e-6) {
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;
const double r = linear / angular;
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(pio0, &quadrature_encoder_substep_program);
substep_init_state(pio0, 0, ENCODER_LEFT_PIN_A , &state_l);
substep_init_state(pio0, 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;
while (true) {
int ch;
if(( ch = stdio_getchar_timeout_us(0)) != PICO_ERROR_TIMEOUT) {
cmd = (cmd << 8) | ch;
if(cmd == (((uint16_t)'g' << 8) | ';')) {
printf("%lf %lf %lf\n", base_x, base_y, base_theta);
cmd = 0;
} else if(cmd == (((uint16_t)'z' << 8) | ';')) {
zero();
}
}
update_pos_cb();
sleep_ms(1);
}
}
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);
}
}
int main()
{
run_init();
multicore_launch_core1(core2_entry);
while (true) {
tud_task();
sleep_ms(1);
}
}

57
mg_odometry/CMakeLists.txt Normal file
View File

@ -0,0 +1,57 @@
cmake_minimum_required(VERSION 3.8)
project(mg_odometry)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2 REQUIRED)
include(FindPkgConfig)
pkg_search_module(LIBSERIAL REQUIRED libserial)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
add_executable(mg_odom_publisher src/mg_odom_publisher.cpp)
ament_target_dependencies(
mg_odom_publisher
tf2
tf2_ros
rclcpp
)
target_include_directories(mg_odom_publisher PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include/${PROJECT_NAME}>
${LIBSERIAL_INCLUDE_DIRS})
target_link_libraries(
mg_odom_publisher
${LIBSERIAL_LIBRARIES}
)
target_compile_features(mg_odom_publisher PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17
install(TARGETS mg_odom_publisher
DESTINATION lib/${PROJECT_NAME})
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# comment the line when a copyright and license is added to all source files
set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# comment the line when this package is in a git repo and when
# a copyright and license is added to all source files
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()

23
mg_odometry/package.xml Normal file
View File

@ -0,0 +1,23 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>mg_odometry</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="82343504+Pimpest@users.noreply.github.com">petar</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>libserial-dev</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

91
mg_odometry/src/mg_odom_publisher.cpp Normal file
View File

@ -0,0 +1,91 @@
#include <chrono>
#include <functional>
#include <memory>
#include <string>
#include <libserial/SerialStream.h>
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/static_transform_broadcaster.h"
#define ENCODER_SERIAL_PATH_DEFAULT \
"/dev/serial/by-id/usb-Mg_Robotics_Magrob_Odometry_MCU_E661AC8863809024-if00"
class MgOdomPublisher : public rclcpp::Node {
public:
MgOdomPublisher()
: Node("mg_odom_publisher")
{
this->declare_parameter("odom", "odom_encoder");
this->declare_parameter("target", "base-link");
this->declare_parameter("serial_path", ENCODER_SERIAL_PATH_DEFAULT);
tf_static_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
timer_ = this->create_wall_timer(
std::chrono::milliseconds(10), std::bind(&MgOdomPublisher::timer_callback, this));
}
private:
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_static_broadcaster_;
rclcpp::TimerBase::SharedPtr timer_;
LibSerial::SerialStream enc_serial_port_;
void timer_callback()
{
RCLCPP_INFO(this->get_logger(), "Callback called");
try {
if(!enc_serial_port_.IsOpen()) {
enc_serial_port_.Open(this->get_parameter("serial_path").as_string());
}
enc_serial_port_ << "g;";
double _x,_y,_theta;
if(!(enc_serial_port_ >> _x >> _y >> _theta)) {
RCLCPP_INFO(this->get_logger(), "Reading Serial Port failed. Closing...");
throw "Unfortunately bug with libserial";
}
RCLCPP_DEBUG(this->get_logger(), "Got following from rpi:{ x: %lf, y: %lf, z: %lf}\n", _x, _y, _theta);
make_transform(_x,_y,_theta);
} catch(const std::exception& e) {
RCLCPP_ERROR(this->get_logger(), "%s\n", e.what());
sleep(1);
}
}
void make_transform(double x, double y, double theta)
{
geometry_msgs::msg::TransformStamped t;
t.header.stamp = this->get_clock()->now();
t.header.frame_id = this->get_parameter("odom").as_string();
t.child_frame_id = this->get_parameter("target").as_string();
t.transform.translation.x = x;
t.transform.translation.y = y;
t.transform.translation.z = 0.0;
tf2::Quaternion q;
q.setRPY(0.0, 0.0, theta);
t.transform.rotation.x = q.x();
t.transform.rotation.y = q.y();
t.transform.rotation.z = q.z();
t.transform.rotation.w = q.w();
tf_static_broadcaster_->sendTransform(t);
}
};
int main(const int argc,const char ** argv)
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<MgOdomPublisher>());
rclcpp::shutdown();
return 0;
}

1
udev/97-magrob-encoder.rules Normal file
View File

@ -0,0 +1 @@
SUBSYSTEM=="tty", ATTRS{idVendor}=="1209", ATTRS{idProduct}=="4d47", MODE="0666"