From c136056ed24e3cc207f0a701dce283f06163577a Mon Sep 17 00:00:00 2001 From: AlexanderHD27 Date: Wed, 1 Jan 2025 16:47:53 +0100 Subject: [PATCH] Got homing sequenc to work, ....again --- .gitmodules | 4 +- i2c-hub/backend/Protocol.md | 4 +- .../backend/src/gobotRPC_ctrl_interface.py | 3 + i2c-hub/backend/src/main.py | 0 i2c-hub/backend/test.txt | 9 + .../i2c-hub-2024-12-30_211118.zip | 3 + i2c-hub/circuit/i2c-hub/i2c-hub.kicad_prl | 3 + i2c-hub/circuit/i2c-hub/i2c-hub.kicad_pro | 4 +- .../ctrl_interface/ctrl_interface_tx.cpp | 2 +- .../i2c-hub-firmware/src/gobotrpc/rx.cpp | 3 +- .../firmware/i2c-hub-firmware/src/main.cpp | 5 +- motor-control/core-xy-firmware/.gitignore | 3 + .../.vscode/c_cpp_properties.json | 22 + .../core-xy-firmware/.vscode/cmake-kits.json | 15 + .../core-xy-firmware/.vscode/extensions.json | 9 + .../core-xy-firmware/.vscode/launch.json | 70 +++ .../core-xy-firmware/.vscode/settings.json | 39 ++ .../core-xy-firmware/.vscode/tasks.json | 94 ++++ motor-control/core-xy-firmware/CMakeLists.txt | 83 +++ .../core-xy-firmware/FreeRTOSConfig.h | 143 +++++ motor-control/core-xy-firmware/blink.pio | 34 ++ .../cmake/FreeRTOS_Kernel_import.cmake | 61 +++ .../core-xy-firmware/cmake/tmc2209.cmake | 19 + .../core-xy-firmware/include/CoreXY.hpp | 57 ++ .../include/tmc2209/registers_map.hpp | 517 ++++++++++++++++++ .../core-xy-firmware/include/tmc2209/step.hpp | 80 +++ .../include/tmc2209/tmc2209.hpp | 125 +++++ .../core-xy-firmware/lib/FreeRTOS-Kernel | 1 + .../core-xy-firmware/pico_sdk_import.cmake | 84 +++ .../core-xy-firmware/src/limitSwitches.cpp | 123 +++++ motor-control/core-xy-firmware/src/main.cpp | 316 +++++++++++ motor-control/core-xy-firmware/src/motors.cpp | 153 ++++++ .../core-xy-firmware/src/tmc2209/pulser.pio | 43 ++ .../core-xy-firmware/src/tmc2209/step.cpp | 261 +++++++++ .../src/tmc2209/uart_interface.cpp | 80 +++ .../src/tmc2209/uart_registers.cpp | 1 + 36 files changed, 2464 insertions(+), 9 deletions(-) create mode 100644 i2c-hub/backend/src/gobotRPC_ctrl_interface.py create mode 100644 i2c-hub/backend/src/main.py create mode 100644 i2c-hub/backend/test.txt create mode 100644 i2c-hub/circuit/i2c-hub/i2c-hub-backups/i2c-hub-2024-12-30_211118.zip create mode 100644 i2c-hub/circuit/i2c-hub/i2c-hub.kicad_prl create mode 100644 motor-control/core-xy-firmware/.gitignore create mode 100644 motor-control/core-xy-firmware/.vscode/c_cpp_properties.json create mode 100644 motor-control/core-xy-firmware/.vscode/cmake-kits.json create mode 100644 motor-control/core-xy-firmware/.vscode/extensions.json create mode 100644 motor-control/core-xy-firmware/.vscode/launch.json create mode 100644 motor-control/core-xy-firmware/.vscode/settings.json create mode 100644 motor-control/core-xy-firmware/.vscode/tasks.json create mode 100644 motor-control/core-xy-firmware/CMakeLists.txt create mode 100644 motor-control/core-xy-firmware/FreeRTOSConfig.h create mode 100644 motor-control/core-xy-firmware/blink.pio create mode 100644 motor-control/core-xy-firmware/cmake/FreeRTOS_Kernel_import.cmake create mode 100644 motor-control/core-xy-firmware/cmake/tmc2209.cmake create mode 100644 motor-control/core-xy-firmware/include/CoreXY.hpp create mode 100644 motor-control/core-xy-firmware/include/tmc2209/registers_map.hpp create mode 100644 motor-control/core-xy-firmware/include/tmc2209/step.hpp create mode 100644 motor-control/core-xy-firmware/include/tmc2209/tmc2209.hpp create mode 160000 motor-control/core-xy-firmware/lib/FreeRTOS-Kernel create mode 100644 motor-control/core-xy-firmware/pico_sdk_import.cmake create mode 100644 motor-control/core-xy-firmware/src/limitSwitches.cpp create mode 100755 motor-control/core-xy-firmware/src/main.cpp create mode 100644 motor-control/core-xy-firmware/src/motors.cpp create mode 100644 motor-control/core-xy-firmware/src/tmc2209/pulser.pio create mode 100644 motor-control/core-xy-firmware/src/tmc2209/step.cpp create mode 100644 motor-control/core-xy-firmware/src/tmc2209/uart_interface.cpp create mode 100644 motor-control/core-xy-firmware/src/tmc2209/uart_registers.cpp diff --git a/.gitmodules b/.gitmodules index e3b8c76..27a7814 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:c3885f497d57d3895555d57e5b538d3230c1e808422cd46b306b11764946093f -size 1376 +oid sha256:02f59f9e47316fce24bc6d45d9676f1bd48fac3b69c51cfbe9dd202a0ddf8303 +size 1555 diff --git a/i2c-hub/backend/Protocol.md b/i2c-hub/backend/Protocol.md index 9f7d9de..03b0d9d 100644 --- a/i2c-hub/backend/Protocol.md +++ b/i2c-hub/backend/Protocol.md @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:1bc599a9a6b30e3b5964f335ec9671c7162ca1fcdf4949b1be726e87e1a04f2b -size 2319 +oid sha256:ab3d0b46f6aab6af43eaf116f560f9418215737437685e2ac3dd3dd3f36ce0eb +size 2307 diff --git a/i2c-hub/backend/src/gobotRPC_ctrl_interface.py b/i2c-hub/backend/src/gobotRPC_ctrl_interface.py new file mode 100644 index 0000000..56cd50a --- /dev/null +++ b/i2c-hub/backend/src/gobotRPC_ctrl_interface.py @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6603666c8610079092cf2b5ff12e322c819ea91082a39db89ff197441d88e143 +size 4543 diff --git a/i2c-hub/backend/src/main.py b/i2c-hub/backend/src/main.py new file mode 100644 index 0000000..e69de29 diff --git a/i2c-hub/backend/test.txt b/i2c-hub/backend/test.txt new file mode 100644 index 0000000..1763faf --- /dev/null +++ b/i2c-hub/backend/test.txt @@ -0,0 +1,9 @@ +fe fe fe fe fc 10 10 10 20 ff 48 65 6c 6c 6f 20 +6f 72 6c 64 21 0a 84 00 01 00 42 00 00 00 12 +00 00 00 c0 84 01 01 01 43 00 00 00 22 00 00 +3f 00 84 00 01 00 42 00 00 00 32 00 00 00 80 +54 68 69 73 20 69 73 20 52 61 6e 64 6f 6d 20 +65 78 74 0a 84 01 01 02 43 00 00 00 63 00 00 +3f 00 84 01 01 00 42 00 00 00 73 00 00 00 bf +81 10 40 42 00 00 00 01 02 03 04 05 06 00 11 +13 14 fe fe fe fe fe fe fc \ No newline at end of file diff --git a/i2c-hub/circuit/i2c-hub/i2c-hub-backups/i2c-hub-2024-12-30_211118.zip b/i2c-hub/circuit/i2c-hub/i2c-hub-backups/i2c-hub-2024-12-30_211118.zip new file mode 100644 index 0000000..b1feee6 --- /dev/null +++ b/i2c-hub/circuit/i2c-hub/i2c-hub-backups/i2c-hub-2024-12-30_211118.zip @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9edd1b0152c012ccb02bf0baad030f82cf3240f63724405c13759c172be7bf16 +size 8838 diff --git a/i2c-hub/circuit/i2c-hub/i2c-hub.kicad_prl b/i2c-hub/circuit/i2c-hub/i2c-hub.kicad_prl new file mode 100644 index 0000000..5f2911b --- /dev/null +++ b/i2c-hub/circuit/i2c-hub/i2c-hub.kicad_prl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:417cfd4eab4641e2ba537e76a376e67ace3cad9c6496d90c9ba01e323a5f70e8 +size 1275 diff --git a/i2c-hub/circuit/i2c-hub/i2c-hub.kicad_pro b/i2c-hub/circuit/i2c-hub/i2c-hub.kicad_pro index 40ec239..2a0c3b8 100644 --- a/i2c-hub/circuit/i2c-hub/i2c-hub.kicad_pro +++ b/i2c-hub/circuit/i2c-hub/i2c-hub.kicad_pro @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:15a17e27a30b0284403a4c96c3b73578ded4be8722e3db25d13fda196e81de87 -size 552 +oid sha256:5834e9a4be48d7235dbad303e84616bc496f322d166fff150b0c8746c4a4229e +size 1644 diff --git a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/ctrl_interface/ctrl_interface_tx.cpp b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/ctrl_interface/ctrl_interface_tx.cpp index a244c34..d374c73 100644 --- a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/ctrl_interface/ctrl_interface_tx.cpp +++ b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/ctrl_interface/ctrl_interface_tx.cpp @@ -6,7 +6,7 @@ void GobotRPC_CtrlInterface::pushPacket(RPCPackage package) { char buffer[MAX_PAGE_SIZES + 7]; buffer[0] = GOBOTRPC_CTRL_CMD_PACKET; - buffer[1] = package.length; + buffer[1] = package.length - 7; buffer[2] = package.type | (package.rpcNum << 4); memcpy(&buffer[3], &package.addr, 4); memcpy(&buffer[7], package.buffer, package.length); diff --git a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/rx.cpp b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/rx.cpp index c74852d..042623f 100644 --- a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/rx.cpp +++ b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/rx.cpp @@ -56,7 +56,7 @@ FrameSubmitionResult GobotRPCParser::submit_frame(char *data, size_t length, uin // Push the update to the control interface ctrlInterface->pushPackageSlotUpdate(res == COMPLEATE, this->buffer[slotNum].is_in_use, slotNum, addr, timestamp, this->buffer[slotNum].used_bit_masked); - if (res == COMPLEATE) { +y if (res == COMPLEATE) { this->buffer[slotNum].is_complete = true; } @@ -118,6 +118,7 @@ void GobotRPCParser::retrivePackage(RPCPackage *dest, int index) { dest->type = buffer[index].type; dest->length = buffer[index].length; + this->ctrlInterface->pushPacket(*dest); freePackageSlot(index); } diff --git a/i2c-hub/firmware/i2c-hub-firmware/src/main.cpp b/i2c-hub/firmware/i2c-hub-firmware/src/main.cpp index e67f73c..8e5333a 100644 --- a/i2c-hub/firmware/i2c-hub-firmware/src/main.cpp +++ b/i2c-hub/firmware/i2c-hub-firmware/src/main.cpp @@ -12,7 +12,7 @@ #include "ctrl_interface_hardware.hpp" void vTaskMain(void * args) { - GobotRPC_CtrlInterface_HardwareHandler_RP2040 uartHandler(uart0, 115200, 500, 0, 1); + GobotRPC_CtrlInterface_HardwareHandler_RP2040 uartHandler(uart0, 9600, 1500, 0, 1); GobotRPC_CtrlInterface ctrlInterface(&uartHandler); GobotRPCParser rpcRXParser(&ctrlInterface); @@ -27,6 +27,9 @@ void vTaskMain(void * args) { rpcRXParser.submit_frame(data3, 7, 0x43, xTaskGetTickCount()); rpcRXParser.submit_frame(data1, 7, 0x42, xTaskGetTickCount()); + + ctrlInterface.printf("This is Random Text\n"); + vTaskDelay(10 / portTICK_PERIOD_MS); rpcRXParser.submit_frame(data3, 7, 0x43, xTaskGetTickCount()); diff --git a/motor-control/core-xy-firmware/.gitignore b/motor-control/core-xy-firmware/.gitignore new file mode 100644 index 0000000..72fda45 --- /dev/null +++ b/motor-control/core-xy-firmware/.gitignore @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f10ec3ffef16a8dace9b58801e0da28819c98af4c8933410d7e7cacc952f9e9b +size 6 diff --git a/motor-control/core-xy-firmware/.vscode/c_cpp_properties.json b/motor-control/core-xy-firmware/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..b24aa9f --- /dev/null +++ b/motor-control/core-xy-firmware/.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/motor-control/core-xy-firmware/.vscode/cmake-kits.json b/motor-control/core-xy-firmware/.vscode/cmake-kits.json new file mode 100644 index 0000000..b0f3815 --- /dev/null +++ b/motor-control/core-xy-firmware/.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/motor-control/core-xy-firmware/.vscode/extensions.json b/motor-control/core-xy-firmware/.vscode/extensions.json new file mode 100644 index 0000000..a940d7c --- /dev/null +++ b/motor-control/core-xy-firmware/.vscode/extensions.json @@ -0,0 +1,9 @@ +{ + "recommendations": [ + "marus25.cortex-debug", + "ms-vscode.cpptools", + "ms-vscode.cpptools-extension-pack", + "ms-vscode.vscode-serial-monitor", + "raspberry-pi.raspberry-pi-pico" + ] +} \ No newline at end of file diff --git a/motor-control/core-xy-firmware/.vscode/launch.json b/motor-control/core-xy-firmware/.vscode/launch.json new file mode 100644 index 0000000..d921c35 --- /dev/null +++ b/motor-control/core-xy-firmware/.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/motor-control/core-xy-firmware/.vscode/settings.json b/motor-control/core-xy-firmware/.vscode/settings.json new file mode 100644 index 0000000..6a94325 --- /dev/null +++ b/motor-control/core-xy-firmware/.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": true, + "cmake.automaticReconfigure": true, + "cmake.configureOnOpen": true, + "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": false, + "raspberry-pi-pico.useCmakeTools": true, + "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/motor-control/core-xy-firmware/.vscode/tasks.json b/motor-control/core-xy-firmware/.vscode/tasks.json new file mode 100644 index 0000000..e4ec47f --- /dev/null +++ b/motor-control/core-xy-firmware/.vscode/tasks.json @@ -0,0 +1,94 @@ +{ + "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", + "focus": true, + "clear": true + }, + "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", + "-c", "reset halt", + "-c", "rp2040.core1 arp_reset assert 0", + "-c", "rp2040.core0 arp_reset assert 0", + "-c", "exit" + ], + "dependsOn": [ + "Compile Project" + ], + "problemMatcher": [], + "windows": { + "command": "${env:USERPROFILE}/.pico-sdk/openocd/0.12.0+dev/openocd.exe", + } + }, + { + "label": "Reset", + "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; init; reset halt;", + "-c", "rp2040.core1 arp_reset assert 0", + "-c", "rp2040.core0 arp_reset assert 0", + "-c", "exit" + ], + "presentation": { + "echo": true, + "reveal": "never", + "focus": false, + "panel": "shared", + "showReuseMessage": true, + "clear": true + }, + "problemMatcher": [], + "windows": { + "command": "${env:USERPROFILE}/.pico-sdk/openocd/0.12.0+dev/openocd.exe", + } + } + ] +} diff --git a/motor-control/core-xy-firmware/CMakeLists.txt b/motor-control/core-xy-firmware/CMakeLists.txt new file mode 100644 index 0000000..0bac787 --- /dev/null +++ b/motor-control/core-xy-firmware/CMakeLists.txt @@ -0,0 +1,83 @@ +# 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") + +# Pull in Raspberry Pi Pico SDK (must be before project) +include(pico_sdk_import.cmake) + +set(FREERTOS_KERNEL_PATH ${CMAKE_CURRENT_LIST_DIR}/lib/FreeRTOS-Kernel) +project(core-xy-firmware C CXX ASM) + +# Add FreeRTOS +include(cmake/FreeRTOS_Kernel_import.cmake) + +# Initialise the Raspberry Pi Pico SDK +pico_sdk_init() + +# Add executable. Default name is the project name, version 0.1 +include(cmake/tmc2209.cmake) + +add_executable(core-xy-firmware + src/main.cpp + src/limitSwitches.cpp + src/motors.cpp +) + +target_include_directories(core-xy-firmware PUBLIC + include/ + build/ + ./ +) + +pico_set_program_name(core-xy-firmware "core-xy-firmware") +pico_set_program_version(core-xy-firmware "0.1") + +# Generate PIO header +pico_generate_pio_header(core-xy-firmware ${CMAKE_CURRENT_LIST_DIR}/src/tmc2209/pulser.pio) + +# Modify the below lines to enable/disable output over UART/USB +pico_enable_stdio_uart(core-xy-firmware 0) +pico_enable_stdio_usb(core-xy-firmware 1) + +# Add any user requested libraries +target_link_libraries(core-xy-firmware + pico_stdlib + pico_time + pico_multicore + pico_stdio_usb + hardware_pio + + tmc2209_driver + FreeRTOS-Kernel-Heap4 +) + +# Add the standard include files to the build +target_include_directories(core-xy-firmware PRIVATE + ${CMAKE_CURRENT_LIST_DIR} +) + +pico_add_extra_outputs(core-xy-firmware) + diff --git a/motor-control/core-xy-firmware/FreeRTOSConfig.h b/motor-control/core-xy-firmware/FreeRTOSConfig.h new file mode 100644 index 0000000..601b4db --- /dev/null +++ b/motor-control/core-xy-firmware/FreeRTOSConfig.h @@ -0,0 +1,143 @@ +/* + * FreeRTOS V202111.00 + * Copyright (C) 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of + * this software and associated documentation files (the "Software"), to deal in + * the Software without restriction, including without limitation the rights to + * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of + * the Software, and to permit persons to whom the Software is furnished to do so, + * subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS + * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR + * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER + * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * + * http://www.FreeRTOS.org + * http://aws.amazon.com/freertos + * + * 1 tab == 4 spaces! + */ + +#ifndef FREERTOS_CONFIG_H +#define FREERTOS_CONFIG_H + +/*----------------------------------------------------------- + * Application specific definitions. + * + * These definitions should be adjusted for your particular hardware and + * application requirements. + * + * THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE + * FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. + * + * See http://www.freertos.org/a00110.html + *----------------------------------------------------------*/ + +/* Scheduler Related */ +#define configUSE_PREEMPTION 1 +#define configUSE_TICKLESS_IDLE 0 +#define configUSE_IDLE_HOOK 0 +#define configUSE_TICK_HOOK 0 +#define configTICK_RATE_HZ ( ( TickType_t ) 1000 ) +#define configMAX_PRIORITIES 32 +#define configMINIMAL_STACK_SIZE ( configSTACK_DEPTH_TYPE ) 256 +#define configUSE_16_BIT_TICKS 0 + +#define configIDLE_SHOULD_YIELD 1 + +/* Synchronization Related */ +#define configUSE_MUTEXES 1 +#define configUSE_RECURSIVE_MUTEXES 1 +#define configUSE_APPLICATION_TASK_TAG 0 +#define configUSE_COUNTING_SEMAPHORES 1 +#define configQUEUE_REGISTRY_SIZE 8 +#define configUSE_QUEUE_SETS 1 +#define configUSE_TIME_SLICING 1 +#define configUSE_NEWLIB_REENTRANT 0 +// todo need this for lwip FreeRTOS sys_arch to compile +#define configENABLE_BACKWARD_COMPATIBILITY 1 +#define configNUM_THREAD_LOCAL_STORAGE_POINTERS 5 + +/* System */ +#define configSTACK_DEPTH_TYPE uint32_t +#define configMESSAGE_BUFFER_LENGTH_TYPE size_t + +/* Memory allocation related definitions. */ +#define configSUPPORT_STATIC_ALLOCATION 0 +#define configSUPPORT_DYNAMIC_ALLOCATION 1 +#define configTOTAL_HEAP_SIZE (1024 * 64) +#define configAPPLICATION_ALLOCATED_HEAP (1024 * 32) + +/* Hook function related definitions. */ +#define configCHECK_FOR_STACK_OVERFLOW 0 +#define configUSE_MALLOC_FAILED_HOOK 0 +#define configUSE_DAEMON_TASK_STARTUP_HOOK 0 + +/* Run time and task stats gathering related definitions. */ +#define configGENERATE_RUN_TIME_STATS 0 +#define configUSE_TRACE_FACILITY 1 +#define configUSE_STATS_FORMATTING_FUNCTIONS 0 + +/* Co-routine related definitions. */ +#define configUSE_CO_ROUTINES 0 +#define configMAX_CO_ROUTINE_PRIORITIES 1 + +/* Software timer related definitions. */ +#define configUSE_TIMERS 1 +#define configTIMER_TASK_PRIORITY ( configMAX_PRIORITIES - 1 ) +#define configTIMER_QUEUE_LENGTH 10 +#define configTIMER_TASK_STACK_DEPTH 1024 + +/* Interrupt nesting behaviour configuration. */ +/* +#define configKERNEL_INTERRUPT_PRIORITY [dependent of processor] +#define configMAX_SYSCALL_INTERRUPT_PRIORITY [dependent on processor and application] +#define configMAX_API_CALL_INTERRUPT_PRIORITY [dependent on processor and application] +*/ + +#if FREE_RTOS_KERNEL_SMP // set by the RP2040 SMP port of FreeRTOS +/* SMP port only */ +#define configUSE_PASSIVE_IDLE_HOOK 0 +#define configNUMBER_OF_CORES 1 +#define configTICK_CORE 0 +#define configRUN_MULTIPLE_PRIORITIES 1 +//#define configUSE_CORE_AFFINITY 1 +#endif + +/* RP2040 specific */ +#define configSUPPORT_PICO_SYNC_INTEROP 1 +#define configSUPPORT_PICO_TIME_INTEROP 1 + +#include +/* Define to trap errors during development. */ +#define configASSERT(x) assert(x) + +/* Set the following definitions to 1 to include the API function, or zero +to exclude the API function. */ +#define INCLUDE_vTaskPrioritySet 1 +#define INCLUDE_uxTaskPriorityGet 1 +#define INCLUDE_vTaskDelete 1 +#define INCLUDE_vTaskSuspend 1 +#define INCLUDE_vTaskDelayUntil 1 +#define INCLUDE_vTaskDelay 1 +#define INCLUDE_xTaskGetSchedulerState 1 +#define INCLUDE_xTaskGetCurrentTaskHandle 1 +#define INCLUDE_uxTaskGetStackHighWaterMark 1 +#define INCLUDE_xTaskGetIdleTaskHandle 1 +#define INCLUDE_eTaskGetState 1 +#define INCLUDE_xTimerPendFunctionCall 1 +#define INCLUDE_xTaskAbortDelay 1 +#define INCLUDE_xTaskGetHandle 1 +#define INCLUDE_xTaskResumeFromISR 1 +#define INCLUDE_xQueueGetMutexHolder 1 + +/* A header file that defines trace macro can be included here. */ + +#endif /* FREERTOS_CONFIG_H */ \ No newline at end of file diff --git a/motor-control/core-xy-firmware/blink.pio b/motor-control/core-xy-firmware/blink.pio new file mode 100644 index 0000000..b174819 --- /dev/null +++ b/motor-control/core-xy-firmware/blink.pio @@ -0,0 +1,34 @@ +; +; Copyright (c) 2020 Raspberry Pi (Trading) Ltd. +; +; SPDX-License-Identifier: BSD-3-Clause +; + +; SET pin 0 should be mapped to your LED GPIO + +.program blink + pull block + out y, 32 +.wrap_target + mov x, y + set pins, 1 ; Turn LED on +lp1: + jmp x-- lp1 ; Delay for (x + 1) cycles, x is a 32 bit number + mov x, y + set pins, 0 ; Turn LED off +lp2: + jmp x-- lp2 ; Delay for the same number of cycles again +.wrap ; Blink forever! + + +% c-sdk { +// this is a raw helper function for use by the user which sets up the GPIO output, and configures the SM to output on a particular pin + +void blink_program_init(PIO pio, uint sm, uint offset, uint pin) { + pio_gpio_init(pio, pin); + pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, true); + pio_sm_config c = blink_program_get_default_config(offset); + sm_config_set_set_pins(&c, pin, 1); + pio_sm_init(pio, sm, offset, &c); +} +%} \ No newline at end of file diff --git a/motor-control/core-xy-firmware/cmake/FreeRTOS_Kernel_import.cmake b/motor-control/core-xy-firmware/cmake/FreeRTOS_Kernel_import.cmake new file mode 100644 index 0000000..109a54e --- /dev/null +++ b/motor-control/core-xy-firmware/cmake/FreeRTOS_Kernel_import.cmake @@ -0,0 +1,61 @@ +# This is a copy of /portable/ThirdParty/GCC/RP2040/FREERTOS_KERNEL_import.cmake + +# This can be dropped into an external project to help locate the FreeRTOS kernel +# It should be include()ed prior to project(). Alternatively this file may +# or the CMakeLists.txt in this directory may be included or added via add_subdirectory +# respectively. + +if (DEFINED ENV{FREERTOS_KERNEL_PATH} AND (NOT FREERTOS_KERNEL_PATH)) + set(FREERTOS_KERNEL_PATH $ENV{FREERTOS_KERNEL_PATH}) + message("Using FREERTOS_KERNEL_PATH from environment ('${FREERTOS_KERNEL_PATH}')") +endif () + +set(FREERTOS_KERNEL_RP2040_RELATIVE_PATH "portable/ThirdParty/GCC/RP2040") +# undo the above +set(FREERTOS_KERNEL_RP2040_BACK_PATH "../../../..") + +if (NOT FREERTOS_KERNEL_PATH) + # check if we are inside the FreeRTOS kernel tree (i.e. this file has been included directly) + get_filename_component(_ACTUAL_PATH ${CMAKE_CURRENT_LIST_DIR} REALPATH) + get_filename_component(_POSSIBLE_PATH ${CMAKE_CURRENT_LIST_DIR}/${FREERTOS_KERNEL_RP2040_BACK_PATH}/${FREERTOS_KERNEL_RP2040_RELATIVE_PATH} REALPATH) + if (_ACTUAL_PATH STREQUAL _POSSIBLE_PATH) + get_filename_component(FREERTOS_KERNEL_PATH ${CMAKE_CURRENT_LIST_DIR}/${FREERTOS_KERNEL_RP2040_BACK_PATH} REALPATH) + endif() + if (_ACTUAL_PATH STREQUAL _POSSIBLE_PATH) + get_filename_component(FREERTOS_KERNEL_PATH ${CMAKE_CURRENT_LIST_DIR}/${FREERTOS_KERNEL_RP2040_BACK_PATH} REALPATH) + message("Setting FREERTOS_KERNEL_PATH to ${FREERTOS_KERNEL_PATH} based on location of FreeRTOS-Kernel-import.cmake") + elseif (PICO_SDK_PATH AND EXISTS "${PICO_SDK_PATH}/../FreeRTOS-Kernel") + set(FREERTOS_KERNEL_PATH ${PICO_SDK_PATH}/../FreeRTOS-Kernel) + message("Defaulting FREERTOS_KERNEL_PATH as sibling of PICO_SDK_PATH: ${FREERTOS_KERNEL_PATH}") + endif() +endif () + +if (NOT FREERTOS_KERNEL_PATH) + foreach(POSSIBLE_SUFFIX Source FreeRTOS-Kernel FreeRTOS/Source) + # check if FreeRTOS-Kernel exists under directory that included us + set(SEARCH_ROOT ${CMAKE_CURRENT_SOURCE_DIR}) + get_filename_component(_POSSIBLE_PATH ${SEARCH_ROOT}/${POSSIBLE_SUFFIX} REALPATH) + if (EXISTS ${_POSSIBLE_PATH}/${FREERTOS_KERNEL_RP2040_RELATIVE_PATH}/CMakeLists.txt) + get_filename_component(FREERTOS_KERNEL_PATH ${_POSSIBLE_PATH} REALPATH) + message("Setting FREERTOS_KERNEL_PATH to '${FREERTOS_KERNEL_PATH}' found relative to enclosing project") + break() + endif() + endforeach() +endif() + +if (NOT FREERTOS_KERNEL_PATH) + message(FATAL_ERROR "FreeRTOS location was not specified. Please set FREERTOS_KERNEL_PATH.") +endif() + +set(FREERTOS_KERNEL_PATH "${FREERTOS_KERNEL_PATH}" CACHE PATH "Path to the FreeRTOS Kernel") + +get_filename_component(FREERTOS_KERNEL_PATH "${FREERTOS_KERNEL_PATH}" REALPATH BASE_DIR "${CMAKE_BINARY_DIR}") +if (NOT EXISTS ${FREERTOS_KERNEL_PATH}) + message(FATAL_ERROR "Directory '${FREERTOS_KERNEL_PATH}' not found") +endif() +if (NOT EXISTS ${FREERTOS_KERNEL_PATH}/${FREERTOS_KERNEL_RP2040_RELATIVE_PATH}/CMakeLists.txt) + message(FATAL_ERROR "Directory '${FREERTOS_KERNEL_PATH}' does not contain an RP2040 port here: ${FREERTOS_KERNEL_RP2040_RELATIVE_PATH}") +endif() +set(FREERTOS_KERNEL_PATH ${FREERTOS_KERNEL_PATH} CACHE PATH "Path to the FreeRTOS_KERNEL" FORCE) + +add_subdirectory(${FREERTOS_KERNEL_PATH}/${FREERTOS_KERNEL_RP2040_RELATIVE_PATH} FREERTOS_KERNEL) diff --git a/motor-control/core-xy-firmware/cmake/tmc2209.cmake b/motor-control/core-xy-firmware/cmake/tmc2209.cmake new file mode 100644 index 0000000..bde90b3 --- /dev/null +++ b/motor-control/core-xy-firmware/cmake/tmc2209.cmake @@ -0,0 +1,19 @@ +add_library(tmc2209_driver STATIC + src/tmc2209/step.cpp + src/tmc2209/uart_interface.cpp + src/tmc2209/uart_registers.cpp +) + +target_include_directories(tmc2209_driver PUBLIC + include/tmc2209/ + build/ + ./ +) + +target_link_libraries(tmc2209_driver + pico_stdlib + hardware_uart + hardware_pio + FreeRTOS-Kernel-Heap4 +) + diff --git a/motor-control/core-xy-firmware/include/CoreXY.hpp b/motor-control/core-xy-firmware/include/CoreXY.hpp new file mode 100644 index 0000000..a1028aa --- /dev/null +++ b/motor-control/core-xy-firmware/include/CoreXY.hpp @@ -0,0 +1,57 @@ +#pragma once +#include +#include "tmc2209/tmc2209.hpp" +#include "tmc2209/step.hpp" + +// LimitSwitch Pins +#define LIMIT0_PIN 10 +#define LIMIT1_PIN 11 +#define LIMIT2_PIN 12 +#define LIMIT3_PIN 13 + +// LimitSwich Physical Wiring +// DONT TOUCH THIS +#define LIMIT_X0 LIMIT1_PIN +#define LIMIT_X1 LIMIT3_PIN +#define LIMIT_Y0 LIMIT2_PIN +#define LIMIT_Y1 LIMIT0_PIN + +// Motor Directions +#define X_DIRECTION_TOP 0b01 +#define X_DIRECTION_BOTTOM 0b00 +#define Y_DIRECTION_LEFT 0b10 +#define Y_DIRECTION_RIGHT 0b00 + +// Motor Mapping +// DONT TOUCH THIS +#define MOTOR_X 1 +#define MOTOR_Y 0 + +// Homing Speeds +#define HOME_SPEED_FAST 10000 +#define HOME_SPEED_SLOW 2000 +#define HOME_SPEED_VERY_SLOW 500 + +enum HOME_AXIS_DIRECTION { + TOP_LEFT = X_DIRECTION_TOP | Y_DIRECTION_LEFT, + TOP_RIGHT = X_DIRECTION_TOP | Y_DIRECTION_RIGHT, + BOTTOM_RIGHT = X_DIRECTION_BOTTOM | Y_DIRECTION_RIGHT, + BOTTOM_LEFT = X_DIRECTION_BOTTOM | Y_DIRECTION_LEFT +}; + +struct MotorPosition { + int x; + int y; +}; + +void homeXY(TMC2209_step_dual * driver, HOME_AXIS_DIRECTION direction, unsigned int speed, unsigned int backupSpace, MotorPosition * last_pos); +MotorPosition homeSequence(TMC2209_step_dual * driver, MotorPosition * last_pos); + + +void initLimitSwitches(); +void isr_LimitSwitch(unsigned int gpio, uint32_t event_mask); + +void waitForSwitch(uint32_t switchPin); + +void debugPrintSwitch(uint32_t switchPin); +void printAllSwitchStates(); \ No newline at end of file diff --git a/motor-control/core-xy-firmware/include/tmc2209/registers_map.hpp b/motor-control/core-xy-firmware/include/tmc2209/registers_map.hpp new file mode 100644 index 0000000..5a25194 --- /dev/null +++ b/motor-control/core-xy-firmware/include/tmc2209/registers_map.hpp @@ -0,0 +1,517 @@ +#pragma once + +// ================================================================================================= +// General Registers (p.23) +// ================================================================================================= +#define REG_ADDR_GENERAL_GCONF 0x00 +#define REG_ADDR_GENERAL_GSTAT 0x01 +#define REG_ADDR_GENERAL_IFCNT 0x02 +#define REG_ADDR_GENERAL_NODECONF 0x03 +#define REG_ADDR_GENERAL_OTP_PROG 0x04 +#define REG_ADDR_GENERAL_OTP_READ 0x05 +#define REG_ADDR_GENERAL_IOIN 0x06 +#define REG_ADDR_GENERAL_FACTORY_CONF 0x07 + +/// @brief Global configuration flags +struct REG_GCONF { + /** + * (Reset default=1) + * 0: Use internal reference derived from 5VOUT + * 1: Use voltage supplied to VREF as current reference + */ + unsigned int I_scale_analog: 1; + + /** + * (Reset default: OTP) + * 0: Operation with external sense resistors + * 1: Internal sense resistors. Use current supplied into VREF as reference for internal sense resistor. VREF pin internally is driven to GND in this mode. + */ + unsigned int internal_Rsense: 1; + + /** + * (Reset default: OTP) + * 0: StealthChop PWM mode enabled (depending on velocity thresholds). Initially switch from off to on state while in stand still, only. + * 1: SpreadCycle mode enabled A high level on the pin SPREAD inverts this flag to switch between both chopper modes. + */ + unsigned int en_SpreadCycle: 1; + + /** + * 1: Inverse motor direction + */ + unsigned int shaft: 1; + + /** + * 0: INDEX shows the first microstep position of sequencer + * 1: INDEX pin outputs overtemperature prewarning flag (otpw) instead + */ + unsigned int index_otpw: 1; + + /** + * 0: INDEX output as selected by index_otpw + * 1: INDEX output shows step pulses from internal pulse generator (toggle upon each step) + */ + unsigned int index_step: 1; + + /** + * 0: PDN_UART controls standstill current reduction + * 1: PDN_UART input function disabled. Set this bit, when using the UART interface! + */ + unsigned int pdn_disable: 1; + + /** + * 0: Microstep resolution selected by pins MS1, MS2 + * 1: Microstep resolution selected by MRES register + */ + unsigned int mstep_reg_select: 1; + + /** + * 0: No filtering of STEP pulses + * 1: Software pulse generator optimization enabled when fullstep frequency > 750Hz (roughly). TSTEP shows filtered step time values when active + */ + unsigned int multistep_filt: 1; + + /** + * 0: Normal operation + * 1: Enable analog test output on pin ENN (pull down resistor off), ENN treated as enabled. IHOLD[1..0] selects the function of DCO: 0…2: T120, DAC, VDDH + */ + unsigned int test_mode: 1; + + unsigned int :22; +}; + +/// @brief Global status flags +struct REG_GSTAT { + /** + * 1: Indicates that the IC has been reset since the last read access to GSTAT. All registers have been cleared to reset values. + */ + unsigned int reset: 1; + + /** + * 1: Indicates, that the driver has been shut down due to overtemperature or short circuit detection since the last read access. Read DRV_STATUS for details. The flag can only be cleared when all error conditions are cleared + */ + unsigned int drv_err: 1; + + /** + * Indicates an undervoltage on the charge pump. The driver is disabled in this case. This flag is not latched and thus does not need to be cleared + */ + unsigned int uv_cp: 1; + + unsigned int :29; +}; + +/// @brief Interface transmission counter +struct REG_IFCNT { + /** + * Interface transmission counter. This register becomes incremented with each successful UART interface write access. Read out to check the serial transmission for lost data. Read accesses do not change the content. The counter wraps around from 255 to 0. + */ + unsigned int mstep: 8; + + unsigned int :24; +}; + +/// @brief Node configuration +struct REG_NODECONF { + unsigned int :8; + + /** + * SENDDELAY for read access (time until reply is sent): + * 0, 1: 8 bit times (Attention: Don’t use in multi-node) + * 2, 3: 3*8 bit times + * 4, 5: 5*8 bit times + * 6, 7: 7*8 bit times + * 8, 9: 9*8 bit times + * 10, 11: 11*8 bit times + * 12, 13: 13*8 bit times + * 14, 15: 15*8 bit times + */ + unsigned int senddelay: 4; + + unsigned int :16; +}; + +/// @brief OTP programming +struct REG_OPT_PROG { + /** + * Selection of OTP bit to be programmed to the selected byte location (n=0..7: programs bit n to a logic 1) + */ + unsigned int otpbit: 3; + + /** + * Selection of OTP programming location (0, 1 or 2) + */ + unsigned int optbyte: 1; + + /** + * Set to 0xbd to enable programming. A programming time of minimum 10ms per bit is recommended (check by reading OTP_READ). + */ + unsigned int optmagic: 8; + + unsigned int :16; +}; + +/// @brief OTP read (Access to OTP memory result and update) +struct OTP_READ { + /// @brief OTP0 byte 0 read data + unsigned int opt0: 8; + + /// @brief OTP1 byte 1 read data + unsigned int opt1: 8; + + /// @brief OTP2 byte 2 read data + unsigned int opt2: 8; + + unsigned int :8; +}; + + +/// @brief Inputs (Reads the state of all input pins available) +struct REG_IOIN { + unsigned int enn: 1; + unsigned int :1; + unsigned int ms1: 1; + unsigned int ms2: 1; + unsigned int diag: 1; + unsigned int :1; + unsigned int pdn_uart: 1; + unsigned int step: 1; + unsigned int spread_en: 1; + unsigned int dir: 1; + + /** + * @brief VERSION: 0x21=first version of the IC Identical numbers mean full digital compatibility. + */ + unsigned int version: 8; +}; + +struct REG_FACTORY_CONF { + /** + * Lowest to highest clock frequency. Check at charge pump output. The frequency span is not guaranteed, but it is tested, that tuning to 12MHz internal clock is possible. The devices come preset to 12MHz clock frequency by OTP programming. + */ + unsigned int fclktrim: 5; + + /** + * %00: OT=143°C, OTPW=120°C + * %01: OT=150°C, OTPW=120°C + * %10: OT=150°C, OTPW=143°C + * %11: OT=157°C, OTPW=143°C + */ + unsigned int ottrim: 2; + + unsigned int :25; +}; + +// ================================================================================================= +// Velocity Dependent Control (p.28) +// ================================================================================================= +#define REG_ADDR_VELOCITY_IHOLD_IRUN 0x10 +#define REG_ADDR_VELOCITY_TPOWERDOWN 0x11 +#define REG_ADDR_VELOCITY_TSTEP 0x12 +#define REG_ADDR_VELOCITY_TPWMTHRS 0x13 +#define REG_ADDR_VELOCITY_VACTUAL 0x22 + +/// @brief Driver current control +struct REG_IHOLD_IRUN { + /** + * (Reset default: OTP) + * IHOLD (Reset default: OTP) + * Standstill current (0=1/32 … 31=32/32) + * In combination with StealthChop mode, setting + * IHOLD=0 allows to choose freewheeling or coil short circuit (passive braking) for motor stand still. + */ + unsigned int ihold: 5; + + /** + * (Reset default=31) + * Motor run current (0=1/32 … 31=32/32) + * + * Tint: Choose sense resistors in a way, that normal IRUN is 16 to 31 for best microstep performance. + */ + unsigned int irun: 5; + + /** + * (Reset default: OTP) + * Controls the number of clock cycles for motor power down after standstill is detected (stst=1) and TPOWERDOWN has expired. The smooth transition avoids a motor jerk upon power down. + * 0: instant power down + * 1..15: Delay per current reduction step in multiple of 2^18 clocks + */ + unsigned int iholddelay: 4; + unsigned int :18; +}; + + +struct REG_TPOWERDOWN { + /** + * (Reset default=20) + * Sets the delay time from stand still (stst) detection to motor current power down. Time range is about 0 to 5.6 seconds. + * 0…((2^8)-1) * 2^18 tCLK + * Attention: A minimum setting of 2 is required to allow automatic tuning of StealthChop PWM_OFFS_AUTO. + */ + unsigned int tpow: 8; + + unsigned int :24; +}; + +/// @brief Time between two 1/256 microsteps +struct REG_TSTEP { + /** + * Actual measured time between two 1/256 microsteps derived from the step input frequency in units of 1/fCLK. Measured value is (2^20)-1 in case of overflow or stand still. TSTEP always relates to 1/256 step, independent of the actual MRES. The TSTEP related threshold uses a hysteresis of 1/16 of the compare value to compensate for jitter in the clock or the step frequency: (Txxx*15/16)-1 is the lower compare value for each TSTEP based comparison. + * This means, that the lower switching velocity equals the calculated setting, but the upper switching velocity is higher as defined by the hysteresis setting. + * + */ + unsigned int tstep: 20; + + unsigned int :12; +}; + +/// @brief Upper velocity for StealthChop voltage PWM mode +struct REG_TPWMTHRS { + /** + * Sets the upper velocity for StealthChop voltage PWM mode. + * TSTEP ≥ TPWMTHRS + * - StealthChop PWM mode is enabled, if configured + * When the velocity exceeds the limit set by TPWMTHRS, the driver switches to SpreadCycle. + * 0: Disabled + */ + unsigned int tpwmthrs: 20; + + unsigned int :12; +}; + +struct REG_VACTUAL { + /** + * ACTUAL allows moving the motor by UART control. + * It gives the motor velocity in +-(2^23)-1 [μsteps / t] + * 0: Normal operation. Driver reacts to STEP input. + * !=0: Motor moves with the velocity given by VACTUAL. Step pulses can be monitored via INDEX output. The motor direction is controlled by the sign of VACTUAL. + */ + int vactual: 24; + + unsigned int :8; +}; + +// ================================================================================================= +// StallGuard (p.29) +// ================================================================================================= + +#define REG_ADDR_STALLGUARD_TCOOLTHRS 0x14 +#define REG_ADDR_STALLGUARD_SGTHRS 0x40 +#define REG_ADDR_STALLGUARD_SG_RESULT 0x41 +#define REG_ADDR_STALLGUARD_COOLCONF 0x42 + +struct REG_TCOOLTHRS { + /** + * This is the lower threshold velocity for switching on smart energy CoolStep and StallGuard to DIAG output. (unsigned) Set this parameter to disable CoolStep at low speeds, where it cannot work reliably. The stall output signal become enabled when exceeding this velocity. It becomes disabled again once the velocity falls below this threshold. + * TCOOLTHRS ≥ TSTEP > TPWMTHRS + * - CoolStep is enabled, if configured (only with StealthChop) + * - Stall output signal on pin DIAG is enabled + */ + unsigned int tcoolthrs: 20; + + unsigned int :12; +}; + +struct REG_SGTHRS { + /** + * Detection threshold for stall. The StallGuard value SG_RESULT becomes compared to the double of this threshold. A stall is signaled with + * SG_RESULT ≤ SGTHRS*2 + */ + unsigned int sgthrs: 8; + + unsigned int :24; +}; + +struct REG_SG_RESULT { + /** + * StallGuard result. SG_RESULT becomes updated with each fullstep, independent of TCOOLTHRS and SGTHRS. A higher value signals a lower motor load and more torque headroom. Intended for StealthChop mode, only. Bits 9 and 0 will always show 0. Scaling to 10 bit is for compatibility to StallGuard2 + */ + unsigned int sg_result: 10; + + unsigned int :22; +}; + +struct REG_COOLCONF { + /** + * @brief CoolStep configuration + */ + unsigned int coolconf: 16; + + unsigned int :16; +}; + +// ================================================================================================= +// Sequencer Registers (p.30) +// =================================================================================================q + +#define REG_ADDR_SEQUENCER_MSCNT 0x6A +#define REG_ADDR_SEQUENCER_MSCURACT 0x6B + +/// @brief Microstep counter +struct REG_MSCNT { + /** + * Microstep counter. Indicates actual position in the microstep table for CUR_A. CUR_B uses an offset of 256 into the table. Reading out MSCNT allows determination of the motor position within the electrical wave. + */ + unsigned int mscnt: 10; + + unsigned int :22; +}; + +/// @brief Microstep current (actual) +struct REG_MSCURACT { + /** + * Actual microstep current for motor phase B (sine wave) as read from the internal sine wave table (not scaled by current setting) + */ + int cur_a: 9; + + /** + * Actual microstep current for motor phase A (co-sine wave) as read from the internal sine wave table (not scaled by current setting) + */ + int cur_b: 9; + + unsigned int :14; +}; + +// ================================================================================================= +// Chopper Registers (p.32) +// ================================================================================================= + +#define REG_ADDR_CHOPPER_CHOPCONF 0x6C +#define REG_ADDR_CHOPPER_DRV_STATUS 0x6F +#define REG_ADDR_CHOPPER_PWMCONF 0x70 +#define REG_ADDR_CHOPPER_PWM_SCALE 0x71 +#define REG_ADDR_CHOPPER_PWM_AUTO 0x72 + +struct REG_CHOPCONF { + /** + * @brief off time and driver enable + * Off time setting controls duration of slow decay phase + * NCLK= 24 + 32*TOFF + * %0000: Driver disable, all bridges off + * %0001: 1 – use only with TBL ≥ 2 + * %0010 ... %1111: 2 ... 15 + * (Default: OTP, resp. 3 in StealthChop mode) + */ + unsigned int toff: 4; + + /** + * @brief hysteresis start value added to HEND + * %000 ... %111: + * Add 1, 2, ..., 8 to hysteresis low value HEND + * (1/512 of this setting adds to current setting) + * Attention: Effective HEND+HSTRT ≤ 16. + * Hint: Hysteresis decrement is done each 16 clocks + */ + unsigned int hstrt: 3; + + /** + * @brief HEND hysteresis low value OFFSET sine wave offset + * %0000 ... %1111: + * Hysteresis is -3, -2, -1, 0, 1, ..., 12 + * (1/512 of this setting adds to current setting) + * This is the hysteresis value which becomes used for the hysteresis chopper. + * (Default: OTP, resp. 0 in StealthChop mode) + */ + unsigned int hend: 4; + + unsigned int :1; + + /** + * @brief TBL blank time select + * %00 ... %11: + * Set comparator blank time to 16, 24, 32 or 40 clocks + * Hint: %00 or %01 is recommended for most applications + * (Default: OTP) + */ + unsigned int tbl: 2; + + /** + * @brief sense resistor voltage based current scaling + * 0: Low sensitivity, high sense resistor voltage + * 1: High sensitivity, low sense resistor voltage + */ + unsigned int vsense: 1; + + unsigned int :6; + + /** + * @brief micro step resolution + * %0000: Native 256 microstep setting. + * %0001 ... %1000: + * 128, 64, 32, 16, 8, 4, 2, FULLSTEP + * Reduced microstep resolution. + * The resolution gives the number of microstep entries per sine quarter wave. + * When choosing a lower microstep resolution, the driver automatically uses microstep positions which result in a symmetrical wave. + * Number of microsteps per step pulse = 2^MRES (Selection by pins unless disabled by GCONF. mstep_reg_select) + */ + unsigned int mres: 4; + + /** + * @brief interpolation to 256 microsteps + * 1: The actual microstep resolution (MRES) becomes extrapolated to 256 microsteps for smoothest motor operation. (Default: 1) + */ + unsigned int intpol: 1; + + /** + * @brief enable double edge step pulses + * 1: Enable step impulse at each step edge to reduce step frequency requirement. This mode is not compatible with the step filtering function (multistep_filt) + */ + unsigned int dedge: 1; + + /** + * @brief short to GND protection disable + * 0: Short to GND protection is on + * 1: Short to GND protection is disabled + */ + unsigned int diss2g: 1; + + /** + * @brief Low side short protection disable + * 0: Short protection low side is on + * 1: Short protection low side is disabled + */ + unsigned int diss2vs: 1; +}; + +struct DRV_STATUS { + unsigned int drv_status: 32; +}; + +struct PWMCONF { + unsigned int pwmconf: 22; + unsigned int :10; +}; + + +/** + * Results of StealthChop amplitude regulator. These values can be used to monitor automatic PWM amplitude scaling (255=max. voltage). + */ +struct PWM_SCALE { + /** + * Actual PWM duty cycle. This value is used for scaling the values CUR_A and CUR_B read from the sine wave table. + */ + unsigned int pwm_scale_sum: 8; + + /** + * 9 Bit signed offset added to the calculated PWM duty cycle. This is the result of the automatic amplitude regulation based on current measurement. + */ + unsigned int pwm_scale_auto: 9; + + unsigned int :15; +}; + +/** + * These automatically generated values can be read out in order to determine a default / power up setting for PWM_GRAD and PWM_OFS. + */ +struct PWM_AUTO { + + /** + * Automatically determined offset value + */ + unsigned int pwm_ofs_auto: 8; + + /** + * Automatically determined gradient value + */ + unsigned int pwm_grad_auto: 8; + + unsigned int :16; +}; \ No newline at end of file diff --git a/motor-control/core-xy-firmware/include/tmc2209/step.hpp b/motor-control/core-xy-firmware/include/tmc2209/step.hpp new file mode 100644 index 0000000..314415b --- /dev/null +++ b/motor-control/core-xy-firmware/include/tmc2209/step.hpp @@ -0,0 +1,80 @@ +#pragma once +#include "hardware/pio.h" +#include "pulser.pio.h" + +#include "FreeRTOS.h" +#include "semphr.h" + +class TMC2209_step_dual { +private: + uint step0_pin; + uint step1_pin; + uint dir0_pin; + uint dir1_pin; + uint enable0_pin; + uint enable1_pin; + + const int sm_num_divider0 = 0; + const int sm_num_divider1 = 1; + const int sm_num_counter0 = 2; + const int sm_num_counter1 = 3; + + PIO pio_core; + + pio_sm_config sm_config_divider0; + pio_sm_config sm_config_counter0; + pio_sm_config sm_config_divider1; + pio_sm_config sm_config_counter1; + + uint pulser_program_offset; + const uint subprogram_offset_divider = 0; + const uint subprogram_offset_counter = 6; + + const float base_clock_freq_khz = 500.0; + + const uint32_t sleep_time_us = 500; + + SemaphoreHandle_t xDoneSemaphore0; + SemaphoreHandle_t xDoneSemaphore1; + + uint remaining_step_count0; + uint remaining_step_count1; + +protected: + void init_pio(); + void init_gpio(); + + +public: + void irq_handler0(); + void irq_handler1(); + + TMC2209_step_dual( + uint step0_pin, + uint step1_pin, + uint dir0_pin, + uint dir1_pin, + uint enable0_pin, + uint enable1_pin, + PIO pio_core + ); + + void set_conf0(uint frequency, bool dir); + void set_conf1(uint frequency, bool dir); + + void pulse0(uint n); + void pulse1(uint n); + + void pulse0_int(int n); + void pulse1_int(int n); + + void wait0(int timeout_ms); + void wait1(int timeout_ms); + + uint get_remaining_steps0(); + uint get_remaining_steps1(); +}; + +extern TMC2209_step_dual * g_step_driver_instance; +void step_irq0_handler(); +void step_irq1_handler(); diff --git a/motor-control/core-xy-firmware/include/tmc2209/tmc2209.hpp b/motor-control/core-xy-firmware/include/tmc2209/tmc2209.hpp new file mode 100644 index 0000000..341ca4f --- /dev/null +++ b/motor-control/core-xy-firmware/include/tmc2209/tmc2209.hpp @@ -0,0 +1,125 @@ +#pragma once +#include "pico/stdlib.h" +#include "hardware/uart.h" + +#include "registers_map.hpp" + +/** + * @brief Interface to communicate with TMC2209 driver via UART + * This implements basic read and write operations to the TMC2209 driver + * It does not implement any register specific operations + */ +class TMC2209_UART { +private: + uint8_t node_address; + uart_inst_t *uart_inst; + uint baudrate; + uint tx_pin; + uint rx_pin; + + +public: + /** + * @brief Construct a new tmc2209 uart object + * This does not initialize the UART peripheral, it just sets the parameters + * + * @param uart_id which UART to use (uart0 or uart1) + * @param node_address this is the address of the TMC2209 driver set by ms1, ms2 pins (0-3) + * @param baudrate UART baudrate to use (250k > baudrate > 2000, good value 115200, the tmc2209 has automatic baudrate detection) + * @param tx_pin TX pin of RP2040. Depending on the UART peripheral, diffrent pins are valid + * @param rx_pin RX pin of RP2040. Depending on the UART peripheral, diffrent pins are valid + */ + TMC2209_UART(uart_inst_t *uart_inst, uint8_t node_address, uint baudrate, uint tx_pin, uint rx_pin); + + /** + * @brief Initialize UART peripheral and pins + * + */ + void init(); + + /** + * @brief Calculate CRC8-ATM checksum for the given data + * the specific implementation is taken from TMC2209 datasheet + * + * @param data Data to calculate CRC for + * @param size Size of the data (without CRC byte) + * @return uint8_t Calculated CRC8 checksum + */ + uint8_t calc_crc8_atm(uint8_t *data, uint8_t size); + + /** + * @brief Perform a read operation to the TMC2209 driver + * This does not check if register is write only. + * It will wait for the response from the driver + * + * @param address + * @return uint32_t Data read from the register + */ + uint32_t read(uint8_t address); + + /** + * @brief Perform a write operation to the TMC2209 driver + * This does not check if register is read only. + * It will wait for the response from the driver + * + * @param address Where to write the data (register Address) + * @param value Data to write to the register + */ + void write(uint8_t address, uint32_t value); + + /** + * @brief This if any CRC error occured during the last read/write operation. Should be checked after each operation + */ + bool crc_error_flag = false; +}; + +/** + * @brief Higher level interface to TMC2209 driver + * This class simplifies dealing with the registers of the TMC2209 driver + * so you can just set the values + */ +class TMC2209_Registers { + +private: + TMC2209_UART uart_driver; + + REG_CHOPCONF chopconfBuffer; + +public: + TMC2209_Registers(TMC2209_UART uart_driver); + + void set_reg_gconf( + bool i_scale_analog, + bool internal_rsense, + bool en_SpreadCycle, + bool shaft, + bool index_otpw, + bool index_step, + bool pdn_disable, + bool mstep_reg_select, + bool multistep_filt + ); + + void set_holdCurrent( + uint8_t currentHold, + uint8_t currentRun, + uint8_t holdDelay + ); + + void set_thresholdPowerdown(uint8_t threshold); + void set_thresholdStealthChop(uint32_t threshold); + void set_thresholdCoolStep(uint32_t threshold); + void set_thresholdStall(uint8_t threshold); + + void set_chopConfig( + bool lowsideShortProtection, + bool GNDShortProtection, + bool doubleEdge, + bool interpolation, + uint8_t microstepResolution, + bool voltageSensatifity, + uint8_t hysteresisOffset, + uint8_t hysteresisStart + ); + +}; diff --git a/motor-control/core-xy-firmware/lib/FreeRTOS-Kernel b/motor-control/core-xy-firmware/lib/FreeRTOS-Kernel new file mode 160000 index 0000000..e55bde2 --- /dev/null +++ b/motor-control/core-xy-firmware/lib/FreeRTOS-Kernel @@ -0,0 +1 @@ +Subproject commit e55bde213348c8911205f81e89016dd4ba2ba79d diff --git a/motor-control/core-xy-firmware/pico_sdk_import.cmake b/motor-control/core-xy-firmware/pico_sdk_import.cmake new file mode 100644 index 0000000..a0721d0 --- /dev/null +++ b/motor-control/core-xy-firmware/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/motor-control/core-xy-firmware/src/limitSwitches.cpp b/motor-control/core-xy-firmware/src/limitSwitches.cpp new file mode 100644 index 0000000..f79d1c1 --- /dev/null +++ b/motor-control/core-xy-firmware/src/limitSwitches.cpp @@ -0,0 +1,123 @@ +#include "CoreXY.hpp" + +#include "pico/stdlib.h" +#include + + +void initLimitSwitches() { + gpio_init(LIMIT_X0); + gpio_init(LIMIT_X1); + gpio_init(LIMIT_Y0); + gpio_init(LIMIT_Y1); + + gpio_set_dir(LIMIT_X0, GPIO_IN); + gpio_set_dir(LIMIT_X1, GPIO_IN); + gpio_set_dir(LIMIT_Y0, GPIO_IN); + gpio_set_dir(LIMIT_Y1, GPIO_IN); + + gpio_pull_up(LIMIT_X0); + gpio_pull_up(LIMIT_X1); + gpio_pull_up(LIMIT_Y0); + gpio_pull_up(LIMIT_Y1); + + gpio_set_input_hysteresis_enabled(LIMIT_X0, true); + gpio_set_input_hysteresis_enabled(LIMIT_X1, true); + gpio_set_input_hysteresis_enabled(LIMIT_Y0, true); + gpio_set_input_hysteresis_enabled(LIMIT_Y1, true); + + gpio_set_irq_enabled_with_callback( + LIMIT_X0, + GPIO_IRQ_EDGE_RISE, + true, + isr_LimitSwitch + ); + + gpio_set_irq_enabled_with_callback( + LIMIT_X1, + GPIO_IRQ_EDGE_RISE, + true, + isr_LimitSwitch + ); + + gpio_set_irq_enabled_with_callback( + LIMIT_Y0, + GPIO_IRQ_EDGE_RISE, + true, + isr_LimitSwitch + ); + + gpio_set_irq_enabled_with_callback( + LIMIT_Y1, + GPIO_IRQ_EDGE_RISE, + true, + isr_LimitSwitch + ); +} + +absolute_time_t debounce_cooldown_ticks= 5000; +absolute_time_t limit_debounce_tick = 0; + +void isr_LimitSwitch(unsigned int gpio, uint32_t event_mask) { + absolute_time_t current_tick = get_absolute_time(); + if((current_tick - limit_debounce_tick) < debounce_cooldown_ticks) { + return; + } else { + limit_debounce_tick = current_tick; + } + + debugPrintSwitch(gpio); +} + +void waitForSwitch(uint32_t switchPin) { + while(!gpio_get(switchPin)) { + sleep_ms(1); + } +} + +void debugPrintSwitch(uint32_t switchPin) { + printf("Switch: "); + + switch (switchPin) { + case LIMIT_X0: + printf("X0"); + break; + case LIMIT_X1: + printf("X1"); + break; + case LIMIT_Y0: + printf("Y0"); + break; + case LIMIT_Y1: + printf("Y1"); + break; + default: + printf("??"); + break; + } + + switch(switchPin) { + case LIMIT0_PIN: + printf(" 0"); + break; + case LIMIT1_PIN: + printf(" 1"); + break; + case LIMIT2_PIN: + printf(" 2"); + break; + case LIMIT3_PIN: + printf(" 3"); + break; + } + + printf(" %d", gpio_get(switchPin)); + + printf(" (%d)\n", switchPin); +} + +void printAllSwitchStates() { + printf("X0: %d\n", gpio_get(LIMIT_X0)); + printf("X1: %d\n", gpio_get(LIMIT_X1)); + printf("Y0: %d\n", gpio_get(LIMIT_Y0)); + printf("Y1: %d\n", gpio_get(LIMIT_Y1)); +} \ No newline at end of file diff --git a/motor-control/core-xy-firmware/src/main.cpp b/motor-control/core-xy-firmware/src/main.cpp new file mode 100755 index 0000000..7e44b6f --- /dev/null +++ b/motor-control/core-xy-firmware/src/main.cpp @@ -0,0 +1,316 @@ +/** + * @file main.c + * @author AlexanderHD27 + * @brief Main file of the RPi Pico Firmware for the Oscilloscope-Renderer + * @version 0.1 + * @date 2024-02-15 + * + * @copyright Copyright (c) 2024 + * + */ + +#include "pico/stdio.h" +#include "pico/stdlib.h" +#include "pico/time.h" +#include "hardware/uart.h" +#include + +#include "pulser.pio.h" +#include "tmc2209/tmc2209.hpp" +#include "tmc2209/step.hpp" + +#include "CoreXY.hpp" + +#include "FreeRTOSConfig.h" +#include "FreeRTOS.h" +#include "task.h" + +#define LED_PIN 25 + +#define RGB1_B_PIN 21 +#define RGB1_G_PIN 22 + +#define RGB2_R_PIN 26 +#define RGB2_G_PIN 27 +#define RGB2_B_PIN 28 + +#define BOARD_LED1_PIN 14 +#define BOARD_LED2_PIN 15 + +#define ENABLE0_PIN 2 +#define ENABLE1_PIN 3 + +#define STEP0_PIN 6 +#define STEP1_PIN 7 +#define DIR0_PIN 8 +#define DIR1_PIN 9 + +enum SYSTEM_STATE { + STARTUP, + HOMEING_FIRST_RUN, + HOMEING_BACKOFF, + HOMEING_SECOND_RUN, + IDLE +}; + +SYSTEM_STATE system_state = STARTUP; + +/* +void homeMotors0(TMC2209_step_dual * driver) { + + driver->set_conf0(10000, 1); + driver->pulse0(10000); + driver->wait0(0); + + driver->set_conf0(10000, 0); + driver->pulse0(100000); + while(!gpio_get(LIMIT2_PIN)) { + sleep_ms(1); + } + driver->pulse0(0); + sleep_ms(500); + + // Second Run + driver->set_conf0(5000, 1); + driver->pulse0(200); + driver->wait0(0); + + driver->set_conf0(500, 0); + driver->pulse0(5000); + while(!gpio_get(LIMIT2_PIN)) { + sleep_ms(1); + } + driver->pulse0(0); + sleep_ms(1); +} + +void homeMotors1(TMC2209_step_dual * driver) { + + driver->set_conf1(10000, 1); + driver->pulse1(5000); + driver->wait1(0); + + driver->set_conf1(10000, 0); + driver->pulse1(100000); + while(!gpio_get(LIMIT3_PIN)) { + sleep_ms(1); + } + driver->pulse1(0); + sleep_ms(500); + + // Second Run + driver->set_conf1(5000, 1); + driver->pulse1(200); + driver->wait1(0); + + driver->set_conf1(500, 0); + driver->pulse1(5000); + while(!gpio_get(LIMIT3_PIN)) { + sleep_ms(1); + } + driver->pulse1(0); + sleep_ms(1); +} + +void task_home0(void *pvParameters) { + TMC2209_step_dual * driver = (TMC2209_step_dual *)pvParameters; + vTaskDelay(500); + homeMotors0(driver); +} + +void task_home1(void *pvParameters) { + TMC2209_step_dual * driver = (TMC2209_step_dual *)pvParameters; + homeMotors1(driver); +} +*/ + +void vTaskInitMotorHome(void *pvParameters) { + TMC2209_step_dual * driver = (TMC2209_step_dual *)pvParameters; + vTaskDelay(500 / portTICK_PERIOD_MS); + + // Drive Forward 1000 Steps + driver->set_conf0(5000, 0); + driver->set_conf1(5000, 0); + driver->pulse0(3000); + driver->pulse1(2000); + driver->wait0(0); + driver->wait1(0); + + + MotorPosition a; + homeSequence(driver, &a); + printf("Home done: %d, %d\n", a.x, a.y); + vTaskDelete(NULL); +} + +void vTaskPrintSwitchStates(void *pvParameters) { + while(true) { + //printAllSwitchStates(); + vTaskDelay(500 / portTICK_PERIOD_MS); + } +} + +int main() { + stdio_init_all(); + + printf("Hello, World!\n"); + + initLimitSwitches(); + + gpio_init(DIR0_PIN); + gpio_set_dir(DIR0_PIN, GPIO_OUT); + gpio_put(DIR0_PIN, 0); + + gpio_init(DIR1_PIN); + gpio_set_dir(DIR1_PIN, GPIO_OUT); + gpio_put(DIR1_PIN, 0); + + TMC2209_step_dual step_driver = TMC2209_step_dual( + STEP0_PIN, STEP1_PIN, + DIR0_PIN, DIR1_PIN, + ENABLE0_PIN, ENABLE1_PIN, + pio0 + ); + TMC2209_UART uart_driver0 = TMC2209_UART(uart0, 0, 19200, 0, 1); + TMC2209_UART uart_driver1 = TMC2209_UART(uart1, 0, 19200, 4, 5); + uart_driver0.init(); + uart_driver1.init(); + + volatile uint8_t a = uart_driver0.read(REG_ADDR_CHOPPER_CHOPCONF); + volatile uint8_t b = uart_driver1.read(REG_ADDR_CHOPPER_CHOPCONF); + + TaskHandle_t initMotorTaskHandle; + TaskHandle_t printSwitchStatesTaskHandle; + + xTaskCreate(vTaskInitMotorHome, "Initial Motor Home", 1024, &step_driver, 5, &initMotorTaskHandle); + xTaskCreate(vTaskPrintSwitchStates, "Print Switch States", 1024, NULL, 2, &printSwitchStatesTaskHandle); + + //vTaskCoreAffinitySet(initMotorTaskHandle, 0); + //vTaskCoreAffinitySet(printSwitchStatesTaskHandle, 0); + + vTaskStartScheduler(); + while(1) { + tight_loop_contents(); + } + + + //step_driver.set_conf1(10000, 0); + //step_driver.set_conf0(10000, 0); + //step_driver.pulse1(1000); + //step_driver.pulse0(1000); + //step_driver.wait1(0); + //step_driver.wait0(0); + +/* + gpio_init(LIMIT0_PIN); + gpio_set_dir(LIMIT0_PIN, GPIO_IN); + gpio_pull_up(LIMIT0_PIN); + gpio_set_input_hysteresis_enabled(LIMIT0_PIN, true); + + gpio_init(LIMIT1_PIN); + gpio_set_dir(LIMIT1_PIN, GPIO_IN); + gpio_pull_up(LIMIT1_PIN); + gpio_set_input_hysteresis_enabled(LIMIT1_PIN, true); + + gpio_init(LIMIT2_PIN); + gpio_set_dir(LIMIT2_PIN, GPIO_IN); + gpio_pull_up(LIMIT2_PIN); + gpio_set_input_hysteresis_enabled(LIMIT2_PIN, true); + + gpio_init(LIMIT3_PIN); + gpio_set_dir(LIMIT3_PIN, GPIO_IN); + gpio_pull_up(LIMIT3_PIN); + gpio_set_input_hysteresis_enabled(LIMIT3_PIN, true); + + gpio_set_irq_enabled_with_callback( + LIMIT0_PIN, + GPIO_IRQ_EDGE_RISE, + true, + isr_LimitSwitch + ); + gpio_set_irq_enabled_with_callback( + LIMIT1_PIN, + GPIO_IRQ_EDGE_RISE, + true, + isr_LimitSwitch + ); + gpio_set_irq_enabled_with_callback( + LIMIT2_PIN, + GPIO_IRQ_EDGE_RISE, + true, + isr_LimitSwitch + ); + gpio_set_irq_enabled_with_callback( + LIMIT3_PIN, + GPIO_IRQ_EDGE_RISE, + true, + isr_LimitSwitch + ); + + gpio_init(RGB2_B_PIN); + gpio_init(RGB2_G_PIN); + gpio_init(RGB2_R_PIN); + gpio_set_dir(RGB2_B_PIN, GPIO_OUT); + gpio_set_dir(RGB2_G_PIN, GPIO_OUT); + gpio_set_dir(RGB2_R_PIN, GPIO_OUT); + + gpio_init(RGB1_B_PIN); + gpio_init(RGB1_G_PIN); + gpio_set_dir(RGB1_B_PIN, GPIO_OUT); + gpio_set_dir(RGB1_G_PIN, GPIO_OUT); + + gpio_put(RGB1_B_PIN, 1); + gpio_put(RGB1_G_PIN, 1); + gpio_put(RGB2_B_PIN, 1); + gpio_put(RGB2_G_PIN, 1); + gpio_put(RGB2_R_PIN, 1); + + + gpio_init(LED_PIN); + gpio_set_dir(LED_PIN, GPIO_OUT); + + //step_driver.set_conf0(10000, 0); + //step_driver.pulse0(1000000); + homeMotors0(&step_driver); + printf("Home0 done\n"); + homeMotors1(&step_driver); + printf("Home1 done\n"); +*/ + //xTaskCreate(task_home0, "Home0", 1024, &step_driver, 5, NULL); + //xTaskCreate(task_home1, "Home1", 1024, &step_driver, 5, NULL); + + /* + while (true) { + sleep_ms(50); + gpio_put(LED_PIN, 0); + sleep_ms(50); + gpio_put(LED_PIN, 1); + + if(gpio_get(LIMIT1_PIN)) { + gpio_put(RGB1_B_PIN, 0); + } else { + gpio_put(RGB1_B_PIN, 1); + } + + if(gpio_get(LIMIT3_PIN)) { + gpio_put(RGB1_G_PIN, 0); + } else { + gpio_put(RGB1_G_PIN, 1); + } + + if(gpio_get(LIMIT0_PIN)) { + gpio_put(RGB2_B_PIN, 0); + } else { + gpio_put(RGB2_B_PIN, 1); + } + + if(gpio_get(LIMIT2_PIN)) { + gpio_put(RGB2_G_PIN, 0); + } else { + gpio_put(RGB2_G_PIN, 1); + } + } + */ + + return 0; +} \ No newline at end of file diff --git a/motor-control/core-xy-firmware/src/motors.cpp b/motor-control/core-xy-firmware/src/motors.cpp new file mode 100644 index 0000000..b2d4051 --- /dev/null +++ b/motor-control/core-xy-firmware/src/motors.cpp @@ -0,0 +1,153 @@ +#include "CoreXY.hpp" + +#include "FreeRTOS.h" +#include "task.h" + +void homeXY(TMC2209_step_dual * driver, HOME_AXIS_DIRECTION direction, unsigned int speed, unsigned int backupSpace, MotorPosition * last_pos) { + + uint8_t x_dir = direction & 0b01; + uint8_t y_dir = (direction & 0b10) >> 1; + + uint8_t x_dir_inv = x_dir ^ 1; + uint8_t y_dir_inv = y_dir ^ 1; + + uint8_t x_inv_limit = x_dir_inv == X_DIRECTION_BOTTOM ? LIMIT_X0 : LIMIT_X1; + uint8_t y_inv_limit = y_dir_inv == Y_DIRECTION_LEFT ? LIMIT_Y0 : LIMIT_Y1; + bool backup_x = !gpio_get(x_inv_limit) && (backupSpace > 0); + bool backup_y = !gpio_get(y_inv_limit) && (backupSpace > 0); + + // Start Backup if necessary + if(backup_x) { + if(MOTOR_X) { + driver->set_conf1(speed, x_dir_inv); + driver->pulse1(backupSpace); + } else { + driver->set_conf0(speed, y_dir_inv); + driver->pulse0(backupSpace); + } + } + + if(backup_y) { + if(MOTOR_Y) { + driver->set_conf1(speed, y_dir_inv); + driver->pulse1(backupSpace); + } else { + driver->set_conf0(speed, y_dir_inv); + driver->pulse0(backupSpace); + } + } + + + // Wait for end of Backup + if(backup_x) { + if(MOTOR_X) { + driver->wait1(0); + } else { + driver->wait0(0); + } + } + + if(backup_y) { + if(MOTOR_Y) { + driver->wait1(0); + } else { + driver->wait0(0); + } + } + + vTaskDelay(100 / portTICK_PERIOD_MS); + + // Start Motors + const unsigned int total_steps = 25000; + // Set Configuration + if(MOTOR_X) { + driver->set_conf1(speed, x_dir); + } else { + driver->set_conf0(speed, x_dir); + } + + if(MOTOR_Y) { + driver->set_conf1(speed, y_dir); + } else { + driver->set_conf0(speed, y_dir); + } + + // Start Motors + if(MOTOR_X) { + driver->pulse1(total_steps); + } else { + driver->pulse0(total_steps); + } + + if(MOTOR_Y) { + driver->pulse1(total_steps); + } else { + driver->pulse0(total_steps); + } + + bool x_done = false; + bool y_done = false; + + volatile int last_x = -1; + volatile int last_y = -1; + + for(int i=0; i<5000; i++) { + if(gpio_get(LIMIT_X0) || gpio_get(LIMIT_X1)) { + x_done = true; + if(MOTOR_X) { + driver->pulse1(0); + } else { + driver->pulse0(0); + } + } + + if(gpio_get(LIMIT_Y0) || gpio_get(LIMIT_Y1)) { + y_done = true; + if(MOTOR_Y) { + driver->pulse1(0); + } else { + driver->pulse0(0); + } + + } + + if(x_done && y_done) { + break; + } + + vTaskDelay(1 / portTICK_PERIOD_MS); + } + + // Emergency Stop + if(!(x_done && y_done)) { + driver->pulse0(0); + driver->pulse1(0); + } + + driver->wait0(0); + driver->wait1(0); + + if(MOTOR_X) { + last_pos->x = driver->get_remaining_steps1(); + } else { + last_pos->x = driver->get_remaining_steps0(); + } + + if(MOTOR_Y) { + last_pos->y = driver->get_remaining_steps1(); + } else { + last_pos->y = driver->get_remaining_steps0(); + } + + last_pos->x = total_steps - last_pos->x; + last_pos->y = total_steps - last_pos->y; +} + +MotorPosition homeSequence(TMC2209_step_dual * driver, MotorPosition * last_pos) { + MotorPosition res = {0, 0}; + + homeXY(driver, TOP_LEFT, HOME_SPEED_FAST, 0, last_pos); + homeXY(driver, TOP_LEFT, HOME_SPEED_SLOW, 500, &res); + + return res; +} \ No newline at end of file diff --git a/motor-control/core-xy-firmware/src/tmc2209/pulser.pio b/motor-control/core-xy-firmware/src/tmc2209/pulser.pio new file mode 100644 index 0000000..2f962d3 --- /dev/null +++ b/motor-control/core-xy-firmware/src/tmc2209/pulser.pio @@ -0,0 +1,43 @@ + +.program pulser +clock_div: + PULL NOBLOCK + MOV X, OSR + MOV Y, X +l0: + JMP Y-- l0 + + IRQ CLEAR 0 REL + ; SM 0 + 0 -> IRQ 0 + ; SM 1 + 0 -> IRQ 1 + + JMP clock_div + +counter: + ; SM 2 + 2 -> IRQ 0 + ; SM 3 + 2 -> IRQ 1 + PULL + MOV X, OSR + JMP !X l1_end + JMP X-- l1 +l1: + PULL NOBLOCK + MOV Y, OSR + JMP !Y l1_end + MOV X, OSR + + IRQ WAIT 2 REL + SET PINS, 1 + IRQ WAIT 2 REL + SET PINS, 0 + + JMP X-- l1 + +l1_end: + MOV ISR, X + PUSH NOBLOCK + IRQ SET 0 REL + ; SM 2 + 0 -> IRQ 2 + ; SM 3 + 0 -> IRQ 3 + JMP counter + diff --git a/motor-control/core-xy-firmware/src/tmc2209/step.cpp b/motor-control/core-xy-firmware/src/tmc2209/step.cpp new file mode 100644 index 0000000..ef46074 --- /dev/null +++ b/motor-control/core-xy-firmware/src/tmc2209/step.cpp @@ -0,0 +1,261 @@ +#include "step.hpp" +#include "pico/stdio.h" +#include "hardware/pio.h" +#include "hardware/irq.h" +#include + +#include + +#include "FreeRTOSConfig.h" +#include "FreeRTOS.h" +#include "task.h" +#include "semphr.h" + +TMC2209_step_dual * g_step_driver_instance; + +void step_irq0_handler() { + g_step_driver_instance->irq_handler0(); +}; + +void step_irq1_handler() { + g_step_driver_instance->irq_handler1(); +}; + +TMC2209_step_dual::TMC2209_step_dual( + uint step0_pin, + uint step1_pin, + uint dir0_pin, + uint dir1_pin, + uint enable0_pin, + uint enable1_pin, + PIO pio_core +) { + this->step0_pin = step0_pin; + this->step1_pin = step1_pin; + this->dir0_pin = dir0_pin; + this->dir1_pin = dir1_pin; + this->enable0_pin = enable0_pin; + this->enable1_pin = enable1_pin; + this->pio_core = pio_core; + + g_step_driver_instance = this; + + this->xDoneSemaphore0 = xSemaphoreCreateBinary(); + this->xDoneSemaphore1 = xSemaphoreCreateBinary(); + + init_gpio(); + init_pio(); +}; + +void TMC2209_step_dual::init_gpio() { + gpio_init(this->enable0_pin); + gpio_init(this->enable1_pin); + gpio_set_dir(this->enable0_pin, GPIO_OUT); + gpio_set_dir(this->enable1_pin, GPIO_OUT); + gpio_put(this->enable0_pin, 0); + gpio_put(this->enable1_pin, 0); + sleep_ms(1); + + gpio_init(this->dir0_pin); + gpio_init(this->dir1_pin); + gpio_set_dir(this->dir0_pin, GPIO_OUT); + gpio_set_dir(this->dir1_pin, GPIO_OUT); + gpio_put(this->dir0_pin, 1); + gpio_put(this->dir1_pin, 1); + sleep_ms(1); +}; + +void TMC2209_step_dual::init_pio() { + pio_sm_set_enabled(this->pio_core, this->sm_num_divider0, false); + pio_sm_set_enabled(this->pio_core, this->sm_num_divider1, false); + pio_sm_set_enabled(this->pio_core, this->sm_num_counter0, false); + pio_sm_set_enabled(this->pio_core, this->sm_num_counter1, false); + + this->pulser_program_offset = pio_add_program(this->pio_core, &pulser_program); + + this->sm_config_divider0 = pulser_program_get_default_config(this->pulser_program_offset); + this->sm_config_divider1 = pulser_program_get_default_config(this->pulser_program_offset); + this->sm_config_counter0 = pulser_program_get_default_config(this->pulser_program_offset); + this->sm_config_counter1 = pulser_program_get_default_config(this->pulser_program_offset); + + pio_gpio_init(this->pio_core, this->step0_pin); + pio_gpio_init(this->pio_core, this->step1_pin); + + pio_sm_set_consecutive_pindirs(this->pio_core, this->sm_num_counter0, this->step0_pin, 1, true); + pio_sm_set_consecutive_pindirs(this->pio_core, this->sm_num_counter1, this->step1_pin, 1, true); + + sm_config_set_set_pins(&this->sm_config_counter0, this->step0_pin, 1); + sm_config_set_set_pins(&this->sm_config_counter1, this->step1_pin, 1); + + pio_sm_init( + this->pio_core, + this->sm_num_divider0, + this->pulser_program_offset + this->subprogram_offset_divider, + &this->sm_config_divider0 + ); + pio_sm_init( + this->pio_core, + this->sm_num_divider1, + this->pulser_program_offset + this->subprogram_offset_divider, + &this->sm_config_divider1 + ); + pio_sm_init( + this->pio_core, + this->sm_num_counter0, + this->pulser_program_offset + this->subprogram_offset_counter, + &this->sm_config_counter0 + ); + pio_sm_init( + this->pio_core, + this->sm_num_counter1, + this->pulser_program_offset + this->subprogram_offset_counter, + &this->sm_config_counter1 + ); + + float clock_div = ((float)(SYS_CLK_KHZ))/(base_clock_freq_khz); + + pio_sm_set_clkdiv(this->pio_core, this->sm_num_divider0, clock_div); + pio_sm_set_clkdiv(this->pio_core, this->sm_num_divider1, clock_div); + pio_sm_set_clkdiv(this->pio_core, this->sm_num_counter0, 1.0); + pio_sm_set_clkdiv(this->pio_core, this->sm_num_counter1, 1.0); + + pio_sm_set_enabled(this->pio_core, this->sm_num_divider0, true); + pio_sm_set_enabled(this->pio_core, this->sm_num_divider1, true); + pio_sm_set_enabled(this->pio_core, this->sm_num_counter0, true); + pio_sm_set_enabled(this->pio_core, this->sm_num_counter1, true); + + pio_set_irq0_source_enabled(this->pio_core, pis_interrupt2, true); + pio_set_irq1_source_enabled(this->pio_core, pis_interrupt3, true); + + if(this->pio_core == pio0) { + irq_set_exclusive_handler(PIO0_IRQ_0, step_irq0_handler); + irq_set_exclusive_handler(PIO0_IRQ_1, step_irq1_handler); + + irq_set_enabled(PIO0_IRQ_0, true); + irq_set_enabled(PIO0_IRQ_1, true); + } else if(this->pio_core == pio1) { + irq_set_exclusive_handler(PIO1_IRQ_0, step_irq0_handler); + irq_set_exclusive_handler(PIO1_IRQ_1, step_irq1_handler); + + irq_set_enabled(PIO1_IRQ_0, true); + irq_set_enabled(PIO1_IRQ_1, true); + } + + set_conf0(1000, 0); + set_conf1(1000, 0); +}; + +void TMC2209_step_dual::set_conf0(uint frequency, bool dir) { + uint32_t cycles = (uint32_t)(base_clock_freq_khz / (((float)(frequency))/1000.0)) - 6; + + gpio_put(this->dir0_pin, dir); + pio_sm_put_blocking( + this->pio_core, + this->sm_num_divider0, + cycles + ); +}; + +void TMC2209_step_dual::set_conf1(uint frequency, bool dir) { + uint32_t cycles = (uint32_t)(base_clock_freq_khz / (((float)(frequency))/1000.0)) - 6; + + gpio_put(this->dir1_pin, dir); + pio_sm_put_blocking( + this->pio_core, + this->sm_num_divider1, + cycles + ); +}; + +void TMC2209_step_dual::pulse0(uint n) { + if(n > 0) + this->remaining_step_count0 = n; + + pio_sm_put_blocking( + this->pio_core, this->sm_num_counter0, n + ); +}; + +void TMC2209_step_dual::pulse1(uint n) { + if(n > 0) + this->remaining_step_count1 = n; + + pio_sm_put_blocking( + this->pio_core, this->sm_num_counter1, n + ); +}; + +void TMC2209_step_dual::pulse0_int(int n) { + if(n == 0) + this->remaining_step_count0 = n; + + if(n < 0) { + n = abs(n); + gpio_put(this->dir1_pin, gpio_get(this->dir1_pin) ^ 1); + } + + pio_sm_put_blocking( + this->pio_core, this->sm_num_counter0, n + ); +}; + +void TMC2209_step_dual::pulse1_int(int n) { + if(n == 0) + this->remaining_step_count1 = n; + + + if(n < 0) { + n = abs(n); + gpio_put(this->dir1_pin, gpio_get(this->dir1_pin) ^ 1); + } + + pio_sm_put_blocking( + this->pio_core, this->sm_num_counter1, n + ); +}; + +void TMC2209_step_dual::irq_handler0() { + uint n = pio_sm_get(this->pio_core, this->sm_num_counter0); + if(n > 0) + this->remaining_step_count0 = n; + + BaseType_t xHigherPriorityTaskWoken = pdFALSE; + xSemaphoreGiveFromISR(this->xDoneSemaphore0, &xHigherPriorityTaskWoken); + + pio_interrupt_clear(this->pio_core, 2); + + portYIELD_FROM_ISR(xHigherPriorityTaskWoken); +} + +void TMC2209_step_dual::irq_handler1() { + uint n = pio_sm_get(this->pio_core, this->sm_num_counter1); + if(n > 0) + this->remaining_step_count1 = n; + + BaseType_t xHigherPriorityTaskWoken = pdFALSE; + xSemaphoreGiveFromISR(this->xDoneSemaphore1, &xHigherPriorityTaskWoken); + + pio_interrupt_clear(this->pio_core, 3); + + portYIELD_FROM_ISR(xHigherPriorityTaskWoken); +} + +void TMC2209_step_dual::wait0(int timeout_ms) { + int t = timeout_ms <= 0 ? portMAX_DELAY : timeout_ms / portTICK_PERIOD_MS; + + xSemaphoreTake(this->xDoneSemaphore0, t); +}; + +void TMC2209_step_dual::wait1(int timeout_ms) { + int t = timeout_ms <= 0 ? portMAX_DELAY : timeout_ms / portTICK_PERIOD_MS; + + xSemaphoreTake(this->xDoneSemaphore1, t); +}; + +uint TMC2209_step_dual::get_remaining_steps0() { + return this->remaining_step_count0; +}; + +uint TMC2209_step_dual::get_remaining_steps1() { + return this->remaining_step_count1; +}; \ No newline at end of file diff --git a/motor-control/core-xy-firmware/src/tmc2209/uart_interface.cpp b/motor-control/core-xy-firmware/src/tmc2209/uart_interface.cpp new file mode 100644 index 0000000..d02b571 --- /dev/null +++ b/motor-control/core-xy-firmware/src/tmc2209/uart_interface.cpp @@ -0,0 +1,80 @@ +#include "pico/stdlib.h" +#include "pico.h" +#include "hardware/uart.h" + +#include "tmc2209.hpp" + +TMC2209_UART::TMC2209_UART( + uart_inst_t *uart_inst, + uint8_t node_address, + uint baudrate, + uint tx_pin, + uint rx_pin +) { + this->uart_inst = uart_inst; + this->node_address = node_address & 0b11; + this->baudrate = baudrate; + this->tx_pin = tx_pin; + this->rx_pin = rx_pin; +} + +void TMC2209_UART::init() { + gpio_set_function(this->tx_pin, UART_FUNCSEL_NUM(this->uart_inst, this->tx_pin)); + gpio_set_function(this->rx_pin, UART_FUNCSEL_NUM(this->uart_inst, this->rx_pin)); + + // UART format: 8 data bits, 1 stop bit, no parity + // -> https://www.analog.com/media/en/technical-documentation/data-sheets/TMC2209_datasheet_rev1.09.pdf + uart_set_format(this->uart_inst, 8, 1, UART_PARITY_NONE); + + // Clear to Send (CTS) and Request to Send (RTS) are not used + // (This is for automaticlly telling the other side that it is ready to receive/send data) + uart_set_hw_flow(this->uart_inst, false, false); + + uart_set_fifo_enabled(this->uart_inst, true); + + uart_init(this->uart_inst, this->baudrate); +} + +uint8_t TMC2209_UART::calc_crc8_atm(uint8_t *data, uint8_t size) { + int i,j; + uint8_t crc = 0; // CRC located in last byte of message + uint8_t currentByte; + + for (i=0; i> 7) ^ (currentByte & 0x01)) { // update CRC based result of XOR operation + crc = (crc << 1) ^ 0x07; + } else { + crc = (crc << 1); + } + currentByte = currentByte >> 1; + } // for CRC bit + } // for message byte + + return crc; +} + +uint32_t TMC2209_UART::read(uint8_t address) { + uint8_t data[4] = { + 0b01010101, // Sync byte + this->node_address, // Node address + (address & (uint8_t)(0x7F)), // Register address, last bit is 0 for read + 0 + }; + + data[3] = this->calc_crc8_atm(data, 3); + + uart_write_blocking(this->uart_inst, data, 4); + + uint8_t response[12]; + uart_read_blocking(this->uart_inst, response, 12); + + if(response[11] != calc_crc8_atm(&response[4], 7)) { + this->crc_error_flag = true; + return 0; + } else { + return *((uint32_t*) &response[7]); + } +} \ No newline at end of file diff --git a/motor-control/core-xy-firmware/src/tmc2209/uart_registers.cpp b/motor-control/core-xy-firmware/src/tmc2209/uart_registers.cpp new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/motor-control/core-xy-firmware/src/tmc2209/uart_registers.cpp @@ -0,0 +1 @@ +