├── .github └── workflows │ └── dist.yml ├── .gitignore ├── .readthedocs.yml ├── LICENSE ├── README.rst ├── devtools ├── build_and_deploy.sh └── exclude_commits_navx ├── docs ├── Makefile ├── api.rst ├── conf.py ├── index.rst ├── magicbot.rst ├── make.bat ├── requirements.txt ├── robotpy_ext.autonomous.rst ├── robotpy_ext.common_drivers.rst ├── robotpy_ext.control.rst └── robotpy_ext.misc.rst ├── magicbot ├── __init__.py ├── inject.py ├── magic_reset.py ├── magic_reset.pyi ├── magic_tunable.py ├── magicbot_tests.py ├── magiccomponent.py ├── magicrobot.py ├── py.typed └── state_machine.py ├── robotpy_ext ├── __init__.py ├── autonomous │ ├── __init__.py │ ├── selector.py │ ├── selector_tests.py │ └── stateful_autonomous.py ├── common_drivers │ ├── README.rst │ ├── __init__.py │ ├── distance_sensors.py │ ├── distance_sensors_sim.py │ ├── driver_base.py │ ├── pressure_sensors.py │ ├── units.py │ └── xl_max_sonar_ez.py ├── control │ ├── __init__.py │ ├── button_debouncer.py │ └── toggle.py └── misc │ ├── __init__.py │ ├── annotations.py │ ├── asyncio_policy.py │ ├── crc7.py │ ├── looptimer.py │ ├── orderedclass.py │ ├── periodic_filter.py │ ├── precise_delay.py │ └── simple_watchdog.py ├── setup.cfg ├── setup.py └── tests ├── conftest.py ├── requirements.txt ├── run_tests.py ├── test_distance_sensors.py ├── test_magicbot_feedback.py ├── test_magicbot_inject.py ├── test_magicbot_injection.py ├── test_magicbot_sm.py ├── test_magicbot_tunable.py ├── test_pressure_sensor.py ├── test_toggle.py ├── test_units.py └── test_xl_max_sonar_ez.py /.github/workflows/dist.yml: -------------------------------------------------------------------------------- 1 | --- 2 | name: dist 3 | 4 | on: 5 | pull_request: 6 | push: 7 | branches: 8 | - main 9 | tags: 10 | - '*' 11 | 12 | jobs: 13 | ci: 14 | uses: robotpy/build-actions/.github/workflows/package-pure.yml@v2025 15 | secrets: 16 | META_REPO_ACCESS_TOKEN: ${{ secrets.REPO_ACCESS_TOKEN }} 17 | PYPI_API_TOKEN: ${{ secrets.PYPI_PASSWORD }} 18 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | 5 | # C extensions 6 | *.so 7 | 8 | # Distribution / packaging 9 | .Python 10 | env/ 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | lib/ 17 | lib64/ 18 | parts/ 19 | sdist/ 20 | var/ 21 | *.egg-info/ 22 | .installed.cfg 23 | *.egg 24 | 25 | # PyInstaller 26 | # Usually these files are written by a python script from a template 27 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 28 | *.manifest 29 | *.spec 30 | 31 | # Installer logs 32 | pip-log.txt 33 | pip-delete-this-directory.txt 34 | 35 | # Unit test / coverage reports 36 | htmlcov/ 37 | .tox/ 38 | .coverage 39 | .cache 40 | nosetests.xml 41 | coverage.xml 42 | 43 | # Translations 44 | *.mo 45 | *.pot 46 | 47 | # Django stuff: 48 | *.log 49 | 50 | # Sphinx documentation 51 | docs/_build/ 52 | docs/_sidebar.rst.inc 53 | 54 | # PyBuilder 55 | target/ 56 | 57 | .project 58 | .pydevproject 59 | 60 | #PyCharm Project files 61 | *.iml 62 | .idea/* 63 | .pycharmproject 64 | 65 | robotpy_ext/version.py 66 | .deploy_cfg 67 | 68 | -------------------------------------------------------------------------------- /.readthedocs.yml: -------------------------------------------------------------------------------- 1 | version: 2 2 | 3 | sphinx: 4 | configuration: docs/conf.py 5 | 6 | build: 7 | os: ubuntu-22.04 8 | tools: 9 | python: "3.11" 10 | 11 | python: 12 | install: 13 | - requirements: docs/requirements.txt 14 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2014-2016, Dustin Spicuzza & Contributors 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of robotpy-wpilib-utilities nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | -------------------------------------------------------------------------------- /README.rst: -------------------------------------------------------------------------------- 1 | robotpy-wpilib-utilities 2 | ======================== 3 | 4 | Useful utility functions/objects for RobotPy WPILib (2015+). 5 | 6 | Documentation 7 | ------------- 8 | 9 | * API Documentation can be found at https://robotpy.readthedocs.io/projects/utilities/en/stable/ 10 | * Example programs can be found at https://github.com/robotpy/examples 11 | 12 | Contribution guidelines 13 | ----------------------- 14 | 15 | This repository is intended to be a common place for high quality code to live 16 | for "things that should be in WPILib, but aren't". The python implementation of 17 | WPILib is intended to be very close to the implementations in the other languages, 18 | which is where packages like this come in. 19 | 20 | * Most anything will be accepted, but ideally full frameworks will be separate 21 | packages and don't belong here 22 | * Ideally, contributions will have unit tests 23 | * Ideally, contributions will not have external python dependencies other than 24 | WPILib -- though, this may change. 25 | * Contributions will work (or at least, not break) on all supported RobotPy 26 | platforms (Windows/Linux/OSX, RoboRio) 27 | * All pull requests will be tested using Travis-CI 28 | 29 | Installation 30 | ------------ 31 | 32 | This library is automatically installed when you install pyfrc or RobotPy 33 | 34 | License 35 | ------- 36 | 37 | BSD License, similar to WPILib. 38 | 39 | Authors 40 | ------- 41 | 42 | - Dustin Spicuzza (dustin@virtualroadside.com) 43 | - Tim Winters (twinters@wpi.edu) 44 | - David Vo (@auscompgeek) 45 | - Insert your name here! 46 | -------------------------------------------------------------------------------- /devtools/build_and_deploy.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -e 4 | cd `dirname $0`/.. 5 | 6 | ROBOTPY_DIR="../robotpy-wpilib" 7 | 8 | source $ROBOTPY_DIR/devtools/_windows_env.sh 9 | 10 | VERSION=`git describe --tags --long --dirty='-dirty'` 11 | 12 | if [[ ! $VERSION =~ ^[0-9]+\.[0-9]\.[0-9]+$ ]]; then 13 | # Convert to PEP440 14 | IFS=- read VTAG VCOMMITS VLOCAL <<< "$VERSION" 15 | VERSION=`printf "%s.post0.dev%s" $VTAG $VCOMMITS` 16 | fi 17 | 18 | python3 setup.py sdist --formats=gztar 19 | 20 | # Run the install now 21 | python3 -m robotpy_installer install -U --force-reinstall --no-deps dist/robotpy-wpilib-utilities-$VERSION.tar.gz 22 | -------------------------------------------------------------------------------- /devtools/exclude_commits_navx: -------------------------------------------------------------------------------- 1 | d34b079bfaeab387915436946233367d6bedb370 # enableLogging method 2 | -------------------------------------------------------------------------------- /docs/Makefile: -------------------------------------------------------------------------------- 1 | # Makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line. 5 | SPHINXOPTS = 6 | SPHINXBUILD = sphinx-build 7 | PAPER = 8 | BUILDDIR = _build 9 | 10 | # User-friendly check for sphinx-build 11 | ifeq ($(shell which $(SPHINXBUILD) >/dev/null 2>&1; echo $$?), 1) 12 | $(error The '$(SPHINXBUILD)' command was not found. Make sure you have Sphinx installed, then set the SPHINXBUILD environment variable to point to the full path of the '$(SPHINXBUILD)' executable. Alternatively you can add the directory with the executable to your PATH. If you don't have Sphinx installed, grab it from http://sphinx-doc.org/) 13 | endif 14 | 15 | # Internal variables. 16 | PAPEROPT_a4 = -D latex_paper_size=a4 17 | PAPEROPT_letter = -D latex_paper_size=letter 18 | ALLSPHINXOPTS = -d $(BUILDDIR)/doctrees $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) . 19 | # the i18n builder cannot share the environment and doctrees with the others 20 | I18NSPHINXOPTS = $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) . 21 | 22 | .PHONY: help clean html dirhtml singlehtml pickle json htmlhelp qthelp devhelp epub latex latexpdf text man changes linkcheck doctest gettext 23 | 24 | help: 25 | @echo "Please use \`make ' where is one of" 26 | @echo " html to make standalone HTML files" 27 | @echo " dirhtml to make HTML files named index.html in directories" 28 | @echo " singlehtml to make a single large HTML file" 29 | @echo " pickle to make pickle files" 30 | @echo " json to make JSON files" 31 | @echo " htmlhelp to make HTML files and a HTML help project" 32 | @echo " qthelp to make HTML files and a qthelp project" 33 | @echo " devhelp to make HTML files and a Devhelp project" 34 | @echo " epub to make an epub" 35 | @echo " latex to make LaTeX files, you can set PAPER=a4 or PAPER=letter" 36 | @echo " latexpdf to make LaTeX files and run them through pdflatex" 37 | @echo " latexpdfja to make LaTeX files and run them through platex/dvipdfmx" 38 | @echo " text to make text files" 39 | @echo " man to make manual pages" 40 | @echo " texinfo to make Texinfo files" 41 | @echo " info to make Texinfo files and run them through makeinfo" 42 | @echo " gettext to make PO message catalogs" 43 | @echo " changes to make an overview of all changed/added/deprecated items" 44 | @echo " xml to make Docutils-native XML files" 45 | @echo " pseudoxml to make pseudoxml-XML files for display purposes" 46 | @echo " linkcheck to check all external links for integrity" 47 | @echo " doctest to run all doctests embedded in the documentation (if enabled)" 48 | 49 | clean: 50 | rm -rf $(BUILDDIR)/* 51 | 52 | html: 53 | $(SPHINXBUILD) -b html $(ALLSPHINXOPTS) $(BUILDDIR)/html 54 | @echo 55 | @echo "Build finished. The HTML pages are in $(BUILDDIR)/html." 56 | 57 | dirhtml: 58 | $(SPHINXBUILD) -b dirhtml $(ALLSPHINXOPTS) $(BUILDDIR)/dirhtml 59 | @echo 60 | @echo "Build finished. The HTML pages are in $(BUILDDIR)/dirhtml." 61 | 62 | singlehtml: 63 | $(SPHINXBUILD) -b singlehtml $(ALLSPHINXOPTS) $(BUILDDIR)/singlehtml 64 | @echo 65 | @echo "Build finished. The HTML page is in $(BUILDDIR)/singlehtml." 66 | 67 | pickle: 68 | $(SPHINXBUILD) -b pickle $(ALLSPHINXOPTS) $(BUILDDIR)/pickle 69 | @echo 70 | @echo "Build finished; now you can process the pickle files." 71 | 72 | json: 73 | $(SPHINXBUILD) -b json $(ALLSPHINXOPTS) $(BUILDDIR)/json 74 | @echo 75 | @echo "Build finished; now you can process the JSON files." 76 | 77 | htmlhelp: 78 | $(SPHINXBUILD) -b htmlhelp $(ALLSPHINXOPTS) $(BUILDDIR)/htmlhelp 79 | @echo 80 | @echo "Build finished; now you can run HTML Help Workshop with the" \ 81 | ".hhp project file in $(BUILDDIR)/htmlhelp." 82 | 83 | qthelp: 84 | $(SPHINXBUILD) -b qthelp $(ALLSPHINXOPTS) $(BUILDDIR)/qthelp 85 | @echo 86 | @echo "Build finished; now you can run "qcollectiongenerator" with the" \ 87 | ".qhcp project file in $(BUILDDIR)/qthelp, like this:" 88 | @echo "# qcollectiongenerator $(BUILDDIR)/qthelp/sphinx.qhcp" 89 | @echo "To view the help file:" 90 | @echo "# assistant -collectionFile $(BUILDDIR)/qthelp/sphinx.qhc" 91 | 92 | devhelp: 93 | $(SPHINXBUILD) -b devhelp $(ALLSPHINXOPTS) $(BUILDDIR)/devhelp 94 | @echo 95 | @echo "Build finished." 96 | @echo "To view the help file:" 97 | @echo "# mkdir -p $$HOME/.local/share/devhelp/sphinx" 98 | @echo "# ln -s $(BUILDDIR)/devhelp $$HOME/.local/share/devhelp/sphinx" 99 | @echo "# devhelp" 100 | 101 | epub: 102 | $(SPHINXBUILD) -b epub $(ALLSPHINXOPTS) $(BUILDDIR)/epub 103 | @echo 104 | @echo "Build finished. The epub file is in $(BUILDDIR)/epub." 105 | 106 | latex: 107 | $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex 108 | @echo 109 | @echo "Build finished; the LaTeX files are in $(BUILDDIR)/latex." 110 | @echo "Run \`make' in that directory to run these through (pdf)latex" \ 111 | "(use \`make latexpdf' here to do that automatically)." 112 | 113 | latexpdf: 114 | $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex 115 | @echo "Running LaTeX files through pdflatex..." 116 | $(MAKE) -C $(BUILDDIR)/latex all-pdf 117 | @echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex." 118 | 119 | latexpdfja: 120 | $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex 121 | @echo "Running LaTeX files through platex and dvipdfmx..." 122 | $(MAKE) -C $(BUILDDIR)/latex all-pdf-ja 123 | @echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex." 124 | 125 | text: 126 | $(SPHINXBUILD) -b text $(ALLSPHINXOPTS) $(BUILDDIR)/text 127 | @echo 128 | @echo "Build finished. The text files are in $(BUILDDIR)/text." 129 | 130 | man: 131 | $(SPHINXBUILD) -b man $(ALLSPHINXOPTS) $(BUILDDIR)/man 132 | @echo 133 | @echo "Build finished. The manual pages are in $(BUILDDIR)/man." 134 | 135 | texinfo: 136 | $(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo 137 | @echo 138 | @echo "Build finished. The Texinfo files are in $(BUILDDIR)/texinfo." 139 | @echo "Run \`make' in that directory to run these through makeinfo" \ 140 | "(use \`make info' here to do that automatically)." 141 | 142 | info: 143 | $(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo 144 | @echo "Running Texinfo files through makeinfo..." 145 | make -C $(BUILDDIR)/texinfo info 146 | @echo "makeinfo finished; the Info files are in $(BUILDDIR)/texinfo." 147 | 148 | gettext: 149 | $(SPHINXBUILD) -b gettext $(I18NSPHINXOPTS) $(BUILDDIR)/locale 150 | @echo 151 | @echo "Build finished. The message catalogs are in $(BUILDDIR)/locale." 152 | 153 | changes: 154 | $(SPHINXBUILD) -b changes $(ALLSPHINXOPTS) $(BUILDDIR)/changes 155 | @echo 156 | @echo "The overview file is in $(BUILDDIR)/changes." 157 | 158 | linkcheck: 159 | $(SPHINXBUILD) -b linkcheck $(ALLSPHINXOPTS) $(BUILDDIR)/linkcheck 160 | @echo 161 | @echo "Link check complete; look for any errors in the above output " \ 162 | "or in $(BUILDDIR)/linkcheck/output.txt." 163 | 164 | doctest: 165 | $(SPHINXBUILD) -b doctest $(ALLSPHINXOPTS) $(BUILDDIR)/doctest 166 | @echo "Testing of doctests in the sources finished, look at the " \ 167 | "results in $(BUILDDIR)/doctest/output.txt." 168 | 169 | xml: 170 | $(SPHINXBUILD) -b xml $(ALLSPHINXOPTS) $(BUILDDIR)/xml 171 | @echo 172 | @echo "Build finished. The XML files are in $(BUILDDIR)/xml." 173 | 174 | pseudoxml: 175 | $(SPHINXBUILD) -b pseudoxml $(ALLSPHINXOPTS) $(BUILDDIR)/pseudoxml 176 | @echo 177 | @echo "Build finished. The pseudo-XML files are in $(BUILDDIR)/pseudoxml." 178 | -------------------------------------------------------------------------------- /docs/api.rst: -------------------------------------------------------------------------------- 1 | 2 | Utilities API 3 | ============= 4 | 5 | .. toctree:: 6 | 7 | magicbot 8 | robotpy_ext.autonomous 9 | robotpy_ext.common_drivers 10 | robotpy_ext.control 11 | robotpy_ext.misc 12 | 13 | 14 | -------------------------------------------------------------------------------- /docs/conf.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import sys 4 | import os 5 | 6 | from os.path import abspath, join, dirname 7 | 8 | # Insert module path here 9 | sys.path.insert(0, abspath(dirname(__file__))) 10 | sys.path.insert(0, abspath(join(dirname(__file__), ".."))) 11 | 12 | import robotpy_ext 13 | 14 | # -- RTD configuration ------------------------------------------------ 15 | 16 | # on_rtd is whether we are on readthedocs.org, this line of code grabbed from docs.readthedocs.org 17 | on_rtd = os.environ.get("READTHEDOCS", None) == "True" 18 | 19 | # This is used for linking and such so we link to the thing we're building 20 | rtd_version = os.environ.get("READTHEDOCS_VERSION", "latest") 21 | if rtd_version not in ["stable", "latest"]: 22 | rtd_version = "stable" 23 | 24 | # -- General configuration ------------------------------------------------ 25 | 26 | # Add any Sphinx extension module names here, as strings. They can be 27 | # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom 28 | # ones. 29 | extensions = [ 30 | "sphinx.ext.autodoc", 31 | "sphinx.ext.viewcode", 32 | "sphinx.ext.intersphinx", 33 | "sphinx_autodoc_typehints", 34 | ] 35 | 36 | # The suffix of source filenames. 37 | source_suffix = ".rst" 38 | 39 | # The master toctree document. 40 | master_doc = "index" 41 | 42 | # General information about the project. 43 | project = "RobotPy WPILib Utilities" 44 | copyright = "2015, RobotPy development team" 45 | 46 | intersphinx_mapping = { 47 | "commandsv1": ( 48 | f"https://robotpy.readthedocs.io/projects/commands-v1/en/{rtd_version}/", 49 | None, 50 | ), 51 | "networktables": ( 52 | f"https://robotpy.readthedocs.io/projects/pynetworktables/en/{rtd_version}/", 53 | None, 54 | ), 55 | "wpilib": ( 56 | f"https://robotpy.readthedocs.io/projects/wpilib/en/{rtd_version}/", 57 | None, 58 | ), 59 | } 60 | 61 | # The version info for the project you're documenting, acts as replacement for 62 | # |version| and |release|, also used in various other places throughout the 63 | # built documents. 64 | # 65 | # The short X.Y version. 66 | version = ".".join(robotpy_ext.__version__.split(".")[:2]) 67 | # The full version, including alpha/beta/rc tags. 68 | release = robotpy_ext.__version__ 69 | 70 | autoclass_content = "both" 71 | 72 | # List of patterns, relative to source directory, that match files and 73 | # directories to ignore when looking for source files. 74 | exclude_patterns = ["_build"] 75 | 76 | # The name of the Pygments (syntax highlighting) style to use. 77 | pygments_style = "sphinx" 78 | 79 | # -- Options for HTML output ---------------------------------------------- 80 | 81 | html_theme = "sphinx_rtd_theme" 82 | 83 | # Output file base name for HTML help builder. 84 | htmlhelp_basename = "sphinxdoc" 85 | 86 | # -- Options for LaTeX output --------------------------------------------- 87 | 88 | latex_elements = {} 89 | 90 | # Grouping the document tree into LaTeX files. List of tuples 91 | # (source start file, target name, title, 92 | # author, documentclass [howto, manual, or own class]). 93 | latex_documents = [("index", "sphinx.tex", ". Documentation", "Author", "manual")] 94 | 95 | # -- Options for manual page output --------------------------------------- 96 | 97 | # One entry per manual page. List of tuples 98 | # (source start file, name, description, authors, manual section). 99 | man_pages = [("index", "sphinx", ". Documentation", ["Author"], 1)] 100 | 101 | # -- Options for Texinfo output ------------------------------------------- 102 | 103 | # Grouping the document tree into Texinfo files. List of tuples 104 | # (source start file, target name, title, author, 105 | # dir menu entry, description, category) 106 | texinfo_documents = [ 107 | ( 108 | "index", 109 | "sphinx", 110 | ". Documentation", 111 | "Author", 112 | "sphinx", 113 | "One line description of project.", 114 | "Miscellaneous", 115 | ) 116 | ] 117 | 118 | # -- Options for Epub output ---------------------------------------------- 119 | 120 | # Bibliographic Dublin Core info. 121 | epub_title = "." 122 | epub_author = "Author" 123 | epub_publisher = "Author" 124 | epub_copyright = "2015, Author" 125 | 126 | # A list of files that should not be packed into the epub file. 127 | epub_exclude_files = ["search.html"] 128 | 129 | # -- Custom Document processing ---------------------------------------------- 130 | 131 | from robotpy_sphinx.sidebar import generate_sidebar 132 | 133 | generate_sidebar( 134 | globals(), 135 | "utilities", 136 | "https://raw.githubusercontent.com/robotpy/docs-sidebar/master/sidebar.toml", 137 | ) 138 | -------------------------------------------------------------------------------- /docs/index.rst: -------------------------------------------------------------------------------- 1 | 2 | robotpy-wpilib-utilities documentation 3 | ====================================== 4 | 5 | This project is intended to be a common place for high quality code to live 6 | for "things that should be in WPILib, but aren't". The python implementation of 7 | WPILib is intended to be very close to the implementations in the other languages, 8 | which is where packages like this come in. 9 | 10 | * Most anything will be accepted, but ideally full frameworks will be separate 11 | packages and don't belong here 12 | * Ideally, contributions will have unit tests 13 | * Ideally, contributions will not have external python dependencies other than 14 | WPILib -- though, this may change. 15 | * Contributions will work (or at least, not break) on all supported RobotPy 16 | platforms (Windows/Linux/OSX, RoboRio) 17 | * All pull requests will be tested using Travis-CI 18 | 19 | .. include:: _sidebar.rst.inc 20 | 21 | Indices and tables 22 | ================== 23 | 24 | * :ref:`genindex` 25 | * :ref:`modindex` 26 | * :ref:`search` 27 | -------------------------------------------------------------------------------- /docs/magicbot.rst: -------------------------------------------------------------------------------- 1 | 2 | .. _magicbot_api: 3 | 4 | magicbot module 5 | ---------------- 6 | 7 | .. module:: magicbot 8 | 9 | .. automodule:: magicbot.magicrobot 10 | :members: 11 | :exclude-members: autonomous, disabled, members, operatorControl, robotInit, test 12 | :show-inheritance: 13 | 14 | Component 15 | ~~~~~~~~~ 16 | 17 | .. automodule:: magicbot.magiccomponent 18 | :members: 19 | :undoc-members: 20 | :show-inheritance: 21 | 22 | Tunable 23 | ~~~~~~~ 24 | 25 | .. automodule:: magicbot.magic_tunable 26 | :members: 27 | :undoc-members: 28 | :show-inheritance: 29 | 30 | Resettable 31 | ~~~~~~~~~~ 32 | 33 | .. automodule:: magicbot.magic_reset 34 | :members: 35 | :undoc-members: 36 | :show-inheritance: 37 | 38 | State machines 39 | ~~~~~~~~~~~~~~ 40 | 41 | .. automodule:: magicbot.state_machine 42 | :members: 43 | :exclude-members: members 44 | :undoc-members: 45 | :show-inheritance: 46 | -------------------------------------------------------------------------------- /docs/make.bat: -------------------------------------------------------------------------------- 1 | @ECHO OFF 2 | 3 | REM Command file for Sphinx documentation 4 | 5 | if "%SPHINXBUILD%" == "" ( 6 | set SPHINXBUILD=sphinx-build 7 | ) 8 | set BUILDDIR=_build 9 | set ALLSPHINXOPTS=-d %BUILDDIR%/doctrees %SPHINXOPTS% . 10 | set I18NSPHINXOPTS=%SPHINXOPTS% . 11 | if NOT "%PAPER%" == "" ( 12 | set ALLSPHINXOPTS=-D latex_paper_size=%PAPER% %ALLSPHINXOPTS% 13 | set I18NSPHINXOPTS=-D latex_paper_size=%PAPER% %I18NSPHINXOPTS% 14 | ) 15 | 16 | if "%1" == "" goto help 17 | 18 | if "%1" == "help" ( 19 | :help 20 | echo.Please use `make ^` where ^ is one of 21 | echo. html to make standalone HTML files 22 | echo. dirhtml to make HTML files named index.html in directories 23 | echo. singlehtml to make a single large HTML file 24 | echo. pickle to make pickle files 25 | echo. json to make JSON files 26 | echo. htmlhelp to make HTML files and a HTML help project 27 | echo. qthelp to make HTML files and a qthelp project 28 | echo. devhelp to make HTML files and a Devhelp project 29 | echo. epub to make an epub 30 | echo. latex to make LaTeX files, you can set PAPER=a4 or PAPER=letter 31 | echo. text to make text files 32 | echo. man to make manual pages 33 | echo. texinfo to make Texinfo files 34 | echo. gettext to make PO message catalogs 35 | echo. changes to make an overview over all changed/added/deprecated items 36 | echo. xml to make Docutils-native XML files 37 | echo. pseudoxml to make pseudoxml-XML files for display purposes 38 | echo. linkcheck to check all external links for integrity 39 | echo. doctest to run all doctests embedded in the documentation if enabled 40 | goto end 41 | ) 42 | 43 | if "%1" == "clean" ( 44 | for /d %%i in (%BUILDDIR%\*) do rmdir /q /s %%i 45 | del /q /s %BUILDDIR%\* 46 | goto end 47 | ) 48 | 49 | 50 | %SPHINXBUILD% 2> nul 51 | if errorlevel 9009 ( 52 | echo. 53 | echo.The 'sphinx-build' command was not found. Make sure you have Sphinx 54 | echo.installed, then set the SPHINXBUILD environment variable to point 55 | echo.to the full path of the 'sphinx-build' executable. Alternatively you 56 | echo.may add the Sphinx directory to PATH. 57 | echo. 58 | echo.If you don't have Sphinx installed, grab it from 59 | echo.http://sphinx-doc.org/ 60 | exit /b 1 61 | ) 62 | 63 | if "%1" == "html" ( 64 | %SPHINXBUILD% -b html %ALLSPHINXOPTS% %BUILDDIR%/html 65 | if errorlevel 1 exit /b 1 66 | echo. 67 | echo.Build finished. The HTML pages are in %BUILDDIR%/html. 68 | goto end 69 | ) 70 | 71 | if "%1" == "dirhtml" ( 72 | %SPHINXBUILD% -b dirhtml %ALLSPHINXOPTS% %BUILDDIR%/dirhtml 73 | if errorlevel 1 exit /b 1 74 | echo. 75 | echo.Build finished. The HTML pages are in %BUILDDIR%/dirhtml. 76 | goto end 77 | ) 78 | 79 | if "%1" == "singlehtml" ( 80 | %SPHINXBUILD% -b singlehtml %ALLSPHINXOPTS% %BUILDDIR%/singlehtml 81 | if errorlevel 1 exit /b 1 82 | echo. 83 | echo.Build finished. The HTML pages are in %BUILDDIR%/singlehtml. 84 | goto end 85 | ) 86 | 87 | if "%1" == "pickle" ( 88 | %SPHINXBUILD% -b pickle %ALLSPHINXOPTS% %BUILDDIR%/pickle 89 | if errorlevel 1 exit /b 1 90 | echo. 91 | echo.Build finished; now you can process the pickle files. 92 | goto end 93 | ) 94 | 95 | if "%1" == "json" ( 96 | %SPHINXBUILD% -b json %ALLSPHINXOPTS% %BUILDDIR%/json 97 | if errorlevel 1 exit /b 1 98 | echo. 99 | echo.Build finished; now you can process the JSON files. 100 | goto end 101 | ) 102 | 103 | if "%1" == "htmlhelp" ( 104 | %SPHINXBUILD% -b htmlhelp %ALLSPHINXOPTS% %BUILDDIR%/htmlhelp 105 | if errorlevel 1 exit /b 1 106 | echo. 107 | echo.Build finished; now you can run HTML Help Workshop with the ^ 108 | .hhp project file in %BUILDDIR%/htmlhelp. 109 | goto end 110 | ) 111 | 112 | if "%1" == "qthelp" ( 113 | %SPHINXBUILD% -b qthelp %ALLSPHINXOPTS% %BUILDDIR%/qthelp 114 | if errorlevel 1 exit /b 1 115 | echo. 116 | echo.Build finished; now you can run "qcollectiongenerator" with the ^ 117 | .qhcp project file in %BUILDDIR%/qthelp, like this: 118 | echo.^> qcollectiongenerator %BUILDDIR%\qthelp\sphinx.qhcp 119 | echo.To view the help file: 120 | echo.^> assistant -collectionFile %BUILDDIR%\qthelp\sphinx.ghc 121 | goto end 122 | ) 123 | 124 | if "%1" == "devhelp" ( 125 | %SPHINXBUILD% -b devhelp %ALLSPHINXOPTS% %BUILDDIR%/devhelp 126 | if errorlevel 1 exit /b 1 127 | echo. 128 | echo.Build finished. 129 | goto end 130 | ) 131 | 132 | if "%1" == "epub" ( 133 | %SPHINXBUILD% -b epub %ALLSPHINXOPTS% %BUILDDIR%/epub 134 | if errorlevel 1 exit /b 1 135 | echo. 136 | echo.Build finished. The epub file is in %BUILDDIR%/epub. 137 | goto end 138 | ) 139 | 140 | if "%1" == "latex" ( 141 | %SPHINXBUILD% -b latex %ALLSPHINXOPTS% %BUILDDIR%/latex 142 | if errorlevel 1 exit /b 1 143 | echo. 144 | echo.Build finished; the LaTeX files are in %BUILDDIR%/latex. 145 | goto end 146 | ) 147 | 148 | if "%1" == "latexpdf" ( 149 | %SPHINXBUILD% -b latex %ALLSPHINXOPTS% %BUILDDIR%/latex 150 | cd %BUILDDIR%/latex 151 | make all-pdf 152 | cd %BUILDDIR%/.. 153 | echo. 154 | echo.Build finished; the PDF files are in %BUILDDIR%/latex. 155 | goto end 156 | ) 157 | 158 | if "%1" == "latexpdfja" ( 159 | %SPHINXBUILD% -b latex %ALLSPHINXOPTS% %BUILDDIR%/latex 160 | cd %BUILDDIR%/latex 161 | make all-pdf-ja 162 | cd %BUILDDIR%/.. 163 | echo. 164 | echo.Build finished; the PDF files are in %BUILDDIR%/latex. 165 | goto end 166 | ) 167 | 168 | if "%1" == "text" ( 169 | %SPHINXBUILD% -b text %ALLSPHINXOPTS% %BUILDDIR%/text 170 | if errorlevel 1 exit /b 1 171 | echo. 172 | echo.Build finished. The text files are in %BUILDDIR%/text. 173 | goto end 174 | ) 175 | 176 | if "%1" == "man" ( 177 | %SPHINXBUILD% -b man %ALLSPHINXOPTS% %BUILDDIR%/man 178 | if errorlevel 1 exit /b 1 179 | echo. 180 | echo.Build finished. The manual pages are in %BUILDDIR%/man. 181 | goto end 182 | ) 183 | 184 | if "%1" == "texinfo" ( 185 | %SPHINXBUILD% -b texinfo %ALLSPHINXOPTS% %BUILDDIR%/texinfo 186 | if errorlevel 1 exit /b 1 187 | echo. 188 | echo.Build finished. The Texinfo files are in %BUILDDIR%/texinfo. 189 | goto end 190 | ) 191 | 192 | if "%1" == "gettext" ( 193 | %SPHINXBUILD% -b gettext %I18NSPHINXOPTS% %BUILDDIR%/locale 194 | if errorlevel 1 exit /b 1 195 | echo. 196 | echo.Build finished. The message catalogs are in %BUILDDIR%/locale. 197 | goto end 198 | ) 199 | 200 | if "%1" == "changes" ( 201 | %SPHINXBUILD% -b changes %ALLSPHINXOPTS% %BUILDDIR%/changes 202 | if errorlevel 1 exit /b 1 203 | echo. 204 | echo.The overview file is in %BUILDDIR%/changes. 205 | goto end 206 | ) 207 | 208 | if "%1" == "linkcheck" ( 209 | %SPHINXBUILD% -b linkcheck %ALLSPHINXOPTS% %BUILDDIR%/linkcheck 210 | if errorlevel 1 exit /b 1 211 | echo. 212 | echo.Link check complete; look for any errors in the above output ^ 213 | or in %BUILDDIR%/linkcheck/output.txt. 214 | goto end 215 | ) 216 | 217 | if "%1" == "doctest" ( 218 | %SPHINXBUILD% -b doctest %ALLSPHINXOPTS% %BUILDDIR%/doctest 219 | if errorlevel 1 exit /b 1 220 | echo. 221 | echo.Testing of doctests in the sources finished, look at the ^ 222 | results in %BUILDDIR%/doctest/output.txt. 223 | goto end 224 | ) 225 | 226 | if "%1" == "xml" ( 227 | %SPHINXBUILD% -b xml %ALLSPHINXOPTS% %BUILDDIR%/xml 228 | if errorlevel 1 exit /b 1 229 | echo. 230 | echo.Build finished. The XML files are in %BUILDDIR%/xml. 231 | goto end 232 | ) 233 | 234 | if "%1" == "pseudoxml" ( 235 | %SPHINXBUILD% -b pseudoxml %ALLSPHINXOPTS% %BUILDDIR%/pseudoxml 236 | if errorlevel 1 exit /b 1 237 | echo. 238 | echo.Build finished. The pseudo-XML files are in %BUILDDIR%/pseudoxml. 239 | goto end 240 | ) 241 | 242 | :end 243 | -------------------------------------------------------------------------------- /docs/requirements.txt: -------------------------------------------------------------------------------- 1 | # This file is intended for use on readthedocs 2 | sphinx-autodoc-typehints 3 | sphinx-rtd-theme 4 | robotpy-sphinx-plugin 5 | -e . 6 | -------------------------------------------------------------------------------- /docs/robotpy_ext.autonomous.rst: -------------------------------------------------------------------------------- 1 | robotpy_ext.autonomous package 2 | ============================== 3 | 4 | .. module:: robotpy_ext.autonomous 5 | 6 | robotpy_ext.autonomous.selector module 7 | -------------------------------------- 8 | 9 | .. automodule:: robotpy_ext.autonomous.selector 10 | :members: 11 | :undoc-members: 12 | :show-inheritance: 13 | 14 | robotpy_ext.autonomous.stateful_autonomous module 15 | ------------------------------------------------- 16 | 17 | .. automodule:: robotpy_ext.autonomous.stateful_autonomous 18 | :members: 19 | :undoc-members: 20 | :show-inheritance: 21 | -------------------------------------------------------------------------------- /docs/robotpy_ext.common_drivers.rst: -------------------------------------------------------------------------------- 1 | robotpy_ext.common_drivers package 2 | ================================== 3 | 4 | .. module:: robotpy_ext.common_drivers 5 | 6 | robotpy_ext.common_drivers.distance_sensors module 7 | -------------------------------------------------- 8 | 9 | .. automodule:: robotpy_ext.common_drivers.distance_sensors 10 | :members: 11 | :undoc-members: 12 | 13 | robotpy_ext.common_drivers.driver_base module 14 | --------------------------------------------- 15 | 16 | .. automodule:: robotpy_ext.common_drivers.driver_base 17 | :members: 18 | :undoc-members: 19 | 20 | robotpy_ext.common_drivers.units module 21 | --------------------------------------- 22 | 23 | .. automodule:: robotpy_ext.common_drivers.units 24 | :members: 25 | :undoc-members: 26 | :show-inheritance: 27 | 28 | robotpy_ext.common_drivers.xl_max_sonar_ez module 29 | ------------------------------------------------- 30 | 31 | .. automodule:: robotpy_ext.common_drivers.xl_max_sonar_ez 32 | :members: 33 | :undoc-members: 34 | :show-inheritance: 35 | -------------------------------------------------------------------------------- /docs/robotpy_ext.control.rst: -------------------------------------------------------------------------------- 1 | robotpy_ext.control package 2 | =========================== 3 | 4 | .. module:: robotpy_ext.control 5 | 6 | robotpy_ext.control.button_debouncer module 7 | ------------------------------------------- 8 | 9 | .. automodule:: robotpy_ext.control.button_debouncer 10 | :members: 11 | :undoc-members: 12 | 13 | robotpy_ext.control.toggle module 14 | --------------------------------- 15 | 16 | .. automodule:: robotpy_ext.control.toggle 17 | :members: 18 | :undoc-members: 19 | -------------------------------------------------------------------------------- /docs/robotpy_ext.misc.rst: -------------------------------------------------------------------------------- 1 | robotpy_ext.misc package 2 | ======================== 3 | 4 | .. module:: robotpy_ext.misc 5 | 6 | robotpy_ext.misc.asyncio_policy module 7 | -------------------------------------- 8 | 9 | .. automodule:: robotpy_ext.misc.asyncio_policy 10 | :members: 11 | :undoc-members: 12 | :show-inheritance: 13 | 14 | robotpy_ext.misc.looptimer module 15 | --------------------------------------- 16 | 17 | .. automodule:: robotpy_ext.misc.looptimer 18 | :members: 19 | :undoc-members: 20 | :show-inheritance: 21 | 22 | 23 | robotpy_ext.misc.precise_delay module 24 | ------------------------------------- 25 | 26 | .. automodule:: robotpy_ext.misc.precise_delay 27 | :members: 28 | :undoc-members: 29 | :show-inheritance: 30 | 31 | robotpy_ext.misc.periodic_filter module 32 | --------------------------------------- 33 | 34 | .. automodule:: robotpy_ext.misc.periodic_filter 35 | :members: 36 | :undoc-members: 37 | :show-inheritance: 38 | 39 | robotpy_ext.misc.simple_watchdog module 40 | --------------------------------------- 41 | 42 | .. automodule:: robotpy_ext.misc.simple_watchdog 43 | :members: 44 | :undoc-members: 45 | :show-inheritance: 46 | -------------------------------------------------------------------------------- /magicbot/__init__.py: -------------------------------------------------------------------------------- 1 | from .magicrobot import MagicRobot 2 | from .magic_tunable import feedback, tunable 3 | from .magic_reset import will_reset_to 4 | 5 | from .state_machine import ( 6 | AutonomousStateMachine, 7 | StateMachine, 8 | default_state, 9 | state, 10 | timed_state, 11 | ) 12 | 13 | __all__ = ( 14 | "MagicRobot", 15 | "feedback", 16 | "tunable", 17 | "will_reset_to", 18 | "AutonomousStateMachine", 19 | "StateMachine", 20 | "default_state", 21 | "state", 22 | "timed_state", 23 | ) 24 | -------------------------------------------------------------------------------- /magicbot/inject.py: -------------------------------------------------------------------------------- 1 | import logging 2 | from typing import Any, Optional 3 | 4 | logger = logging.getLogger(__name__) 5 | 6 | 7 | class MagicInjectError(ValueError): 8 | pass 9 | 10 | 11 | def get_injection_requests( 12 | type_hints: dict[str, type], cname: str, component: Optional[Any] = None 13 | ) -> dict[str, type]: 14 | """ 15 | Given a dict of type hints, filter it to the requested injection types. 16 | 17 | :param type_hints: The type hints to inspect. 18 | :param cname: The component name. 19 | :param component: The component if it has been instantiated. 20 | """ 21 | requests = {} 22 | 23 | for n, inject_type in type_hints.items(): 24 | # If the variable is private ignore it 25 | if n.startswith("_"): 26 | if component is None: 27 | message = f"Cannot inject into component {cname} __init__ param {n}" 28 | raise MagicInjectError(message) 29 | continue 30 | 31 | # If the variable has been set, skip it 32 | if component is not None and hasattr(component, n): 33 | continue 34 | 35 | # Check for generic types from the typing module 36 | origin = getattr(inject_type, "__origin__", None) 37 | if origin is not None: 38 | inject_type = origin 39 | 40 | # If the type is not actually a type, give a meaningful error 41 | if not isinstance(inject_type, type): 42 | message = ( 43 | f"Component {cname} has a non-type annotation {n}: {inject_type!r}" 44 | ) 45 | if component is not None: 46 | message += "\nLone non-injection variable annotations are disallowed. Did you mean to assign a static variable?" 47 | raise TypeError(message) 48 | 49 | requests[n] = inject_type 50 | 51 | return requests 52 | 53 | 54 | def find_injections( 55 | requests: dict[str, type], injectables: dict[str, Any], cname: str 56 | ) -> dict[str, Any]: 57 | """ 58 | Get a dict of the variables to inject into a given component. 59 | 60 | :param requests: The mapping of requested variables to types, 61 | as returned by :func:`get_injection_requests`. 62 | :param injectables: The available variables to inject. 63 | :param cname: The name of the component. 64 | """ 65 | to_inject = {} 66 | 67 | for n, inject_type in requests.items(): 68 | injectable = injectables.get(n) 69 | if injectable is None: 70 | # Try prefixing the component name 71 | injectable = injectables.get(f"{cname}_{n}") 72 | 73 | # Raise error if injectable syntax used but no injectable was found. 74 | if injectable is None: 75 | raise MagicInjectError( 76 | f"Component {cname} has variable {n} (type {inject_type}), which is absent from robot" 77 | ) 78 | 79 | # Raise error if injectable declared with type different than the initial type 80 | if not isinstance(injectable, inject_type): 81 | raise MagicInjectError( 82 | f"Component {cname} variable {n} does not match type in robot! (Got {type(injectable)}, expected {inject_type})" 83 | ) 84 | 85 | to_inject[n] = injectable 86 | logger.debug("-> %s.%s = %s", cname, n, injectable) 87 | 88 | return to_inject 89 | -------------------------------------------------------------------------------- /magicbot/magic_reset.py: -------------------------------------------------------------------------------- 1 | from typing import Any, Generic, TypeVar 2 | 3 | V = TypeVar("V") 4 | 5 | 6 | class will_reset_to(Generic[V]): 7 | """ 8 | This marker indicates that this variable on a component will be 9 | reset to a default value at the very end of each control loop. 10 | 11 | Example usage:: 12 | 13 | class Component: 14 | 15 | foo = will_reset_to(False) 16 | 17 | def control_fn(self): 18 | self.foo = True 19 | 20 | def execute(self): 21 | if self.foo: 22 | # ... 23 | 24 | # after all components are executed, foo is reset 25 | # back to the default value (False) 26 | 27 | 28 | .. note:: This will only work for MagicRobot components 29 | 30 | .. warning:: This will not work on classes that set ``__slots__``. 31 | """ 32 | 33 | __slots__ = ("default",) 34 | 35 | def __init__(self, default: V) -> None: 36 | self.default = default 37 | 38 | 39 | def collect_resets(cls: type) -> dict[str, Any]: 40 | """ 41 | Get all the ``will_reset_to`` variables and their values from a class. 42 | 43 | .. note:: This isn't useful for normal use. 44 | """ 45 | 46 | result = {} 47 | 48 | for n in dir(cls): 49 | v = getattr(cls, n) 50 | if isinstance(v, will_reset_to): 51 | result[n] = v.default 52 | 53 | return result 54 | -------------------------------------------------------------------------------- /magicbot/magic_reset.pyi: -------------------------------------------------------------------------------- 1 | from typing import Any, Generic, TypeVar, overload 2 | 3 | _V = TypeVar("_V") 4 | 5 | class will_reset_to(Generic[_V]): 6 | default: _V 7 | 8 | def __init__(self, default: _V) -> None: ... 9 | # we don't really have __get__, but this makes code in methods using these 10 | # to type-check, whilst giving correct behaviour for code at the class level 11 | @overload 12 | def __get__(self, instance: None, owner=...) -> will_reset_to: ... 13 | @overload 14 | def __get__(self, instance, owner=...) -> _V: ... 15 | def __set__(self, instance, value: _V) -> None: ... 16 | 17 | def collect_resets(cls: type) -> dict[str, Any]: ... 18 | -------------------------------------------------------------------------------- /magicbot/magic_tunable.py: -------------------------------------------------------------------------------- 1 | import collections.abc 2 | import functools 3 | import inspect 4 | import typing 5 | import warnings 6 | from typing import Callable, Generic, Optional, TypeVar, Union, overload 7 | from collections.abc import Sequence 8 | 9 | import ntcore 10 | from ntcore import NetworkTableInstance 11 | from ntcore.types import ValueT 12 | 13 | 14 | class StructSerializable(typing.Protocol): 15 | """Any type that is a wpiutil.wpistruct.""" 16 | 17 | WPIStruct: typing.ClassVar 18 | 19 | 20 | T = TypeVar("T") 21 | V = TypeVar("V", bound=Union[ValueT, StructSerializable, Sequence[StructSerializable]]) 22 | 23 | 24 | class tunable(Generic[V]): 25 | """ 26 | This allows you to define simple properties that allow you to easily 27 | communicate with other programs via NetworkTables. 28 | 29 | The following example will define a NetworkTable variable at 30 | ``/components/my_component/foo``:: 31 | 32 | class MyRobot(magicbot.MagicRobot): 33 | 34 | my_component: MyComponent 35 | 36 | ... 37 | 38 | from magicbot import tunable 39 | 40 | class MyComponent: 41 | 42 | # define the tunable property 43 | foo = tunable(True) 44 | 45 | def execute(self): 46 | 47 | # set the variable 48 | self.foo = True 49 | 50 | # get the variable 51 | foo = self.foo 52 | 53 | The key of the NetworkTables variable will vary based on what kind of 54 | object the decorated method belongs to: 55 | 56 | * A component: ``/components/COMPONENTNAME/VARNAME`` 57 | * An autonomous mode: ``/autonomous/MODENAME/VARNAME`` 58 | * Your main robot class: ``/robot/VARNAME`` 59 | 60 | .. note:: When executing unit tests on objects that create tunables, 61 | you will want to use setup_tunables to set the object up. 62 | In normal usage, MagicRobot does this for you, so you don't 63 | have to do anything special. 64 | 65 | .. versionchanged:: 2024.1.0 66 | Added support for WPILib Struct serializable types. 67 | Integer defaults now create integer topics instead of double topics. 68 | """ 69 | 70 | # the way this works is we use a special class to indicate that it 71 | # is a tunable, and MagicRobot adds _ntattr and _global_table variables 72 | # to the class property 73 | 74 | # The tricky bit is that you need to do late binding on these, because 75 | # the networktables key is not known when the object is created. Instead, 76 | # the name of the key is related to the name of the variable name in the 77 | # robot class 78 | 79 | __slots__ = ( 80 | "_ntdefault", 81 | "_ntsubtable", 82 | "_ntwritedefault", 83 | # "__doc__", 84 | "__orig_class__", 85 | "_topic_type", 86 | "_nt", 87 | ) 88 | 89 | def __init__( 90 | self, 91 | default: V, 92 | *, 93 | writeDefault: bool = True, 94 | subtable: Optional[str] = None, 95 | doc=None, 96 | ) -> None: 97 | if doc is not None: 98 | warnings.warn("tunable no longer uses the doc argument", stacklevel=2) 99 | 100 | self._ntdefault = default 101 | self._ntsubtable = subtable 102 | self._ntwritedefault = writeDefault 103 | # self.__doc__ = doc 104 | 105 | # Defer checks for empty sequences to check type hints. 106 | # Report errors here when we can so the error points to the tunable line. 107 | if default or not isinstance(default, collections.abc.Sequence): 108 | topic_type = _get_topic_type_for_value(default) 109 | if topic_type is None: 110 | checked_type: type = type(default) 111 | raise TypeError( 112 | f"tunable is not publishable to NetworkTables, type: {checked_type.__name__}" 113 | ) 114 | self._topic_type = topic_type 115 | 116 | def __set_name__(self, owner: type, name: str) -> None: 117 | type_hint: Optional[type] = None 118 | # __orig_class__ is set after __init__, check it here. 119 | orig_class = getattr(self, "__orig_class__", None) 120 | if orig_class is not None: 121 | # Accept field = tunable[Sequence[int]]([]) 122 | type_hint = typing.get_args(orig_class)[0] 123 | else: 124 | type_hint = typing.get_type_hints(owner).get(name) 125 | origin = typing.get_origin(type_hint) 126 | if origin is typing.ClassVar: 127 | # Accept field: ClassVar[tunable[Sequence[int]]] = tunable([]) 128 | type_hint = typing.get_args(type_hint)[0] 129 | origin = typing.get_origin(type_hint) 130 | if origin is tunable: 131 | # Accept field: tunable[Sequence[int]] = tunable([]) 132 | type_hint = typing.get_args(type_hint)[0] 133 | 134 | if type_hint is not None: 135 | topic_type = _get_topic_type(type_hint) 136 | else: 137 | topic_type = _get_topic_type_for_value(self._ntdefault) 138 | 139 | if topic_type is None: 140 | checked_type: type = type_hint or type(self._ntdefault) 141 | raise TypeError( 142 | f"tunable is not publishable to NetworkTables, type: {checked_type.__name__}" 143 | ) 144 | 145 | self._topic_type = topic_type 146 | 147 | @overload 148 | def __get__(self, instance: None, owner=None) -> "tunable[V]": ... 149 | 150 | @overload 151 | def __get__(self, instance, owner=None) -> V: ... 152 | 153 | def __get__(self, instance, owner=None): 154 | if instance is not None: 155 | return instance._tunables[self].get() 156 | return self 157 | 158 | def __set__(self, instance, value: V) -> None: 159 | instance._tunables[self].set(value) 160 | 161 | 162 | def _get_topic_type_for_value(value) -> Optional[Callable[[ntcore.Topic], typing.Any]]: 163 | topic_type = _get_topic_type(type(value)) 164 | # bytes and str are Sequences. They must be checked before Sequence. 165 | if topic_type is None and isinstance(value, collections.abc.Sequence): 166 | if not value: 167 | raise ValueError( 168 | f"tunable default cannot be an empty sequence, got {value}" 169 | ) 170 | topic_type = _get_topic_type(Sequence[type(value[0])]) # type: ignore [misc] 171 | return topic_type 172 | 173 | 174 | def setup_tunables(component, cname: str, prefix: Optional[str] = "components") -> None: 175 | """ 176 | Connects the tunables on an object to NetworkTables. 177 | 178 | :param component: Component object 179 | :param cname: Name of component 180 | :param prefix: Prefix to use, or no prefix if None 181 | 182 | .. note:: This is not needed in normal use, only useful 183 | for testing 184 | """ 185 | 186 | cls = component.__class__ 187 | 188 | if prefix is None: 189 | prefix = f"/{cname}" 190 | else: 191 | prefix = f"/{prefix}/{cname}" 192 | 193 | NetworkTables = NetworkTableInstance.getDefault() 194 | 195 | tunables: dict[tunable, ntcore.Topic] = {} 196 | 197 | for n in dir(cls): 198 | if n.startswith("_"): 199 | continue 200 | 201 | prop = getattr(cls, n) 202 | if not isinstance(prop, tunable): 203 | continue 204 | 205 | if prop._ntsubtable: 206 | key = f"{prefix}/{prop._ntsubtable}/{n}" 207 | else: 208 | key = f"{prefix}/{n}" 209 | 210 | topic = prop._topic_type(NetworkTables.getTopic(key)) 211 | ntvalue = topic.getEntry(prop._ntdefault) 212 | if prop._ntwritedefault: 213 | ntvalue.set(prop._ntdefault) 214 | else: 215 | ntvalue.setDefault(prop._ntdefault) 216 | tunables[prop] = ntvalue 217 | 218 | component._tunables = tunables 219 | 220 | 221 | @overload 222 | def feedback(f: Callable[[T], V]) -> Callable[[T], V]: ... 223 | 224 | 225 | @overload 226 | def feedback(*, key: str) -> Callable[[Callable[[T], V]], Callable[[T], V]]: ... 227 | 228 | 229 | def feedback(f=None, *, key: Optional[str] = None) -> Callable: 230 | """ 231 | This decorator allows you to create NetworkTables values that are 232 | automatically updated with the return value of a method. 233 | 234 | ``key`` is an optional parameter, and if it is not supplied, 235 | the key will default to the method name with a leading ``get_`` removed. 236 | If the method does not start with ``get_``, the key will be the full 237 | name of the method. 238 | 239 | The key of the NetworkTables value will vary based on what kind of 240 | object the decorated method belongs to: 241 | 242 | * A component: ``/components/COMPONENTNAME/VARNAME`` 243 | * Your main robot class: ``/robot/VARNAME`` 244 | 245 | The NetworkTables value will be auto-updated in all modes. 246 | 247 | .. warning:: The function should only act as a getter, and must not 248 | take any arguments (other than self). 249 | 250 | Example:: 251 | 252 | from magicbot import feedback 253 | 254 | class MyComponent: 255 | navx: ... 256 | 257 | @feedback 258 | def get_angle(self) -> float: 259 | return self.navx.getYaw() 260 | 261 | class MyRobot(magicbot.MagicRobot): 262 | my_component: MyComponent 263 | 264 | ... 265 | 266 | In this example, the NetworkTable key is stored at 267 | ``/components/my_component/angle``. 268 | 269 | .. seealso:: :class:`~wpilib.LiveWindow` may suit your needs, 270 | especially if you wish to monitor WPILib objects. 271 | 272 | .. versionadded:: 2018.1.0 273 | 274 | .. versionchanged:: 2024.1.0 275 | WPILib Struct serializable types are supported when the return type is type hinted. 276 | An ``int`` return type hint now creates an integer topic. 277 | """ 278 | if f is None: 279 | return functools.partial(feedback, key=key) 280 | 281 | if not callable(f): 282 | raise TypeError(f"Illegal use of feedback decorator on non-callable {f!r}") 283 | sig = inspect.signature(f) 284 | name = f.__name__ 285 | 286 | if len(sig.parameters) != 1: 287 | raise ValueError( 288 | f"{name} may not take arguments other than 'self' (must be a simple getter method)" 289 | ) 290 | 291 | # Set attributes to be checked during injection 292 | f._magic_feedback = True 293 | f._magic_feedback_key = key 294 | 295 | return f 296 | 297 | 298 | _topic_types = { 299 | bool: ntcore.BooleanTopic, 300 | int: ntcore.IntegerTopic, 301 | float: ntcore.DoubleTopic, 302 | str: ntcore.StringTopic, 303 | bytes: ntcore.RawTopic, 304 | } 305 | _array_topic_types = { 306 | bool: ntcore.BooleanArrayTopic, 307 | int: ntcore.IntegerArrayTopic, 308 | float: ntcore.DoubleArrayTopic, 309 | str: ntcore.StringArrayTopic, 310 | } 311 | 312 | 313 | def _get_topic_type( 314 | return_annotation, 315 | ) -> Optional[Callable[[ntcore.Topic], typing.Any]]: 316 | if return_annotation in _topic_types: 317 | return _topic_types[return_annotation] 318 | if hasattr(return_annotation, "WPIStruct"): 319 | return lambda topic: ntcore.StructTopic(topic, return_annotation) 320 | 321 | # Check for PEP 484 generic types 322 | origin = getattr(return_annotation, "__origin__", None) 323 | args = typing.get_args(return_annotation) 324 | if origin in (list, tuple, collections.abc.Sequence) and args: 325 | # Ensure tuples are tuple[T, ...] or homogenous 326 | if origin is tuple and not ( 327 | (len(args) == 2 and args[1] is Ellipsis) or len(set(args)) == 1 328 | ): 329 | return None 330 | 331 | inner_type = args[0] 332 | if inner_type in _array_topic_types: 333 | return _array_topic_types[inner_type] 334 | if hasattr(inner_type, "WPIStruct"): 335 | return lambda topic: ntcore.StructArrayTopic(topic, inner_type) 336 | 337 | return None 338 | 339 | 340 | def collect_feedbacks(component, cname: str, prefix: Optional[str] = "components"): 341 | """ 342 | Finds all methods decorated with :func:`feedback` on an object 343 | and returns a list of 2-tuples (method, NetworkTables entry setter). 344 | 345 | .. note:: This isn't useful for normal use. 346 | """ 347 | if prefix is None: 348 | prefix = f"/{cname}" 349 | else: 350 | prefix = f"/{prefix}/{cname}" 351 | 352 | nt = NetworkTableInstance.getDefault().getTable(prefix) 353 | feedbacks = [] 354 | 355 | for name, method in inspect.getmembers(component, inspect.ismethod): 356 | if getattr(method, "_magic_feedback", False): 357 | key = method._magic_feedback_key 358 | if key is None: 359 | if name.startswith("get_"): 360 | key = name[4:] 361 | else: 362 | key = name 363 | 364 | return_annotation = typing.get_type_hints(method).get("return", None) 365 | if return_annotation is not None: 366 | topic_type = _get_topic_type(return_annotation) 367 | else: 368 | topic_type = None 369 | 370 | if topic_type is None: 371 | entry = nt.getEntry(key) 372 | setter = entry.setValue 373 | else: 374 | publisher = topic_type(nt.getTopic(key)).publish() 375 | setter = publisher.set 376 | 377 | feedbacks.append((method, setter)) 378 | 379 | return feedbacks 380 | -------------------------------------------------------------------------------- /magicbot/magicbot_tests.py: -------------------------------------------------------------------------------- 1 | from robotpy_ext.autonomous.selector_tests import * 2 | -------------------------------------------------------------------------------- /magicbot/magiccomponent.py: -------------------------------------------------------------------------------- 1 | import logging 2 | 3 | 4 | class MagicComponent: 5 | """ 6 | To automagically retrieve variables defined in your base robot 7 | object, you can add the following:: 8 | 9 | class MyComponent: 10 | 11 | # other variables 'imported' automatically from MagicRobot 12 | elevator_motor: Talon 13 | other_component: MyOtherComponent 14 | 15 | ... 16 | 17 | def execute(self): 18 | 19 | # This will be automatically set to the Talon 20 | # instance created in robot.py 21 | self.elevator_motor.set(self.value) 22 | 23 | 24 | What this says is "find the variable in the robot class called 25 | 'elevator_motor', which is a Talon". If the name and type match, 26 | then the variable will automatically be injected into your 27 | component when it is created. 28 | 29 | .. note:: You don't need to inherit from ``MagicComponent``, it is only 30 | provided for documentation's sake 31 | """ 32 | 33 | logger: logging.Logger 34 | 35 | def setup(self) -> None: 36 | """ 37 | This function is called after ``createObjects`` has been called in 38 | the main robot class, and after all components have been created 39 | 40 | The setup function is optional and components do not have to define 41 | one. ``setup()`` functions are called in order of component definition 42 | in the main robot class. 43 | 44 | .. note:: For technical reasons, variables imported from 45 | MagicRobot are not initialized when your component's 46 | constructor is called. However, they will be initialized 47 | by the time this function is called. 48 | """ 49 | 50 | def on_enable(self) -> None: 51 | """ 52 | Called when the robot enters autonomous, teleoperated or test mode mode. This 53 | function should initialize your component to a "safe" state so 54 | that unexpected things don't happen when enabling the robot. 55 | 56 | .. note:: You'll note that there isn't a separate initialization 57 | function for autonomous and teleoperated modes. This is 58 | intentional, as they should be the same. 59 | """ 60 | 61 | def on_disable(self) -> None: 62 | """ 63 | Called when the robot enters disabled mode. 64 | """ 65 | 66 | def execute(self) -> None: 67 | """ 68 | This function is called at the end of the control loop 69 | """ 70 | -------------------------------------------------------------------------------- /magicbot/magicrobot.py: -------------------------------------------------------------------------------- 1 | import contextlib 2 | import inspect 3 | import logging 4 | import sys 5 | import types 6 | import typing 7 | 8 | from typing import Any, Callable 9 | 10 | import hal 11 | import wpilib 12 | 13 | from ntcore import NetworkTableInstance 14 | 15 | # from wpilib.shuffleboard import Shuffleboard 16 | 17 | from robotpy_ext.autonomous import AutonomousModeSelector 18 | from robotpy_ext.misc import NotifierDelay 19 | from robotpy_ext.misc.simple_watchdog import SimpleWatchdog 20 | 21 | from .inject import get_injection_requests, find_injections 22 | from .magic_tunable import setup_tunables, tunable, collect_feedbacks 23 | from .magic_reset import collect_resets 24 | 25 | __all__ = ["MagicRobot"] 26 | 27 | 28 | class MagicInjectError(ValueError): 29 | pass 30 | 31 | 32 | class MagicRobot(wpilib.RobotBase): 33 | """ 34 | Robots that use the MagicBot framework should use this as their 35 | base robot class. If you use this as your base, you must 36 | implement the following methods: 37 | 38 | - :meth:`createObjects` 39 | - :meth:`teleopPeriodic` 40 | 41 | MagicRobot uses the :class:`.AutonomousModeSelector` to allow you 42 | to define multiple autonomous modes and to select one of them via 43 | the SmartDashboard/Shuffleboard. 44 | 45 | MagicRobot will set the following NetworkTables variables 46 | automatically: 47 | 48 | - ``/robot/mode``: one of 'disabled', 'auto', 'teleop', or 'test' 49 | - ``/robot/is_simulation``: True/False 50 | - ``/robot/is_ds_attached``: True/False 51 | 52 | """ 53 | 54 | #: Amount of time each loop takes (default is 20ms) 55 | control_loop_wait_time = 0.020 56 | 57 | #: Error report interval: when an FMS is attached, how often should 58 | #: uncaught exceptions be reported? 59 | error_report_interval = 0.5 60 | 61 | #: A Python logging object that you can use to send messages to the log. 62 | #: It is recommended to use this instead of print statements. 63 | logger = logging.getLogger("robot") 64 | 65 | #: If True, teleopPeriodic will be called in autonomous mode 66 | use_teleop_in_autonomous = False 67 | 68 | def __init__(self) -> None: 69 | super().__init__() 70 | hal.report( 71 | hal.tResourceType.kResourceType_Framework.value, 72 | hal.tInstances.kFramework_MagicBot.value, 73 | ) 74 | 75 | self._exclude_from_injection = ["logger"] 76 | 77 | self.__last_error_report = -10.0 78 | 79 | self._components: list[tuple[str, Any]] = [] 80 | self._feedbacks: list[tuple[Callable[[], Any], Callable[[Any], Any]]] = [] 81 | self._reset_components: list[tuple[dict[str, Any], Any]] = [] 82 | 83 | self.__done = False 84 | 85 | # cache these 86 | self.__is_ds_attached = wpilib.DriverStation.isDSAttached 87 | self.__sd_update = wpilib.SmartDashboard.updateValues 88 | self.__lv_update = wpilib.LiveWindow.updateValues 89 | # self.__sf_update = Shuffleboard.update 90 | 91 | def _simulationInit(self) -> None: 92 | pass 93 | 94 | def _simulationPeriodic(self) -> None: 95 | pass 96 | 97 | def __simulationPeriodic(self) -> None: 98 | hal.simPeriodicBefore() 99 | self._simulationPeriodic() 100 | hal.simPeriodicAfter() 101 | 102 | def robotInit(self) -> None: 103 | """ 104 | .. warning:: Internal API, don't override; use :meth:`createObjects` instead 105 | """ 106 | 107 | # Create the user's objects and stuff here 108 | self.createObjects() 109 | 110 | # Load autonomous modes 111 | self._automodes = AutonomousModeSelector("autonomous") 112 | 113 | # Next, create the robot components and wire them together 114 | self._create_components() 115 | 116 | # cache these 117 | self.__is_ds_attached = wpilib.DriverStation.isDSAttached 118 | self.__sd_update = wpilib.SmartDashboard.updateValues 119 | self.__lv_update = wpilib.LiveWindow.updateValues 120 | # self.__sf_update = Shuffleboard.update 121 | 122 | self.__nt = NetworkTableInstance.getDefault().getTable("/robot") 123 | 124 | self.__nt_put_is_ds_attached = self.__nt.getEntry("is_ds_attached").setBoolean 125 | self.__nt_put_mode = self.__nt.getEntry("mode").setString 126 | 127 | self.__nt.putBoolean("is_simulation", self.isSimulation()) 128 | self.__nt_put_is_ds_attached(self.__is_ds_attached()) 129 | 130 | self.watchdog = SimpleWatchdog(self.control_loop_wait_time) 131 | 132 | self.__periodics: list[tuple[Callable[[], None], str]] = [ 133 | (self.robotPeriodic, "robotPeriodic()"), 134 | ] 135 | 136 | if self.isSimulation(): 137 | self._simulationInit() 138 | self.__periodics.append((self.__simulationPeriodic, "simulationPeriodic()")) 139 | 140 | def createObjects(self) -> None: 141 | """ 142 | You should override this and initialize all of your wpilib 143 | objects here (and not in your components, for example). This 144 | serves two purposes: 145 | 146 | - It puts all of your motor/sensor initialization in the same 147 | place, so that if you need to change a port/pin number it 148 | makes it really easy to find it. Additionally, if you want 149 | to create a simplified robot program to test a specific 150 | thing, it makes it really easy to copy/paste it elsewhere 151 | 152 | - It allows you to use the magic injection mechanism to share 153 | variables between components 154 | 155 | .. note:: Do not access your magic components in this function, 156 | as their instances have not been created yet. Do not 157 | create them either. 158 | """ 159 | raise NotImplementedError 160 | 161 | def autonomousInit(self) -> None: 162 | """Initialization code for autonomous mode may go here. 163 | 164 | Users may override this method for initialization code which 165 | will be called each time the robot enters autonomous mode, 166 | regardless of the selected autonomous mode. 167 | 168 | This can be useful for code that must be run at the beginning of a match. 169 | 170 | .. note:: 171 | 172 | This method is called after every component's ``on_enable`` method, 173 | but before the selected autonomous mode's ``on_enable`` method. 174 | """ 175 | pass 176 | 177 | def teleopInit(self) -> None: 178 | """ 179 | Initialization code for teleop control code may go here. 180 | 181 | Users may override this method for initialization code which will be 182 | called each time the robot enters teleop mode. 183 | 184 | .. note:: The ``on_enable`` functions of all components are called 185 | before this function is called. 186 | """ 187 | pass 188 | 189 | def teleopPeriodic(self): 190 | """ 191 | Periodic code for teleop mode should go here. 192 | 193 | Users should override this method for code which will be called 194 | periodically at a regular rate while the robot is in teleop mode. 195 | 196 | This code executes before the ``execute`` functions of all 197 | components are called. 198 | 199 | .. note:: If you want this function to be called in autonomous 200 | mode, set ``use_teleop_in_autonomous`` to True in your 201 | robot class. 202 | """ 203 | func = self.teleopPeriodic.__func__ 204 | if not hasattr(func, "firstRun"): 205 | self.logger.warning( 206 | "Default MagicRobot.teleopPeriodic() method... Override me!" 207 | ) 208 | func.firstRun = False 209 | 210 | def disabledInit(self) -> None: 211 | """ 212 | Initialization code for disabled mode may go here. 213 | 214 | Users may override this method for initialization code which will be 215 | called each time the robot enters disabled mode. 216 | 217 | .. note:: The ``on_disable`` functions of all components are called 218 | before this function is called. 219 | """ 220 | pass 221 | 222 | def disabledPeriodic(self): 223 | """ 224 | Periodic code for disabled mode should go here. 225 | 226 | Users should override this method for code which will be called 227 | periodically at a regular rate while the robot is in disabled mode. 228 | 229 | This code executes before the ``execute`` functions of all 230 | components are called. 231 | """ 232 | func = self.disabledPeriodic.__func__ 233 | if not hasattr(func, "firstRun"): 234 | self.logger.warning( 235 | "Default MagicRobot.disabledPeriodic() method... Override me!" 236 | ) 237 | func.firstRun = False 238 | 239 | def testInit(self) -> None: 240 | """Initialization code for test mode should go here. 241 | 242 | Users should override this method for initialization code which will be 243 | called each time the robot enters disabled mode. 244 | """ 245 | pass 246 | 247 | def testPeriodic(self) -> None: 248 | """Periodic code for test mode should go here.""" 249 | pass 250 | 251 | def robotPeriodic(self) -> None: 252 | """ 253 | Periodic code for all modes should go here. 254 | 255 | Users must override this method to utilize it 256 | but it is not required. 257 | 258 | This function gets called last in each mode. 259 | You may use it for any code you need to run 260 | during all modes of the robot (e.g NetworkTables updates) 261 | 262 | The default implementation will update 263 | SmartDashboard, LiveWindow and Shuffleboard. 264 | """ 265 | watchdog = self.watchdog 266 | self.__sd_update() 267 | watchdog.addEpoch("SmartDashboard") 268 | self.__lv_update() 269 | watchdog.addEpoch("LiveWindow") 270 | # self.__sf_update() 271 | # watchdog.addEpoch("Shuffleboard") 272 | 273 | def onException(self, forceReport: bool = False) -> None: 274 | """ 275 | This function must *only* be called when an unexpected exception 276 | has occurred that would otherwise crash the robot code. Use this 277 | inside your :meth:`operatorActions` function. 278 | 279 | If the FMS is attached (eg, during a real competition match), 280 | this function will return without raising an error. However, 281 | it will try to report one-off errors to the Driver Station so 282 | that it will be recorded in the Driver Station Log Viewer. 283 | Repeated errors may not get logged. 284 | 285 | Example usage:: 286 | 287 | def teleopPeriodic(self): 288 | try: 289 | if self.joystick.getTrigger(): 290 | self.shooter.shoot() 291 | except: 292 | self.onException() 293 | 294 | try: 295 | if self.joystick.getRawButton(2): 296 | self.ball_intake.run() 297 | except: 298 | self.onException() 299 | 300 | # and so on... 301 | 302 | :param forceReport: Always report the exception to the DS. Don't 303 | set this to True 304 | """ 305 | # If the FMS is not attached, crash the robot program 306 | if not wpilib.DriverStation.isFMSAttached(): 307 | raise 308 | 309 | # Otherwise, if the FMS is attached then try to report the error via 310 | # the driver station console. Maybe. 311 | now = wpilib.Timer.getFPGATimestamp() 312 | 313 | try: 314 | if ( 315 | forceReport 316 | or (now - self.__last_error_report) > self.error_report_interval 317 | ): 318 | wpilib.reportError("Unexpected exception", True) 319 | except: 320 | pass # ok, can't do anything here 321 | 322 | self.__last_error_report = now 323 | 324 | @contextlib.contextmanager 325 | def consumeExceptions(self, forceReport: bool = False): 326 | """ 327 | This returns a context manager which will consume any uncaught 328 | exceptions that might otherwise crash the robot. 329 | 330 | Example usage:: 331 | 332 | def teleopPeriodic(self): 333 | with self.consumeExceptions(): 334 | if self.joystick.getTrigger(): 335 | self.shooter.shoot() 336 | 337 | with self.consumeExceptions(): 338 | if self.joystick.getRawButton(2): 339 | self.ball_intake.run() 340 | 341 | # and so on... 342 | 343 | :param forceReport: Always report the exception to the DS. Don't 344 | set this to True 345 | 346 | .. seealso:: :meth:`onException` for more details 347 | """ 348 | try: 349 | yield 350 | except: 351 | self.onException(forceReport=forceReport) 352 | 353 | # 354 | # Internal API 355 | # 356 | 357 | def startCompetition(self) -> None: 358 | """ 359 | This runs the mode-switching loop. 360 | 361 | .. warning:: Internal API, don't override 362 | """ 363 | 364 | # TODO: usage reporting? 365 | self.robotInit() 366 | 367 | # Tell the DS the robot is ready to be enabled 368 | hal.observeUserProgramStarting() 369 | 370 | while not self.__done: 371 | isEnabled, isAutonomous, isTest = self.getControlState() 372 | 373 | if not isEnabled: 374 | self._disabled() 375 | elif isAutonomous: 376 | self.autonomous() 377 | elif isTest: 378 | self._test() 379 | else: 380 | self._operatorControl() 381 | 382 | def endCompetition(self) -> None: 383 | self.__done = True 384 | self._automodes.endCompetition() 385 | 386 | def autonomous(self) -> None: 387 | """ 388 | MagicRobot will do The Right Thing and automatically load all 389 | autonomous mode routines defined in the autonomous folder. 390 | 391 | .. warning:: Internal API, don't override 392 | """ 393 | 394 | self.__nt_put_mode("auto") 395 | self.__nt_put_is_ds_attached(self.__is_ds_attached()) 396 | 397 | self._on_mode_enable_components() 398 | 399 | try: 400 | self.autonomousInit() 401 | except: 402 | self.onException(forceReport=True) 403 | 404 | auto_functions: tuple[Callable[[], None], ...] = (self._enabled_periodic,) 405 | 406 | if self.use_teleop_in_autonomous: 407 | auto_functions = (self.teleopPeriodic,) + auto_functions 408 | 409 | self._automodes.run( 410 | self.control_loop_wait_time, 411 | auto_functions, 412 | self.onException, 413 | watchdog=self.watchdog, 414 | ) 415 | 416 | def _disabled(self) -> None: 417 | """ 418 | This function is called in disabled mode. You should not 419 | override this function; rather, you should override the 420 | :meth:`disabledPeriodic` function instead. 421 | 422 | .. warning:: Internal API, don't override 423 | """ 424 | watchdog = self.watchdog 425 | watchdog.reset() 426 | 427 | self.__nt_put_mode("disabled") 428 | ds_attached = None 429 | 430 | self._on_mode_disable_components() 431 | try: 432 | self.disabledInit() 433 | except: 434 | self.onException(forceReport=True) 435 | watchdog.addEpoch("disabledInit()") 436 | 437 | refreshData = wpilib.DriverStation.refreshData 438 | DSControlWord = wpilib.DSControlWord 439 | 440 | with NotifierDelay(self.control_loop_wait_time) as delay: 441 | while not self.__done: 442 | refreshData() 443 | cw = DSControlWord() 444 | if cw.isEnabled(): 445 | break 446 | 447 | if ds_attached != cw.isDSAttached(): 448 | ds_attached = not ds_attached 449 | self.__nt_put_is_ds_attached(ds_attached) 450 | 451 | hal.observeUserProgramDisabled() 452 | try: 453 | self.disabledPeriodic() 454 | except: 455 | self.onException() 456 | watchdog.addEpoch("disabledPeriodic()") 457 | 458 | self._do_periodics() 459 | # watchdog.disable() 460 | watchdog.printIfExpired() 461 | 462 | delay.wait() 463 | watchdog.reset() 464 | 465 | def _operatorControl(self) -> None: 466 | """ 467 | This function is called in teleoperated mode. You should not 468 | override this function; rather, you should override the 469 | :meth:`teleopPeriodics` function instead. 470 | 471 | .. warning:: Internal API, don't override 472 | """ 473 | watchdog = self.watchdog 474 | watchdog.reset() 475 | 476 | self.__nt_put_mode("teleop") 477 | # don't need to update this during teleop -- presumably will switch 478 | # modes when ds is no longer attached 479 | self.__nt_put_is_ds_attached(self.__is_ds_attached()) 480 | 481 | # initialize things 482 | self._on_mode_enable_components() 483 | 484 | try: 485 | self.teleopInit() 486 | except: 487 | self.onException(forceReport=True) 488 | watchdog.addEpoch("teleopInit()") 489 | 490 | observe = hal.observeUserProgramTeleop 491 | refreshData = wpilib.DriverStation.refreshData 492 | isTeleopEnabled = wpilib.DriverStation.isTeleopEnabled 493 | 494 | with NotifierDelay(self.control_loop_wait_time) as delay: 495 | while not self.__done: 496 | refreshData() 497 | if not isTeleopEnabled(): 498 | break 499 | 500 | observe() 501 | try: 502 | self.teleopPeriodic() 503 | except: 504 | self.onException() 505 | watchdog.addEpoch("teleopPeriodic()") 506 | 507 | self._enabled_periodic() 508 | # watchdog.disable() 509 | watchdog.printIfExpired() 510 | 511 | delay.wait() 512 | watchdog.reset() 513 | 514 | def _test(self) -> None: 515 | """Called when the robot is in test mode""" 516 | watchdog = self.watchdog 517 | watchdog.reset() 518 | 519 | self.__nt_put_mode("test") 520 | self.__nt_put_is_ds_attached(self.__is_ds_attached()) 521 | 522 | wpilib.LiveWindow.setEnabled(True) 523 | # Shuffleboard.enableActuatorWidgets() 524 | 525 | # initialize things 526 | self._on_mode_enable_components() 527 | 528 | try: 529 | self.testInit() 530 | except: 531 | self.onException(forceReport=True) 532 | watchdog.addEpoch("testInit()") 533 | 534 | refreshData = wpilib.DriverStation.refreshData 535 | DSControlWord = wpilib.DSControlWord 536 | 537 | with NotifierDelay(self.control_loop_wait_time) as delay: 538 | while not self.__done: 539 | refreshData() 540 | cw = DSControlWord() 541 | if not (cw.isTest() and cw.isEnabled()): 542 | break 543 | 544 | hal.observeUserProgramTest() 545 | try: 546 | self.testPeriodic() 547 | except: 548 | self.onException() 549 | watchdog.addEpoch("testPeriodic()") 550 | 551 | self._do_periodics() 552 | # watchdog.disable() 553 | watchdog.printIfExpired() 554 | 555 | delay.wait() 556 | watchdog.reset() 557 | 558 | wpilib.LiveWindow.setEnabled(False) 559 | # Shuffleboard.disableActuatorWidgets() 560 | 561 | def _on_mode_enable_components(self) -> None: 562 | # initialize things 563 | for _, component in self._components: 564 | on_enable = getattr(component, "on_enable", None) 565 | if on_enable is not None: 566 | try: 567 | on_enable() 568 | except: 569 | self.onException(forceReport=True) 570 | 571 | def _on_mode_disable_components(self) -> None: 572 | # deinitialize things 573 | for _, component in self._components: 574 | on_disable = getattr(component, "on_disable", None) 575 | if on_disable is not None: 576 | try: 577 | on_disable() 578 | except: 579 | self.onException(forceReport=True) 580 | 581 | def _create_components(self) -> None: 582 | # 583 | # TODO: Will need to inject into any autonomous mode component 584 | # too, as they're a bit different 585 | # 586 | 587 | # TODO: Will need to order state machine components before 588 | # other components just in case 589 | 590 | components = [] 591 | 592 | self.logger.info("Creating magic components") 593 | 594 | # Identify all of the types, and create them 595 | cls = type(self) 596 | 597 | # - Iterate over class variables with type annotations 598 | # .. this hack is necessary for pybind11 based modules 599 | sys.modules["pybind11_builtins"] = types.SimpleNamespace() # type: ignore 600 | 601 | injectables = self._collect_injectables() 602 | 603 | for m, ctyp in typing.get_type_hints(cls).items(): 604 | # Ignore private variables 605 | if m.startswith("_"): 606 | continue 607 | 608 | # If the variable has been set, skip it 609 | if hasattr(self, m): 610 | continue 611 | 612 | # If the type is not actually a type, give a meaningful error 613 | if not isinstance(ctyp, type): 614 | raise TypeError( 615 | f"{cls.__name__} has a non-type annotation on {m} ({ctyp!r}); lone non-injection variable annotations are disallowed, did you want to assign a static variable?" 616 | ) 617 | 618 | component = self._create_component(m, ctyp, injectables) 619 | 620 | # Store for later 621 | components.append((m, component)) 622 | injectables[m] = component 623 | 624 | # For each new component, perform magic injection 625 | for cname, component in components: 626 | setup_tunables(component, cname, "components") 627 | self._setup_vars(cname, component, injectables) 628 | self._setup_reset_vars(component) 629 | 630 | # Do it for autonomous modes too 631 | for mode in self._automodes.modes.values(): 632 | mode.logger = logging.getLogger(mode.MODE_NAME) 633 | setup_tunables(mode, mode.MODE_NAME, "autonomous") 634 | self._setup_vars(mode.MODE_NAME, mode, injectables) 635 | 636 | # And for self too 637 | setup_tunables(self, "robot", None) 638 | self._feedbacks += collect_feedbacks(self, "robot", None) 639 | 640 | # Call setup functions for components 641 | for cname, component in components: 642 | setup = getattr(component, "setup", None) 643 | if setup is not None: 644 | setup() 645 | # ... and grab all the feedback methods 646 | self._feedbacks += collect_feedbacks(component, cname, "components") 647 | 648 | # Call setup functions for autonomous modes 649 | for mode in self._automodes.modes.values(): 650 | if hasattr(mode, "setup"): 651 | mode.setup() 652 | 653 | self._components = components 654 | 655 | def _collect_injectables(self) -> dict[str, Any]: 656 | injectables = {} 657 | cls = type(self) 658 | 659 | for n in dir(self): 660 | if ( 661 | n.startswith("_") 662 | or n in self._exclude_from_injection 663 | or isinstance(getattr(cls, n, None), (property, tunable)) 664 | ): 665 | continue 666 | 667 | o = getattr(self, n) 668 | 669 | # Don't inject methods 670 | # TODO: This could actually be a cool capability.. 671 | if inspect.ismethod(o): 672 | continue 673 | 674 | injectables[n] = o 675 | 676 | return injectables 677 | 678 | def _create_component(self, name: str, ctyp: type, injectables: dict[str, Any]): 679 | type_hints = typing.get_type_hints(ctyp.__init__) 680 | NoneType = type(None) 681 | init_return_type = type_hints.pop("return", NoneType) 682 | assert ( 683 | init_return_type is NoneType 684 | ), f"{ctyp!r} __init__ had an unexpected non-None return type hint" 685 | requests = get_injection_requests(type_hints, name) 686 | injections = find_injections(requests, injectables, name) 687 | 688 | # Create instance, set it on self 689 | component = ctyp(**injections) 690 | setattr(self, name, component) 691 | 692 | # Ensure that mandatory methods are there 693 | if not callable(getattr(component, "execute", None)): 694 | raise ValueError( 695 | f"Component {name} ({component!r}) must have a method named 'execute'" 696 | ) 697 | 698 | # Automatically inject a logger object 699 | component.logger = logging.getLogger(name) 700 | 701 | self.logger.info("-> %s (class: %s)", name, ctyp.__name__) 702 | 703 | return component 704 | 705 | def _setup_vars(self, cname: str, component, injectables: dict[str, Any]) -> None: 706 | self.logger.debug("Injecting magic variables into %s", cname) 707 | 708 | type_hints = typing.get_type_hints(type(component)) 709 | requests = get_injection_requests(type_hints, cname, component) 710 | injections = find_injections(requests, injectables, cname) 711 | component.__dict__.update(injections) 712 | 713 | def _setup_reset_vars(self, component) -> None: 714 | reset_dict = collect_resets(type(component)) 715 | 716 | if reset_dict: 717 | component.__dict__.update(reset_dict) 718 | self._reset_components.append((reset_dict, component)) 719 | 720 | def _do_periodics(self) -> None: 721 | """Run periodic methods which run in every mode.""" 722 | watchdog = self.watchdog 723 | 724 | for method, setter in self._feedbacks: 725 | try: 726 | value = method() 727 | except: 728 | self.onException() 729 | else: 730 | setter(value) 731 | 732 | watchdog.addEpoch("@magicbot.feedback") 733 | 734 | for periodic, name in self.__periodics: 735 | periodic() 736 | watchdog.addEpoch(name) 737 | 738 | for reset_dict, component in self._reset_components: 739 | component.__dict__.update(reset_dict) 740 | 741 | def _enabled_periodic(self) -> None: 742 | """Run components and all periodic methods.""" 743 | watchdog = self.watchdog 744 | 745 | for name, component in self._components: 746 | try: 747 | component.execute() 748 | except: 749 | self.onException() 750 | watchdog.addEpoch(name) 751 | 752 | self._do_periodics() 753 | -------------------------------------------------------------------------------- /magicbot/py.typed: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotpy/robotpy-wpilib-utilities/2c535d4b92478776e59b8f731df190af5a41513c/magicbot/py.typed -------------------------------------------------------------------------------- /magicbot/state_machine.py: -------------------------------------------------------------------------------- 1 | import inspect 2 | import logging 3 | from typing import ( 4 | Any, 5 | Callable, 6 | ClassVar, 7 | NoReturn, 8 | Optional, 9 | Union, 10 | overload, 11 | ) 12 | from collections.abc import Sequence 13 | 14 | import wpilib 15 | 16 | from .magic_tunable import tunable 17 | 18 | if wpilib.RobotBase.isSimulation(): 19 | getTime = wpilib.Timer.getFPGATimestamp 20 | else: 21 | from time import monotonic as getTime 22 | 23 | 24 | class IllegalCallError(TypeError): 25 | pass 26 | 27 | 28 | class NoFirstStateError(ValueError): 29 | pass 30 | 31 | 32 | class MultipleFirstStatesError(ValueError): 33 | pass 34 | 35 | 36 | class MultipleDefaultStatesError(ValueError): 37 | pass 38 | 39 | 40 | class InvalidStateName(ValueError): 41 | pass 42 | 43 | 44 | class _State: 45 | def __init__( 46 | self, 47 | f: "StateMethod", 48 | first: bool = False, 49 | must_finish: bool = False, 50 | *, 51 | duration: Optional[float] = None, 52 | is_default: bool = False, 53 | ) -> None: 54 | name = f.__name__ 55 | 56 | # Can't define states that are named the same as things in the 57 | # base class, will cause issues. Catch it early. 58 | if hasattr(StateMachine, name): 59 | raise InvalidStateName(f"cannot have a state named '{name}'") 60 | 61 | # inspect the args, provide a correct call implementation 62 | allowed_args = "self", "tm", "state_tm", "initial_call" 63 | sig = inspect.signature(f) 64 | args = [] 65 | invalid_args = [] 66 | for i, arg in enumerate(sig.parameters.values()): 67 | if i == 0 and arg.name != "self": 68 | raise ValueError(f"First argument to {name} must be 'self'") 69 | if arg.kind is arg.VAR_POSITIONAL: 70 | raise ValueError(f"Cannot use *args in signature for function {name}") 71 | if arg.kind is arg.VAR_KEYWORD: 72 | raise ValueError( 73 | f"Cannot use **kwargs in signature for function {name}" 74 | ) 75 | if arg.kind is arg.KEYWORD_ONLY: 76 | raise ValueError( 77 | f"Cannot use keyword-only parameters for function {name}" 78 | ) 79 | if arg.name in allowed_args: 80 | args.append(arg.name) 81 | else: 82 | invalid_args.append(arg.name) 83 | 84 | if invalid_args: 85 | raise ValueError( 86 | "Invalid parameter names in {}: {}".format(name, ",".join(invalid_args)) 87 | ) 88 | 89 | self.name = name 90 | self.description = inspect.getdoc(f) 91 | self.first = first 92 | self.must_finish = must_finish 93 | self.is_default = is_default 94 | self.duration = duration 95 | 96 | varlist = {"f": f} 97 | args_code = ",".join(args) 98 | wrapper_code = f"lambda self, tm, state_tm, initial_call: f({args_code})" 99 | self.run = eval(wrapper_code, varlist, varlist) 100 | 101 | self.next_state: Optional[StateRef] 102 | 103 | def __call__(self, *args, **kwargs) -> NoReturn: 104 | raise IllegalCallError( 105 | "Do not call states directly, use begin/next_state instead" 106 | ) 107 | 108 | def __set_name__(self, owner: type, name: str) -> None: 109 | # Don't allow aliasing of states, it probably won't do what users expect 110 | if name != self.name: 111 | raise InvalidStateName( 112 | f"magicbot state '{self.name}' defined as attribute '{name}'" 113 | ) 114 | 115 | if not issubclass(owner, StateMachine): 116 | raise TypeError(f"magicbot state {name} defined in non-StateMachine") 117 | 118 | # make durations tunable 119 | if self.duration is not None: 120 | duration_attr = name + "_duration" 121 | # don't create it twice (in case of inheritance overriding) 122 | if getattr(owner, duration_attr, None) is None: 123 | setattr( 124 | owner, 125 | duration_attr, 126 | tunable(self.duration, writeDefault=False, subtable="state"), 127 | ) 128 | 129 | 130 | StateRef = Union[str, _State] 131 | StateMethod = Callable[..., None] 132 | 133 | 134 | class _StateData: 135 | def __init__(self, wrapper: _State) -> None: 136 | self.name = wrapper.name 137 | self.duration_attr = f"{self.name}_duration" 138 | self.expires: float = 0xFFFFFFFF 139 | self.ran = False 140 | self.run = wrapper.run 141 | self.must_finish = wrapper.must_finish 142 | 143 | if hasattr(wrapper, "next_state"): 144 | self.next_state = wrapper.next_state 145 | 146 | self.start_time: float 147 | 148 | 149 | def timed_state( 150 | *, 151 | duration: float, 152 | next_state: Optional[StateRef] = None, 153 | first: bool = False, 154 | must_finish: bool = False, 155 | ) -> Callable[[StateMethod], _State]: 156 | """ 157 | If this decorator is applied to a function in an object that inherits 158 | from :class:`.StateMachine`, it indicates that the function 159 | is a state that will run for a set amount of time unless interrupted. 160 | 161 | It is guaranteed that a timed_state will execute at least once, even if 162 | it expires prior to being executed. 163 | 164 | The decorated function can have the following arguments in any order: 165 | 166 | - ``tm`` - The number of seconds since the state machine has started 167 | - ``state_tm`` - The number of seconds since this state has been active 168 | (note: it may not start at zero!) 169 | - ``initial_call`` - Set to True when the state is initially called, 170 | False otherwise. If the state is switched to multiple times, this 171 | will be set to True at the start of each state execution. 172 | 173 | :param duration: The length of time to run the state before progressing 174 | to the next state 175 | :param next_state: The name of the next state. If not specified, then 176 | this will be the last state executed if time expires 177 | :param first: If True, this state will be ran first 178 | :param must_finish: If True, then this state will continue executing 179 | even if ``engage()`` is not called. However, 180 | if ``done()`` is called, execution will stop 181 | regardless of whether this is set. 182 | """ 183 | 184 | def decorator(f: StateMethod) -> _State: 185 | wrapper = _State(f, first, must_finish, duration=duration) 186 | 187 | wrapper.next_state = next_state 188 | 189 | return wrapper 190 | 191 | return decorator 192 | 193 | 194 | @overload 195 | def state( 196 | *, 197 | first: bool = ..., 198 | must_finish: bool = ..., 199 | ) -> Callable[[StateMethod], _State]: ... 200 | 201 | 202 | @overload 203 | def state(f: StateMethod) -> _State: ... 204 | 205 | 206 | def state( 207 | f: Optional[StateMethod] = None, 208 | *, 209 | first: bool = False, 210 | must_finish: bool = False, 211 | ) -> Union[Callable[[StateMethod], _State], _State]: 212 | """ 213 | If this decorator is applied to a function in an object that inherits 214 | from :class:`.StateMachine`, it indicates that the function 215 | is a state. The state will continue to be executed until the 216 | ``next_state`` function is executed. 217 | 218 | The decorated function can have the following arguments in any order: 219 | 220 | - ``tm`` - The number of seconds since the state machine has started 221 | - ``state_tm`` - The number of seconds since this state has been active 222 | (note: it may not start at zero!) 223 | - ``initial_call`` - Set to True when the state is initially called, 224 | False otherwise. If the state is switched to multiple times, this 225 | will be set to True at the start of each state execution. 226 | 227 | :param first: If True, this state will be ran first 228 | :param must_finish: If True, then this state will continue executing 229 | even if ``engage()`` is not called. However, 230 | if ``done()`` is called, execution will stop 231 | regardless of whether this is set. 232 | """ 233 | 234 | if f is None: 235 | return lambda f: _State(f, first, must_finish) 236 | 237 | return _State(f, first, must_finish) 238 | 239 | 240 | def default_state(f: StateMethod) -> _State: 241 | """ 242 | If this decorator is applied to a method in an object that inherits 243 | from :class:`.StateMachine`, it indicates that the method 244 | is a default state; that is, if no other states are executing, this 245 | state will execute. If the state machine is always executing, the 246 | default state will never execute. 247 | 248 | There can only be a single default state in a StateMachine object. 249 | 250 | The decorated function can have the following arguments in any order: 251 | 252 | - ``tm`` - The number of seconds since the state machine has started 253 | - ``state_tm`` - The number of seconds since this state has been active 254 | (note: it may not start at zero!) 255 | - ``initial_call`` - Set to True when the state is initially called, 256 | False otherwise. If the state is switched to multiple times, this 257 | will be set to True at the start of each state execution. 258 | """ 259 | return _State(f, first=False, must_finish=True, is_default=True) 260 | 261 | 262 | def _get_class_members(cls: type) -> dict[str, Any]: 263 | """Get the members of the given class in definition order, bases first.""" 264 | d = {} 265 | for cls in reversed(cls.__mro__): 266 | d.update(cls.__dict__) 267 | return d 268 | 269 | 270 | class StateMachine: 271 | ''' 272 | The StateMachine class is used to implement magicbot components that 273 | allow one to easily define a `finite state machine (FSM) 274 | `_ that can be 275 | executed via the magicbot framework. 276 | 277 | You create a component class that inherits from ``StateMachine``. 278 | Each state is represented as a single function, and you indicate that 279 | a function is a particular state by decorating it with one of the 280 | following decorators: 281 | 282 | * :func:`@default_state <.default_state>` 283 | * :func:`@state <.state>` 284 | * :func:`@timed_state <.timed_state>` 285 | 286 | As the state machine executes, the decorated function representing the 287 | current state will be called. Decorated state functions can receive the 288 | following parameters (all of which are optional): 289 | 290 | - ``tm`` - The number of seconds since autonomous has started 291 | - ``state_tm`` - The number of seconds since this state has been active 292 | (note: it may not start at zero!) 293 | - ``initial_call`` - Set to True when the state is initially called, 294 | False otherwise. If the state is switched to multiple times, this 295 | will be set to True at the start of each state. 296 | 297 | To be consistent with the magicbot philosophy, in order for the 298 | state machine to execute its states you must call the :func:`engage` 299 | function upon each execution of the main robot control loop. If you do 300 | not call this function, then execution of the FSM will cease. 301 | 302 | .. note:: If you wish for the FSM to continue executing state functions 303 | regardless whether ``engage()`` is called, you must set the 304 | ``must_finish`` parameter in your state decorator to be True. 305 | 306 | When execution ceases (because ``engage()`` was not called), the 307 | :func:`done` function will be called and the FSM will be reset to the 308 | starting state. The state functions will not be called again unless 309 | ``engage`` is called. 310 | 311 | As a magicbot component, StateMachine contains an ``execute`` function that 312 | will be called on each control loop. All state execution occurs from 313 | within that function call. If you call other components from a 314 | StateMachine, you should ensure that your StateMachine is declared 315 | *before* the other components in your Robot class. 316 | 317 | .. warning:: As StateMachine already contains an execute function, 318 | there is no need to define your own ``execute`` function for 319 | a state machine component -- if you override ``execute``, 320 | then the state machine may not work correctly. Instead, 321 | use the :func:`@default_state <.default_state>` decorator. 322 | 323 | Here's a very simple example of how you might implement a shooter 324 | automation component that moves a ball into a shooter when the 325 | shooter is ready:: 326 | 327 | class ShooterAutomation(magicbot.StateMachine): 328 | 329 | # Some other component 330 | shooter: Shooter 331 | ball_pusher: BallPusher 332 | 333 | def fire(self): 334 | """This is called from the main loop.""" 335 | self.engage() 336 | 337 | @state(first=True) 338 | def begin_firing(self): 339 | """ 340 | This function will only be called IFF fire is called and 341 | the FSM isn't currently in the 'firing' state. If fire 342 | was not called, this function will not execute. 343 | """ 344 | self.shooter.enable() 345 | if self.shooter.ready(): 346 | self.next_state('firing') 347 | 348 | @timed_state(duration=1.0, must_finish=True) 349 | def firing(self): 350 | """ 351 | Because must_finish=True, once the FSM has reached this state, 352 | this state will continue executing even if engage isn't called. 353 | """ 354 | self.shooter.enable() 355 | self.ball_pusher.push() 356 | 357 | # 358 | # Note that there is no execute function defined as part of 359 | # this component 360 | # 361 | 362 | ... 363 | 364 | class MyRobot(magicbot.MagicRobot): 365 | shooter_automation: ShooterAutomation 366 | 367 | shooter: Shooter 368 | ball_pusher: BallPusher 369 | 370 | def teleopPeriodic(self): 371 | 372 | if self.joystick.getTrigger(): 373 | self.shooter_automation.fire() 374 | 375 | This object has a lot of really useful NetworkTables integration as well: 376 | 377 | - tunables are created in /components/NAME/state 378 | - state durations can be tuned here 379 | - The 'current state' is output as it happens 380 | - Descriptions and names of the states are here (for dashboard use) 381 | 382 | 383 | .. warning:: This object is not intended to be threadsafe and should not 384 | be accessed from multiple threads 385 | ''' 386 | 387 | VERBOSE_LOGGING = False 388 | 389 | #: A Python logging object automatically injected by magicbot. 390 | #: It can be used to send messages to the log, instead of using print statements. 391 | logger: logging.Logger 392 | 393 | #: NT variable that indicates which state will be executed next (though, 394 | #: does not guarantee that it will be executed). Will return an empty 395 | #: string if the state machine is not currently engaged. 396 | current_state = tunable("", subtable="state") 397 | 398 | state_names: ClassVar[tunable[Sequence[str]]] 399 | state_descriptions: ClassVar[tunable[Sequence[str]]] 400 | 401 | def __new__(cls) -> "StateMachine": 402 | # choose to use __new__ instead of __init__ 403 | o = super().__new__(cls) 404 | o._build_states() 405 | return o 406 | 407 | # TODO: when this gets invoked, tunables need to be setup on 408 | # the object first 409 | 410 | def _build_states(self) -> None: 411 | has_first = False 412 | 413 | # problem: the user interface won't know which entries are the 414 | # current variables being used by the robot. So, we setup 415 | # an array with the names, and the dashboard uses that 416 | # to determine the ordering too 417 | 418 | nt_names = [] 419 | nt_desc = [] 420 | 421 | states = {} 422 | cls = type(self) 423 | 424 | default_state = None 425 | 426 | # for each state function: 427 | for name, state in _get_class_members(cls).items(): 428 | if not isinstance(state, _State): 429 | continue 430 | 431 | # is this the first state to execute? 432 | if state.first: 433 | if has_first: 434 | raise MultipleFirstStatesError( 435 | "Multiple states were specified as the first state!" 436 | ) 437 | 438 | self.__first = name 439 | has_first = True 440 | 441 | state_data = _StateData(state) 442 | states[name] = state_data 443 | nt_names.append(name) 444 | nt_desc.append(state.description or "") 445 | 446 | if state.is_default: 447 | if default_state is not None: 448 | raise MultipleDefaultStatesError( 449 | "Multiple default states are not allowed" 450 | ) 451 | default_state = state_data 452 | 453 | if not has_first: 454 | raise NoFirstStateError( 455 | "Starting state not defined! Use first=True on a state decorator" 456 | ) 457 | 458 | # NOTE: this depends on tunables being bound after this function is called 459 | cls.state_names = tunable(nt_names, subtable="state") 460 | cls.state_descriptions = tunable(nt_desc, subtable="state") 461 | 462 | # Indicates that an external party wishes the state machine to execute 463 | self.__should_engage = False 464 | 465 | # Indicates that the state machine is currently executing 466 | self.__engaged = False 467 | 468 | # A dictionary of states 469 | self.__states = states 470 | 471 | # The currently executing state, or None if not executing 472 | self.__state: Optional[_StateData] = None 473 | 474 | # The default state 475 | self.__default_state = default_state 476 | 477 | # Variable to store time in 478 | self.__start = 0 479 | 480 | @property 481 | def is_executing(self) -> bool: 482 | """:returns: True if the state machine is executing states""" 483 | # return self.__state is not None 484 | return self.__engaged 485 | 486 | def on_enable(self) -> None: 487 | """ 488 | magicbot component API: called when autonomous/teleop is enabled 489 | """ 490 | pass 491 | 492 | def on_disable(self) -> None: 493 | """ 494 | magicbot component API: called when autonomous/teleop is disabled 495 | """ 496 | self.done() 497 | 498 | def engage( 499 | self, 500 | initial_state: Optional[StateRef] = None, 501 | force: bool = False, 502 | ) -> None: 503 | """ 504 | This signals that you want the state machine to execute its 505 | states. 506 | 507 | :param initial_state: If specified and execution is not currently 508 | occurring, start in this state instead of 509 | in the 'first' state 510 | :param force: If True, will transition even if the state 511 | machine is currently active. 512 | """ 513 | self.__should_engage = True 514 | 515 | if force or self.__state is None or self.__state is self.__default_state: 516 | if initial_state: 517 | self.next_state(initial_state) 518 | else: 519 | self.next_state(self.__first) 520 | 521 | def next_state(self, state: StateRef) -> None: 522 | """Call this function to transition to the next state 523 | 524 | :param state: Name of the state to transition to 525 | 526 | .. note:: This should only be called from one of the state functions 527 | """ 528 | if isinstance(state, _State): 529 | state = state.name 530 | 531 | state_data = self.__states[state] 532 | state_data.ran = False 533 | self.current_state = state 534 | 535 | self.__state = state_data 536 | 537 | def next_state_now(self, state: StateRef) -> None: 538 | """Call this function to transition to the next state, and call the next 539 | state function immediately. Prefer to use :meth:`next_state` instead. 540 | 541 | :param state: Name of the state to transition to 542 | 543 | .. note:: This should only be called from one of the state functions 544 | """ 545 | self.next_state(state) 546 | # TODO: may want to do this differently? 547 | self.execute() 548 | 549 | def done(self) -> None: 550 | """Call this function to end execution of the state machine. 551 | 552 | This function will always be called when a state machine ends. Even if 553 | the engage function is called repeatedly, done() will be called. 554 | 555 | .. note:: If you wish to do something each time execution ceases, 556 | override this function (but be sure to call 557 | ``super().done()``!) 558 | """ 559 | if self.VERBOSE_LOGGING and self.__state is not None: 560 | self.logger.info("Stopped state machine execution") 561 | 562 | self.__state = None 563 | self.__engaged = False 564 | self.current_state = "" 565 | 566 | def execute(self) -> None: 567 | """ 568 | magicbot component API: This is called on each iteration of the 569 | control loop. Most of the time, you will not want to override 570 | this function. If you find you want to, you may want to use the 571 | @default_state mechanism instead. 572 | """ 573 | 574 | now = getTime() 575 | 576 | if not self.__engaged: 577 | if self.__should_engage: 578 | self.__start = now 579 | self.__engaged = True 580 | elif self.__default_state is None: 581 | return 582 | 583 | # tm is the number of seconds that the state machine has been executing 584 | tm = now - self.__start 585 | state = self.__state 586 | done_called = False 587 | 588 | # we adjust this so that if we have states chained together, 589 | # then the total time it runs is the amount of time of the 590 | # states. Otherwise, the time drifts. 591 | new_state_start = tm 592 | 593 | # determine if the time has passed to execute the next state 594 | # -> intentionally comes first 595 | if state is not None and state.ran and state.expires < tm: 596 | new_state_start = state.expires 597 | 598 | if state.next_state is None: 599 | # If the state expires and it's the last state, if the machine 600 | # is still engaged then it should cycle back to the beginning 601 | # ... but we should call done() first 602 | done_called = True 603 | self.done() 604 | 605 | if self.__should_engage: 606 | self.next_state(self.__first) 607 | state = self.__state 608 | else: 609 | state = None 610 | else: 611 | self.next_state(state.next_state) 612 | state = self.__state 613 | 614 | # deactivate the current state unless engage was called or 615 | # must_finish was set 616 | if not (self.__should_engage or state is not None and state.must_finish): 617 | state = None 618 | 619 | # if there is no state to execute and there is a default 620 | # state, do the default state 621 | if state is None and self.__default_state is not None: 622 | state = self.__default_state 623 | if self.__state != state: 624 | state.ran = False 625 | self.__state = state 626 | 627 | if state is not None: 628 | # is this the first time this was executed? 629 | initial_call = not state.ran 630 | if initial_call: 631 | state.ran = True 632 | state.start_time = new_state_start 633 | state.expires = new_state_start + getattr( 634 | self, state.duration_attr, 0xFFFFFFFF 635 | ) 636 | 637 | if self.VERBOSE_LOGGING: 638 | self.logger.info("%.3fs: Entering state: %s", tm, state.name) 639 | 640 | # execute the state function, passing it the arguments 641 | state.run(self, tm, tm - state.start_time, initial_call) 642 | elif not done_called: 643 | # or clear the state 644 | self.done() 645 | 646 | # Reset this each time 647 | self.__should_engage = False 648 | 649 | 650 | class AutonomousStateMachine(StateMachine): 651 | """ 652 | This is a specialized version of the StateMachine that is designed 653 | to be used as an autonomous mode. There are a few key differences: 654 | 655 | - The :func:`.engage` function is always called, so the state machine 656 | will always run to completion unless done() is called 657 | - VERBOSE_LOGGING is set to True, so a log message will be printed out upon 658 | each state transition 659 | 660 | """ 661 | 662 | VERBOSE_LOGGING = True 663 | 664 | def on_enable(self) -> None: 665 | super().on_enable() 666 | self.__engaged = True 667 | 668 | def on_iteration(self, tm: float) -> None: 669 | # TODO, remove the on_iteration function in 2017? 670 | 671 | # Only engage the state machine until its execution finishes, otherwise 672 | # it will just keep repeating 673 | # 674 | # This is because if you keep calling engage(), the state machine will 675 | # loop. I'm tempted to change that, but I think it would lead to unexpected 676 | # side effects. Will have to contemplate this... 677 | 678 | if self.__engaged: 679 | self.engage() 680 | self.execute() 681 | self.__engaged = self.is_executing 682 | 683 | def done(self) -> None: 684 | super().done() 685 | self._StateMachine__should_engage = False 686 | self.__engaged = False 687 | -------------------------------------------------------------------------------- /robotpy_ext/__init__.py: -------------------------------------------------------------------------------- 1 | try: 2 | from .version import version as __version__ 3 | except ImportError: # pragma: nocover 4 | __version__ = "master" 5 | -------------------------------------------------------------------------------- /robotpy_ext/autonomous/__init__.py: -------------------------------------------------------------------------------- 1 | # Autonomous mode switcher 2 | from .selector import AutonomousModeSelector 3 | 4 | # Autonomous mode state machine 5 | from .stateful_autonomous import state, timed_state, StatefulAutonomous 6 | -------------------------------------------------------------------------------- /robotpy_ext/autonomous/selector.py: -------------------------------------------------------------------------------- 1 | import importlib 2 | import inspect 3 | import logging 4 | import os 5 | from glob import glob 6 | from typing import Callable, Union 7 | from collections.abc import Sequence 8 | 9 | import hal 10 | import wpilib 11 | 12 | from ..misc.precise_delay import NotifierDelay 13 | from ..misc.simple_watchdog import SimpleWatchdog 14 | 15 | logger = logging.getLogger("autonomous") 16 | 17 | 18 | class AutonomousModeSelector: 19 | """ 20 | This object loads all modules in a specified python package, and tries 21 | to automatically discover autonomous modes from them. Each module is 22 | added to a ``SendableChooser`` object, which allows the user to select 23 | one of them via SmartDashboard. 24 | 25 | Autonomous mode objects must implement the following functions: 26 | 27 | - ``on_enable`` - Called when autonomous mode is initially enabled 28 | - ``on_disable`` - Called when autonomous mode is no longer active 29 | - ``on_iteration`` - Called for each iteration of the autonomous control loop 30 | 31 | Your autonomous object may have the following attributes: 32 | 33 | - ``MODE_NAME`` - The name of the autonomous mode to display to users 34 | - ``DISABLED`` - If True, don't allow this mode to be selected 35 | - ``DEFAULT`` - If True, this is the default autonomous mode selected 36 | 37 | Here is an example of using ``AutonomousModeSelector`` in ``TimedRobot``: 38 | 39 | .. code-block:: python 40 | 41 | class MyRobot(wpilib.TimedRobot): 42 | 43 | def robotInit(self): 44 | self.automodes = AutonomousModeSelector('autonomous') 45 | 46 | def autonomousInit(self): 47 | self.automodes.start() 48 | 49 | def autonomousPeriodic(self): 50 | self.automodes.periodic() 51 | 52 | def disabledInit(self): 53 | self.automodes.disable() 54 | 55 | If you use AutonomousModeSelector, you may also be interested in 56 | the autonomous state machine helper (:class:`.StatefulAutonomous`). 57 | 58 | Check out the samples in our github repository that show some basic 59 | usage of ``AutonomousModeSelector``. 60 | 61 | .. note:: If you use AutonomousModeSelector, then you should add 62 | ``robotpy_ext.autonomous.selector_tests`` to your pyfrc 63 | unit tests like so:: 64 | 65 | from robotpy_ext.autonomous.selector_tests import * 66 | 67 | .. note:: 68 | 69 | For your autonomous mode's ``on_disable`` method to be called, 70 | you must call :meth:`disable` in ``disabledInit``. 71 | 72 | It is okay to not call :meth:`disable` if you do not need ``on_disable``. 73 | """ 74 | 75 | def __init__(self, autonomous_pkgname, *args, **kwargs): 76 | """ 77 | :param autonomous_pkgname: Module to load autonomous modes from 78 | :param args: Args to pass to created autonomous modes 79 | :param kwargs: Keyword args to pass to created autonomous modes 80 | """ 81 | 82 | self.modes = {} 83 | self.active_mode = None 84 | self.robot_exit = False 85 | 86 | logger.info("Begin initializing autonomous mode switcher") 87 | 88 | # load all modules in specified module 89 | modules = [] 90 | 91 | try: 92 | autonomous_pkg = importlib.import_module(autonomous_pkgname) 93 | except ImportError as e: 94 | if e.name not in [autonomous_pkgname, autonomous_pkgname.split(".")[0]]: 95 | raise 96 | 97 | # Don't kill the robot because they didn't create an autonomous package 98 | logger.warning("Cannot load the '%s' package", autonomous_pkgname) 99 | else: 100 | pkgdirs = [] 101 | pkgfile = getattr(autonomous_pkg, "__file__", None) 102 | if pkgfile: 103 | pkgdirs = [os.path.dirname(os.path.abspath(pkgfile))] 104 | else: 105 | # implicit packages have no __file__, just __path__ 106 | pkgpath = getattr(autonomous_pkg, "__path__", None) 107 | if pkgpath: 108 | pkgdirs = list(set(pkgpath)) 109 | 110 | if pkgdirs: 111 | for pkgdir in pkgdirs: 112 | modules.extend(glob(os.path.join(pkgdir, "*.py"))) 113 | 114 | for module_filename in modules: 115 | module = None 116 | module_name = os.path.basename(module_filename[:-3]) 117 | 118 | if module_name in ["__init__"]: 119 | continue 120 | 121 | try: 122 | module = importlib.import_module("." + module_name, autonomous_pkgname) 123 | # module = imp.load_source('.' + module_name, module_filename) 124 | except: 125 | if not wpilib.DriverStation.isFMSAttached(): 126 | raise 127 | 128 | # 129 | # Find autonomous mode classes in the modules that are present 130 | # -> note that we actually create the instance of the objects here, 131 | # so that way we find out about any errors *before* we get out 132 | # on the field.. 133 | 134 | for name, obj in inspect.getmembers(module, inspect.isclass): 135 | mode_name = getattr(obj, "MODE_NAME", None) 136 | if mode_name is not None: 137 | # don't allow the driver to select this mode 138 | if getattr(obj, "DISABLED", False): 139 | logger.warning( 140 | "autonomous mode %s is marked as disabled", obj.MODE_NAME 141 | ) 142 | continue 143 | 144 | try: 145 | instance = obj(*args, **kwargs) 146 | except: 147 | if not wpilib.DriverStation.isFMSAttached(): 148 | raise 149 | else: 150 | continue 151 | 152 | if mode_name in self.modes: 153 | if not wpilib.DriverStation.isFMSAttached(): 154 | raise RuntimeError( 155 | f"Duplicate name {mode_name} in {module_filename}" 156 | ) 157 | 158 | logger.error( 159 | "Duplicate name %s specified by object type %s in module %s", 160 | mode_name, 161 | name, 162 | module_filename, 163 | ) 164 | self.modes[name + "_" + module_filename] = instance 165 | else: 166 | self.modes[instance.MODE_NAME] = instance 167 | 168 | # now that we have a bunch of valid autonomous mode objects, let 169 | # the user select one using the SmartDashboard. 170 | 171 | # SmartDashboard interface 172 | self.chooser = wpilib.SendableChooser() 173 | 174 | default_modes = [] 175 | mode_names = [] 176 | 177 | logger.info("Loaded autonomous modes:") 178 | for k, v in sorted(self.modes.items()): 179 | if getattr(v, "DEFAULT", False): 180 | logger.info(" -> %s [Default]", k) 181 | self.chooser.setDefaultOption(k, v) 182 | default_modes.append(k) 183 | else: 184 | logger.info(" -> %s", k) 185 | self.chooser.addOption(k, v) 186 | 187 | mode_names.append(k) 188 | 189 | if len(self.modes) == 0: 190 | logger.warning("-- no autonomous modes were loaded!") 191 | 192 | self.chooser.addOption("None", None) 193 | 194 | if len(default_modes) == 0: 195 | self.chooser.setDefaultOption("None", None) 196 | elif len(default_modes) != 1: 197 | if not wpilib.DriverStation.isFMSAttached(): 198 | raise RuntimeError( 199 | "More than one autonomous mode was specified as default! (modes: {})".format( 200 | ", ".join(default_modes) 201 | ) 202 | ) 203 | 204 | # must PutData after setting up objects 205 | wpilib.SmartDashboard.putData("Autonomous Mode", self.chooser) 206 | 207 | # XXX: Compatibility with the FRC dashboard 208 | wpilib.SmartDashboard.putStringArray("Auto List", mode_names) 209 | 210 | logger.info("Autonomous switcher initialized") 211 | 212 | def endCompetition(self): 213 | """Call this function when your robot's endCompetition function is called""" 214 | self.robot_exit = True 215 | 216 | def run( 217 | self, 218 | control_loop_wait_time: float = 0.020, 219 | iter_fn: Union[Callable[[], None], Sequence[Callable[[], None]]] = None, 220 | on_exception: Callable = None, 221 | watchdog: Union[wpilib.Watchdog, SimpleWatchdog] = None, 222 | ) -> None: 223 | """ 224 | This method implements the entire autonomous loop. 225 | 226 | Do not call this from ``TimedRobot`` as this will break the 227 | timing of your control loop when your robot switches to teleop. 228 | 229 | This function will NOT exit until autonomous mode has ended. If 230 | you need to execute code in all autonomous modes, pass a function 231 | or list of functions as the ``iter_fn`` parameter, and they will be 232 | called once per autonomous mode iteration. 233 | 234 | :param control_loop_wait_time: Amount of time between iterations in seconds 235 | :param iter_fn: Called at the end of every iteration while 236 | autonomous mode is executing 237 | :param on_exception: Called when an uncaught exception is raised, 238 | must take a single keyword arg "forceReport" 239 | :param watchdog: a WPILib Watchdog to feed every iteration 240 | """ 241 | if watchdog is not None: 242 | watchdog.reset() 243 | 244 | if isinstance(watchdog, SimpleWatchdog): 245 | watchdog_check_expired = watchdog.printIfExpired 246 | else: 247 | 248 | def watchdog_check_expired(): 249 | if watchdog.isExpired(): 250 | watchdog.printEpochs() 251 | 252 | logger.info("Begin autonomous") 253 | 254 | if iter_fn is None: 255 | iter_fn = (lambda: None,) 256 | elif callable(iter_fn): 257 | iter_fn = (iter_fn,) 258 | 259 | if on_exception is None: 260 | on_exception = self._on_exception 261 | 262 | # keep track of how much time has passed in autonomous mode 263 | timer = wpilib.Timer() 264 | timer.start() 265 | 266 | try: 267 | self._on_autonomous_enable() 268 | except: 269 | on_exception(forceReport=True) 270 | if watchdog is not None: 271 | watchdog.addEpoch("auto on_enable") 272 | 273 | # 274 | # Autonomous control loop 275 | # 276 | 277 | observe = hal.observeUserProgramAutonomous 278 | refreshData = wpilib.DriverStation.refreshData 279 | isAutonomousEnabled = wpilib.DriverStation.isAutonomousEnabled 280 | 281 | with NotifierDelay(control_loop_wait_time) as delay: 282 | while not self.robot_exit: 283 | refreshData() 284 | if not isAutonomousEnabled(): 285 | break 286 | 287 | observe() 288 | try: 289 | self._on_iteration(timer.get()) 290 | except: 291 | on_exception() 292 | if watchdog is not None: 293 | watchdog.addEpoch("auto on_iteration") 294 | 295 | for fn in iter_fn: 296 | fn() 297 | 298 | if watchdog is not None: 299 | watchdog.disable() 300 | 301 | watchdog_check_expired() 302 | 303 | delay.wait() 304 | if watchdog is not None: 305 | watchdog.reset() 306 | 307 | # 308 | # Done with autonomous, finish up 309 | # 310 | 311 | try: 312 | self.disable() 313 | except: 314 | on_exception(forceReport=True) 315 | 316 | logger.info("Autonomous mode ended") 317 | 318 | def start(self) -> None: 319 | """Start autonomous mode. 320 | 321 | This initialises the selected autonomous mode. 322 | Call this from your ``autonomousInit`` method. 323 | 324 | .. versionadded:: 2020.1.5 325 | """ 326 | self.timer = wpilib.Timer() 327 | self.timer.start() 328 | 329 | self._on_autonomous_enable() 330 | 331 | def periodic(self) -> None: 332 | """Execute one control loop iteration of the active autonomous mode. 333 | 334 | Call this from your ``autonomousPeriodic`` method. 335 | 336 | .. versionadded:: 2020.1.5 337 | """ 338 | self._on_iteration(self.timer.get()) 339 | 340 | def disable(self) -> None: 341 | """Disables the active autonomous mode. 342 | 343 | You can call this from your ``disabledInit`` method 344 | to call your autonomous mode's ``on_disable`` method. 345 | 346 | .. versionadded:: 2020.1.5 347 | """ 348 | if self.active_mode is not None: 349 | logger.info("Disabling '%s'", self.active_mode.MODE_NAME) 350 | self.active_mode.on_disable() 351 | 352 | self.active_mode = None 353 | 354 | # 355 | # Internal methods used to implement autonomous mode switching, and 356 | # are called automatically 357 | # 358 | 359 | def _on_autonomous_enable(self) -> None: 360 | """Selects the active autonomous mode and enables it""" 361 | 362 | # XXX: FRC Dashboard compatibility 363 | # -> if you set it here, you're stuck using it. The FRC Dashboard 364 | # doesn't seem to have a default (nor will it show a default), 365 | # so the key will only get set if you set it. 366 | auto_mode = wpilib.SmartDashboard.getString("Auto Selector", None) 367 | if auto_mode is not None and auto_mode in self.modes: 368 | logger.info("Using autonomous mode set by LabVIEW dashboard") 369 | self.active_mode = self.modes[auto_mode] 370 | else: 371 | self.active_mode = self.chooser.getSelected() 372 | 373 | if self.active_mode is not None: 374 | logger.info("Enabling '%s'", self.active_mode.MODE_NAME) 375 | self.active_mode.on_enable() 376 | else: 377 | logger.warning( 378 | "No autonomous modes were selected, not enabling autonomous mode" 379 | ) 380 | 381 | def _on_iteration(self, time_elapsed: float) -> None: 382 | """Run the code for the current autonomous mode""" 383 | if self.active_mode is not None: 384 | self.active_mode.on_iteration(time_elapsed) 385 | 386 | def _on_exception(self, forceReport: bool = False): 387 | if not wpilib.DriverStation.isFMSAttached(): 388 | raise 389 | -------------------------------------------------------------------------------- /robotpy_ext/autonomous/selector_tests.py: -------------------------------------------------------------------------------- 1 | import logging 2 | from pyfrc.test_support.controller import TestController as PyfrcTestController 3 | from ntcore.util import ChooserControl 4 | 5 | 6 | autonomous_seconds = 15 7 | 8 | 9 | def test_all_autonomous(control: PyfrcTestController): 10 | """ 11 | This test runs all possible autonomous modes that can be selected 12 | by the autonomous switcher. 13 | 14 | This should work for most robots. If it doesn't work for yours, 15 | and it's not a code issue with your robot, please file a bug on 16 | github. 17 | """ 18 | 19 | logger = logging.getLogger("test-all-autonomous") 20 | 21 | with control.run_robot(): 22 | # Run disabled for a short period, chooser needs to be 23 | # initialized in robotInit 24 | control.step_timing(seconds=0.5, autonomous=True, enabled=False) 25 | 26 | # retrieve autonomous modes from chooser here 27 | chooser = ChooserControl("Autonomous Mode") 28 | choices = chooser.getChoices() 29 | if len(choices) == 0: 30 | return 31 | 32 | for choice in choices: 33 | chooser.setSelected(choice) 34 | logger.info(f"{'='*10} Testing '{choice}' {'='*10}") 35 | 36 | # Run disabled for a short period 37 | control.step_timing(seconds=0.5, autonomous=True, enabled=False) 38 | 39 | # Run enabled for 15 seconds 40 | control.step_timing( 41 | seconds=autonomous_seconds, autonomous=True, enabled=True 42 | ) 43 | 44 | # Disabled for another short period 45 | control.step_timing(seconds=0.5, autonomous=True, enabled=False) 46 | -------------------------------------------------------------------------------- /robotpy_ext/autonomous/stateful_autonomous.py: -------------------------------------------------------------------------------- 1 | import functools 2 | import inspect 3 | import logging 4 | from typing import Callable, Optional 5 | 6 | import ntcore 7 | import wpilib 8 | 9 | logger = logging.getLogger("autonomous") 10 | 11 | 12 | # use this to track ordering of functions, so that we can display them 13 | # properly in the tuning widget on the dashboard 14 | __global_cnt_serial = [0] 15 | 16 | 17 | def _get_state_serial(): 18 | __global_cnt_serial[0] = __global_cnt_serial[0] + 1 19 | return __global_cnt_serial[0] 20 | 21 | 22 | class _State: 23 | def __init__(self, f: Callable, first: bool): 24 | # inspect the args, provide a correct call implementation 25 | allowed_args = "self", "tm", "state_tm", "initial_call" 26 | sig = inspect.signature(f) 27 | name = f.__name__ 28 | 29 | args = [] 30 | invalid_args = [] 31 | for i, arg in enumerate(sig.parameters.values()): 32 | if i == 0 and arg.name != "self": 33 | raise ValueError(f"First argument to {name} must be 'self'") 34 | if arg.kind is arg.VAR_POSITIONAL: 35 | raise ValueError(f"Cannot use *args in signature for function {name}") 36 | if arg.kind is arg.VAR_KEYWORD: 37 | raise ValueError( 38 | f"Cannot use **kwargs in signature for function {name}" 39 | ) 40 | if arg.kind is arg.KEYWORD_ONLY: 41 | raise ValueError( 42 | f"Cannot use keyword-only parameters for function {name}" 43 | ) 44 | if arg.name in allowed_args: 45 | args.append(arg.name) 46 | else: 47 | invalid_args.append(arg.name) 48 | 49 | if invalid_args: 50 | raise ValueError( 51 | "Invalid parameter names in {}: {}".format(name, ",".join(invalid_args)) 52 | ) 53 | 54 | functools.update_wrapper(self, f) 55 | 56 | # store state variables here 57 | self._func = f 58 | self.name = name 59 | self.description = f.__doc__ 60 | self.ran = False 61 | self.first = first 62 | self.expires = 0xFFFFFFFF 63 | self.serial = _get_state_serial() 64 | 65 | varlist = {"f": f} 66 | args_code = ",".join(args) 67 | wrapper_code = f"lambda self, tm, state_tm, initial_call: f({args_code})" 68 | self.run = eval(wrapper_code, varlist, varlist) 69 | 70 | def __call__(self, *args, **kwargs): 71 | self._func(*args, **kwargs) 72 | 73 | def __set_name__(self, owner: type, name: str) -> None: 74 | if not issubclass(owner, StatefulAutonomous): 75 | raise TypeError( 76 | f"StatefulAutonomous state {name} defined in non-StatefulAutonomous" 77 | ) 78 | 79 | 80 | # 81 | # Decorators: 82 | # 83 | # state 84 | # timed_state 85 | # 86 | 87 | 88 | def timed_state( 89 | f: Optional[Callable] = None, 90 | duration: float = None, 91 | next_state: str = None, 92 | first: bool = False, 93 | ): 94 | """ 95 | If this decorator is applied to a function in an object that inherits 96 | from :class:`.StatefulAutonomous`, it indicates that the function 97 | is a state that will run for a set amount of time unless interrupted 98 | 99 | The decorated function can have the following arguments in any order: 100 | 101 | - ``tm`` - The number of seconds since autonomous has started 102 | - ``state_tm`` - The number of seconds since this state has been active 103 | (note: it may not start at zero!) 104 | - ``initial_call`` - Set to True when the state is initially called, 105 | False otherwise. If the state is switched to multiple times, this 106 | will be set to True at the start of each state. 107 | 108 | :param duration: The length of time to run the state before progressing 109 | to the next state 110 | :param next_state: The name of the next state. If not specified, then 111 | this will be the last state executed if time expires 112 | :param first: If True, this state will be ran first 113 | """ 114 | 115 | if f is None: 116 | return functools.partial( 117 | timed_state, duration=duration, next_state=next_state, first=first 118 | ) 119 | 120 | if duration is None: 121 | raise ValueError("timed_state functions must specify a duration") 122 | 123 | wrapper = _State(f, first) 124 | 125 | wrapper.next_state = next_state 126 | wrapper.duration = duration 127 | 128 | return wrapper 129 | 130 | 131 | def state(f: Optional[Callable] = None, first: bool = False): 132 | """ 133 | If this decorator is applied to a function in an object that inherits 134 | from :class:`.StatefulAutonomous`, it indicates that the function 135 | is a state. The state will continue to be executed until the 136 | ``next_state`` function is executed. 137 | 138 | The decorated function can have the following arguments in any order: 139 | 140 | - ``tm`` - The number of seconds since autonomous has started 141 | - ``state_tm`` - The number of seconds since this state has been active 142 | (note: it may not start at zero!) 143 | - ``initial_call`` - Set to True when the state is initially called, 144 | False otherwise. If the state is switched to multiple times, this 145 | will be set to True at the start of each state. 146 | 147 | :param first: If True, this state will be ran first 148 | """ 149 | 150 | if f is None: 151 | return functools.partial(state, first=first) 152 | 153 | return _State(f, first) 154 | 155 | 156 | class StatefulAutonomous: 157 | """ 158 | This object is designed to be used to implement autonomous modes that 159 | can be used with the :class:`.AutonomousModeSelector` object to select 160 | an appropriate autonomous mode. However, you don't have to. 161 | 162 | This object is designed to meet the following goals: 163 | 164 | - Supports simple built-in tuning of autonomous mode parameters via 165 | SmartDashboard 166 | - Easy to create autonomous modes that support state machine or 167 | time-based operation 168 | - Autonomous modes that are easy to read and understand 169 | 170 | You use this by defining a class that inherits from ``StatefulAutonomous``. 171 | To define each state, you use the :func:`timed_state` decorator on a 172 | function. When each state is run, the decorated function will be 173 | called. Decorated functions can receive the following parameters: 174 | 175 | - ``tm`` - The number of seconds since autonomous has started 176 | - ``state_tm`` - The number of seconds since this state has been active 177 | (note: it may not start at zero!) 178 | - ``initial_call`` - Set to True when the state is initially called, 179 | False otherwise. If the state is switched to multiple times, this 180 | will be set to True at the start of each state. 181 | 182 | An example autonomous mode that drives the robot forward for 5 seconds 183 | might look something like this:: 184 | 185 | from robotpy_ext.autonomous import StatefulAutonomous 186 | 187 | class DriveForward(StatefulAutonomous): 188 | 189 | MODE_NAME = 'Drive Forward' 190 | 191 | def initialize(self): 192 | pass 193 | 194 | @timed_state(duration=0.5, next_state='drive_forward', first=True) 195 | def drive_wait(self): 196 | pass 197 | 198 | @timed_state(duration=5) 199 | def drive_forward(self): 200 | self.drive.move(0, 1, 0) 201 | 202 | Note that in this example, it is assumed that the DriveForward object 203 | is initialized with a dictionary with a value 'drive' that contains 204 | an object that has a move function:: 205 | 206 | components = {'drive': SomeObject() } 207 | mode = DriveForward(components) 208 | 209 | If you use this object with :class:`.AutonomousModeSelector`, make sure 210 | to initialize it with the dictionary, and it will be passed to this 211 | autonomous mode object when initialized. 212 | 213 | .. seealso:: Check out the samples in our github repository that show 214 | some basic usage of ``AutonomousModeSelector``. 215 | """ 216 | 217 | __built = False 218 | __done = False 219 | 220 | def __init__(self, components=None): 221 | """ 222 | :param components: A dictionary of values that will be assigned 223 | as attributes to this object, using the key 224 | names in the dictionary 225 | :type components: dict 226 | """ 227 | 228 | if not hasattr(self, "MODE_NAME"): 229 | raise ValueError("Must define MODE_NAME class variable") 230 | 231 | if components: 232 | for k, v in components.items(): 233 | setattr(self, k, v) 234 | 235 | NetworkTables = ntcore.NetworkTableInstance.getDefault() 236 | self.__table = NetworkTables.getTable("SmartDashboard") 237 | self.__sd_args = [] 238 | 239 | self.__build_states() 240 | self.__tunables = [] 241 | 242 | if hasattr(self, "initialize"): 243 | self.initialize() 244 | 245 | def register_sd_var(self, name, default, add_prefix=True, vmin=-1, vmax=1): 246 | """ 247 | Register a variable that is tunable via NetworkTables/SmartDashboard 248 | 249 | When this autonomous mode is enabled, all of the SmartDashboard 250 | settings will be read and stored as attributes of this object. For 251 | example, to register a variable 'foo' with a default value of 1:: 252 | 253 | self.register_sd_var('foo', 1) 254 | 255 | This value will show up on NetworkTables as the key ``MODE_NAME\\foo`` 256 | if add_prefix is specified, otherwise as ``foo``. 257 | 258 | :param name: Name of variable to display to user, cannot have a 259 | space in it. 260 | :param default: Default value of variable 261 | :param add_prefix: Prefix this setting with the mode name 262 | :type add_prefix: bool 263 | :param vmin: For tuning: minimum value of this variable 264 | :param vmax: For tuning: maximum value of this variable 265 | """ 266 | 267 | is_number = self.__register_sd_var_internal(name, default, add_prefix, True) 268 | 269 | if not add_prefix: 270 | return 271 | 272 | # communicate the min/max value for numbers to the dashboard 273 | if is_number: 274 | name = f"{name}|{vmin:0.3f}|{vmax:0.3f}" 275 | 276 | self.__tunables.append(name) 277 | self.__table.putStringArray(self.MODE_NAME + "_tunables", self.__tunables) 278 | 279 | def __register_sd_var_internal(self, name, default, add_prefix, readback): 280 | if " " in name: 281 | raise ValueError( 282 | f"ERROR: Cannot use spaces in a tunable variable name ({name})" 283 | ) 284 | 285 | is_number = False 286 | sd_name = name 287 | 288 | if add_prefix: 289 | sd_name = f"{self.MODE_NAME}\\{name}" 290 | 291 | if isinstance(default, bool): 292 | self.__table.putBoolean(sd_name, default) 293 | args = (name, sd_name, self.__table.getBoolean, default) 294 | 295 | elif isinstance(default, int) or isinstance(default, float): 296 | self.__table.putNumber(sd_name, default) 297 | args = (name, sd_name, self.__table.getNumber, default) 298 | is_number = True 299 | 300 | elif isinstance(default, str): 301 | self.__table.putString(sd_name, default) 302 | args = (name, sd_name, self.__table.getString, default) 303 | 304 | else: 305 | raise ValueError("Invalid default value") 306 | 307 | if readback: 308 | self.__sd_args.append(args) 309 | return is_number 310 | 311 | def __build_states(self): 312 | has_first = False 313 | 314 | states = {} 315 | cls = type(self) 316 | 317 | # for each state function: 318 | for name in dir(cls): 319 | state = getattr(cls, name) 320 | if not isinstance(state, _State): 321 | continue 322 | 323 | # is this the first state to execute? 324 | if state.first: 325 | if has_first: 326 | raise ValueError( 327 | "Multiple states were specified as the first state!" 328 | ) 329 | 330 | self.__first = name 331 | has_first = True 332 | 333 | # problem: how do we expire old entries? 334 | # -> what if we just use json? more flexible, but then we can't tune it 335 | # via SmartDashboard 336 | 337 | # make the time tunable 338 | if hasattr(state, "duration"): 339 | self.__register_sd_var_internal( 340 | state.name + "_duration", state.duration, True, True 341 | ) 342 | 343 | description = "" 344 | if state.description is not None: 345 | description = state.description 346 | 347 | states[state.serial] = (state.name, description) 348 | 349 | # problem: the user interface won't know which entries are the 350 | # current variables being used by the robot. So, we setup 351 | # an array with the names, and the dashboard uses that 352 | # to determine the ordering too 353 | 354 | sorted_states = sorted(states.items()) 355 | 356 | self.__table.putStringArray( 357 | self.MODE_NAME + "_durations", [name for _, (name, desc) in sorted_states] 358 | ) 359 | 360 | self.__table.putStringArray( 361 | self.MODE_NAME + "_descriptions", 362 | [desc for _, (name, desc) in sorted_states], 363 | ) 364 | 365 | if not has_first: 366 | raise ValueError( 367 | "Starting state not defined! Use first=True on a state decorator" 368 | ) 369 | 370 | self.__built = True 371 | 372 | def _validate(self): 373 | # TODO: make sure the state machine can be executed 374 | # - run at robot time? Probably not. Run this as part of a unit test 375 | pass 376 | 377 | # how long does introspection take? do this in the constructor? 378 | 379 | # can do things like add all of the timed states, and project how long 380 | # it will take to execute it (don't forget about cycles!) 381 | 382 | def on_enable(self): 383 | """ 384 | Called when autonomous mode is enabled, and initializes the 385 | state machine internals. 386 | 387 | If you override this function, be sure to call it from your 388 | customized ``on_enable`` function:: 389 | 390 | super().on_enable() 391 | """ 392 | 393 | if not self.__built: 394 | raise ValueError("super().__init__(components) was never called!") 395 | 396 | # print out the details of this autonomous mode, and any tunables 397 | 398 | self.battery_voltage = wpilib.DriverStation.getBatteryVoltage() 399 | logger.info("Battery voltage: %.02fv", self.battery_voltage) 400 | 401 | logger.info("Tunable values:") 402 | 403 | # read smart dashboard values, print them 404 | for name, sd_name, fn, default in self.__sd_args: 405 | val = fn(sd_name, default) 406 | setattr(self, name, val) 407 | logger.info("-> %25s: %s" % (name, val)) 408 | 409 | # set the starting state 410 | self.next_state(self.__first) 411 | self.__done = False 412 | 413 | def on_disable(self): 414 | """Called when the autonomous mode is disabled""" 415 | pass 416 | 417 | def done(self): 418 | """Call this function to indicate that no more states should be called""" 419 | self.next_state(None) 420 | 421 | def next_state(self, name): 422 | """Call this function to transition to the next state 423 | 424 | :param name: Name of the state to transition to 425 | """ 426 | if name is not None: 427 | self.__state = getattr(self.__class__, name) 428 | else: 429 | self.__state = None 430 | 431 | if self.__state is None: 432 | return 433 | 434 | self.__state.ran = False 435 | 436 | def on_iteration(self, tm): 437 | """This function is called by the autonomous mode switcher, should 438 | not be called by enduser code. It is called once per control 439 | loop iteration.""" 440 | 441 | # if you get an error here, then you probably overrode on_enable, 442 | # but didn't call super().on_enable(). Don't do that. 443 | try: 444 | state = self.__state 445 | except AttributeError: 446 | raise ValueError("super().on_enable was never called!") 447 | 448 | # we adjust this so that if we have states chained together, 449 | # then the total time it runs is the amount of time of the 450 | # states. Otherwise, the time drifts. 451 | new_state_start = tm 452 | 453 | # determine if the time has passed to execute the next state 454 | if state is not None and state.expires < tm: 455 | self.next_state(state.next_state) 456 | new_state_start = state.expires 457 | state = self.__state 458 | 459 | if state is None: 460 | if not self.__done: 461 | logger.info("%.3fs: Done with autonomous mode", tm) 462 | self.__done = True 463 | return 464 | 465 | # is this the first time this was executed? 466 | initial_call = not state.ran 467 | if initial_call: 468 | state.ran = True 469 | state.start_time = new_state_start 470 | state.expires = state.start_time + getattr( 471 | self, state.name + "_duration", 0xFFFFFFFF 472 | ) 473 | 474 | logger.info("%.3fs: Entering state: %s", tm, state.name) 475 | 476 | # execute the state function, passing it the arguments 477 | state.run(self, tm, tm - state.start_time, initial_call) 478 | -------------------------------------------------------------------------------- /robotpy_ext/common_drivers/README.rst: -------------------------------------------------------------------------------- 1 | CDL or Common Driver Library 2 | ============================ 3 | This is a group effort to provide an extended collection of drivers for devices commonly used in FRC. 4 | 5 | Drivers 6 | ======= 7 | You might ask why there are so few drivers in this library. That is because no one developer can possibly create a driver for every possible sensor for use in FRC, there are just too many. 8 | 9 | That is where the community comes in. If you have a device that is not properly represented in wpilib, and that you want to use, why not add a driver here for it? 10 | 11 | 12 | Authors 13 | ======= 14 | 15 | - Dustin Spicuzza (dustin@virtualroadside.com) 16 | - Christian Balcom (robot.inventor@gmail.com) 17 | - Insert your name here! 18 | -------------------------------------------------------------------------------- /robotpy_ext/common_drivers/__init__.py: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /robotpy_ext/common_drivers/distance_sensors.py: -------------------------------------------------------------------------------- 1 | import wpilib 2 | import math 3 | 4 | 5 | class SharpIR2Y0A02: 6 | """ 7 | Sharp GP2Y0A02YK0F is an analog IR sensor capable of measuring 8 | distances from 20cm to 150cm. Output distance is measured in 9 | centimeters. 10 | 11 | Distance is calculated using the following equation derived from 12 | the graph provided in the datasheet:: 13 | 14 | 62.28*x ^ -1.092 15 | 16 | .. warning:: FRC Teams: the case on these sensors is conductive and 17 | grounded, and should not be mounted on a metallic 18 | surface! 19 | """ 20 | 21 | def __init__(self, port): 22 | """:param port: Analog port number""" 23 | self.distance = wpilib.AnalogInput(port) 24 | 25 | def getDistance(self): 26 | """ 27 | :returns: distance in centimeters. The output is constrained to 28 | be between 22.5 and 145 29 | """ 30 | 31 | # Don't allow zero/negative values 32 | v = max(self.distance.getVoltage(), 0.00001) 33 | d = 62.28 * math.pow(v, -1.092) 34 | 35 | # Constrain output 36 | return max(min(d, 145.0), 22.5) 37 | 38 | 39 | class SharpIR2Y0A21: 40 | """ 41 | Sharp GP2Y0A21YK0F is an analog IR sensor capable of measuring 42 | distances from 10cm to 80cm. Output distance is measured in 43 | centimeters. 44 | 45 | Distance is calculated using the following equation derived from 46 | the graph provided in the datasheet:: 47 | 48 | 26.449*x ^ -1.226 49 | 50 | .. warning:: FRC Teams: the case on these sensors is conductive and 51 | grounded, and should not be mounted on a metallic 52 | surface! 53 | """ 54 | 55 | def __init__(self, port): 56 | """:param port: Analog port number""" 57 | self.distance = wpilib.AnalogInput(port) 58 | 59 | def getDistance(self): 60 | """ 61 | :returns: distance in centimeters. The output is constrained to 62 | be between 10 and 80 63 | """ 64 | 65 | # Don't allow zero/negative values 66 | v = max(self.distance.getVoltage(), 0.00001) 67 | d = 26.449 * math.pow(v, -1.226) 68 | 69 | # Constrain output 70 | return max(min(d, 80.0), 10.0) 71 | 72 | 73 | class SharpIR2Y0A41: 74 | """ 75 | Sharp GP2Y0A41SK0F is an analog IR sensor capable of measuring 76 | distances from 4cm to 40cm. Output distance is measured in 77 | centimeters. 78 | 79 | Distance is calculated using the following equation derived from 80 | the graph provided in the datasheet:: 81 | 82 | 12.84*x ^ -0.9824 83 | 84 | .. warning:: FRC Teams: the case on these sensors is conductive and 85 | grounded, and should not be mounted on a metallic 86 | surface! 87 | """ 88 | 89 | def __init__(self, port): 90 | """:param port: Analog port number""" 91 | self.distance = wpilib.AnalogInput(port) 92 | 93 | def getDistance(self): 94 | """ 95 | :returns: distance in centimeters. The output is constrained to 96 | be between 4.5 and 35 97 | """ 98 | 99 | # Don't allow zero/negative values 100 | v = max(self.distance.getVoltage(), 0.00001) 101 | d = 12.84 * math.pow(v, -0.9824) 102 | 103 | # Constrain output 104 | return max(min(d, 35.0), 4.5) 105 | 106 | 107 | # backwards compat 108 | SharpIRGP2Y0A41SK0F = SharpIR2Y0A41 109 | -------------------------------------------------------------------------------- /robotpy_ext/common_drivers/distance_sensors_sim.py: -------------------------------------------------------------------------------- 1 | import math 2 | 3 | from wpilib.simulation import AnalogInputSim 4 | 5 | from .distance_sensors import SharpIR2Y0A02, SharpIR2Y0A21, SharpIR2Y0A41 6 | 7 | 8 | class SharpIR2Y0A02Sim: 9 | """ 10 | An easy to use simulation interface for a Sharp GP2Y0A02YK0F 11 | """ 12 | 13 | def __init__(self, sensor: SharpIR2Y0A02) -> None: 14 | assert isinstance(sensor, SharpIR2Y0A02) 15 | self._sim = AnalogInputSim(sensor.distance) 16 | self._distance = 0 17 | 18 | def getDistance(self) -> float: 19 | """Get set distance (not distance sensor sees) in centimeters""" 20 | return self._distance 21 | 22 | def setDistance(self, d) -> None: 23 | """Set distance in centimeters""" 24 | self._distance = d 25 | d = max(min(d, 145.0), 22.5) 26 | v = math.pow(d / 62.28, 1 / -1.092) 27 | self._sim.setVoltage(v) 28 | 29 | 30 | class SharpIR2Y0A21Sim: 31 | """ 32 | An easy to use simulation interface for a Sharp GP2Y0A21YK0F 33 | """ 34 | 35 | def __init__(self, sensor: SharpIR2Y0A21) -> None: 36 | assert isinstance(sensor, SharpIR2Y0A21) 37 | self._sim = AnalogInputSim(sensor.distance) 38 | self._distance = 0 39 | 40 | def getDistance(self) -> float: 41 | """Get set distance (not distance sensor sees) in centimeters""" 42 | return self._distance 43 | 44 | def setDistance(self, d) -> None: 45 | """Set distance in centimeters""" 46 | self._distance = d 47 | d = max(min(d, 80.0), 10.0) 48 | v = math.pow(d / 26.449, 1 / -1.226) 49 | self._sim.setVoltage(v) 50 | 51 | 52 | class SharpIR2Y0A41Sim: 53 | """ 54 | An easy to use simulation interface for a Sharp GP2Y0A41SK0F 55 | """ 56 | 57 | def __init__(self, sensor: SharpIR2Y0A41) -> None: 58 | assert isinstance(sensor, SharpIR2Y0A41) 59 | self._sim = AnalogInputSim(sensor.distance) 60 | self._distance = 0 61 | 62 | def getDistance(self) -> float: 63 | """Get set distance (not distance sensor sees) in centimeters""" 64 | return self._distance 65 | 66 | def setDistance(self, d) -> None: 67 | """Set distance in centimeters""" 68 | self._distance = d 69 | d = max(min(d, 35.0), 4.5) 70 | v = math.pow(d / 12.84, 1 / -0.9824) 71 | self._sim.setVoltage(v) 72 | -------------------------------------------------------------------------------- /robotpy_ext/common_drivers/driver_base.py: -------------------------------------------------------------------------------- 1 | class DriverBase: 2 | """ 3 | This should be the base class for all drivers in the cdl, 4 | currently all it does is spit out a warning message if the driver has not been verified. 5 | """ 6 | 7 | #:This should be overloaded by the driver, 8 | # It is just a mechanism to ensure code quality for device drivers. Upon creation of a new driver, 9 | # it will be either left alone, or overloaded to be False. Once the functionality of the driver is verified, 10 | # this will get overloaded to true by whoever verifies it. 11 | verified = False 12 | 13 | def __init__(self): 14 | """ 15 | Constructor for DriverBase, all this does is print a message to console if the driver has not been verified yet. 16 | """ 17 | if not self.verified: 18 | print( 19 | f"Warning, device driver {self.__class__.__name__} has not been verified yet, please use with caution!" 20 | ) 21 | -------------------------------------------------------------------------------- /robotpy_ext/common_drivers/pressure_sensors.py: -------------------------------------------------------------------------------- 1 | from wpilib import AnalogInput 2 | 3 | 4 | class REVAnalogPressureSensor: 5 | """ 6 | The REV Robotics Analog Pressure Sensor is a 5V sensor 7 | that can measure pressures up to 200 PSI. It outputs an 8 | analog voltage that is proportional to the measured pressure. 9 | 10 | Pressure is derived from the following equation, taken from 11 | the data sheet:: 12 | 13 | Pressure = 250 * (Vout / Vcc) -25 14 | 15 | where Vout is the output voltage of the sensor, and Vcc is the input 16 | voltage 17 | 18 | To calibrate the sensor, supply the sensor with a known 19 | pressure and call the calibrate method with the given 20 | pressure as the parameter 21 | """ 22 | 23 | def __init__(self, channel, voltage_in=5): 24 | """ 25 | :param voltage_in: Supply voltage to sensor from roboRIO 26 | """ 27 | 28 | self.sensor = AnalogInput(channel) 29 | self.voltage_in = voltage_in 30 | 31 | @property 32 | def pressure(self): 33 | try: 34 | v = max(self.sensor.getAverageVoltage(), 0.00001) 35 | return (250 * (v / getattr(self, "Vn", self.voltage_in))) - 25 36 | except ZeroDivisionError: 37 | return 0 38 | 39 | def calibrate(self, known_pressure): 40 | """ 41 | Calibrate calculated pressure to known pressure 42 | 43 | :param known_pressure: Known current pressure to calibrate to 44 | """ 45 | 46 | Vo = max(self.sensor.getAverageVoltage(), 0.00001) 47 | self.Vn = Vo / (0.004 * known_pressure + 0.1) 48 | -------------------------------------------------------------------------------- /robotpy_ext/common_drivers/units.py: -------------------------------------------------------------------------------- 1 | class Unit: 2 | """The class for all of the units here""" 3 | 4 | def __init__(self, base_unit, base_to_unit, unit_to_base): 5 | """ 6 | Unit constructor, used as a mechanism to convert between various measurements 7 | :param base_unit: The instance of Unit to base conversions from. If None, then assume it is the ultimate base unit 8 | :param base_to_unit: A callable to convert measurements between this unit and the base unit 9 | :param unit_to_base: A callable to convert measurements between the base unit and this unit 10 | """ 11 | self.base_unit = base_unit 12 | self.base_to_unit = base_to_unit 13 | self.unit_to_base = unit_to_base 14 | 15 | 16 | def convert(source_unit, target_unit, value): 17 | """ 18 | Convert between units, returns value in target_unit 19 | :param source_unit: The unit of value 20 | :param target_unit: The desired output unit 21 | :param value: The value, in source_unit, to convert 22 | """ 23 | # Convert value from source_unit to the ultimate base unit 24 | current_unit = source_unit 25 | current_value = value 26 | while current_unit.base_unit is not None: 27 | current_value = current_unit.unit_to_base(current_value) 28 | next_unit = current_unit.base_unit 29 | current_unit = next_unit 30 | 31 | # Get the chain of conversions between target_unit and the ultimate base unit 32 | current_unit = target_unit 33 | unit_chain = [] 34 | while current_unit.base_unit is not None: 35 | unit_chain.append(current_unit) 36 | next_unit = current_unit.base_unit 37 | current_unit = next_unit 38 | 39 | # Follow the chain of conversions back to target_unit 40 | for unit in reversed(unit_chain): 41 | current_value = unit.base_to_unit(current_value) 42 | 43 | # Return it! 44 | return current_value 45 | 46 | 47 | # Some typical units to be used 48 | meter = Unit(base_unit=None, base_to_unit=lambda x: None, unit_to_base=lambda x: None) 49 | centimeter = Unit( 50 | base_unit=meter, base_to_unit=lambda x: x * 100, unit_to_base=lambda x: x / 100 51 | ) 52 | 53 | foot = Unit( 54 | base_unit=meter, 55 | base_to_unit=lambda x: x / 0.3048, 56 | unit_to_base=lambda x: x * 0.3048, 57 | ) 58 | inch = Unit( 59 | base_unit=foot, base_to_unit=lambda x: x * 12, unit_to_base=lambda x: x / 12 60 | ) 61 | -------------------------------------------------------------------------------- /robotpy_ext/common_drivers/xl_max_sonar_ez.py: -------------------------------------------------------------------------------- 1 | """ 2 | These are a set of drivers for the XL-MaxSonar EZ series of sonar modules. 3 | The devices have a few different ways of reading from them, and the these drivers attempt to cover 4 | some of the methods 5 | """ 6 | 7 | import wpilib 8 | 9 | from . import driver_base 10 | from . import units 11 | 12 | 13 | class MaxSonarEZPulseWidth(driver_base.DriverBase): 14 | """ 15 | This is a driver for the MaxSonar EZ series of sonar sensors, using the pulse-width output of the sensor. 16 | 17 | To use this driver, pin 2 on the sensor must be mapped to a dio pin. 18 | """ 19 | 20 | verified = True 21 | 22 | def __init__(self, channel, output_units=units.inch): 23 | """Sonar sensor constructor 24 | 25 | :param channel: The digital input index which is wired to the pulse-width output pin (pin 2) on the sensor. 26 | :param output_units: The Unit instance specifying the format of value to return 27 | """ 28 | 29 | # Save value 30 | self.output_units = output_units 31 | 32 | # Setup the counter 33 | self.counter = wpilib.Counter(channel) 34 | self.counter.setSemiPeriodMode(highSemiPeriod=True) 35 | 36 | # Call the parents 37 | super().__init__() 38 | 39 | def get(self): 40 | """Return the current sonar sensor reading, in the units specified from the constructor""" 41 | inches = self.counter.getPeriod() / 0.000147 42 | return units.convert(units.inch, self.output_units, inches) 43 | 44 | 45 | class MaxSonarEZAnalog(driver_base.DriverBase): 46 | """ 47 | This is a driver for the MaxSonar EZ series of sonar sensors, using the analog output of the sensor. 48 | 49 | To use this driver, pin 3 on the sensor must be mapped to an analog pin, and the sensor must be on a 5v supply. 50 | """ 51 | 52 | # This code has actually never been run, so it is extra not-verified! 53 | verified = False 54 | 55 | def __init__(self, channel, output_units=units.inch): 56 | """Sonar sensor constructor 57 | 58 | :param channel: The analog input index which is wired to the analog output pin (pin 3) on the sensor. 59 | :param output_units: The Unit instance specifying the format of value to return 60 | """ 61 | 62 | # Save value 63 | self.output_units = output_units 64 | 65 | # Setup the analog input 66 | self.analog = wpilib.AnalogInput(channel) 67 | 68 | # Call the parents 69 | super().__init__() 70 | 71 | def get(self): 72 | """Return the current sonar sensor reading, in the units specified from the constructor""" 73 | centimeters = self.analog.getVoltage() / 0.0049 74 | return units.convert(units.centimeter, self.output_units, centimeters) 75 | -------------------------------------------------------------------------------- /robotpy_ext/control/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotpy/robotpy-wpilib-utilities/2c535d4b92478776e59b8f731df190af5a41513c/robotpy_ext/control/__init__.py -------------------------------------------------------------------------------- /robotpy_ext/control/button_debouncer.py: -------------------------------------------------------------------------------- 1 | import wpilib 2 | 3 | 4 | class ButtonDebouncer: 5 | """Useful utility class for debouncing buttons""" 6 | 7 | def __init__(self, joystick, buttonnum, period=0.5): 8 | """ 9 | :param joystick: Joystick object 10 | :type joystick: :class:`wpilib.Joystick` 11 | :param buttonnum: Number of button to retrieve 12 | :type buttonnum: int 13 | :param period: Period of time (in seconds) to wait before allowing new button 14 | presses. Defaults to 0.5 seconds. 15 | :type period: float 16 | """ 17 | self.joystick = joystick 18 | self.buttonnum = buttonnum 19 | self.latest = 0 20 | self.debounce_period = float(period) 21 | self.timer = wpilib.Timer 22 | 23 | def set_debounce_period(self, period): 24 | """Set number of seconds to wait before returning True for the 25 | button again""" 26 | self.debounce_period = float(period) 27 | 28 | def get(self): 29 | """Returns the value of the joystick button. If the button is held down, then 30 | True will only be returned once every ``debounce_period`` seconds""" 31 | 32 | now = self.timer.getFPGATimestamp() 33 | if self.joystick.getRawButton(self.buttonnum): 34 | if (now - self.latest) > self.debounce_period: 35 | self.latest = now 36 | return True 37 | return False 38 | 39 | __bool__ = get 40 | -------------------------------------------------------------------------------- /robotpy_ext/control/toggle.py: -------------------------------------------------------------------------------- 1 | from functools import partial 2 | 3 | import wpilib 4 | 5 | 6 | class Toggle: 7 | """Utility class for joystick button toggle 8 | 9 | Usage:: 10 | 11 | foo = Toggle(joystick, 3) 12 | 13 | if foo: 14 | toggleFunction() 15 | 16 | if foo.on: 17 | onToggle() 18 | 19 | if foo.off: 20 | offToggle() 21 | """ 22 | 23 | class _SteadyDebounce: 24 | """ 25 | Similar to ButtonDebouncer, but the output stays steady for 26 | the given periodic_filter. E.g, if you set the period to 2 27 | and press the button, the value will return true for 2 seconds. 28 | 29 | Steady debounce will return true for the given period, allowing it to be 30 | used with Toggle 31 | """ 32 | 33 | def __init__(self, joystick: wpilib.Joystick, button: int, period: float): 34 | """ 35 | :param joystick: Joystick object 36 | :type joystick: :class:`wpilib.Joystick` 37 | :param button: Number of button to retrieve 38 | :type button: int 39 | :param period: Period of time (in seconds) to wait before allowing new button 40 | presses. Defaults to 0.5 seconds. 41 | :type period: float 42 | """ 43 | self.joystick = joystick 44 | self.button = button 45 | 46 | self.debounce_period = float(period) 47 | self.latest = ( 48 | -self.debounce_period 49 | ) # Negative latest prevents get from returning true until joystick is presed for the first time 50 | self.enabled = False 51 | 52 | def get(self): 53 | """ 54 | :returns: The value of the joystick button. Once the button is pressed, 55 | the return value will be `True` until the time expires 56 | """ 57 | 58 | now = wpilib.Timer.getFPGATimestamp() 59 | if now - self.latest < self.debounce_period: 60 | return True 61 | 62 | if self.joystick.getRawButton(self.button): 63 | self.latest = now 64 | return True 65 | else: 66 | return False 67 | 68 | def __init__( 69 | self, joystick: wpilib.Joystick, button: int, debounce_period: float = None 70 | ): 71 | """ 72 | :param joystick: :class:`wpilib.Joystick` that contains the button to toggle 73 | :param button: Number of button that will act as toggle. Same value used in `getRawButton()` 74 | :param debounce_period: Period in seconds to wait before registering a new button press. 75 | """ 76 | 77 | if debounce_period is not None: 78 | self.joystickget = Toggle._SteadyDebounce( 79 | joystick, button, debounce_period 80 | ).get 81 | else: 82 | self.joystick = joystick 83 | self.joystickget = partial(self.joystick.getRawButton, button) 84 | 85 | self.released = False 86 | self.toggle = False 87 | self.state = False 88 | 89 | def get(self): 90 | """ 91 | :return: State of toggle 92 | :rtype: bool 93 | """ 94 | current_state = self.joystickget() 95 | 96 | if current_state and not self.released: 97 | self.released = True 98 | self.toggle = not self.toggle 99 | self.state = not self.state # Toggles between 1 and 0. 100 | 101 | elif not current_state and self.released: 102 | self.released = False 103 | 104 | return self.toggle 105 | 106 | @property 107 | def on(self): 108 | """ 109 | Equates to true if toggle is in the 'on' state 110 | """ 111 | self.get() 112 | return self.state 113 | 114 | @property 115 | def off(self): 116 | """ 117 | Equates to true if toggle is in the 'off' state 118 | """ 119 | self.get() 120 | return not self.state 121 | 122 | __bool__ = get 123 | -------------------------------------------------------------------------------- /robotpy_ext/misc/__init__.py: -------------------------------------------------------------------------------- 1 | from .precise_delay import NotifierDelay 2 | from .periodic_filter import PeriodicFilter 3 | -------------------------------------------------------------------------------- /robotpy_ext/misc/annotations.py: -------------------------------------------------------------------------------- 1 | def get_class_annotations(cls): 2 | """Get variable annotations in a class, inheriting from superclasses.""" 3 | result = {} 4 | for base in reversed(cls.__mro__): 5 | result.update(getattr(base, "__annotations__", {})) 6 | return result 7 | -------------------------------------------------------------------------------- /robotpy_ext/misc/asyncio_policy.py: -------------------------------------------------------------------------------- 1 | """ 2 | This is a replacement event loop and policy for asyncio that uses FPGA time, 3 | rather than native python time. 4 | """ 5 | 6 | from asyncio.events import AbstractEventLoopPolicy 7 | from asyncio import SelectorEventLoop, set_event_loop_policy 8 | from wpilib import Timer 9 | 10 | 11 | class FPGATimedEventLoop(SelectorEventLoop): 12 | """An asyncio event loop that uses wpilib time rather than python time""" 13 | 14 | def time(self): 15 | return Timer.getFPGATimestamp() 16 | 17 | 18 | class FPGATimedEventLoopPolicy(AbstractEventLoopPolicy): 19 | """An asyncio event loop policy that uses FPGATimedEventLoop""" 20 | 21 | _loop_factory = FPGATimedEventLoop 22 | 23 | 24 | def patch_asyncio_policy(): 25 | """ 26 | Sets an instance of FPGATimedEventLoopPolicy as the default asyncio event 27 | loop policy 28 | """ 29 | set_event_loop_policy(FPGATimedEventLoopPolicy()) 30 | -------------------------------------------------------------------------------- /robotpy_ext/misc/crc7.py: -------------------------------------------------------------------------------- 1 | # fmt: off 2 | _crc7_table = [ 3 | 0, 65, 19, 82, 38, 103, 53, 116, 76, 13, 95, 30, 106, 43, 121, 56, 9, 4 | 72, 26, 91, 47, 110, 60, 125, 69, 4, 86, 23, 99, 34, 112, 49, 18, 5 | 83, 1, 64, 52, 117, 39, 102, 94, 31, 77, 12, 120, 57, 107, 42, 27, 6 | 90, 8, 73, 61, 124, 46, 111, 87, 22, 68, 5, 113, 48, 98, 35, 36, 7 | 101, 55, 118, 2, 67, 17, 80, 104, 41, 123, 58, 78, 15, 93, 28, 45, 8 | 108, 62, 127, 11, 74, 24, 89, 97, 32, 114, 51, 71, 6, 84, 21, 54, 9 | 119, 37, 100, 16, 81, 3, 66, 122, 59, 105, 40, 92, 29, 79, 14, 63, 10 | 126, 44, 109, 25, 88, 10, 75, 115, 50, 96, 33, 85, 20, 70, 7, 72, 11 | 9, 91, 26, 110, 47, 125, 60, 4, 69, 23, 86, 34, 99, 49, 112, 65, 12 | 0, 82, 19, 103, 38, 116, 53, 13, 76, 30, 95, 43, 106, 56, 121, 90, 13 | 27, 73, 8, 124, 61, 111, 46, 22, 87, 5, 68, 48, 113, 35, 98, 83, 14 | 18, 64, 1, 117, 52, 102, 39, 31, 94, 12, 77, 57, 120, 42, 107, 108, 15 | 45, 127, 62, 74, 11, 89, 24, 32, 97, 51, 114, 6, 71, 21, 84, 101, 16 | 36, 118, 55, 67, 2, 80, 17, 41, 104, 58, 123, 15, 78, 28, 93, 126, 17 | 63, 109, 44, 88, 25, 75, 10, 50, 115, 33, 96, 20, 85, 7, 70, 119, 18 | 54, 100, 37, 81, 16, 66, 3, 59, 122, 40, 105, 29, 92, 14, 79 19 | ] 20 | # fmt: on 21 | 22 | 23 | def crc7(data): 24 | csum = 0 25 | for d in data: 26 | csum = _crc7_table[d ^ csum] 27 | 28 | return csum 29 | -------------------------------------------------------------------------------- /robotpy_ext/misc/looptimer.py: -------------------------------------------------------------------------------- 1 | import logging 2 | import math 3 | import wpilib 4 | 5 | _getFPGATimestamp = wpilib.Timer.getFPGATimestamp 6 | 7 | 8 | class LoopTimer: 9 | """ 10 | A utility class that measures the number of loops that a robot program 11 | executes, and computes the min/max/average period for loops in the last 12 | second. 13 | 14 | Example usage:: 15 | 16 | class Robot(wpilib.TimedRobot): 17 | 18 | def teleopInit(self): 19 | self.loop_timer = LoopTimer(self.logger) 20 | 21 | def teleopPeriodic(self): 22 | self.loop_timer.measure() 23 | 24 | Mainly intended for debugging purposes to measure how much lag. 25 | """ 26 | 27 | def __init__(self, logger: logging.Logger): 28 | self.logger = logger 29 | self.timer = wpilib.Timer() 30 | self.timer.start() 31 | 32 | self.reset() 33 | 34 | def reset(self) -> None: 35 | self.timer.reset() 36 | 37 | self.start = self.last = _getFPGATimestamp() 38 | self.min_time = math.inf 39 | self.max_time = -1 40 | self.loops = 0 41 | 42 | def measure(self) -> None: 43 | """ 44 | Computes loop performance information and periodically dumps it to 45 | the info logger. 46 | """ 47 | 48 | # compute min/max/count 49 | now = _getFPGATimestamp() 50 | diff = now - self.last 51 | self.min_time = min(self.min_time, diff) 52 | self.max_time = max(self.max_time, diff) 53 | 54 | self.loops += 1 55 | self.last = now 56 | 57 | if self.timer.advanceIfElapsed(1): 58 | self.logger.info( 59 | "Loops: %d; min: %.3f; max: %.3f; period: %.3f; avg: %.3f", 60 | self.loops, 61 | self.min_time, 62 | self.max_time, 63 | now - self.start, 64 | (now - self.start) / self.loops, 65 | ) 66 | 67 | self.min_time = math.inf 68 | self.max_time = -1 69 | self.start = now 70 | self.loops = 0 71 | -------------------------------------------------------------------------------- /robotpy_ext/misc/orderedclass.py: -------------------------------------------------------------------------------- 1 | """ 2 | Taken from the python documentation, distributed under 3 | that same license 4 | """ 5 | 6 | import collections 7 | 8 | 9 | class OrderedClass(type): 10 | """ 11 | Metaclass that stores class attributes in a member called 12 | 'members'. If you subclass something that uses this metaclass, 13 | the base class members will be listed before the subclass 14 | """ 15 | 16 | # TODO: this isn't required in Python 3.6 due to PEP 520 17 | 18 | @classmethod 19 | def __prepare__(metacls, name, bases, **kwds): 20 | return collections.OrderedDict() 21 | 22 | def __new__(cls, name, bases, namespace, **kwds): 23 | result = type.__new__(cls, name, bases, dict(namespace)) 24 | 25 | members = [] 26 | members.extend(getattr(result, "members", ())) 27 | 28 | # because of the potential of multiple inheritance/mixins, need 29 | # to grab members list from all bases 30 | for base in bases: 31 | members.extend(getattr(base, "members", ())) 32 | 33 | members.extend(namespace) 34 | 35 | seen = set() 36 | result.members = tuple(m for m in members if not (m in seen or seen.add(m))) 37 | return result 38 | -------------------------------------------------------------------------------- /robotpy_ext/misc/periodic_filter.py: -------------------------------------------------------------------------------- 1 | import logging 2 | import time 3 | 4 | 5 | class PeriodicFilter: 6 | """ 7 | Periodic Filter to help keep down clutter in the console. 8 | Simply add this filter to your logger and the logger will 9 | only print periodically. 10 | 11 | The logger will always print logging levels of WARNING or higher, 12 | unless given a different bypass level 13 | 14 | Example:: 15 | 16 | class Component1: 17 | 18 | def setup(self): 19 | # Set period to 3 seconds, set bypass_level to WARN 20 | self.logger.addFilter(PeriodicFilter(3, bypass_level=logging.WARN)) 21 | 22 | def execute(self): 23 | # This message will be printed once every three seconds 24 | self.logger.info('Component1 Executing') 25 | 26 | # This message will be printed out every loop 27 | self.logger.warn("Uh oh, this shouldn't have happened...") 28 | 29 | """ 30 | 31 | def __init__(self, period, bypass_level=logging.WARN): 32 | """ 33 | :param period: Wait period (in seconds) between logs 34 | :param bypass_level: Lowest logging level that the filter should not catch 35 | """ 36 | 37 | self._period = period 38 | self._loggingLoop = True 39 | self._last_log = -period 40 | self._bypass_level = bypass_level 41 | 42 | def filter(self, record): 43 | """Performs filtering action for logger""" 44 | self._refresh_logger() 45 | return self._loggingLoop or record.levelno >= self._bypass_level 46 | 47 | def _refresh_logger(self): 48 | """Determine if the log wait period has passed""" 49 | now = time.monotonic() 50 | self._loggingLoop = False 51 | if now - self._last_log > self._period: 52 | self._loggingLoop = True 53 | self._last_log = now 54 | -------------------------------------------------------------------------------- /robotpy_ext/misc/precise_delay.py: -------------------------------------------------------------------------------- 1 | import hal 2 | import wpilib 3 | 4 | 5 | class NotifierDelay: 6 | """Synchronizes a timing loop against interrupts from the FPGA. 7 | 8 | This will delay so that the next invocation of your loop happens at 9 | precisely the same period, assuming that your loop does not take longer 10 | than the specified period. 11 | 12 | Example:: 13 | 14 | with NotifierDelay(0.02) as delay: 15 | while something: 16 | # do things here 17 | delay.wait() 18 | """ 19 | 20 | def __init__(self, delay_period: float) -> None: 21 | """:param delay_period: The period's amount of time (in seconds).""" 22 | if delay_period < 0.001: 23 | raise ValueError("You probably don't want to delay less than 1ms!") 24 | 25 | # Convert the delay period to microseconds, as FPGA timestamps are microseconds 26 | self.delay_period = int(delay_period * 1e6) 27 | self._notifier = hal.initializeNotifier()[0] 28 | self._expiry_time = wpilib.RobotController.getFPGATime() + self.delay_period 29 | self._update_alarm(self._notifier) 30 | 31 | # wpilib.Resource._add_global_resource(self) 32 | 33 | def __del__(self): 34 | self.free() 35 | 36 | def __enter__(self) -> "NotifierDelay": 37 | return self 38 | 39 | def __exit__(self, exc_type, exc_val, exc_tb): 40 | self.free() 41 | 42 | def free(self) -> None: 43 | """Clean up the notifier. 44 | 45 | Do not use this object after this method is called. 46 | """ 47 | handle = self._notifier 48 | if handle is None: 49 | return 50 | hal.stopNotifier(handle) 51 | hal.cleanNotifier(handle) 52 | self._notifier = None 53 | 54 | def wait(self) -> None: 55 | """Wait until the delay period has passed.""" 56 | handle = self._notifier 57 | if handle is None: 58 | return 59 | hal.waitForNotifierAlarm(handle) 60 | self._expiry_time += self.delay_period 61 | self._update_alarm(handle) 62 | 63 | def _update_alarm(self, handle) -> None: 64 | hal.updateNotifierAlarm(handle, self._expiry_time) 65 | -------------------------------------------------------------------------------- /robotpy_ext/misc/simple_watchdog.py: -------------------------------------------------------------------------------- 1 | import wpilib 2 | import logging 3 | 4 | logger = logging.getLogger("simple_watchdog") 5 | 6 | __all__ = ["SimpleWatchdog"] 7 | 8 | 9 | class SimpleWatchdog: 10 | """A class that's a wrapper around a watchdog timer. 11 | 12 | When the timer expires, a message is printed to the console and an optional user-provided 13 | callback is invoked. 14 | 15 | The watchdog is initialized disabled, so the user needs to call enable() before use. 16 | 17 | .. note:: This is a simpler replacement for the :class:`wpilib.Watchdog`, 18 | and should function mostly the same (except that this watchdog will 19 | not detect infinite loops). 20 | 21 | .. warning:: This watchdog is not threadsafe 22 | 23 | """ 24 | 25 | # Used for timeout print rate-limiting 26 | kMinPrintPeriod = 1000000 # us 27 | 28 | def __init__(self, timeout: float): 29 | """Watchdog constructor. 30 | 31 | :param timeout: The watchdog's timeout in seconds with microsecond resolution. 32 | """ 33 | self._get_time = wpilib.RobotController.getFPGATime 34 | 35 | self._startTime = 0 # us 36 | self._timeout = int(timeout * 1e6) # us 37 | self._expirationTime = 0 # us 38 | self._lastTimeoutPrintTime = 0 # us 39 | self._lastEpochsPrintTime = 0 # us 40 | self._epochs: list[tuple[str, int]] = [] 41 | 42 | def getTime(self) -> float: 43 | """Returns the time in seconds since the watchdog was last fed.""" 44 | return (self._get_time() - self._startTime) / 1e6 45 | 46 | def setTimeout(self, timeout: float) -> None: 47 | """Sets the watchdog's timeout. 48 | 49 | :param timeout: The watchdog's timeout in seconds with microsecond 50 | resolution. 51 | """ 52 | self._epochs.clear() 53 | timeout = int(timeout * 1e6) # us 54 | self._timeout = timeout 55 | self._startTime = self._get_time() 56 | self._expirationTime = self._startTime + timeout 57 | 58 | def getTimeout(self) -> float: 59 | """Returns the watchdog's timeout in seconds.""" 60 | return self._timeout / 1e6 61 | 62 | def isExpired(self) -> bool: 63 | """Returns true if the watchdog timer has expired.""" 64 | return self._get_time() > self._expirationTime 65 | 66 | def addEpoch(self, epochName: str) -> None: 67 | """ 68 | Adds time since last epoch to the list printed by printEpochs(). 69 | 70 | Epochs are a way to partition the time elapsed so that when 71 | overruns occur, one can determine which parts of an operation 72 | consumed the most time. 73 | 74 | :param epochName: The name to associate with the epoch. 75 | """ 76 | self._epochs.append((epochName, self._get_time())) 77 | 78 | def printIfExpired(self) -> None: 79 | """Prints list of epochs added so far and their times.""" 80 | now = self._get_time() 81 | if ( 82 | now > self._expirationTime 83 | and now - self._lastEpochsPrintTime > self.kMinPrintPeriod 84 | ): 85 | self._lastEpochsPrintTime = now 86 | prev = self._startTime 87 | logger.warning("Watchdog not fed after %.6fs", (now - prev) / 1e6) 88 | epoch_logs = [] 89 | for key, value in self._epochs: 90 | time = (value - prev) / 1e6 91 | epoch_logs.append(f"\t{key}: {time:.6f}") 92 | prev = value 93 | logger.info("Epochs:\n%s", "\n".join(epoch_logs)) 94 | 95 | def reset(self) -> None: 96 | """Resets the watchdog timer. 97 | 98 | This also enables the timer if it was previously disabled. 99 | """ 100 | self.enable() 101 | 102 | def enable(self) -> None: 103 | """Enables the watchdog timer.""" 104 | self._epochs.clear() 105 | self._startTime = self._get_time() 106 | self._expirationTime = self._startTime + self._timeout 107 | 108 | def disable(self) -> None: 109 | """Disables the watchdog timer.""" 110 | # .. this doesn't do anything 111 | -------------------------------------------------------------------------------- /setup.cfg: -------------------------------------------------------------------------------- 1 | [metadata] 2 | name = robotpy-wpilib-utilities 3 | description = Useful utility functions/objects for RobotPy 4 | long_description = file: README.rst 5 | long_description_content_type = text/x-rst 6 | author = RobotPy Development Team 7 | author_email = robotpy@googlegroups.com 8 | url = https://github.com/robotpy/robotpy-wpilib-utilities 9 | license = BSD-3-Clause 10 | # Include the license file in wheels. 11 | license_file = LICENSE 12 | 13 | classifiers = 14 | Development Status :: 5 - Production/Stable 15 | Intended Audience :: Developers 16 | License :: OSI Approved :: BSD License 17 | Operating System :: OS Independent 18 | Programming Language :: Python :: 3 :: Only 19 | Topic :: Software Development 20 | Topic :: Scientific/Engineering 21 | 22 | [options] 23 | zip_safe = False 24 | include_package_data = True 25 | packages = find: 26 | install_requires = 27 | wpilib>=2025.1.1,<2026 28 | setup_requires = 29 | setuptools_scm > 6 30 | python_requires = >=3.9 31 | 32 | [options.package_data] 33 | magicbot = py.typed, magic_reset.pyi 34 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | from setuptools import setup 4 | 5 | setup(use_scm_version={"write_to": "robotpy_ext/version.py"}) 6 | -------------------------------------------------------------------------------- /tests/conftest.py: -------------------------------------------------------------------------------- 1 | # TODO: remove this once WPILib is public, and use the real thing 2 | 3 | import sys 4 | 5 | import pytest 6 | from unittest.mock import MagicMock 7 | 8 | 9 | def pytest_runtest_setup(): 10 | pass 11 | 12 | 13 | def pytest_runtest_teardown(): 14 | pass 15 | 16 | 17 | @pytest.fixture(scope="function") 18 | def wpimock(monkeypatch): 19 | mock = MagicMock(name="wpimock") 20 | monkeypatch.setitem(sys.modules, "wpilib", mock) 21 | return mock 22 | 23 | 24 | @pytest.fixture(scope="function") 25 | def wpitime(): 26 | import hal.simulation 27 | 28 | class FakeTime: 29 | def step(self, seconds): 30 | delta = int(seconds * 1000000) 31 | hal.simulation.stepTimingAsync(delta) 32 | 33 | hal.simulation.pauseTiming() 34 | hal.simulation.restartTiming() 35 | 36 | yield FakeTime() 37 | 38 | hal.simulation.resumeTiming() 39 | 40 | 41 | @pytest.fixture(scope="function") 42 | def hal(wpitime): 43 | import hal.simulation 44 | 45 | yield 46 | 47 | # Reset the HAL handles 48 | hal.simulation.resetGlobalHandles() 49 | 50 | # Reset the HAL data 51 | hal.simulation.resetAllSimData() 52 | -------------------------------------------------------------------------------- /tests/requirements.txt: -------------------------------------------------------------------------------- 1 | pytest -------------------------------------------------------------------------------- /tests/run_tests.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import os 4 | from os.path import abspath, dirname 5 | import sys 6 | import subprocess 7 | 8 | if __name__ == "__main__": 9 | root = abspath(dirname(__file__)) 10 | os.chdir(root) 11 | 12 | subprocess.check_call([sys.executable, "-m", "pytest"]) 13 | -------------------------------------------------------------------------------- /tests/test_distance_sensors.py: -------------------------------------------------------------------------------- 1 | from robotpy_ext.common_drivers.distance_sensors import ( 2 | SharpIR2Y0A02, 3 | SharpIR2Y0A21, 4 | SharpIR2Y0A41, 5 | ) 6 | 7 | from robotpy_ext.common_drivers.distance_sensors_sim import ( 8 | SharpIR2Y0A02Sim, 9 | SharpIR2Y0A21Sim, 10 | SharpIR2Y0A41Sim, 11 | ) 12 | 13 | import pytest 14 | 15 | 16 | def test_2Y0A02(hal): 17 | sensor = SharpIR2Y0A02(0) 18 | sim = SharpIR2Y0A02Sim(sensor) 19 | 20 | # 22 to 145cm 21 | 22 | # min 23 | sim.setDistance(10) 24 | assert sensor.getDistance() == pytest.approx(22.5) 25 | 26 | # max 27 | sim.setDistance(200) 28 | assert sensor.getDistance() == pytest.approx(145) 29 | 30 | # middle 31 | sim.setDistance(50) 32 | assert sensor.getDistance() == pytest.approx(50) 33 | 34 | sim.setDistance(100) 35 | assert sensor.getDistance() == pytest.approx(100) 36 | 37 | 38 | def test_2Y0A021(hal): 39 | sensor = SharpIR2Y0A21(0) 40 | sim = SharpIR2Y0A21Sim(sensor) 41 | 42 | # 10 to 80cm 43 | 44 | # min 45 | sim.setDistance(5) 46 | assert sensor.getDistance() == pytest.approx(10) 47 | 48 | # max 49 | sim.setDistance(100) 50 | assert sensor.getDistance() == pytest.approx(80) 51 | 52 | # middle 53 | sim.setDistance(30) 54 | assert sensor.getDistance() == pytest.approx(30) 55 | 56 | sim.setDistance(60) 57 | assert sensor.getDistance() == pytest.approx(60) 58 | 59 | 60 | def test_2Y0A041(hal): 61 | sensor = SharpIR2Y0A41(0) 62 | sim = SharpIR2Y0A41Sim(sensor) 63 | 64 | # 4.5 to 35 cm 65 | 66 | # min 67 | sim.setDistance(2) 68 | assert sensor.getDistance() == pytest.approx(4.5) 69 | 70 | # max 71 | sim.setDistance(50) 72 | assert sensor.getDistance() == pytest.approx(35) 73 | 74 | # middle 75 | sim.setDistance(10) 76 | assert sensor.getDistance() == pytest.approx(10) 77 | 78 | sim.setDistance(25) 79 | assert sensor.getDistance() == pytest.approx(25) 80 | -------------------------------------------------------------------------------- /tests/test_magicbot_feedback.py: -------------------------------------------------------------------------------- 1 | from typing import Sequence, Tuple 2 | 3 | import ntcore 4 | from wpimath import geometry 5 | 6 | import magicbot 7 | 8 | 9 | class BasicComponent: 10 | @magicbot.feedback 11 | def get_number(self): 12 | return 0 13 | 14 | @magicbot.feedback 15 | def get_ints(self): 16 | return (0,) 17 | 18 | @magicbot.feedback 19 | def get_floats(self): 20 | return (0.0, 0) 21 | 22 | def execute(self): 23 | pass 24 | 25 | 26 | class TypeHintedComponent: 27 | @magicbot.feedback 28 | def get_rotation(self) -> geometry.Rotation2d: 29 | return geometry.Rotation2d() 30 | 31 | @magicbot.feedback 32 | def get_rotation_array(self) -> Sequence[geometry.Rotation2d]: 33 | return [geometry.Rotation2d()] 34 | 35 | @magicbot.feedback 36 | def get_rotation_2_tuple(self) -> Tuple[geometry.Rotation2d, geometry.Rotation2d]: 37 | return (geometry.Rotation2d(), geometry.Rotation2d()) 38 | 39 | @magicbot.feedback 40 | def get_int(self) -> int: 41 | return 0 42 | 43 | @magicbot.feedback 44 | def get_float(self) -> float: 45 | return 0.5 46 | 47 | @magicbot.feedback 48 | def get_ints(self) -> Sequence[int]: 49 | return (0,) 50 | 51 | @magicbot.feedback 52 | def get_empty_strings(self) -> Sequence[str]: 53 | return () 54 | 55 | def execute(self): 56 | pass 57 | 58 | 59 | class Robot(magicbot.MagicRobot): 60 | basic: BasicComponent 61 | type_hinted: TypeHintedComponent 62 | 63 | def createObjects(self): 64 | pass 65 | 66 | 67 | def test_feedbacks_with_type_hints(): 68 | robot = Robot() 69 | robot.robotInit() 70 | nt = ntcore.NetworkTableInstance.getDefault().getTable("components") 71 | 72 | robot._do_periodics() 73 | 74 | for name, type_str, value in ( 75 | ("basic/number", "double", 0.0), 76 | ("basic/ints", "int[]", [0]), 77 | ("basic/floats", "double[]", [0.0, 0.0]), 78 | ("type_hinted/int", "int", 0), 79 | ("type_hinted/float", "double", 0.5), 80 | ("type_hinted/ints", "int[]", [0]), 81 | ("type_hinted/empty_strings", "string[]", []), 82 | ): 83 | topic = nt.getTopic(name) 84 | assert topic.getTypeString() == type_str 85 | assert topic.genericSubscribe().get().value() == value 86 | 87 | for name, value in [ 88 | ("type_hinted/rotation", geometry.Rotation2d()), 89 | ]: 90 | struct_type = type(value) 91 | assert nt.getTopic(name).getTypeString() == f"struct:{struct_type.__name__}" 92 | topic = nt.getStructTopic(name, struct_type) 93 | assert topic.subscribe(None).get() == value 94 | 95 | for name, struct_type, value in ( 96 | ("type_hinted/rotation_array", geometry.Rotation2d, [geometry.Rotation2d()]), 97 | ( 98 | "type_hinted/rotation_2_tuple", 99 | geometry.Rotation2d, 100 | [geometry.Rotation2d(), geometry.Rotation2d()], 101 | ), 102 | ): 103 | assert nt.getTopic(name).getTypeString() == f"struct:{struct_type.__name__}[]" 104 | topic = nt.getStructArrayTopic(name, struct_type) 105 | assert topic.subscribe([]).get() == value 106 | -------------------------------------------------------------------------------- /tests/test_magicbot_inject.py: -------------------------------------------------------------------------------- 1 | import typing 2 | 3 | import pytest 4 | 5 | from magicbot.inject import get_injection_requests 6 | 7 | 8 | def test_ctor_invalid_type_hint_message(): 9 | """ 10 | class Component: 11 | def __init__(self, foo: 1): ... 12 | """ 13 | type_hints = { 14 | "foo": typing.cast(type, 1), 15 | } 16 | 17 | with pytest.raises(TypeError) as exc_info: 18 | get_injection_requests(type_hints, "bar") 19 | 20 | assert exc_info.value.args[0] == "Component bar has a non-type annotation foo: 1" 21 | -------------------------------------------------------------------------------- /tests/test_magicbot_injection.py: -------------------------------------------------------------------------------- 1 | from typing import List, Tuple, Type, TypeVar 2 | from unittest.mock import Mock 3 | 4 | import magicbot 5 | 6 | 7 | class Injectable: 8 | def __init__(self, num): 9 | self.num = num 10 | 11 | 12 | class DumbComponent: 13 | def execute(self): 14 | pass 15 | 16 | 17 | class Component1: 18 | intvar: int 19 | tupvar: tuple 20 | 21 | injectable: Injectable 22 | 23 | def execute(self): 24 | pass 25 | 26 | 27 | class Component2: 28 | tupvar: tuple 29 | 30 | component1: Component1 31 | 32 | def execute(self): 33 | pass 34 | 35 | 36 | class Component3: 37 | intvar: int 38 | 39 | def __init__( 40 | self, 41 | tupvar: Tuple[int, int], 42 | injectable: Injectable, 43 | component2: Component2, 44 | ) -> None: 45 | self.tuple_ = tupvar 46 | self.injectable_ = injectable 47 | self.component_2 = component2 48 | 49 | def execute(self): 50 | pass 51 | 52 | 53 | class SimpleBot(magicbot.MagicRobot): 54 | intvar = 1 55 | tupvar = 1, 2 56 | 57 | component1: Component1 58 | component2: Component2 59 | component3: Component3 60 | 61 | def createObjects(self): 62 | self.injectable = Injectable(42) 63 | 64 | 65 | class DuplicateComponent: 66 | var: tuple 67 | injectable: Injectable 68 | 69 | def execute(self): 70 | pass 71 | 72 | 73 | class MultilevelBot(magicbot.MagicRobot): 74 | dup1: DuplicateComponent 75 | dup1_var = 1, 2 76 | 77 | dup2: DuplicateComponent 78 | dup2_var = 3, 4 79 | 80 | dup3: DuplicateComponent 81 | dup3_var = 5, 6 82 | 83 | def createObjects(self): 84 | self.injectable = Injectable(42) 85 | 86 | 87 | class SuperComponent: 88 | intvar: int 89 | 90 | def execute(self): 91 | pass 92 | 93 | 94 | class InheritedComponent(SuperComponent): 95 | tupvar: tuple 96 | 97 | def execute(self): 98 | pass 99 | 100 | 101 | class InheritBot(magicbot.MagicRobot): 102 | component: InheritedComponent 103 | 104 | def createObjects(self): 105 | self.intvar = 1 106 | self.tupvar = 1, 2 107 | 108 | 109 | class BotBase(magicbot.MagicRobot): 110 | component_a: DumbComponent 111 | 112 | def createObjects(self): 113 | pass 114 | 115 | 116 | class InheritedBot(BotBase): 117 | component_b: DumbComponent 118 | 119 | 120 | class TypeHintedBot(magicbot.MagicRobot): 121 | some_int: int = 1 122 | some_float: float 123 | 124 | component: DumbComponent 125 | 126 | def createObjects(self): 127 | self.some_float = 0.5 128 | 129 | 130 | class TypeHintedComponent: 131 | injectable: Injectable 132 | numbers: List[float] 133 | 134 | some_int: int = 1 135 | maybe_float: float = None 136 | calculated_num: float 137 | 138 | def __init__(self): 139 | self.calculated_num = 1 - self.some_int 140 | self.numbers = [1, 2.0, 3, 5.0] 141 | 142 | def execute(self): 143 | pass 144 | 145 | 146 | class TypeHintsBot(magicbot.MagicRobot): 147 | injectable: Injectable 148 | injectables: List[Injectable] 149 | 150 | component: TypeHintedComponent 151 | 152 | def createObjects(self): 153 | self.injectable = Injectable(42) 154 | self.injectables = [self.injectable] 155 | 156 | 157 | R = TypeVar("R", bound=magicbot.MagicRobot) 158 | 159 | 160 | def _make_bot(cls: Type[R]) -> R: 161 | bot = cls() 162 | bot.createObjects() 163 | bot._automodes = Mock() 164 | bot._automodes.modes = {} 165 | bot._create_components() 166 | return bot 167 | 168 | 169 | def test_simple_annotation_inject(): 170 | bot = _make_bot(SimpleBot) 171 | 172 | assert bot.component1.intvar == 1 173 | assert isinstance(bot.component1.injectable, Injectable) 174 | assert bot.component1.injectable.num == 42 175 | 176 | assert bot.component2.tupvar == (1, 2) 177 | assert bot.component2.component1 is bot.component1 178 | 179 | assert bot.component3.intvar == 1 180 | assert bot.component3.tuple_ == (1, 2) 181 | assert isinstance(bot.component3.injectable_, Injectable) 182 | assert bot.component3.injectable_.num == 42 183 | assert bot.component3.component_2 is bot.component2 184 | 185 | # Check the method hasn't been mutated 186 | assert str(Component3.__init__.__annotations__["return"]) == "None" 187 | 188 | 189 | def test_multilevel_annotation_inject(): 190 | bot = _make_bot(MultilevelBot) 191 | 192 | assert bot.dup1 is not bot.dup2 193 | assert bot.dup1.var == (1, 2) 194 | assert bot.dup2.var == (3, 4) 195 | assert bot.dup3.var == (5, 6) 196 | 197 | 198 | def test_inherited_annotation_inject(): 199 | bot = _make_bot(InheritBot) 200 | 201 | assert bot.component.tupvar == (1, 2) 202 | assert bot.component.intvar == 1 203 | 204 | 205 | def test_botinherit_annotation_inject(): 206 | bot = _make_bot(InheritedBot) 207 | 208 | assert isinstance(bot.component_a, DumbComponent) 209 | assert isinstance(bot.component_b, DumbComponent) 210 | assert bot.component_a is not bot.component_b 211 | 212 | 213 | def test_typehintedbot(): 214 | bot = _make_bot(TypeHintedBot) 215 | 216 | assert isinstance(bot.component, DumbComponent) 217 | assert bot.some_int == 1 218 | assert bot.some_float == 0.5 219 | 220 | 221 | def test_typehints_inject(): 222 | bot = _make_bot(TypeHintsBot) 223 | 224 | assert isinstance(bot.component, TypeHintedComponent) 225 | assert bot.component.some_int == 1 226 | assert isinstance(bot.component.injectable, Injectable) 227 | assert bot.component.injectable.num == 42 228 | -------------------------------------------------------------------------------- /tests/test_magicbot_sm.py: -------------------------------------------------------------------------------- 1 | from magicbot.state_machine import ( 2 | default_state, 3 | state, 4 | timed_state, 5 | AutonomousStateMachine, 6 | StateMachine, 7 | IllegalCallError, 8 | NoFirstStateError, 9 | MultipleFirstStatesError, 10 | MultipleDefaultStatesError, 11 | InvalidStateName, 12 | ) 13 | 14 | from magicbot.magic_tunable import setup_tunables 15 | 16 | import pytest 17 | 18 | 19 | def test_no_timed_state_duration(): 20 | with pytest.raises(TypeError): 21 | 22 | class _TM(StateMachine): 23 | @timed_state() 24 | def tmp(self): 25 | pass 26 | 27 | 28 | def test_no_start_state(): 29 | class _TM(StateMachine): 30 | pass 31 | 32 | with pytest.raises(NoFirstStateError): 33 | _TM() 34 | 35 | 36 | def test_multiple_first_states(): 37 | class _TM(StateMachine): 38 | @state(first=True) 39 | def tmp1(self): 40 | pass 41 | 42 | @state(first=True) 43 | def tmp2(self): 44 | pass 45 | 46 | with pytest.raises(MultipleFirstStatesError): 47 | _TM() 48 | 49 | 50 | def test_sm(wpitime): 51 | class _TM(StateMachine): 52 | def __init__(self): 53 | self.executed = [] 54 | 55 | def some_fn(self): 56 | self.executed.append("sf") 57 | 58 | @state(first=True) 59 | def first_state(self): 60 | self.executed.append(1) 61 | self.next_state("second_state") 62 | 63 | @timed_state(duration=1, next_state="third_state") 64 | def second_state(self): 65 | self.executed.append(2) 66 | 67 | @state 68 | def third_state(self): 69 | self.executed.append(3) 70 | 71 | sm = _TM() 72 | setup_tunables(sm, "test_sm") 73 | sm.some_fn() 74 | 75 | # should not be able to directly call 76 | with pytest.raises(IllegalCallError): 77 | sm.first_state() 78 | 79 | assert sm.current_state == "" 80 | assert not sm.is_executing 81 | 82 | sm.engage() 83 | assert sm.current_state == "first_state" 84 | assert not sm.is_executing 85 | 86 | sm.execute() 87 | assert sm.current_state == "second_state" 88 | assert sm.is_executing 89 | 90 | # should not change 91 | sm.engage() 92 | assert sm.current_state == "second_state" 93 | assert sm.is_executing 94 | 95 | sm.execute() 96 | assert sm.current_state == "second_state" 97 | assert sm.is_executing 98 | 99 | wpitime.step(1.5) 100 | sm.engage() 101 | sm.execute() 102 | assert sm.current_state == "third_state" 103 | assert sm.is_executing 104 | 105 | sm.engage() 106 | sm.execute() 107 | assert sm.current_state == "third_state" 108 | assert sm.is_executing 109 | 110 | # should be done 111 | sm.done() 112 | assert sm.current_state == "" 113 | assert not sm.is_executing 114 | 115 | # should be able to start directly at second state 116 | sm.engage(initial_state="second_state") 117 | sm.execute() 118 | assert sm.current_state == "second_state" 119 | assert sm.is_executing 120 | 121 | wpitime.step(1.5) 122 | sm.engage() 123 | sm.execute() 124 | assert sm.current_state == "third_state" 125 | assert sm.is_executing 126 | 127 | # test force 128 | sm.engage() 129 | sm.execute() 130 | assert sm.current_state == "third_state" 131 | assert sm.is_executing 132 | 133 | sm.engage(force=True) 134 | assert sm.current_state == "first_state" 135 | assert sm.is_executing 136 | 137 | sm.execute() 138 | sm.execute() 139 | assert not sm.is_executing 140 | assert sm.current_state == "" 141 | 142 | assert sm.executed == ["sf", 1, 2, 3, 3, 2, 3, 3, 1] 143 | 144 | 145 | def test_sm_inheritance(): 146 | class _TM1(StateMachine): 147 | @state 148 | def second_state(self): 149 | self.done() 150 | 151 | class _TM2(_TM1): 152 | @state(first=True) 153 | def first_state(self): 154 | self.next_state("second_state") 155 | 156 | sm = _TM2() 157 | setup_tunables(sm, "test_sm_inheritance") 158 | sm.engage() 159 | assert sm.current_state == "first_state" 160 | 161 | sm.execute() 162 | assert sm.current_state == "second_state" 163 | 164 | sm.execute() 165 | assert sm.current_state == "" 166 | 167 | 168 | def test_must_finish(wpitime): 169 | class _TM(StateMachine): 170 | def __init__(self): 171 | self.executed = [] 172 | 173 | @state(first=True) 174 | def ordinary1(self): 175 | self.next_state("ordinary2") 176 | self.executed.append(1) 177 | 178 | @state 179 | def ordinary2(self): 180 | self.next_state("must_finish") 181 | self.executed.append(2) 182 | 183 | @state(must_finish=True) 184 | def must_finish(self): 185 | self.executed.append("mf") 186 | 187 | @state 188 | def ordinary3(self): 189 | self.executed.append(3) 190 | self.next_state_now("timed_must_finish") 191 | 192 | @timed_state(duration=1, must_finish=True) 193 | def timed_must_finish(self): 194 | self.executed.append("tmf") 195 | 196 | sm = _TM() 197 | setup_tunables(sm, "test_must_finish") 198 | 199 | sm.engage() 200 | sm.execute() 201 | sm.execute() 202 | 203 | assert sm.current_state == "" 204 | assert not sm.is_executing 205 | 206 | sm.engage() 207 | sm.execute() 208 | sm.engage() 209 | sm.execute() 210 | sm.execute() 211 | sm.execute() 212 | 213 | assert sm.current_state == "must_finish" 214 | assert sm.is_executing 215 | 216 | sm.next_state("ordinary3") 217 | sm.engage() 218 | sm.execute() 219 | 220 | assert sm.current_state == "timed_must_finish" 221 | 222 | sm.execute() 223 | assert sm.is_executing 224 | assert sm.current_state == "timed_must_finish" 225 | 226 | for _ in range(7): 227 | wpitime.step(0.1) 228 | 229 | sm.execute() 230 | assert sm.is_executing 231 | assert sm.current_state == "timed_must_finish" 232 | 233 | wpitime.step(1) 234 | sm.execute() 235 | assert not sm.is_executing 236 | 237 | assert sm.executed == [1, 1, 2, "mf", "mf", 3] + ["tmf"] * 9 238 | 239 | 240 | def test_autonomous_sm(): 241 | class _TM(AutonomousStateMachine): 242 | i = 0 243 | VERBOSE_LOGGING = False 244 | 245 | @state(first=True) 246 | def something(self): 247 | self.i += 1 248 | if self.i == 6: 249 | self.done() 250 | 251 | sm = _TM() 252 | setup_tunables(sm, "test_autonomous_sm") 253 | 254 | sm.on_enable() 255 | 256 | for _ in range(5): 257 | sm.on_iteration(None) 258 | assert sm.is_executing 259 | 260 | sm.on_iteration(None) 261 | assert not sm.is_executing 262 | 263 | for _ in range(5): 264 | sm.on_iteration(None) 265 | assert not sm.is_executing 266 | 267 | assert sm.i == 6 268 | 269 | 270 | def test_autonomous_sm_end_timed_state(wpitime): 271 | class _TM(AutonomousStateMachine): 272 | i = 0 273 | j = 0 274 | VERBOSE_LOGGING = False 275 | 276 | @state(first=True) 277 | def something(self): 278 | self.i += 1 279 | if self.i == 3: 280 | self.next_state("timed") 281 | 282 | @timed_state(duration=1) 283 | def timed(self): 284 | self.j += 1 285 | 286 | sm = _TM() 287 | setup_tunables(sm, "test_autonomous_sm_end_timed_state") 288 | 289 | sm.on_enable() 290 | 291 | for _ in range(5): 292 | wpitime.step(0.7) 293 | sm.on_iteration(None) 294 | assert sm.is_executing 295 | 296 | for _ in range(5): 297 | wpitime.step(0.7) 298 | sm.on_iteration(None) 299 | assert not sm.is_executing 300 | 301 | assert sm.i == 3 302 | assert sm.j == 2 303 | 304 | 305 | def test_next_fn(): 306 | class _TM(StateMachine): 307 | @state(first=True) 308 | def first_state(self): 309 | self.next_state(self.second_state) 310 | 311 | @state 312 | def second_state(self): 313 | self.done() 314 | 315 | sm = _TM() 316 | setup_tunables(sm, "test_next_fn") 317 | sm.engage() 318 | assert sm.current_state == "first_state" 319 | 320 | sm.execute() 321 | assert sm.current_state == "second_state" 322 | 323 | sm.engage() 324 | sm.execute() 325 | assert sm.current_state == "" 326 | 327 | 328 | def test_next_fn2(wpitime): 329 | class _TM(StateMachine): 330 | @state 331 | def second_state(self): 332 | pass 333 | 334 | @timed_state(first=True, duration=0.1, next_state=second_state) 335 | def first_state(self): 336 | pass 337 | 338 | sm = _TM() 339 | setup_tunables(sm, "test_next_fn2") 340 | sm.engage() 341 | sm.execute() 342 | assert sm.current_state == "first_state" 343 | assert sm.is_executing 344 | 345 | wpitime.step(0.5) 346 | 347 | sm.engage() 348 | sm.execute() 349 | assert sm.current_state == "second_state" 350 | assert sm.is_executing 351 | 352 | sm.execute() 353 | assert sm.current_state == "" 354 | assert not sm.is_executing 355 | 356 | 357 | def test_mixup(): 358 | from robotpy_ext.autonomous import state as _ext_state 359 | from robotpy_ext.autonomous import timed_state as _ext_timed_state 360 | 361 | with pytest.raises((RuntimeError, TypeError)) as exc_info: 362 | 363 | class _SM1(StateMachine): 364 | @_ext_state(first=True) 365 | def the_state(self): 366 | pass 367 | 368 | if isinstance(exc_info.value, RuntimeError): 369 | assert isinstance(exc_info.value.__cause__, TypeError) 370 | 371 | with pytest.raises((RuntimeError, TypeError)) as exc_info: 372 | 373 | class _SM2(StateMachine): 374 | @_ext_timed_state(first=True, duration=1) 375 | def the_state(self): 376 | pass 377 | 378 | if isinstance(exc_info.value, RuntimeError): 379 | assert isinstance(exc_info.value.__cause__, TypeError) 380 | 381 | 382 | def test_forbidden_state_names(): 383 | with pytest.raises(InvalidStateName): 384 | 385 | class _SM(StateMachine): 386 | @state 387 | def done(self): 388 | pass 389 | 390 | 391 | def test_mixins(): 392 | class _SM1(StateMachine): 393 | @state 394 | def state1(self): 395 | pass 396 | 397 | class _SM2(StateMachine): 398 | @state 399 | def state2(self): 400 | pass 401 | 402 | class _SM(_SM1, _SM2): 403 | @state(first=True) 404 | def first_state(self): 405 | pass 406 | 407 | s = _SM() 408 | states = s._StateMachine__states 409 | 410 | assert "state1" in states 411 | assert "state2" in states 412 | assert "first_state" in states 413 | 414 | 415 | def test_multiple_default_states(): 416 | class _SM(StateMachine): 417 | @state(first=True) 418 | def state(self): 419 | pass 420 | 421 | @default_state 422 | def state1(self): 423 | pass 424 | 425 | @default_state 426 | def state2(self): 427 | pass 428 | 429 | with pytest.raises(MultipleDefaultStatesError): 430 | _SM() 431 | 432 | 433 | def test_default_state_machine(): 434 | class _SM(StateMachine): 435 | def __init__(self): 436 | self.didOne = None 437 | self.didDefault = None 438 | self.defaultInit = None 439 | self.didDone = None 440 | 441 | @state(first=True) 442 | def stateOne(self): 443 | self.didOne = True 444 | self.didDefault = False 445 | self.didDone = False 446 | 447 | @state 448 | def doneState(self): 449 | self.didOne = False 450 | self.didDefault = False 451 | self.didDone = True 452 | self.done() 453 | 454 | @default_state 455 | def defaultState(self, initial_call): 456 | self.didOne = False 457 | self.didDefault = True 458 | self.defaultInit = initial_call 459 | self.didDone = False 460 | 461 | sm = _SM() 462 | setup_tunables(sm, "test_default_state_machine") 463 | 464 | sm.execute() 465 | assert sm.didOne == False 466 | assert sm.didDefault == True 467 | assert sm.defaultInit == True 468 | assert sm.didDone == False 469 | 470 | sm.execute() 471 | assert sm.didOne == False 472 | assert sm.didDefault == True 473 | assert sm.defaultInit == False 474 | assert sm.didDone == False 475 | 476 | # do a thing 477 | sm.engage() 478 | sm.execute() 479 | assert sm.didOne == True 480 | assert sm.didDefault == False 481 | assert sm.didDone == False 482 | 483 | # should go back (test for initial) 484 | sm.execute() 485 | assert sm.didOne == False 486 | assert sm.didDefault == True 487 | assert sm.defaultInit == True 488 | assert sm.didDone == False 489 | 490 | # should happen again (no initial) 491 | sm.execute() 492 | assert sm.didOne == False 493 | assert sm.didDefault == True 494 | assert sm.defaultInit == False 495 | assert sm.didDone == False 496 | 497 | # do another thing 498 | sm.engage() 499 | sm.execute() 500 | assert sm.didOne == True 501 | assert sm.didDefault == False 502 | assert sm.didDone == False 503 | 504 | # should go back (test for initial) 505 | sm.execute() 506 | assert sm.didOne == False 507 | assert sm.didDefault == True 508 | assert sm.defaultInit == True 509 | assert sm.didDone == False 510 | 511 | # should happen again (no initial) 512 | sm.execute() 513 | assert sm.didOne == False 514 | assert sm.didDefault == True 515 | assert sm.defaultInit == False 516 | assert sm.didDone == False 517 | 518 | # enagage a state that will call done, check to see 519 | # if we come back 520 | sm.engage("doneState") 521 | sm.execute() 522 | assert sm.didOne == False 523 | assert sm.didDefault == False 524 | assert sm.defaultInit == False 525 | assert sm.didDone == True 526 | 527 | # should go back (test for initial) 528 | sm.execute() 529 | assert sm.didOne == False 530 | assert sm.didDefault == True 531 | assert sm.defaultInit == True 532 | assert sm.didDone == False 533 | 534 | # should happen again (no initial) 535 | sm.execute() 536 | assert sm.didOne == False 537 | assert sm.didDefault == True 538 | assert sm.defaultInit == False 539 | assert sm.didDone == False 540 | 541 | 542 | def test_short_timed_state(wpitime): 543 | """ 544 | Tests two things: 545 | - A timed state that expires before it executes 546 | - Ensures that the default state won't execute if the machine is always 547 | executing 548 | """ 549 | 550 | class _SM(StateMachine): 551 | def __init__(self): 552 | self.executed = [] 553 | 554 | @default_state 555 | def d(self): 556 | self.executed.append("d") 557 | 558 | @state(first=True) 559 | def a(self): 560 | self.executed.append("a") 561 | self.next_state("b") 562 | 563 | @timed_state(duration=0.01) 564 | def b(self): 565 | self.executed.append("b") 566 | 567 | def done(self): 568 | super().done() 569 | self.executed.append("d") 570 | 571 | sm = _SM() 572 | setup_tunables(sm, "test_short_timed_state") 573 | assert sm.current_state == "" 574 | assert not sm.is_executing 575 | 576 | for _ in [1, 2, 3, 4]: 577 | sm.engage() 578 | sm.execute() 579 | assert sm.current_state == "b" 580 | 581 | wpitime.step(0.02) 582 | 583 | sm.engage() 584 | sm.execute() 585 | assert sm.current_state == "b" 586 | 587 | wpitime.step(0.02) 588 | 589 | assert sm.executed == ["a", "b", "d", "a", "b", "d", "a", "b", "d", "a", "b"] 590 | -------------------------------------------------------------------------------- /tests/test_magicbot_tunable.py: -------------------------------------------------------------------------------- 1 | from typing import ClassVar, List, Sequence 2 | 3 | import ntcore 4 | import pytest 5 | from wpimath import geometry 6 | 7 | from magicbot.magic_tunable import setup_tunables, tunable 8 | 9 | 10 | def test_tunable() -> None: 11 | class Component: 12 | an_int = tunable(1) 13 | ints = tunable([0]) 14 | floats = tunable([1.0, 2.0]) 15 | rotation = tunable(geometry.Rotation2d()) 16 | rotations = tunable([geometry.Rotation2d()]) 17 | 18 | component = Component() 19 | setup_tunables(component, "test_tunable") 20 | nt = ntcore.NetworkTableInstance.getDefault().getTable("/components/test_tunable") 21 | 22 | for name, type_str, value in [ 23 | ("an_int", "int", 1), 24 | ("ints", "int[]", [0]), 25 | ("floats", "double[]", [1.0, 2.0]), 26 | ]: 27 | topic = nt.getTopic(name) 28 | assert topic.getTypeString() == type_str 29 | assert topic.genericSubscribe().get().value() == value 30 | assert getattr(component, name) == value 31 | 32 | for name, value in [ 33 | ("rotation", geometry.Rotation2d()), 34 | ]: 35 | struct_type = type(value) 36 | assert nt.getTopic(name).getTypeString() == f"struct:{struct_type.__name__}" 37 | topic = nt.getStructTopic(name, struct_type) 38 | assert topic.subscribe(None).get() == value 39 | assert getattr(component, name) == value 40 | 41 | for name, struct_type, value in [ 42 | ("rotations", geometry.Rotation2d, [geometry.Rotation2d()]), 43 | ]: 44 | assert nt.getTopic(name).getTypeString() == f"struct:{struct_type.__name__}[]" 45 | topic = nt.getStructArrayTopic(name, struct_type) 46 | assert topic.subscribe([]).get() == value 47 | assert getattr(component, name) == value 48 | 49 | 50 | def test_tunable_errors(): 51 | with pytest.raises(TypeError): 52 | 53 | class Component: 54 | invalid = tunable(None) 55 | 56 | 57 | def test_tunable_errors_with_empty_sequence(): 58 | with pytest.raises((RuntimeError, ValueError)): 59 | 60 | class Component: 61 | empty = tunable([]) 62 | 63 | 64 | def test_type_hinted_empty_sequences() -> None: 65 | class Component: 66 | generic_seq = tunable[Sequence[int]](()) 67 | class_var_seq: ClassVar[tunable[Sequence[int]]] = tunable(()) 68 | inst_seq: Sequence[int] = tunable(()) 69 | 70 | generic_typing_list = tunable[List[int]]([]) 71 | class_var_typing_list: ClassVar[tunable[List[int]]] = tunable([]) 72 | inst_typing_list: List[int] = tunable([]) 73 | 74 | generic_list = tunable[list[int]]([]) 75 | class_var_list: ClassVar[tunable[list[int]]] = tunable([]) 76 | inst_list: list[int] = tunable([]) 77 | 78 | component = Component() 79 | setup_tunables(component, "test_type_hinted_sequences") 80 | NetworkTables = ntcore.NetworkTableInstance.getDefault() 81 | nt = NetworkTables.getTable("/components/test_type_hinted_sequences") 82 | 83 | for name in [ 84 | "generic_seq", 85 | "class_var_seq", 86 | "inst_seq", 87 | "generic_typing_list", 88 | "class_var_typing_list", 89 | "inst_typing_list", 90 | "generic_list", 91 | "class_var_list", 92 | "inst_list", 93 | ]: 94 | assert nt.getTopic(name).getTypeString() == "int[]" 95 | entry = nt.getEntry(name) 96 | assert entry.getIntegerArray(None) == [] 97 | assert getattr(component, name) == [] 98 | -------------------------------------------------------------------------------- /tests/test_pressure_sensor.py: -------------------------------------------------------------------------------- 1 | def test_sensor(wpimock): 2 | from robotpy_ext.common_drivers.pressure_sensors import REVAnalogPressureSensor 3 | 4 | sensor = REVAnalogPressureSensor(3, 3.3) 5 | sensor.sensor.getAverageVoltage.return_value = 2.0 6 | assert int(sensor.pressure) == 126 7 | 8 | 9 | def test_calibration(wpimock): 10 | from robotpy_ext.common_drivers.pressure_sensors import REVAnalogPressureSensor 11 | 12 | sensor = REVAnalogPressureSensor(3, 3.3) 13 | sensor.sensor.getAverageVoltage.return_value = 2.0 14 | sensor.calibrate(50) 15 | assert int(sensor.pressure) == 50 16 | -------------------------------------------------------------------------------- /tests/test_toggle.py: -------------------------------------------------------------------------------- 1 | from robotpy_ext.control.toggle import Toggle 2 | from robotpy_ext.misc.precise_delay import NotifierDelay 3 | 4 | 5 | class FakeJoystick: 6 | def __init__(self): 7 | self._pressed = [False] * 2 8 | 9 | def getRawButton(self, num): 10 | return self._pressed[num] 11 | 12 | def press(self, num): 13 | self._pressed[num] = True 14 | 15 | def release(self, num): 16 | self._pressed[num] = False 17 | 18 | 19 | def test_toggle(): 20 | joystick = FakeJoystick() 21 | toggleButton = Toggle(joystick, 0) 22 | toggleButton2 = Toggle(joystick, 1) 23 | assert toggleButton.off 24 | joystick.press(0) 25 | assert toggleButton.on 26 | assert toggleButton2.off 27 | joystick.release(0) 28 | assert toggleButton.on 29 | joystick.press(0) 30 | assert toggleButton.off 31 | joystick.release(0) 32 | assert toggleButton.off 33 | joystick.press(1) 34 | assert toggleButton.off 35 | assert toggleButton2.on 36 | 37 | 38 | def test_toggle_debounce(): 39 | # TODO: use simulated time 40 | delay = NotifierDelay(0.5) 41 | joystick = FakeJoystick() 42 | toggleButton = Toggle(joystick, 1, 0.1) 43 | assert toggleButton.off 44 | joystick.press(1) 45 | assert toggleButton.on 46 | joystick.release(1) 47 | joystick.press(1) 48 | joystick.release(1) 49 | assert toggleButton.on 50 | delay.wait() 51 | assert toggleButton.on 52 | joystick.press(1) 53 | assert toggleButton.off 54 | -------------------------------------------------------------------------------- /tests/test_units.py: -------------------------------------------------------------------------------- 1 | from robotpy_ext.common_drivers import units 2 | 3 | 4 | def close_enough(val_1, val_2): 5 | """Quick function to determine if val_1 ~= val_2""" 6 | return abs(val_1 - val_2) < 0.001 7 | 8 | 9 | def test_meters_to_cm(): 10 | assert units.convert(units.meter, units.centimeter, 1) == 100 11 | assert units.convert(units.meter, units.centimeter, 50) == 5000 12 | 13 | 14 | def test_cm_to_meters(): 15 | assert units.convert(units.centimeter, units.meter, 100) == 1 16 | assert units.convert(units.centimeter, units.meter, 500) == 5 17 | 18 | 19 | def test_feet_to_inches(): 20 | assert close_enough(units.convert(units.foot, units.inch, 1), 12) 21 | assert close_enough(units.convert(units.foot, units.inch, 4), 48) 22 | 23 | 24 | def test_inches_to_feet(): 25 | assert close_enough(units.convert(units.inch, units.foot, 12), 1) 26 | assert close_enough(units.convert(units.inch, units.foot, 48), 4) 27 | 28 | 29 | def test_feet_to_cm(): 30 | assert close_enough(units.convert(units.foot, units.centimeter, 1), 30.48) 31 | assert close_enough(units.convert(units.foot, units.centimeter, 3), 91.44) 32 | 33 | 34 | def test_cm_to_feet(): 35 | assert close_enough(units.convert(units.centimeter, units.foot, 30.48), 1) 36 | assert close_enough(units.convert(units.centimeter, units.foot, 91.44), 3) 37 | -------------------------------------------------------------------------------- /tests/test_xl_max_sonar_ez.py: -------------------------------------------------------------------------------- 1 | def test_digital_sensor(wpimock): 2 | wpimock.Counter().getPeriod.return_value = 1 * 0.000147 3 | 4 | from robotpy_ext.common_drivers import xl_max_sonar_ez 5 | 6 | digital = xl_max_sonar_ez.MaxSonarEZPulseWidth(1) 7 | assert digital.get() == 1 8 | 9 | 10 | def test_analog_sensor(wpimock): 11 | from robotpy_ext.common_drivers import xl_max_sonar_ez 12 | 13 | analog = xl_max_sonar_ez.MaxSonarEZAnalog(1) 14 | analog.analog.getVoltage.return_value = 1 * 0.0049 * 2.54 15 | 16 | assert analog.get() == 1 17 | --------------------------------------------------------------------------------