From fff7576e18d3b0713ed5949645cdaf57ea740bb4 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+Pimpest@users.noreply.github.com> Date: Wed, 18 Dec 2024 12:29:19 +0100 Subject: [PATCH] Initial commit --- .gitignore | 4 + .vscode/c_cpp_properties.json | 21 ++ firmware/COLCON_IGNORE | 0 firmware/usb-test/.COLCON_IGNORE | 0 firmware/usb-test/.gitignore | 1 + .../usb-test/.vscode/c_cpp_properties.json | 22 ++ firmware/usb-test/.vscode/cmake-kits.json | 15 ++ firmware/usb-test/.vscode/extensions.json | 9 + firmware/usb-test/.vscode/launch.json | 70 ++++++ firmware/usb-test/.vscode/settings.json | 39 ++++ firmware/usb-test/.vscode/tasks.json | 58 +++++ firmware/usb-test/CMakeLists.txt | 89 ++++++++ firmware/usb-test/pico_sdk_import.cmake | 84 +++++++ firmware/usb-test/src/pico_sdk_import.cmake | 84 +++++++ firmware/usb-test/src/quadrature.c | 184 ++++++++++++++++ firmware/usb-test/src/quadrature.h | 27 +++ firmware/usb-test/src/quadrature.pio | 208 ++++++++++++++++++ firmware/usb-test/src/quadrature.pio.h | 144 ++++++++++++ firmware/usb-test/src/tusb_config.h | 42 ++++ firmware/usb-test/src/tusb_descriptors.c | 132 +++++++++++ firmware/usb-test/src/usb-test.c | 158 +++++++++++++ mg_odometry/CMakeLists.txt | 57 +++++ mg_odometry/package.xml | 23 ++ mg_odometry/src/mg_odom_publisher.cpp | 91 ++++++++ udev/97-magrob-encoder.rules | 1 + 25 files changed, 1563 insertions(+) create mode 100644 .gitignore create mode 100644 .vscode/c_cpp_properties.json create mode 100644 firmware/COLCON_IGNORE create mode 100644 firmware/usb-test/.COLCON_IGNORE create mode 100644 firmware/usb-test/.gitignore create mode 100644 firmware/usb-test/.vscode/c_cpp_properties.json create mode 100644 firmware/usb-test/.vscode/cmake-kits.json create mode 100644 firmware/usb-test/.vscode/extensions.json create mode 100644 firmware/usb-test/.vscode/launch.json create mode 100644 firmware/usb-test/.vscode/settings.json create mode 100644 firmware/usb-test/.vscode/tasks.json create mode 100644 firmware/usb-test/CMakeLists.txt create mode 100644 firmware/usb-test/pico_sdk_import.cmake create mode 100644 firmware/usb-test/src/pico_sdk_import.cmake create mode 100644 firmware/usb-test/src/quadrature.c create mode 100644 firmware/usb-test/src/quadrature.h create mode 100644 firmware/usb-test/src/quadrature.pio create mode 100644 firmware/usb-test/src/quadrature.pio.h create mode 100644 firmware/usb-test/src/tusb_config.h create mode 100644 firmware/usb-test/src/tusb_descriptors.c create mode 100644 firmware/usb-test/src/usb-test.c create mode 100644 mg_odometry/CMakeLists.txt create mode 100644 mg_odometry/package.xml create mode 100644 mg_odometry/src/mg_odom_publisher.cpp create mode 100644 udev/97-magrob-encoder.rules diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..556525f --- /dev/null +++ b/.gitignore @@ -0,0 +1,4 @@ +build +install +firmware/usb-test/build +firmware/usb-test/install diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..4674311 --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -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 +} \ No newline at end of file diff --git a/firmware/COLCON_IGNORE b/firmware/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/firmware/usb-test/.COLCON_IGNORE b/firmware/usb-test/.COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/firmware/usb-test/.gitignore b/firmware/usb-test/.gitignore new file mode 100644 index 0000000..378eac2 --- /dev/null +++ b/firmware/usb-test/.gitignore @@ -0,0 +1 @@ +build diff --git a/firmware/usb-test/.vscode/c_cpp_properties.json b/firmware/usb-test/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..b24aa9f --- /dev/null +++ b/firmware/usb-test/.vscode/c_cpp_properties.json @@ -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 +} diff --git a/firmware/usb-test/.vscode/cmake-kits.json b/firmware/usb-test/.vscode/cmake-kits.json new file mode 100644 index 0000000..b0f3815 --- /dev/null +++ b/firmware/usb-test/.vscode/cmake-kits.json @@ -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}" + } + } +] \ No newline at end of file diff --git a/firmware/usb-test/.vscode/extensions.json b/firmware/usb-test/.vscode/extensions.json new file mode 100644 index 0000000..96867a8 --- /dev/null +++ b/firmware/usb-test/.vscode/extensions.json @@ -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" + ] +} \ No newline at end of file diff --git a/firmware/usb-test/.vscode/launch.json b/firmware/usb-test/.vscode/launch.json new file mode 100644 index 0000000..d921c35 --- /dev/null +++ b/firmware/usb-test/.vscode/launch.json @@ -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" + }, + ] +} diff --git a/firmware/usb-test/.vscode/settings.json b/firmware/usb-test/.vscode/settings.json new file mode 100644 index 0000000..992389d --- /dev/null +++ b/firmware/usb-test/.vscode/settings.json @@ -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" +} diff --git a/firmware/usb-test/.vscode/tasks.json b/firmware/usb-test/.vscode/tasks.json new file mode 100644 index 0000000..1b6a876 --- /dev/null +++ b/firmware/usb-test/.vscode/tasks.json @@ -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", + } + } + ] +} diff --git a/firmware/usb-test/CMakeLists.txt b/firmware/usb-test/CMakeLists.txt new file mode 100644 index 0000000..70d91f9 --- /dev/null +++ b/firmware/usb-test/CMakeLists.txt @@ -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) + diff --git a/firmware/usb-test/pico_sdk_import.cmake b/firmware/usb-test/pico_sdk_import.cmake new file mode 100644 index 0000000..a0721d0 --- /dev/null +++ b/firmware/usb-test/pico_sdk_import.cmake @@ -0,0 +1,84 @@ +# 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() + +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}) diff --git a/firmware/usb-test/src/pico_sdk_import.cmake b/firmware/usb-test/src/pico_sdk_import.cmake new file mode 100644 index 0000000..a0721d0 --- /dev/null +++ b/firmware/usb-test/src/pico_sdk_import.cmake @@ -0,0 +1,84 @@ +# 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() + +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}) diff --git a/firmware/usb-test/src/quadrature.c b/firmware/usb-test/src/quadrature.c new file mode 100644 index 0000000..b2c3965 --- /dev/null +++ b/firmware/usb-test/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/usb-test/src/quadrature.h b/firmware/usb-test/src/quadrature.h new file mode 100644 index 0000000..44e2c1b --- /dev/null +++ b/firmware/usb-test/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/usb-test/src/quadrature.pio b/firmware/usb-test/src/quadrature.pio new file mode 100644 index 0000000..f86b98d --- /dev/null +++ b/firmware/usb-test/src/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/usb-test/src/quadrature.pio.h b/firmware/usb-test/src/quadrature.pio.h new file mode 100644 index 0000000..91636a7 --- /dev/null +++ b/firmware/usb-test/src/quadrature.pio.h @@ -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 + diff --git a/firmware/usb-test/src/tusb_config.h b/firmware/usb-test/src/tusb_config.h new file mode 100644 index 0000000..7c41b14 --- /dev/null +++ b/firmware/usb-test/src/tusb_config.h @@ -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 diff --git a/firmware/usb-test/src/tusb_descriptors.c b/firmware/usb-test/src/tusb_descriptors.c new file mode 100644 index 0000000..a7900fb --- /dev/null +++ b/firmware/usb-test/src/tusb_descriptors.c @@ -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; +} \ No newline at end of file diff --git a/firmware/usb-test/src/usb-test.c b/firmware/usb-test/src/usb-test.c new file mode 100644 index 0000000..ec211bd --- /dev/null +++ b/firmware/usb-test/src/usb-test.c @@ -0,0 +1,158 @@ +#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" + +//===================== 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); + } +} diff --git a/mg_odometry/CMakeLists.txt b/mg_odometry/CMakeLists.txt new file mode 100644 index 0000000..0794779 --- /dev/null +++ b/mg_odometry/CMakeLists.txt @@ -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( 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 + $ + $ + ${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() diff --git a/mg_odometry/package.xml b/mg_odometry/package.xml new file mode 100644 index 0000000..2f453ed --- /dev/null +++ b/mg_odometry/package.xml @@ -0,0 +1,23 @@ + + + + mg_odometry + 0.0.0 + TODO: Package description + petar + TODO: License declaration + + ament_cmake + + rclcpp + tf2 + tf2_ros + libserial-dev + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/mg_odometry/src/mg_odom_publisher.cpp b/mg_odometry/src/mg_odom_publisher.cpp new file mode 100644 index 0000000..d43b3b7 --- /dev/null +++ b/mg_odometry/src/mg_odom_publisher.cpp @@ -0,0 +1,91 @@ +#include +#include +#include +#include +#include + +#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(this); + + timer_ = this->create_wall_timer( + std::chrono::milliseconds(10), std::bind(&MgOdomPublisher::timer_callback, this)); + } + +private: + std::shared_ptr 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()); + rclcpp::shutdown(); + return 0; +} diff --git a/udev/97-magrob-encoder.rules b/udev/97-magrob-encoder.rules new file mode 100644 index 0000000..0f23b24 --- /dev/null +++ b/udev/97-magrob-encoder.rules @@ -0,0 +1 @@ +SUBSYSTEM=="tty", ATTRS{idVendor}=="1209", ATTRS{idProduct}=="4d47", MODE="0666" \ No newline at end of file