diff --git a/.flake8.cfg b/.flake8.cfg new file mode 100644 index 0000000..ead0c3d --- /dev/null +++ b/.flake8.cfg @@ -0,0 +1,10 @@ +[flake8] +max-line-length = 120 +max-complexity = 40 +ignore = + W504 +exclude = + .git + ./ev3dev2/auto.py + ./tests/fake-sys/ + ./venv diff --git a/.readthedocs.yml b/.readthedocs.yml new file mode 100644 index 0000000..75c0d10 --- /dev/null +++ b/.readthedocs.yml @@ -0,0 +1,12 @@ +version: 2 + +# Build documentation in the docs/ directory with Sphinx +sphinx: + configuration: docs/conf.py + +formats: all + +python: + version: 3.7 + install: + - requirements: docs/requirements.txt \ No newline at end of file diff --git a/.travis.yml b/.travis.yml index 7329875..87ed261 100644 --- a/.travis.yml +++ b/.travis.yml @@ -3,7 +3,7 @@ env: - USE_MICROPYTHON=false - USE_MICROPYTHON=true python: -- 3.4 +- 3.8 cache: pip: true directories: @@ -13,7 +13,7 @@ sudo: false git: depth: 100 install: -- if [ $USE_MICROPYTHON = false ]; then pip install Pillow Sphinx sphinx_bootstrap_theme recommonmark evdev==1.0.0 && pip install -r ./docs/requirements.txt; fi +- if [ $USE_MICROPYTHON = false ]; then pip install Pillow Sphinx==2.4.4 sphinx_bootstrap_theme recommonmark evdev==1.0.0 && pip install -r ./docs/requirements.txt; fi - if [ $USE_MICROPYTHON = true ]; then ./.travis/install-micropython.sh && MICROPYTHON=~/micropython/ports/unix/micropython; fi script: - chmod -R g+rw ./tests/fake-sys/devices/**/* diff --git a/.yapf.cfg b/.yapf.cfg new file mode 100644 index 0000000..c30b99c --- /dev/null +++ b/.yapf.cfg @@ -0,0 +1,3 @@ +[style] +based_on_style = pep8 +COLUMN_LIMIT = 120 diff --git a/CONTRIBUTING.rst b/CONTRIBUTING.rst index 7c650b1..bad507f 100644 --- a/CONTRIBUTING.rst +++ b/CONTRIBUTING.rst @@ -70,34 +70,71 @@ sudo make install To update the module, use the following commands: -```shell -cd ev3dev-lang-python -git pull -sudo make install -``` +.. code-block:: bash + + cd ev3dev-lang-python + git pull + sudo make install + If you are developing micropython support, you can take a shortcut and use the following command to build and deploy the micropython files only: -```shell -cd ev3dev-lang-python -sudo make micropython-install -``` + +.. code-block:: bash + + cd ev3dev-lang-python + sudo make micropython-install To re-install the latest release, use the following command: -```shell -sudo apt-get --reinstall install python3-ev3dev2 -``` + +.. code-block:: bash + + sudo apt-get --reinstall install python3-ev3dev2 Or, to update your current ev3dev2 to the latest release, use the following commands: -```shell -sudo apt update -sudo apt install --only-upgrade micropython-ev3dev2 -``` + +.. code-block:: bash + + sudo apt update + sudo apt install --only-upgrade micropython-ev3dev2 + +Publishing releases +------------------- + +#. Update the changelog, including a correct release date. Use ``date -R`` to get correctly-formatted date. +#. Commit the changelog. By convention, use message like ``Update changelog for 2.1.0 release``. +#. Build/update pbuilder base images: ``OS=debian DIST=stretch ARCH=amd64 pbuilder-ev3dev base`` and ``OS=raspbian DIST=stretch ARCH=armhf pbuilder-ev3dev base``. +#. ``./debian/release.sh`` and enter passwords/key passphrases as needed +#. ``git push`` +#. ``git push --tags`` +#. ``git tag -d stable`` +#. ``git tag stable`` +#. ``git push --tags --force`` +#. ``git tag -a 2.1.0 -m "python-ev3dev2 PyPi release 2.1.0"`` +#. ``git push --tags`` + +Note that push order is important; the CI server will get confused if you push +other tags pointing to the same commit after you push the PyPi release tag. This +doesn't actually cause release issues, but does mark the CI builds as "failed" +because it tried to publish the same release again. + +**Check all of the following after release is complete:** + +- Emails from package server don't include any errors +- All Travis CI builds succeeded +- New release is available on PyPi +- Release tags are up on GitHub +- ReadTheDocs is updated + + - ReadTheDocs "stable" version points to latest release + - There is an explicit version tag for the last-released version (exeption: ``2.1.0``) + - There is an explicit version tag for this version (you will likely need to manually activate it) + - All ReadTheDocs builds succeeded .. _ev3dev: http://ev3dev.org -.. _FAQ: https://python-ev3dev.readthedocs.io/en/ev3dev-stretch/faq.html \ No newline at end of file +.. _FAQ: https://python-ev3dev.readthedocs.io/en/ev3dev-stretch/faq.html diff --git a/Makefile b/Makefile index 697b250..f16352c 100644 --- a/Makefile +++ b/Makefile @@ -16,3 +16,11 @@ install: micropython-install: ${MPYS} cp -R $(OUT)/* /usr/lib/micropython/ + +clean: + find . -name __pycache__ | xargs rm -rf + find . -name *.pyc | xargs rm -rf + +format: + yapf --style .yapf.cfg --in-place --exclude tests/fake-sys/ --recursive . + python3 -m flake8 --config=.flake8.cfg . diff --git a/README.rst b/README.rst index 23af819..a1c414a 100644 --- a/README.rst +++ b/README.rst @@ -50,6 +50,9 @@ to get started: .. code-block:: python #!/usr/bin/env python3 + + from time import sleep + from ev3dev2.motor import LargeMotor, OUTPUT_A, OUTPUT_B, SpeedPercent, MoveTank from ev3dev2.sensor import INPUT_1 from ev3dev2.sensor.lego import TouchSensor @@ -107,6 +110,8 @@ before trying this out. else: leds.set_color("LEFT", "RED") leds.set_color("RIGHT", "RED") + # don't let this loop use 100% CPU + sleep(0.01) If you'd like to use a sensor on a specific port, specify the port like this: @@ -114,6 +119,8 @@ If you'd like to use a sensor on a specific port, specify the port like this: ts = TouchSensor(INPUT_1) +*Heads-up:* If you are using a BrickPi instead of an EV3, you will need to manually configure the sensor. See the example here: https://github.com/ev3dev/ev3dev-lang-python-demo/blob/stretch/platform/brickpi3-motor-and-sensor.py + Running a single motor ~~~~~~~~~~~~~~~~~~~~~~ diff --git a/debian/changelog b/debian/changelog index 5848b81..a88a243 100644 --- a/debian/changelog +++ b/debian/changelog @@ -1,3 +1,35 @@ +python-ev3dev2 (2.1.0) stretch; urgency=medium + + [Daniel Walton] + * RPyC update docs and make it easier to use + * use double backticks consistently in docstrings + * format code via yapf + * MoveDifferential use gyro for better accuracy. Make MoveTank and + MoveDifferential "turn" APIs more consistent. + * MoveTank turn_degrees convert speed to SpeedValue object + * correct flake8 errors + + [Matěj Volf] + * LED animation fix duration None + * Add rest notes to sound.Sound.play_song() + + [Kaelin Laundry] + * Add "value_secs" and "is_elapsed_*" methods to StopWatch + * Rename "value_hms" to "hms_str". New "value_hms" returns original tuple. + * Fix bug in Button.process() on Micropython + + [Nithin Shenoy] + * Add Gyro-based driving support to MoveTank + + -- Kaelin Laundry Sun, 22 Mar 2020 19:14:35 -0700 + +python-ev3dev2 (2.0.0) stretch; urgency=medium + + [David Lechner] + * Fix gyro sensor reset + + -- Kaelin Laundry Sun, 24 Nov 2019 20:41:18 -0800 + python-ev3dev2 (2.0.0~beta5) stretch; urgency=medium [Brady Merkel] diff --git a/docs/conf.py b/docs/conf.py index 5af6eb7..b9e9a8b 100644 --- a/docs/conf.py +++ b/docs/conf.py @@ -15,30 +15,23 @@ import sys import os -import shlex import subprocess - -sys.path.append(os.path.join(os.path.dirname(__file__), '..')) -from git_version import git_version - -on_rtd = os.environ.get('READTHEDOCS', None) == 'True' - -if on_rtd: - subprocess.check_call([sys.executable, '-m', 'pip', 'install', 'sphinx_bootstrap_theme', 'recommonmark', 'evdev']) - import sphinx_bootstrap_theme from recommonmark.parser import CommonMarkParser from recommonmark.transform import AutoStructify +sys.path.append(os.path.join(os.path.dirname(__file__), '..')) +from git_version import git_version # noqa: E402 + # If extensions (or modules to document with autodoc) are in another directory, # add these directories to sys.path here. If the directory is relative to the # documentation root, use os.path.abspath to make it absolute, like shown here. -#sys.path.insert(0, os.path.abspath('.')) +# sys.path.insert(0, os.path.abspath('.')) # -- General configuration ------------------------------------------------ # If your documentation needs a minimal Sphinx version, state it here. -#needs_sphinx = '1.0' +# needs_sphinx = '1.0' # Add any Sphinx extension module names here, as strings. They can be # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom @@ -61,7 +54,7 @@ source_suffix = ['.rst', '.md'] # The encoding of source files. -#source_encoding = 'utf-8-sig' +# source_encoding = 'utf-8-sig' # The master toctree document. master_doc = 'index' @@ -89,9 +82,9 @@ # There are two options for replacing |today|: either, you set today to some # non-false value, then it is used: -#today = '' +# today = '' # Else, today_fmt is used as the format for a strftime call. -#today_fmt = '%B %d, %Y' +# today_fmt = '%B %d, %Y' # List of patterns, relative to source directory, that match files and # directories to ignore when looking for source files. @@ -99,32 +92,31 @@ # The reST default role (used for this markup: `text`) to use for all # documents. -#default_role = None +# default_role = None # If true, '()' will be appended to :func: etc. cross-reference text. -#add_function_parentheses = True +# add_function_parentheses = True # If true, the current module name will be prepended to all description # unit titles (such as .. function::). -#add_module_names = True +# add_module_names = True # If true, sectionauthor and moduleauthor directives will be shown in the # output. They are ignored by default. -#show_authors = False +# show_authors = False # The name of the Pygments (syntax highlighting) style to use. pygments_style = 'sphinx' # A list of ignored prefixes for module index sorting. -#modindex_common_prefix = [] +# modindex_common_prefix = [] # If true, keep warnings as "system message" paragraphs in the built documents. -#keep_warnings = False +# keep_warnings = False # If true, `todo` and `todoList` produce output, else they produce nothing. todo_include_todos = False - # -- Options for HTML output ---------------------------------------------- # The theme to use for HTML and HTML Help pages. See the documentation for @@ -132,35 +124,33 @@ html_theme_path = sphinx_bootstrap_theme.get_html_theme_path() html_theme = 'bootstrap' html_theme_options = { - 'bootswatch_theme': 'yeti', - 'navbar_links' : [ - ("GitHub", "https://github.com/ev3dev/ev3dev-lang-python", True) - ] - } + 'bootswatch_theme': 'yeti', + 'navbar_links': [("GitHub", "https://github.com/ev3dev/ev3dev-lang-python", True)] +} # Theme options are theme-specific and customize the look and feel of a theme # further. For a list of options available for each theme, see the # documentation. -#html_theme_options = {} +# html_theme_options = {} # Add any paths that contain custom themes here, relative to this directory. -#html_theme_path = [] +# html_theme_path = [] # The name for this set of Sphinx documents. If None, it defaults to # " v documentation". -#html_title = None +# html_title = None # A shorter title for the navigation bar. Default is the same as html_title. -#html_short_title = None +# html_short_title = None # The name of an image file (relative to this directory) to place at the top # of the sidebar. -#html_logo = None +# html_logo = None # The name of an image file (within the static path) to use as favicon of the # docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 # pixels large. -#html_favicon = None +# html_favicon = None # Add any paths that contain custom static files (such as style sheets) here, # relative to this directory. They are copied after the builtin static files, @@ -170,62 +160,62 @@ # Add any extra paths that contain custom files (such as robots.txt or # .htaccess) here, relative to this directory. These files are copied # directly to the root of the documentation. -#html_extra_path = [] +# html_extra_path = [] # If not '', a 'Last updated on:' timestamp is inserted at every page bottom, # using the given strftime format. -#html_last_updated_fmt = '%b %d, %Y' +# html_last_updated_fmt = '%b %d, %Y' # If true, SmartyPants will be used to convert quotes and dashes to # typographically correct entities. -#html_use_smartypants = True +# html_use_smartypants = True # Custom sidebar templates, maps document names to template names. -#html_sidebars = {} +# html_sidebars = {} # Additional templates that should be rendered to pages, maps page names to # template names. -#html_additional_pages = {} +# html_additional_pages = {} # If false, no module index is generated. -#html_domain_indices = True +# html_domain_indices = True # If false, no index is generated. -#html_use_index = True +# html_use_index = True # If true, the index is split into individual pages for each letter. -#html_split_index = False +# html_split_index = False # If true, links to the reST sources are added to the pages. -#html_show_sourcelink = True +# html_show_sourcelink = True # If true, "Created using Sphinx" is shown in the HTML footer. Default is True. -#html_show_sphinx = True +# html_show_sphinx = True # If true, "(C) Copyright ..." is shown in the HTML footer. Default is True. -#html_show_copyright = True +# html_show_copyright = True # If true, an OpenSearch description file will be output, and all pages will # contain a tag referring to it. The value of this option must be the # base URL from which the finished HTML is served. -#html_use_opensearch = '' +# html_use_opensearch = '' # This is the file name suffix for HTML files (e.g. ".xhtml"). -#html_file_suffix = None +# html_file_suffix = None # Language to be used for generating the HTML full-text search index. # Sphinx supports the following languages: # 'da', 'de', 'en', 'es', 'fi', 'fr', 'h', 'it', 'ja' # 'nl', 'no', 'pt', 'ro', 'r', 'sv', 'tr' -#html_search_language = 'en' +# html_search_language = 'en' # A dictionary with options for the search language support, empty by default. # Now only 'ja' uses this config value -#html_search_options = {'type': 'default'} +# html_search_options = {'type': 'default'} # The name of a javascript file (relative to the configuration directory) that # implements a search results scorer. If empty, the default will be used. -#html_search_scorer = 'scorer.js' +# html_search_scorer = 'scorer.js' # Output file base name for HTML help builder. htmlhelp_basename = 'python-ev3devdoc' @@ -233,60 +223,54 @@ # -- Options for LaTeX output --------------------------------------------- latex_elements = { -# The paper size ('letterpaper' or 'a4paper'). -#'papersize': 'letterpaper', + # The paper size ('letterpaper' or 'a4paper'). + # 'papersize': 'letterpaper', -# The font size ('10pt', '11pt' or '12pt'). -#'pointsize': '10pt', + # The font size ('10pt', '11pt' or '12pt'). + # 'pointsize': '10pt', -# Additional stuff for the LaTeX preamble. -#'preamble': '', + # Additional stuff for the LaTeX preamble. + # 'preamble': '', -# Latex figure (float) alignment -#'figure_align': 'htbp', + # Latex figure (float) alignment + # 'figure_align': 'htbp', } # Grouping the document tree into LaTeX files. List of tuples # (source start file, target name, title, # author, documentclass [howto, manual, or own class]). latex_documents = [ - (master_doc, 'python-ev3dev.tex', 'python-ev3dev Documentation', - 'Ralph Hempel et al', 'manual'), + (master_doc, 'python-ev3dev.tex', 'python-ev3dev Documentation', 'Ralph Hempel et al', 'manual'), ] # The name of an image file (relative to this directory) to place at the top of # the title page. -#latex_logo = None +# latex_logo = None # For "manual" documents, if this is true, then toplevel headings are parts, # not chapters. -#latex_use_parts = False +# latex_use_parts = False # If true, show page references after internal links. -#latex_show_pagerefs = False +# latex_show_pagerefs = False # If true, show URL addresses after external links. -#latex_show_urls = False +# latex_show_urls = False # Documents to append as an appendix to all manuals. -#latex_appendices = [] +# latex_appendices = [] # If false, no module index is generated. -#latex_domain_indices = True - +# latex_domain_indices = True # -- Options for manual page output --------------------------------------- # One entry per manual page. List of tuples # (source start file, name, description, authors, manual section). -man_pages = [ - (master_doc, 'python-ev3dev', 'python-ev3dev Documentation', - [author], 1) -] +man_pages = [(master_doc, 'python-ev3dev', 'python-ev3dev Documentation', [author], 1)] # If true, show URL addresses after external links. -#man_show_urls = False - +# man_show_urls = False # -- Options for Texinfo output ------------------------------------------- @@ -294,41 +278,33 @@ # (source start file, target name, title, author, # dir menu entry, description, category) texinfo_documents = [ - (master_doc, 'python-ev3dev', 'python-ev3dev Documentation', - author, 'python-ev3dev', 'One line description of project.', - 'Miscellaneous'), + (master_doc, 'python-ev3dev', 'python-ev3dev Documentation', author, 'python-ev3dev', + 'One line description of project.', 'Miscellaneous'), ] # Documents to append as an appendix to all manuals. -#texinfo_appendices = [] +# texinfo_appendices = [] # If false, no module index is generated. -#texinfo_domain_indices = True +# texinfo_domain_indices = True # How to display URL addresses: 'footnote', 'no', or 'inline'. -#texinfo_show_urls = 'footnote' +# texinfo_show_urls = 'footnote' # If true, do not generate a @detailmenu in the "Top" node's menu. -#texinfo_no_detailmenu = False +# texinfo_no_detailmenu = False autodoc_member_order = 'bysource' suppress_warnings = ['image.nonlocal_uri'] -nitpick_ignore = [ - ('py:class', 'ev3dev2.display.FbMem'), - ('py:class', 'ev3dev2.button.ButtonBase'), - ('py:class', 'int'), - ('py:class', 'float'), - ('py:class', 'string'), - ('py:class', 'iterable'), - ('py:class', 'tuple'), - ('py:class', 'list'), - ('py:exc', 'ValueError') -] +nitpick_ignore = [('py:class', 'ev3dev2.display.FbMem'), ('py:class', 'ev3dev2.button.ButtonBase'), ('py:class', 'int'), + ('py:class', 'float'), ('py:class', 'string'), ('py:class', 'iterable'), ('py:class', 'tuple'), + ('py:class', 'list'), ('py:exc', 'ValueError')] + def setup(app): app.add_config_value('recommonmark_config', { - 'enable_eval_rst': True, - }, True) + 'enable_eval_rst': True, + }, True) app.add_transform(AutoStructify) diff --git a/docs/ports.rst b/docs/ports.rst index 33f5d93..92c5b6a 100644 --- a/docs/ports.rst +++ b/docs/ports.rst @@ -2,7 +2,8 @@ Lego Port ========= The `LegoPort` class is only needed when manually reconfiguring input/output -ports. Most users can ignore this page. +ports. This is necessary on the BrickPi but not other platforms, such as the +EV3. Most users can ignore this page. .. autoclass:: ev3dev2.port.LegoPort :members: \ No newline at end of file diff --git a/docs/requirements.txt b/docs/requirements.txt index 341fd90..1d58f15 100644 --- a/docs/requirements.txt +++ b/docs/requirements.txt @@ -1 +1,4 @@ -docutils==0.14 \ No newline at end of file +docutils==0.14 +sphinx_bootstrap_theme +recommonmark +evdev \ No newline at end of file diff --git a/docs/rpyc.rst b/docs/rpyc.rst index 58ff0c2..46a41d6 100644 --- a/docs/rpyc.rst +++ b/docs/rpyc.rst @@ -1,87 +1,115 @@ -Working with ev3dev remotely using RPyC -======================================= +************** +RPyC on ev3dev +************** -RPyC_ (pronounced as are-pie-see), or Remote Python Call, is a transparent -python library for symmetrical remote procedure calls, clustering and -distributed-computing. RPyC makes use of object-proxying, a technique that -employs python’s dynamic nature, to overcome the physical boundaries between -processes and computers, so that remote objects can be manipulated as if they -were local. Here are simple steps you need to follow in order to install and -use RPyC with ev3dev: +`RPyC_ `_ (pronounced as are-pie-see) can be used to: +* run a python program on an ev3dev device that controls another ev3dev device. +This is more commonly known as daisy chaining. +* run a python program on your laptop that controls an ev3dev device. This can be +useful if your robot requires CPU intensive code that would be slow to run on the +EV3. A good example of this is a Rubik's cube solver, calculating the solution to +solve a Rubik's cube can be slow on an EV3. -1. Install RPyC both on the EV3 and on your desktop PC. For the EV3, enter the - following command at the command prompt (after you `connect with SSH`_): +For both of these scenarios you can use RPyC to control multiple remote ev3dev devices. - .. code-block:: shell - - sudo easy_install3 rpyc - On the desktop PC, it really depends on your operating system. In case it is - some flavor of linux, you should be able to do +Networking +========== +You will need IP connectivity between the device where your python code runs +(laptop, an ev3dev device, etc) and the remote ev3dev devices. Some common scenarios +might be: +* Multiple EV3s on the same WiFi network +* A laptop and an EV3 on the same WiFi network +* A bluetooth connection between two EV3s - .. code-block:: shell +The `ev3dev networking documentation `_ should get +you up and running in terms of networking connectivity. - sudo pip3 install rpyc - In case it is Windows, there is a win32 installer on the project's - `sourceforge page`_. Also, have a look at the `Download and Install`_ page - on their site. +Install +======= -2. Create file ``rpyc_server.sh`` with the following contents on the EV3: +1. RPyC is installed on ev3dev but we need to create a service that launches + ``rpyc_classic.py`` at bootup. `SSH `_ to your remote ev3dev devices and + cut-n-paste the following commands at the bash prompt. .. code-block:: shell - #!/bin/bash - python3 `which rpyc_classic.py` + echo "[Unit] + Description=RPyC Classic Service + After=multi-user.target - and make the file executable: + [Service] + Type=simple + ExecStart=/usr/bin/rpyc_classic.py - .. code-block:: shell + [Install] + WantedBy=multi-user.target" > rpyc-classic.service - chmod +x rpyc_server.sh + sudo cp rpyc-classic.service /lib/systemd/system/ + sudo systemctl daemon-reload + sudo systemctl enable rpyc-classic.service + sudo systemctl start rpyc-classic.service - Launch the created file either from SSH session (with - ``./rpyc_server.sh`` command), or from brickman. It should output something - like - .. code-block:: none +2. If you will be using an ev3dev device to control another ev3dev device you + can skip this step. If you will be using your desktop PC to control an ev3dev + device you must install RPyC on your desktop PC. How you install RPyC depends + on your operating system. For Linux you should be able to do: - INFO:SLAVE/18812:server started on [0.0.0.0]:18812 + .. code-block:: shell + + sudo apt-get install python3-rpyc - and keep running. + For Windows there is a win32 installer on the project's `sourceforge page`_. + Also, have a look at the `Download and Install`_ page on their site. -3. Now you are ready to connect to the RPyC server from your desktop PC. The - following python script should make a large motor connected to output port - ``A`` spin for a second. +Example +======= +We will run code on our laptop to control the remote ev3dev device with IP +address X.X.X.X. The goal is to have the LargeMotor connected to ``OUTPUT_A`` +run when the TouchSensor on ``INPUT_1`` is pressed. .. code-block:: py import rpyc - conn = rpyc.classic.connect('ev3dev') # host name or IP address of the EV3 - ev3 = conn.modules['ev3dev2.ev3'] # import ev3dev2.ev3 remotely - m = ev3.LargeMotor('outA') - m.run_timed(time_sp=1000, speed_sp=600) - -You can run scripts like this from any interactive python environment, like -ipython shell/notebook, spyder, pycharm, etc. - -Some *advantages* of using RPyC with ev3dev are: - -* It uses much less resources than running ipython notebook on EV3; RPyC server - is lightweight, and only requires an IP connection to the EV3 once set up (no - ssh required). -* The scripts you are working with are actually stored and edited on your - desktop PC, with your favorite editor/IDE. -* Some robots may need much more computational power than what EV3 can give - you. A notable example is the Rubics cube solver: there is an algorithm that - provides almost optimal solution (in terms of number of cube rotations), but - it takes more RAM than is available on EV3. With RPYC, you could run the - heavy-duty computations on your desktop. - -The most obvious *disadvantage* is latency introduced by network connection. -This may be a show stopper for robots where reaction speed is essential. - -.. _RPyC: http://rpyc.readthedocs.io/ -.. _sourceforge page: http://sourceforge.net/projects/rpyc/files/main -.. _Download and Install: http://rpyc.readthedocs.io/en/latest/install.html -.. _connect with SSH: http://www.ev3dev.org/docs/tutorials/connecting-to-ev3dev-with-ssh/ + + # Create a RPyC connection to the remote ev3dev device. + # Use the hostname or IP address of the ev3dev device. + # If this fails, verify your IP connectivty via ``ping X.X.X.X`` + conn = rpyc.classic.connect('X.X.X.X') + + # import ev3dev2 on the remote ev3dev device + ev3dev2_motor = conn.modules['ev3dev2.motor'] + ev3dev2_sensor = conn.modules['ev3dev2.sensor'] + ev3dev2_sensor_lego = conn.modules['ev3dev2.sensor.lego'] + + # Use the LargeMotor and TouchSensor on the remote ev3dev device + motor = ev3dev2_motor.LargeMotor(ev3dev2_motor.OUTPUT_A) + ts = ev3dev2_sensor_lego.TouchSensor(ev3dev2_sensor.INPUT_1) + + # If the TouchSensor is pressed, run the motor + while True: + ts.wait_for_pressed() + motor.run_forever(speed_sp=200) + + ts.wait_for_released() + motor.stop() + +Pros +==== +* RPyC is lightweight and only requires an IP connection (no ssh required). +* Some robots may need much more computational power than an EV3 can give + you. A notable example is the Rubik's cube solver. + +Cons +==== +* Latency will be introduced by the network connection. This may be a show stopper for robots where reaction speed is essential. +* RPyC is only supported by python, it is *NOT* supported by micropython + +References +========== +* `RPyC `_ +* `sourceforge page `_ +* `Download and Install `_ +* `connect with SSH `_ diff --git a/docs/sensors.rst b/docs/sensors.rst index 368ee3e..0c86881 100644 --- a/docs/sensors.rst +++ b/docs/sensors.rst @@ -3,6 +3,11 @@ Sensor classes .. contents:: :local: +*Note:* If you are using a BrickPi rather than an EV3, you will need to manually +configure the ports before interacting with your sensors. See the example +`here `_. + + Dedicated sensor classes ------------------------ diff --git a/docs/sphinx3-build b/docs/sphinx3-build index 9c3e5eb..89b4cee 100755 --- a/docs/sphinx3-build +++ b/docs/sphinx3-build @@ -15,4 +15,3 @@ if __name__ == '__main__': sys.exit(make_main(sys.argv)) else: sys.exit(main(sys.argv)) - diff --git a/ev3dev2/__init__.py b/ev3dev2/__init__.py index 2d2895e..5e776fb 100644 --- a/ev3dev2/__init__.py +++ b/ev3dev2/__init__.py @@ -24,33 +24,35 @@ # ----------------------------------------------------------------------------- import sys +import os +import io +import fnmatch +import re +import stat +import errno +from os.path import abspath + +try: + # if we are in a released build, there will be an auto-generated "version" + # module + from .version import __version__ +except ImportError: + __version__ = "" -if sys.version_info < (3,4): +if sys.version_info < (3, 4): raise SystemError('Must be using Python 3.4 or higher') + def is_micropython(): return sys.implementation.name == "micropython" + def chain_exception(exception, cause): if is_micropython(): raise exception else: raise exception from cause -try: - # if we are in a released build, there will be an auto-generated "version" - # module - from .version import __version__ -except ImportError: - __version__ = "" - -import os -import io -import fnmatch -import re -import stat -import errno -from os.path import abspath def get_current_platform(): """ @@ -121,7 +123,7 @@ def matches(attribute, pattern): try: with io.FileIO(attribute) as f: value = f.read().strip().decode() - except: + except Exception: return False if isinstance(pattern, list): @@ -139,12 +141,23 @@ def matches(attribute, pattern): def library_load_warning_message(library_name, dependent_class): return 'Import warning: Failed to import "{}". {} will be unusable!'.format(library_name, dependent_class) + class DeviceNotFound(Exception): pass + +class DeviceNotDefined(Exception): + pass + + +class ThreadNotRunning(Exception): + pass + + # ----------------------------------------------------------------------------- # Define the base class from which all other ev3dev classes are defined. + class Device(object): """The ev3dev device base class""" @@ -214,7 +227,7 @@ def __str__(self): return "%s(%s)" % (self.__class__.__name__, self.kwargs.get('address')) else: return self.__class__.__name__ - + def __repr__(self): return self.__str__() @@ -241,7 +254,7 @@ def _get_attribute(self, attribute, name): """Device attribute getter""" try: if attribute is None: - attribute = self._attribute_file_open( name ) + attribute = self._attribute_file_open(name) else: attribute.seek(0) return attribute, attribute.read().strip().decode() @@ -252,7 +265,7 @@ def _set_attribute(self, attribute, name, value): """Device attribute setter""" try: if attribute is None: - attribute = self._attribute_file_open( name ) + attribute = self._attribute_file_open(name) else: attribute.seek(0) @@ -275,11 +288,13 @@ def _raise_friendly_access_error(self, driver_error, attribute, value): try: max_speed = self.max_speed except (AttributeError, Exception): - chain_exception(ValueError("The given speed value {} was out of range".format(value)), - driver_error) + chain_exception(ValueError("The given speed value {} was out of range".format(value)), driver_error) else: - chain_exception(ValueError("The given speed value {} was out of range. Max speed: +/-{}".format(value, max_speed)), driver_error) - chain_exception(ValueError("One or more arguments were out of range or invalid, value {}".format(value)), driver_error) + chain_exception( + ValueError("The given speed value {} was out of range. Max speed: +/-{}".format( + value, max_speed)), driver_error) + chain_exception(ValueError("One or more arguments were out of range or invalid, value {}".format(value)), + driver_error) elif driver_errorno == errno.ENODEV or driver_errorno == errno.ENOENT: # We will assume that a file-not-found error is the result of a disconnected device # rather than a library error. If that isn't the case, at a minimum the underlying @@ -369,5 +384,4 @@ def list_devices(class_name, name_pattern, **kwargs): """ classpath = abspath(Device.DEVICE_ROOT_PATH + '/' + class_name) - return (Device(class_name, name, name_exact=True) - for name in list_device_names(classpath, name_pattern, **kwargs)) + return (Device(class_name, name, name_exact=True) for name in list_device_names(classpath, name_pattern, **kwargs)) diff --git a/ev3dev2/_platform/brickpi.py b/ev3dev2/_platform/brickpi.py index 47768ae..75e6991 100644 --- a/ev3dev2/_platform/brickpi.py +++ b/ev3dev2/_platform/brickpi.py @@ -21,7 +21,6 @@ # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. # ----------------------------------------------------------------------------- - """ An assortment of classes modeling specific features of the BrickPi. """ @@ -46,11 +45,11 @@ LEDS['blue_led2'] = 'led2:blue:brick-status' LED_GROUPS = OrderedDict() -LED_GROUPS['LED1'] = ('blue_led1',) -LED_GROUPS['LED2'] = ('blue_led2',) +LED_GROUPS['LED1'] = ('blue_led1', ) +LED_GROUPS['LED2'] = ('blue_led2', ) LED_COLORS = OrderedDict() -LED_COLORS['BLACK'] = (0,) -LED_COLORS['BLUE'] = (1,) +LED_COLORS['BLACK'] = (0, ) +LED_COLORS['BLUE'] = (1, ) LED_DEFAULT_COLOR = 'BLUE' diff --git a/ev3dev2/_platform/brickpi3.py b/ev3dev2/_platform/brickpi3.py index 25a414c..b875b9b 100644 --- a/ev3dev2/_platform/brickpi3.py +++ b/ev3dev2/_platform/brickpi3.py @@ -48,10 +48,10 @@ LEDS['amber_led'] = 'led1:amber:brick-status' LED_GROUPS = OrderedDict() -LED_GROUPS['LED'] = ('amber_led',) +LED_GROUPS['LED'] = ('amber_led', ) LED_COLORS = OrderedDict() -LED_COLORS['BLACK'] = (0,) -LED_COLORS['AMBER'] = (1,) +LED_COLORS['BLACK'] = (0, ) +LED_COLORS['AMBER'] = (1, ) LED_DEFAULT_COLOR = 'AMBER' diff --git a/ev3dev2/_platform/ev3.py b/ev3dev2/_platform/ev3.py index 6bf166b..92e0c6c 100644 --- a/ev3dev2/_platform/ev3.py +++ b/ev3dev2/_platform/ev3.py @@ -21,7 +21,6 @@ # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. # ----------------------------------------------------------------------------- - """ An assortment of classes modeling specific features of the EV3 brick. """ diff --git a/ev3dev2/_platform/evb.py b/ev3dev2/_platform/evb.py index 9350470..7c6470e 100644 --- a/ev3dev2/_platform/evb.py +++ b/ev3dev2/_platform/evb.py @@ -1,4 +1,3 @@ - """ An assortment of classes modeling specific features of the EVB. """ diff --git a/ev3dev2/_platform/fake.py b/ev3dev2/_platform/fake.py index d13a1fd..26a0498 100644 --- a/ev3dev2/_platform/fake.py +++ b/ev3dev2/_platform/fake.py @@ -1,4 +1,3 @@ - OUTPUT_A = 'outA' OUTPUT_B = 'outB' OUTPUT_C = 'outC' diff --git a/ev3dev2/_platform/pistorms.py b/ev3dev2/_platform/pistorms.py index 38a4f91..1dfa0b2 100644 --- a/ev3dev2/_platform/pistorms.py +++ b/ev3dev2/_platform/pistorms.py @@ -1,4 +1,3 @@ - """ An assortment of classes modeling specific features of the PiStorms. """ @@ -14,11 +13,9 @@ INPUT_3 = 'pistorms:BBS1' INPUT_4 = 'pistorms:BBS2' - BUTTONS_FILENAME = '/dev/input/by-path/platform-3f804000.i2c-event' EVDEV_DEVICE_NAME = 'PiStorms' - LEDS = OrderedDict() LEDS['red_left'] = 'pistorms:BB:red:brick-status' LEDS['red_right'] = 'pistorms:BA:red:brick-status' diff --git a/ev3dev2/button.py b/ev3dev2/button.py index 5b742e8..50dd673 100644 --- a/ev3dev2/button.py +++ b/ev3dev2/button.py @@ -24,16 +24,10 @@ # ----------------------------------------------------------------------------- import sys - -if sys.version_info < (3,4): - raise SystemError('Must be using Python 3.4 or higher') - from ev3dev2.stopwatch import StopWatch from ev3dev2 import get_current_platform, is_micropython, library_load_warning_message from logging import getLogger -log = getLogger(__name__) - # Import the button filenames, this is platform specific platform = get_current_platform() @@ -58,21 +52,25 @@ else: raise Exception("Unsupported platform '%s'" % platform) +if sys.version_info < (3, 4): + raise SystemError('Must be using Python 3.4 or higher') + +log = getLogger(__name__) + class MissingButton(Exception): pass class ButtonCommon(object): - def __str__(self): return self.__class__.__name__ @staticmethod def on_change(changed_buttons): """ - This handler is called by `process()` whenever state of any button has - changed since last `process()` call. `changed_buttons` is a list of + This handler is called by ``process()`` whenever state of any button has + changed since last ``process()`` call. ``changed_buttons`` is a list of tuples of changed button names and their states. """ pass @@ -89,7 +87,7 @@ def any(self): def check_buttons(self, buttons=[]): """ - Check if currently pressed buttons exactly match the given list. + Check if currently pressed buttons exactly match the given list ``buttons``. """ return set(self.buttons_pressed) == set(buttons) @@ -98,20 +96,20 @@ def _wait(self, wait_for_button_press, wait_for_button_release, timeout_ms): def wait_for_pressed(self, buttons, timeout_ms=None): """ - Wait for the button to be pressed down. + Wait for ``buttons`` to be pressed down. """ return self._wait(buttons, [], timeout_ms) def wait_for_released(self, buttons, timeout_ms=None): """ - Wait for the button to be released. + Wait for ``buttons`` to be released. """ return self._wait([], buttons, timeout_ms) def wait_for_bump(self, buttons, timeout_ms=None): """ - Wait for the button to be pressed down and then released. - Both actions must happen within timeout_ms. + Wait for ``buttons`` to be pressed down and then released. + Both actions must happen within ``timeout_ms``. """ stopwatch = StopWatch() stopwatch.start() @@ -125,12 +123,12 @@ def wait_for_bump(self, buttons, timeout_ms=None): def process(self, new_state=None): """ - Check for currenly pressed buttons. If the new state differs from the + Check for currenly pressed buttons. If the ``new_state`` differs from the old state, call the appropriate button event handlers (on_up, on_down, etc). """ if new_state is None: new_state = set(self.buttons_pressed) - old_state = self._state + old_state = self._state if hasattr(self, '_state') else set() self._state = new_state state_diff = new_state.symmetric_difference(old_state) @@ -146,9 +144,9 @@ def process(self, new_state=None): class EV3ButtonCommon(object): - # These handlers are called by `ButtonCommon.process()` whenever the + # These handlers are called by ButtonCommon.process() whenever the # state of 'up', 'down', etc buttons have changed since last - # `ButtonCommon.process()` call + # ButtonCommon.process() call on_up = None on_down = None on_left = None @@ -159,48 +157,48 @@ class EV3ButtonCommon(object): @property def up(self): """ - Check if 'up' button is pressed. + Check if ``up`` button is pressed. """ return 'up' in self.buttons_pressed @property def down(self): """ - Check if 'down' button is pressed. + Check if ``down`` button is pressed. """ return 'down' in self.buttons_pressed @property def left(self): """ - Check if 'left' button is pressed. + Check if ``left`` button is pressed. """ return 'left' in self.buttons_pressed @property def right(self): """ - Check if 'right' button is pressed. + Check if ``right`` button is pressed. """ return 'right' in self.buttons_pressed @property def enter(self): """ - Check if 'enter' button is pressed. + Check if ``enter`` button is pressed. """ return 'enter' in self.buttons_pressed @property def backspace(self): """ - Check if 'backspace' button is pressed. + Check if ``backspace`` button is pressed. """ return 'backspace' in self.buttons_pressed # micropython implementation -if is_micropython(): +if is_micropython(): # noqa: C901 try: # This is a linux-specific module. @@ -213,13 +211,11 @@ def backspace(self): if platform not in ("ev3", "fake"): raise Exception("micropython button support has not been implemented for '%s'" % platform) - def _test_bit(buf, index): byte = buf[int(index >> 3)] bit = byte & (1 << (index % 8)) return bool(bit) - class ButtonBase(ButtonCommon): pass @@ -241,12 +237,12 @@ class Button(ButtonCommon, EV3ButtonCommon): _BUTTON_DEV = '/dev/input/by-path/platform-gpio_keys-event' _BUTTON_TO_STRING = { - UP : "up", - DOWN : "down", - LEFT : "left", - RIGHT : "right", - ENTER : "enter", - BACK : "backspace", + UP: "up", + DOWN: "down", + LEFT: "left", + RIGHT: "right", + ENTER: "enter", + BACK: "backspace", } # stuff from linux/input.h and linux/input-event-codes.h @@ -285,10 +281,14 @@ def _wait(self, wait_for_button_press, wait_for_button_release, timeout_ms): # with the name of a single button. If it is a string of a single # button convert that to a list. if isinstance(wait_for_button_press, str): - wait_for_button_press = [wait_for_button_press, ] + wait_for_button_press = [ + wait_for_button_press, + ] if isinstance(wait_for_button_release, str): - wait_for_button_release = [wait_for_button_release, ] + wait_for_button_release = [ + wait_for_button_release, + ] while True: all_pressed = True @@ -311,6 +311,7 @@ def _wait(self, wait_for_button_press, wait_for_button_release, timeout_ms): if timeout_ms is not None and stopwatch.value_ms >= timeout_ms: return False + # python3 implementation else: import array @@ -331,7 +332,6 @@ def _wait(self, wait_for_button_press, wait_for_button_release, timeout_ms): except ImportError: log.warning(library_load_warning_message("evdev", "Button")) - class ButtonBase(ButtonCommon): """ Abstract button interface. @@ -356,7 +356,6 @@ def process_forever(self): if event.type == evdev.ecodes.EV_KEY: self.process() - class ButtonEVIO(ButtonBase): """ Provides a generic button reading mechanism that works with event interface @@ -420,10 +419,14 @@ def _wait(self, wait_for_button_press, wait_for_button_release, timeout_ms): # with the name of a single button. If it is a string of a single # button convert that to a list. if isinstance(wait_for_button_press, str): - wait_for_button_press = [wait_for_button_press, ] + wait_for_button_press = [ + wait_for_button_press, + ] if isinstance(wait_for_button_release, str): - wait_for_button_release = [wait_for_button_release, ] + wait_for_button_release = [ + wait_for_button_release, + ] for event in self.evdev_device.read_loop(): if event.type == evdev.ecodes.EV_KEY: @@ -447,18 +450,35 @@ def _wait(self, wait_for_button_press, wait_for_button_release, timeout_ms): if timeout_ms is not None and stopwatch.value_ms >= timeout_ms: return False - class Button(ButtonEVIO, EV3ButtonCommon): """ EV3 Buttons """ _buttons = { - 'up': {'name': BUTTONS_FILENAME, 'value': 103}, - 'down': {'name': BUTTONS_FILENAME, 'value': 108}, - 'left': {'name': BUTTONS_FILENAME, 'value': 105}, - 'right': {'name': BUTTONS_FILENAME, 'value': 106}, - 'enter': {'name': BUTTONS_FILENAME, 'value': 28}, - 'backspace': {'name': BUTTONS_FILENAME, 'value': 14}, + 'up': { + 'name': BUTTONS_FILENAME, + 'value': 103 + }, + 'down': { + 'name': BUTTONS_FILENAME, + 'value': 108 + }, + 'left': { + 'name': BUTTONS_FILENAME, + 'value': 105 + }, + 'right': { + 'name': BUTTONS_FILENAME, + 'value': 106 + }, + 'enter': { + 'name': BUTTONS_FILENAME, + 'value': 28 + }, + 'backspace': { + 'name': BUTTONS_FILENAME, + 'value': 14 + }, } evdev_device_name = EVDEV_DEVICE_NAME diff --git a/ev3dev2/console.py b/ev3dev2/console.py index d7c0324..4dec21e 100644 --- a/ev3dev2/console.py +++ b/ev3dev2/console.py @@ -27,14 +27,13 @@ class Console(): for cursor positioning, text color, and resetting the screen. Supports changing the console font using standard system fonts. """ - def __init__(self, font="Lat15-TerminusBold24x12"): """ Construct the Console instance, optionally with a font name specified. Parameter: - - `font` (string): Font name, as found in `/usr/share/consolefonts/` + - ``font`` (string): Font name, as found in ``/usr/share/consolefonts/`` """ self._font = None @@ -94,7 +93,7 @@ def cursor(self, value): def text_at(self, text, column=1, row=1, reset_console=False, inverse=False, alignment="L"): """ - Display `text` (string) at grid position (`column`, `row`). + Display ``text`` (string) at grid position (``column``, ``row``). Note that the grid locations are 1-based (not 0-based). Depending on the font, the number of columns and rows supported by the EV3 LCD console @@ -102,30 +101,30 @@ def text_at(self, text, column=1, row=1, reset_console=False, inverse=False, ali 44 columns and 21 rows. The default font for the Console() class results in a grid that is 14 columns and 5 rows. - Using the `inverse=True` parameter will display the `text` with more emphasis and contrast, + Using the ``inverse=True`` parameter will display the ``text`` with more emphasis and contrast, as the background of the text will be black, and the foreground is white. Using inverse can help in certain situations, such as to indicate when a color sensor senses black, or the gyro sensor is pointing to zero. - Use the `alignment` parameter to enable the function to align the `text` differently to the - column/row values passed-in. Use `L` for left-alignment (default), where the first character - in the `text` will show at the column/row position. Use `R` for right-alignment, where the - last character will show at the column/row position. Use `C` for center-alignment, where the + Use the ``alignment`` parameter to enable the function to align the ``text`` differently to the + column/row values passed-in. Use ``L`` for left-alignment (default), where the first character + in the ``text`` will show at the column/row position. Use ``R`` for right-alignment, where the + last character will show at the column/row position. Use ``C`` for center-alignment, where the text string will centered at the column/row position (as close as possible using integer division--odd-length text string will center better than even-length). Parameters: - - `text` (string): Text to display - - `column` (int): LCD column position to start the text (1 = left column); + - ``text`` (string): Text to display + - ``column`` (int): LCD column position to start the text (1 = left column); text will wrap when it reaches the right edge - - `row` (int): LCD row position to start the text (1 = top row) - - `reset_console` (bool): ``True`` to reset the EV3 LCD console before showing + - ``row`` (int): LCD row position to start the text (1 = top row) + - ``reset_console`` (bool): ``True`` to reset the EV3 LCD console before showing the text; default is ``False`` - - `inverse` (bool): ``True`` for white on black, otherwise black on white; + - ``inverse`` (bool): ``True`` for white on black, otherwise black on white; default is ``False`` - - `alignment` (string): Align the `text` horizontally. Use `L` for left-alignment (default), - `R` for right-alignment, or `C` for center-alignment + - ``alignment`` (string): Align the ``text`` horizontally. Use ``L`` for left-alignment (default), + ``R`` for right-alignment, or ``C`` for center-alignment """ @@ -149,8 +148,8 @@ def set_font(self, font="Lat15-TerminusBold24x12", reset_console=True): Parameters: - - `font` (string): Font name, as found in `/usr/share/consolefonts/` - - `reset_console` (bool): ``True`` to reset the EV3 LCD console + - ``font`` (string): Font name, as found in ``/usr/share/consolefonts/`` + - ``reset_console`` (bool): ``True`` to reset the EV3 LCD console after the font change; default is ``True`` """ @@ -166,13 +165,13 @@ def set_font(self, font="Lat15-TerminusBold24x12", reset_console=True): def clear_to_eol(self, column=None, row=None): """ - Clear to the end of line from the `column` and `row` position + Clear to the end of line from the ``column`` and ``row`` position on the EV3 LCD console. Default to current cursor position. Parameters: - - `column` (int): LCD column position to move to before clearing - - `row` (int): LCD row position to move to before clearing + - ``column`` (int): LCD column position to move to before clearing + - ``row`` (int): LCD row position to move to before clearing """ if column is not None and row is not None: diff --git a/ev3dev2/control/GyroBalancer.py b/ev3dev2/control/GyroBalancer.py index 1d4929e..6df1169 100644 --- a/ev3dev2/control/GyroBalancer.py +++ b/ev3dev2/control/GyroBalancer.py @@ -81,7 +81,6 @@ class GyroBalancer(object): Robot will keep its balance. """ - def __init__(self, gain_gyro_angle=1700, gain_gyro_rate=120, @@ -138,13 +137,10 @@ def __init__(self, # Open sensor and motor files self.gyro_file = open(self.gyro._path + "/value0", "rb") self.touch_file = open(self.touch._path + "/value0", "rb") - self.encoder_left_file = open(self.motor_left._path + "/position", - "rb") - self.encoder_right_file = open(self.motor_right._path + "/position", - "rb") + self.encoder_left_file = open(self.motor_left._path + "/position", "rb") + self.encoder_right_file = open(self.motor_right._path + "/position", "rb") self.dc_left_file = open(self.motor_left._path + "/duty_cycle_sp", "w") - self.dc_right_file = open(self.motor_right._path + "/duty_cycle_sp", - "w") + self.dc_right_file = open(self.motor_right._path + "/duty_cycle_sp", "w") # Drive queue self.drive_queue = queue.Queue() @@ -174,7 +170,7 @@ def shutdown(self): def _fast_read(self, infile): """Function for fast reading from sensor files.""" infile.seek(0) - return(int(infile.read().decode().strip())) + return (int(infile.read().decode().strip())) def _fast_write(self, outfile, value): """Function for fast writing to motor files.""" @@ -182,11 +178,10 @@ def _fast_write(self, outfile, value): outfile.write(str(int(value))) outfile.flush() - def _set_duty(self, motor_duty_file, duty, friction_offset, - voltage_comp): + def _set_duty(self, motor_duty_file, duty, friction_offset, voltage_comp): """Function to set the duty cycle of the motors.""" # Compensate for nominal voltage and round the input - duty_int = int(round(duty*voltage_comp)) + duty_int = int(round(duty * voltage_comp)) # Add or subtract offset and clamp the value between -100 and 100 if duty_int > 0: @@ -298,8 +293,7 @@ def _balance(self): voltage_comp = self.power_voltage_nominal / voltage_idle # Offset to limit friction deadlock - friction_offset = int(round(self.pwr_friction_offset_nom * - voltage_comp)) + friction_offset = int(round(self.pwr_friction_offset_nom * voltage_comp)) # Timing settings for the program # Time of each loop, measured in seconds. @@ -375,9 +369,8 @@ def _balance(self): touch_pressed = self._fast_read(self.touch_file) # Read the Motor Position - motor_angle_raw = ((self._fast_read(self.encoder_left_file) + - self._fast_read(self.encoder_right_file)) / - 2.0) + motor_angle_raw = ( + (self._fast_read(self.encoder_left_file) + self._fast_read(self.encoder_right_file)) / 2.0) motor_angle = motor_angle_raw * RAD_PER_RAW_MOTOR_UNIT # Read the Gyro @@ -385,7 +378,7 @@ def _balance(self): # Busy wait for the loop to reach target time length loop_time = 0 - while(loop_time < loop_time_target): + while (loop_time < loop_time_target): loop_time = time.time() - loop_start_time time.sleep(0.001) @@ -430,10 +423,8 @@ def _balance(self): motor_angle_error_acc) # Apply the signal to the motor, and add steering - self._set_duty(self.dc_right_file, motor_duty_cycle + steering, - friction_offset, voltage_comp) - self._set_duty(self.dc_left_file, motor_duty_cycle - steering, - friction_offset, voltage_comp) + self._set_duty(self.dc_right_file, motor_duty_cycle + steering, friction_offset, voltage_comp) + self._set_duty(self.dc_left_file, motor_duty_cycle - steering, friction_offset, voltage_comp) # Update angle estimate and gyro offset estimate gyro_est_angle = gyro_est_angle + gyro_rate *\ diff --git a/ev3dev2/control/rc_tank.py b/ev3dev2/control/rc_tank.py index 4e367b8..6b93a42 100644 --- a/ev3dev2/control/rc_tank.py +++ b/ev3dev2/control/rc_tank.py @@ -1,4 +1,3 @@ - import logging from ev3dev2.motor import MoveTank from ev3dev2.sensor.lego import InfraredSensor @@ -6,11 +5,11 @@ log = logging.getLogger(__name__) + # ============ # Tank classes # ============ class RemoteControlledTank(MoveTank): - def __init__(self, left_motor_port, right_motor_port, polarity='inversed', speed=400, channel=1): MoveTank.__init__(self, left_motor_port, right_motor_port) self.set_polarity(polarity) @@ -20,7 +19,7 @@ def __init__(self, left_motor_port, right_motor_port, polarity='inversed', speed self.speed_sp = speed self.remote = InfraredSensor() self.remote.on_channel1_top_left = self.make_move(left_motor, self.speed_sp) - self.remote.on_channel1_bottom_left = self.make_move(left_motor, self.speed_sp* -1) + self.remote.on_channel1_bottom_left = self.make_move(left_motor, self.speed_sp * -1) self.remote.on_channel1_top_right = self.make_move(right_motor, self.speed_sp) self.remote.on_channel1_bottom_right = self.make_move(right_motor, self.speed_sp * -1) self.channel = channel @@ -31,6 +30,7 @@ def move(state): motor.run_forever(speed_sp=dc_sp) else: motor.stop() + return move def main(self): diff --git a/ev3dev2/control/webserver.py b/ev3dev2/control/webserver.py index 36142bc..d148af4 100644 --- a/ev3dev2/control/webserver.py +++ b/ev3dev2/control/webserver.py @@ -24,13 +24,13 @@ class to handle REST APIish GETs via their do_GET() # File extension to mimetype mimetype = { - 'css' : 'text/css', - 'gif' : 'image/gif', - 'html' : 'text/html', - 'ico' : 'image/x-icon', - 'jpg' : 'image/jpg', - 'js' : 'application/javascript', - 'png' : 'image/png' + 'css': 'text/css', + 'gif': 'image/gif', + 'html': 'text/html', + 'ico': 'image/x-icon', + 'jpg': 'image/jpg', + 'js': 'application/javascript', + 'png': 'image/png' } def do_GET(self): @@ -83,8 +83,8 @@ def log_message(self, format, *args): medium_motor_max_speed = None joystick_engaged = False -class TankWebHandler(RobotWebHandler): +class TankWebHandler(RobotWebHandler): def __str__(self): return "%s-TankWebHandler" % self.robot @@ -108,7 +108,6 @@ def do_GET(self): medium_motor_max_speed = self.robot.medium_motor.max_speed else: medium_motor_max_speed = 0 - ''' Sometimes we get AJAX requests out of order like this: 2016-09-06 02:29:35,846 DEBUG: seq 65: (x, y): 0, 44 -> speed 462 462 @@ -145,8 +144,8 @@ def do_GET(self): speed_percentage = path[4] log.debug("seq %d: move %s" % (seq, direction)) - left_speed = int(int(speed_percentage) * motor_max_speed)/100.0 - right_speed = int(int(speed_percentage) * motor_max_speed)/100.0 + left_speed = int(int(speed_percentage) * motor_max_speed) / 100.0 + right_speed = int(int(speed_percentage) * motor_max_speed) / 100.0 if direction == 'forward': self.robot.left_motor.run_forever(speed_sp=left_speed) @@ -186,16 +185,17 @@ def do_GET(self): motor = path[3] direction = path[4] speed_percentage = path[5] - log.debug("seq %d: start motor %s, direction %s, speed_percentage %s" % (seq, motor, direction, speed_percentage)) + log.debug("seq %d: start motor %s, direction %s, speed_percentage %s" % + (seq, motor, direction, speed_percentage)) if motor == 'medium': if hasattr(self.robot, 'medium_motor'): if direction == 'clockwise': - medium_speed = int(int(speed_percentage) * medium_motor_max_speed)/100.0 + medium_speed = int(int(speed_percentage) * medium_motor_max_speed) / 100.0 self.robot.medium_motor.run_forever(speed_sp=medium_speed) elif direction == 'counter-clockwise': - medium_speed = int(int(speed_percentage) * medium_motor_max_speed)/100.0 + medium_speed = int(int(speed_percentage) * medium_motor_max_speed) / 100.0 self.robot.medium_motor.run_forever(speed_sp=medium_speed * -1) else: log.info("we do not have a medium_motor") @@ -213,18 +213,16 @@ def do_GET(self): max_move_xy_seq = seq log.debug("seq %d: (x, y) (%4d, %4d)" % (seq, x, y)) else: - log.debug("seq %d: (x, y) %4d, %4d (ignore, max seq %d)" % - (seq, x, y, max_move_xy_seq)) + log.debug("seq %d: (x, y) %4d, %4d (ignore, max seq %d)" % (seq, x, y, max_move_xy_seq)) else: - log.debug("seq %d: (x, y) %4d, %4d (ignore, joystick idle)" % - (seq, x, y)) + log.debug("seq %d: (x, y) %4d, %4d (ignore, joystick idle)" % (seq, x, y)) elif action == 'joystick-engaged': joystick_engaged = True elif action == 'log': msg = ''.join(path[3:]) - re_msg = re.search('^(.*)\?', msg) + re_msg = re.search(r'^(.*)\?', msg) if re_msg: msg = re_msg.group(1) @@ -247,7 +245,6 @@ class RobotWebServer(object): """ A Web server so that 'robot' can be controlled via 'handler_class' """ - def __init__(self, robot, handler_class, port_number=8000): self.content_server = None self.handler_class = handler_class @@ -277,7 +274,6 @@ class WebControlledTank(MoveJoystick): """ A tank that is controlled via a web browser """ - def __init__(self, left_motor, right_motor, port_number=8000, desc=None, motor_class=LargeMotor): MoveJoystick.__init__(self, left_motor, right_motor, desc, motor_class) self.www = RobotWebServer(self, TankWebHandler, port_number) diff --git a/ev3dev2/display.py b/ev3dev2/display.py index 41d4ccf..de875f2 100644 --- a/ev3dev2/display.py +++ b/ev3dev2/display.py @@ -24,10 +24,6 @@ # ----------------------------------------------------------------------------- import sys - -if sys.version_info < (3,4): - raise SystemError('Must be using Python 3.4 or higher') - import os import mmap import ctypes @@ -37,6 +33,9 @@ from . import get_current_platform, library_load_warning_message from struct import pack +if sys.version_info < (3, 4): + raise SystemError('Must be using Python 3.4 or higher') + log = logging.getLogger(__name__) try: @@ -49,7 +48,6 @@ class FbMem(object): - """The framebuffer memory object. Made of: @@ -83,7 +81,6 @@ class FbMem(object): FB_VISUAL_MONO10 = 1 class FixScreenInfo(ctypes.Structure): - """The fb_fix_screeninfo from fb.h.""" _fields_ = [ @@ -104,9 +101,7 @@ class FixScreenInfo(ctypes.Structure): ] class VarScreenInfo(ctypes.Structure): - class FbBitField(ctypes.Structure): - """The fb_bitfield struct from fb.h.""" _fields_ = [ @@ -128,10 +123,8 @@ def __str__(self): ('yres_virtual', ctypes.c_uint32), ('xoffset', ctypes.c_uint32), ('yoffset', ctypes.c_uint32), - ('bits_per_pixel', ctypes.c_uint32), ('grayscale', ctypes.c_uint32), - ('red', FbBitField), ('green', FbBitField), ('blue', FbBitField), @@ -140,8 +133,8 @@ def __str__(self): def __str__(self): return ("%sx%s at (%s,%s), bpp %s, grayscale %s, red %s, green %s, blue %s, transp %s" % - (self.xres, self.yres, self.xoffset, self.yoffset, self.bits_per_pixel, self.grayscale, - self.red, self.green, self.blue, self.transp)) + (self.xres, self.yres, self.xoffset, self.yoffset, self.bits_per_pixel, self.grayscale, self.red, + self.green, self.blue, self.transp)) def __init__(self, fbdev=None): """Create the FbMem framebuffer memory object.""" @@ -157,9 +150,8 @@ def __init__(self, fbdev=None): def _open_fbdev(fbdev=None): """Return the framebuffer file descriptor. - Try to use the FRAMEBUFFER - environment variable if fbdev is not given. Use '/dev/fb0' by - default. + Try to use the FRAMEBUFFER environment variable if fbdev is + not given. Use '/dev/fb0' by default. """ dev = fbdev or os.getenv('FRAMEBUFFER', '/dev/fb0') fbfid = os.open(dev, os.O_RDWR) @@ -182,13 +174,7 @@ def _get_var_info(fbfid): @staticmethod def _map_fb_memory(fbfid, fix_info): """Map the framebuffer memory.""" - return mmap.mmap( - fbfid, - fix_info.smem_len, - mmap.MAP_SHARED, - mmap.PROT_READ | mmap.PROT_WRITE, - offset=0 - ) + return mmap.mmap(fbfid, fix_info.smem_len, mmap.MAP_SHARED, mmap.PROT_READ | mmap.PROT_WRITE, offset=0) class Display(FbMem): @@ -218,12 +204,10 @@ def __init__(self, desc='Display'): else: raise Exception("Not supported - platform %s with bits_per_pixel %s" % - (self.platform, self.var_info.bits_per_pixel)) + (self.platform, self.var_info.bits_per_pixel)) - self._img = Image.new( - im_type, - (self.fix_info.line_length * 8 // self.var_info.bits_per_pixel, self.yres), - "white") + self._img = Image.new(im_type, (self.fix_info.line_length * 8 // self.var_info.bits_per_pixel, self.yres), + "white") self._draw = ImageDraw.Draw(self._img) self.desc = desc @@ -308,7 +292,7 @@ def update(self): else: raise Exception("Not supported - platform %s with bits_per_pixel %s" % - (self.platform, self.var_info.bits_per_pixel)) + (self.platform, self.var_info.bits_per_pixel)) def image_filename(self, filename, clear_screen=True, x1=0, y1=0, x2=None, y2=None): @@ -370,19 +354,19 @@ def point(self, clear_screen=True, x=10, y=10, point_color='black'): def text_pixels(self, text, clear_screen=True, x=0, y=0, text_color='black', font=None): """ - Display `text` starting at pixel (x, y). + Display ``text`` starting at pixel (x, y). The EV3 display is 178x128 pixels - (0, 0) would be the top left corner of the display - (89, 64) would be right in the middle of the display - 'text_color' : PIL says it supports "common HTML color names". There + ``text_color`` : PIL says it supports "common HTML color names". There are 140 HTML color names listed here that are supported by all modern browsers. This is probably a good list to start with. https://www.w3schools.com/colors/colors_names.asp - 'font' : can be any font displayed here + ``font`` : can be any font displayed here http://ev3dev-lang.readthedocs.io/projects/python-ev3dev/en/ev3dev-stretch/display.html#bitmap-fonts - If font is a string, it is the name of a font to be loaded. @@ -404,18 +388,18 @@ def text_pixels(self, text, clear_screen=True, x=0, y=0, text_color='black', fon def text_grid(self, text, clear_screen=True, x=0, y=0, text_color='black', font=None): """ - Display 'text' starting at grid (x, y) + Display ``text`` starting at grid (x, y) The EV3 display can be broken down in a grid that is 22 columns wide and 12 rows tall. Each column is 8 pixels wide and each row is 10 pixels tall. - 'text_color' : PIL says it supports "common HTML color names". There + ``text_color`` : PIL says it supports "common HTML color names". There are 140 HTML color names listed here that are supported by all modern browsers. This is probably a good list to start with. https://www.w3schools.com/colors/colors_names.asp - 'font' : can be any font displayed here + ``font`` : can be any font displayed here http://ev3dev-lang.readthedocs.io/projects/python-ev3dev/en/ev3dev-stretch/display.html#bitmap-fonts - If font is a string, it is the name of a font to be loaded. @@ -432,9 +416,7 @@ def text_grid(self, text, clear_screen=True, x=0, y=0, text_color='black', font= "grid rows must be between 0 and %d, %d was requested" %\ ((Display.GRID_ROWS - 1), y) - return self.text_pixels(text, clear_screen, - x * Display.GRID_COLUMN_PIXELS, - y * Display.GRID_ROW_PIXELS, + return self.text_pixels(text, clear_screen, x * Display.GRID_COLUMN_PIXELS, y * Display.GRID_ROW_PIXELS, text_color, font) def reset_screen(self): diff --git a/ev3dev2/fonts/__init__.py b/ev3dev2/fonts/__init__.py index 2ec97cd..95b4804 100644 --- a/ev3dev2/fonts/__init__.py +++ b/ev3dev2/fonts/__init__.py @@ -2,15 +2,16 @@ from glob import glob from PIL import ImageFont + def available(): """ Returns list of available font names. """ font_dir = os.path.dirname(__file__) - names = [os.path.basename(os.path.splitext(f)[0]) - for f in glob(os.path.join(font_dir, '*.pil'))] + names = [os.path.basename(os.path.splitext(f)[0]) for f in glob(os.path.join(font_dir, '*.pil'))] return sorted(names) + def load(name): """ Loads the font specified by name and returns it as an instance of @@ -20,8 +21,7 @@ def load(name): try: font_dir = os.path.dirname(__file__) pil_file = os.path.join(font_dir, '{}.pil'.format(name)) - pbm_file = os.path.join(font_dir, '{}.pbm'.format(name)) return ImageFont.load(pil_file) except FileNotFoundError: raise Exception('Failed to load font "{}". '.format(name) + - 'Check ev3dev.fonts.available() for the list of available fonts') + 'Check ev3dev.fonts.available() for the list of available fonts') diff --git a/ev3dev2/led.py b/ev3dev2/led.py index d6669cc..48e0409 100644 --- a/ev3dev2/led.py +++ b/ev3dev2/led.py @@ -24,10 +24,6 @@ # ----------------------------------------------------------------------------- import sys - -if sys.version_info < (3,4): - raise SystemError('Must be using Python 3.4 or higher') - import os import stat import time @@ -37,6 +33,9 @@ from ev3dev2.stopwatch import StopWatch from time import sleep +if sys.version_info < (3, 4): + raise SystemError('Must be using Python 3.4 or higher') + # Import the LED settings, this is platform specific platform = get_current_platform() @@ -72,18 +71,16 @@ class Led(Device): SYSTEM_CLASS_NAME = 'leds' SYSTEM_DEVICE_NAME_CONVENTION = '*' __slots__ = [ - '_max_brightness', - '_brightness', - '_triggers', - '_trigger', - '_delay_on', - '_delay_off', - 'desc', + '_max_brightness', + '_brightness', + '_triggers', + '_trigger', + '_delay_on', + '_delay_off', + 'desc', ] - def __init__(self, - name_pattern=SYSTEM_DEVICE_NAME_CONVENTION, name_exact=False, - desc=None, **kwargs): + def __init__(self, name_pattern=SYSTEM_DEVICE_NAME_CONVENTION, name_exact=False, desc=None, **kwargs): self.desc = desc super(Led, self).__init__(self.SYSTEM_CLASS_NAME, name_pattern, name_exact, **kwargs) self._max_brightness = None @@ -110,7 +107,7 @@ def max_brightness(self): @property def brightness(self): """ - Sets the brightness level. Possible values are from 0 to `max_brightness`. + Sets the brightness level. Possible values are from 0 to ``max_brightness``. """ self._brightness, value = self.get_attr_int(self._brightness, 'brightness') return value @@ -133,17 +130,17 @@ def trigger(self): Sets the LED trigger. A trigger is a kernel based source of LED events. Triggers can either be simple or complex. A simple trigger isn't configurable and is designed to slot into existing subsystems with - minimal additional code. Examples are the `ide-disk` and `nand-disk` + minimal additional code. Examples are the ``ide-disk`` and ``nand-disk`` triggers. Complex triggers whilst available to all LEDs have LED specific - parameters and work on a per LED basis. The `timer` trigger is an example. - The `timer` trigger will periodically change the LED brightness between - 0 and the current brightness setting. The `on` and `off` time can - be specified via `delay_{on,off}` attributes in milliseconds. + parameters and work on a per LED basis. The ``timer`` trigger is an example. + The ``timer`` trigger will periodically change the LED brightness between + 0 and the current brightness setting. The ``on`` and ``off`` time can + be specified via ``delay_{on,off}`` attributes in milliseconds. You can change the brightness value of a LED independently of the timer trigger. However, if you set the brightness value to 0 it will - also disable the `timer` trigger. + also disable the ``timer`` trigger. """ self._trigger, value = self.get_attr_from_set(self._trigger, 'trigger') return value @@ -179,9 +176,9 @@ def trigger(self, value): @property def delay_on(self): """ - The `timer` trigger will periodically change the LED brightness between - 0 and the current brightness setting. The `on` time can - be specified via `delay_on` attribute in milliseconds. + The ``timer`` trigger will periodically change the LED brightness between + 0 and the current brightness setting. The ``on`` time can + be specified via ``delay_on`` attribute in milliseconds. """ # Workaround for ev3dev/ev3dev#225. @@ -219,9 +216,9 @@ def delay_on(self, value): @property def delay_off(self): """ - The `timer` trigger will periodically change the LED brightness between - 0 and the current brightness setting. The `off` time can - be specified via `delay_off` attribute in milliseconds. + The ``timer`` trigger will periodically change the LED brightness between + 0 and the current brightness setting. The ``off`` time can + be specified via ``delay_off`` attribute in milliseconds. """ # Workaround for ev3dev/ev3dev#225. @@ -241,11 +238,13 @@ def delay_off(self): @delay_off.setter def delay_off(self, value): - # Workaround for ev3dev/ev3dev#225. - # 'delay_on' and 'delay_off' attributes are created when trigger is set - # to 'timer', and destroyed when it is set to anything else. - # This means the file cache may become outdated, and we may have to - # reopen the file. + """ + Workaround for ev3dev/ev3dev#225. + ``delay_on`` and ``delay_off`` attributes are created when trigger is set + to ``timer``, and destroyed when it is set to anything else. + This means the file cache may become outdated, and we may have to + reopen the file. + """ for retry in (True, False): try: self._delay_off = self.set_attr_int(self._delay_off, 'delay_off', value) @@ -269,7 +268,6 @@ def brightness_pct(self, value): class Leds(object): - def __init__(self): self.leds = OrderedDict() self.led_groups = OrderedDict() @@ -354,6 +352,8 @@ def all_off(self): if not self.leds: return + self.animate_stop() + for led in self.leds.values(): led.brightness = 0 @@ -381,7 +381,14 @@ def animate_stop(self): while self.animate_thread_id: pass - def animate_police_lights(self, color1, color2, group1='LEFT', group2='RIGHT', sleeptime=0.5, duration=5, block=True): + def animate_police_lights(self, + color1, + color2, + group1='LEFT', + group2='RIGHT', + sleeptime=0.5, + duration=5, + block=True): """ Cycle the ``group1`` and ``group2`` LEDs between ``color1`` and ``color2`` to give the effect of police lights. Alternate the ``group1`` and ``group2`` @@ -397,11 +404,10 @@ def animate_police_lights(self, color1, color2, group1='LEFT', group2='RIGHT', s leds = Leds() leds.animate_police_lights('RED', 'GREEN', sleeptime=0.75, duration=10) """ - def _animate_police_lights(): self.all_off() even = True - duration_ms = duration * 1000 + duration_ms = duration * 1000 if duration is not None else None stopwatch = StopWatch() stopwatch.start() @@ -413,7 +419,7 @@ def _animate_police_lights(): self.set_color(group1, color2) self.set_color(group2, color1) - if self.animate_thread_stop or stopwatch.value_ms >= duration_ms: + if self.animate_thread_stop or stopwatch.is_elapsed_ms(duration_ms): break even = not even @@ -443,10 +449,9 @@ def animate_flash(self, color, groups=('LEFT', 'RIGHT'), sleeptime=0.5, duration leds = Leds() leds.animate_flash('AMBER', sleeptime=0.75, duration=10) """ - def _animate_flash(): even = True - duration_ms = duration * 1000 + duration_ms = duration * 1000 if duration is not None else None stopwatch = StopWatch() stopwatch.start() @@ -457,7 +462,7 @@ def _animate_flash(): else: self.all_off() - if self.animate_thread_stop or stopwatch.value_ms >= duration_ms: + if self.animate_thread_stop or stopwatch.is_elapsed_ms(duration_ms): break even = not even @@ -486,12 +491,12 @@ def animate_cycle(self, colors, groups=('LEFT', 'RIGHT'), sleeptime=0.5, duratio from ev3dev2.led import Leds leds = Leds() - leds.animate_cyle(('RED', 'GREEN', 'AMBER')) + leds.animate_cycle(('RED', 'GREEN', 'AMBER')) """ def _animate_cycle(): index = 0 max_index = len(colors) - duration_ms = duration * 1000 + duration_ms = duration * 1000 if duration is not None else None stopwatch = StopWatch() stopwatch.start() @@ -504,7 +509,7 @@ def _animate_cycle(): if index == max_index: index = 0 - if self.animate_thread_stop or stopwatch.value_ms >= duration_ms: + if self.animate_thread_stop or stopwatch.is_elapsed_ms(duration_ms): break sleep(sleeptime) @@ -533,7 +538,6 @@ def animate_rainbow(self, group1='LEFT', group2='RIGHT', increment_by=0.1, sleep leds = Leds() leds.animate_rainbow() """ - def _animate_rainbow(): # state 0: (LEFT,RIGHT) from (0,0) to (1,0)...RED # state 1: (LEFT,RIGHT) from (1,0) to (1,1)...AMBER @@ -545,7 +549,7 @@ def _animate_rainbow(): MIN_VALUE = 0 MAX_VALUE = 1 self.all_off() - duration_ms = duration * 1000 + duration_ms = duration * 1000 if duration is not None else None stopwatch = StopWatch() stopwatch.start() @@ -580,7 +584,7 @@ def _animate_rainbow(): elif state == 3 and right_value == MIN_VALUE: state = 0 - if self.animate_thread_stop or stopwatch.value_ms >= duration_ms: + if self.animate_thread_stop or stopwatch.is_elapsed_ms(duration_ms): break sleep(sleeptime) diff --git a/ev3dev2/motor.py b/ev3dev2/motor.py index 4a8e430..6e61ecf 100644 --- a/ev3dev2/motor.py +++ b/ev3dev2/motor.py @@ -21,10 +21,6 @@ # ----------------------------------------------------------------------------- import sys - -if sys.version_info < (3,4): - raise SystemError('Must be using Python 3.4 or higher') - import math import select import time @@ -39,43 +35,48 @@ from logging import getLogger from os.path import abspath -from ev3dev2 import get_current_platform, Device, list_device_names +from ev3dev2 import get_current_platform, Device, list_device_names, DeviceNotDefined, ThreadNotRunning from ev3dev2.stopwatch import StopWatch -log = getLogger(__name__) - -# The number of milliseconds we wait for the state of a motor to -# update to 'running' in the "on_for_XYZ" methods of the Motor class -WAIT_RUNNING_TIMEOUT = 100 - - # OUTPUT ports have platform specific values that we must import platform = get_current_platform() if platform == 'ev3': - from ev3dev2._platform.ev3 import OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D + from ev3dev2._platform.ev3 import OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D # noqa: F401 elif platform == 'evb': - from ev3dev2._platform.evb import OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D + from ev3dev2._platform.evb import OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D # noqa: F401 elif platform == 'pistorms': - from ev3dev2._platform.pistorms import OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D + from ev3dev2._platform.pistorms import OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D # noqa: F401 elif platform == 'brickpi': - from ev3dev2._platform.brickpi import OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D + from ev3dev2._platform.brickpi import OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D # noqa: F401 elif platform == 'brickpi3': - from ev3dev2._platform.brickpi3 import OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D, \ - OUTPUT_E, OUTPUT_F, OUTPUT_G, OUTPUT_H, \ - OUTPUT_I, OUTPUT_J, OUTPUT_K, OUTPUT_L, \ - OUTPUT_M, OUTPUT_N, OUTPUT_O, OUTPUT_P + from ev3dev2._platform.brickpi3 import ( # noqa: F401 + OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D, OUTPUT_E, OUTPUT_F, OUTPUT_G, OUTPUT_H, OUTPUT_I, OUTPUT_J, OUTPUT_K, + OUTPUT_L, OUTPUT_M, OUTPUT_N, OUTPUT_O, OUTPUT_P) elif platform == 'fake': - from ev3dev2._platform.fake import OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D + from ev3dev2._platform.fake import OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D # noqa: F401 else: raise Exception("Unsupported platform '%s'" % platform) +if sys.version_info < (3, 4): + raise SystemError('Must be using Python 3.4 or higher') + +log = getLogger(__name__) + +# The number of milliseconds we wait for the state of a motor to +# update to 'running' in the "on_for_XYZ" methods of the Motor class +WAIT_RUNNING_TIMEOUT = 100 + + +class SpeedInvalid(ValueError): + pass + class SpeedValue(object): """ @@ -83,7 +84,6 @@ class SpeedValue(object): :class:`SpeedPercent`, :class:`SpeedRPS`, :class:`SpeedRPM`, :class:`SpeedDPS`, and :class:`SpeedDPM`. """ - def __eq__(self, other): return self.to_native_units() == other.to_native_units() @@ -110,15 +110,14 @@ class SpeedPercent(SpeedValue): """ Speed as a percentage of the motor's maximum rated speed. """ - - def __init__(self, percent): - assert -100 <= percent <= 100,\ - "{} is an invalid percentage, must be between -100 and 100 (inclusive)".format(percent) - + def __init__(self, percent, desc=None): + if percent < -100 or percent > 100: + raise SpeedInvalid("invalid percentage {}, must be between -100 and 100 (inclusive)".format(percent)) self.percent = percent + self.desc = desc def __str__(self): - return str(self.percent) + "%" + return "{} ".format(self.desc) if self.desc else "" + str(self.percent) + "%" def __mul__(self, other): assert isinstance(other, (float, int)), "{} can only be multiplied by an int or float".format(self) @@ -135,12 +134,12 @@ class SpeedNativeUnits(SpeedValue): """ Speed in tacho counts per second. """ - - def __init__(self, native_counts): + def __init__(self, native_counts, desc=None): self.native_counts = native_counts + self.desc = desc def __str__(self): - return "{:.2f}".format(self.native_counts) + " counts/sec" + return "{} ".format(self.desc) if self.desc else "" + "{:.2f}".format(self.native_counts) + " counts/sec" def __mul__(self, other): assert isinstance(other, (float, int)), "{} can only be multiplied by an int or float".format(self) @@ -150,6 +149,9 @@ def to_native_units(self, motor=None): """ Return this SpeedNativeUnits as a number """ + if self.native_counts > motor.max_speed: + raise SpeedInvalid("invalid native-units: {} max speed {}, {} was requested".format( + motor, motor.max_speed, self.native_counts)) return self.native_counts @@ -157,12 +159,12 @@ class SpeedRPS(SpeedValue): """ Speed in rotations-per-second. """ - - def __init__(self, rotations_per_second): + def __init__(self, rotations_per_second, desc=None): self.rotations_per_second = rotations_per_second + self.desc = desc def __str__(self): - return str(self.rotations_per_second) + " rot/sec" + return "{} ".format(self.desc) if self.desc else "" + str(self.rotations_per_second) + " rot/sec" def __mul__(self, other): assert isinstance(other, (float, int)), "{} can only be multiplied by an int or float".format(self) @@ -172,22 +174,22 @@ def to_native_units(self, motor): """ Return the native speed measurement required to achieve desired rotations-per-second """ - assert abs(self.rotations_per_second) <= motor.max_rps,\ - "invalid rotations-per-second: {} max RPS is {}, {} was requested".format( - motor, motor.max_rps, self.rotations_per_second) - return self.rotations_per_second/motor.max_rps * motor.max_speed + if abs(self.rotations_per_second) > motor.max_rps: + raise SpeedInvalid("invalid rotations-per-second: {} max RPS is {}, {} was requested".format( + motor, motor.max_rps, self.rotations_per_second)) + return self.rotations_per_second / motor.max_rps * motor.max_speed class SpeedRPM(SpeedValue): """ Speed in rotations-per-minute. """ - - def __init__(self, rotations_per_minute): + def __init__(self, rotations_per_minute, desc=None): self.rotations_per_minute = rotations_per_minute + self.desc = desc def __str__(self): - return str(self.rotations_per_minute) + " rot/min" + return "{} ".format(self.desc) if self.desc else "" + str(self.rotations_per_minute) + " rot/min" def __mul__(self, other): assert isinstance(other, (float, int)), "{} can only be multiplied by an int or float".format(self) @@ -197,22 +199,22 @@ def to_native_units(self, motor): """ Return the native speed measurement required to achieve desired rotations-per-minute """ - assert abs(self.rotations_per_minute) <= motor.max_rpm,\ - "invalid rotations-per-minute: {} max RPM is {}, {} was requested".format( - motor, motor.max_rpm, self.rotations_per_minute) - return self.rotations_per_minute/motor.max_rpm * motor.max_speed + if abs(self.rotations_per_minute) > motor.max_rpm: + raise SpeedInvalid("invalid rotations-per-minute: {} max RPM is {}, {} was requested".format( + motor, motor.max_rpm, self.rotations_per_minute)) + return self.rotations_per_minute / motor.max_rpm * motor.max_speed class SpeedDPS(SpeedValue): """ Speed in degrees-per-second. """ - - def __init__(self, degrees_per_second): + def __init__(self, degrees_per_second, desc=None): self.degrees_per_second = degrees_per_second + self.desc = desc def __str__(self): - return str(self.degrees_per_second) + " deg/sec" + return "{} ".format(self.desc) if self.desc else "" + str(self.degrees_per_second) + " deg/sec" def __mul__(self, other): assert isinstance(other, (float, int)), "{} can only be multiplied by an int or float".format(self) @@ -222,22 +224,22 @@ def to_native_units(self, motor): """ Return the native speed measurement required to achieve desired degrees-per-second """ - assert abs(self.degrees_per_second) <= motor.max_dps,\ - "invalid degrees-per-second: {} max DPS is {}, {} was requested".format( - motor, motor.max_dps, self.degrees_per_second) - return self.degrees_per_second/motor.max_dps * motor.max_speed + if abs(self.degrees_per_second) > motor.max_dps: + raise SpeedInvalid("invalid degrees-per-second: {} max DPS is {}, {} was requested".format( + motor, motor.max_dps, self.degrees_per_second)) + return self.degrees_per_second / motor.max_dps * motor.max_speed class SpeedDPM(SpeedValue): """ Speed in degrees-per-minute. """ - - def __init__(self, degrees_per_minute): + def __init__(self, degrees_per_minute, desc=None): self.degrees_per_minute = degrees_per_minute + self.desc = desc def __str__(self): - return str(self.degrees_per_minute) + " deg/min" + return "{} ".format(self.desc) if self.desc else "" + str(self.degrees_per_minute) + " deg/min" def __mul__(self, other): assert isinstance(other, (float, int)), "{} can only be multiplied by an int or float".format(self) @@ -247,83 +249,93 @@ def to_native_units(self, motor): """ Return the native speed measurement required to achieve desired degrees-per-minute """ - assert abs(self.degrees_per_minute) <= motor.max_dpm,\ - "invalid degrees-per-minute: {} max DPM is {}, {} was requested".format( - motor, motor.max_dpm, self.degrees_per_minute) - return self.degrees_per_minute/motor.max_dpm * motor.max_speed + if abs(self.degrees_per_minute) > motor.max_dpm: + raise SpeedInvalid("invalid degrees-per-minute: {} max DPM is {}, {} was requested".format( + motor, motor.max_dpm, self.degrees_per_minute)) + return self.degrees_per_minute / motor.max_dpm * motor.max_speed -class Motor(Device): +def speed_to_speedvalue(speed, desc=None): + """ + If ``speed`` is not a ``SpeedValue`` object, treat it as a percentage. + Returns a ``SpeedValue`` object. + """ + if isinstance(speed, SpeedValue): + return speed + else: + return SpeedPercent(speed, desc) + +class Motor(Device): """ The motor class provides a uniform interface for using motors with positional and directional feedback such as the EV3 and NXT motors. This feedback allows for precise control of the motors. This is the - most common type of motor, so we just call it `motor`. + most common type of motor, so we just call it ``motor``. """ SYSTEM_CLASS_NAME = 'tacho-motor' SYSTEM_DEVICE_NAME_CONVENTION = '*' __slots__ = [ - '_address', - '_command', - '_commands', - '_count_per_rot', - '_count_per_m', - '_driver_name', - '_duty_cycle', - '_duty_cycle_sp', - '_full_travel_count', - '_polarity', - '_position', - '_position_p', - '_position_i', - '_position_d', - '_position_sp', - '_max_speed', - '_speed', - '_speed_sp', - '_ramp_up_sp', - '_ramp_down_sp', - '_speed_p', - '_speed_i', - '_speed_d', - '_state', - '_stop_action', - '_stop_actions', - '_time_sp', - '_poll', - 'max_rps', - 'max_rpm', - 'max_dps', - 'max_dpm', + '_address', + '_command', + '_commands', + '_count_per_rot', + '_count_per_m', + '_driver_name', + '_duty_cycle', + '_duty_cycle_sp', + '_full_travel_count', + '_polarity', + '_position', + '_position_p', + '_position_i', + '_position_d', + '_position_sp', + '_max_speed', + '_speed', + '_speed_sp', + '_ramp_up_sp', + '_ramp_down_sp', + '_speed_p', + '_speed_i', + '_speed_d', + '_state', + '_stop_action', + '_stop_actions', + '_time_sp', + '_poll', + 'max_rps', + 'max_rpm', + 'max_dps', + 'max_dpm', ] #: Run the motor until another command is sent. COMMAND_RUN_FOREVER = 'run-forever' - #: Run to an absolute position specified by `position_sp` and then - #: stop using the action specified in `stop_action`. + #: Run to an absolute position specified by ``position_sp`` and then + #: stop using the action specified in ``stop_action``. COMMAND_RUN_TO_ABS_POS = 'run-to-abs-pos' - #: Run to a position relative to the current `position` value. - #: The new position will be current `position` + `position_sp`. + #: Run to a position relative to the current ``position`` value. + #: The new position will be current ``position`` + ``position_sp``. #: When the new position is reached, the motor will stop using - #: the action specified by `stop_action`. + #: the action specified by ``stop_action``. COMMAND_RUN_TO_REL_POS = 'run-to-rel-pos' - #: Run the motor for the amount of time specified in `time_sp` - #: and then stop the motor using the action specified by `stop_action`. + #: Run the motor for the amount of time specified in ``time_sp`` + #: and then stop the motor using the action specified by ``stop_action``. COMMAND_RUN_TIMED = 'run-timed' - #: Run the motor at the duty cycle specified by `duty_cycle_sp`. - #: Unlike other run commands, changing `duty_cycle_sp` while running *will* + #: Run the motor at the duty cycle specified by ``duty_cycle_sp``. + #: Unlike other run commands, changing ``duty_cycle_sp`` while running *will* #: take effect immediately. COMMAND_RUN_DIRECT = 'run-direct' #: Stop any of the run commands before they are complete using the - #: action specified by `stop_action`. + #: action specified by ``stop_action``. COMMAND_STOP = 'stop' #: Reset all of the motor parameter attributes to their default value. @@ -336,11 +348,11 @@ class Motor(Device): #: Sets the inversed polarity of the rotary encoder. ENCODER_POLARITY_INVERSED = 'inversed' - #: With `normal` polarity, a positive duty cycle will + #: With ``normal`` polarity, a positive duty cycle will #: cause the motor to rotate clockwise. POLARITY_NORMAL = 'normal' - #: With `inversed` polarity, a positive duty cycle will + #: With ``inversed`` polarity, a positive duty cycle will #: cause the motor to rotate counter-clockwise. POLARITY_INVERSED = 'inversed' @@ -353,7 +365,7 @@ class Motor(Device): #: The motor is not turning, but rather attempting to hold a fixed position. STATE_HOLDING = 'holding' - #: The motor is turning, but cannot reach its `speed_sp`. + #: The motor is turning, but cannot reach its ``speed_sp``. STATE_OVERLOADED = 'overloaded' #: The motor is not turning when it should be. @@ -370,7 +382,7 @@ class Motor(Device): #: Does not remove power from the motor. Instead it actively try to hold the motor #: at the current position. If an external force tries to turn the motor, the motor - #: will `push back` to maintain its position. + #: will ``push back`` to maintain its position. STOP_ACTION_HOLD = 'hold' def __init__(self, address=None, name_pattern=SYSTEM_DEVICE_NAME_CONVENTION, name_exact=False, **kwargs): @@ -410,7 +422,7 @@ def __init__(self, address=None, name_pattern=SYSTEM_DEVICE_NAME_CONVENTION, nam self._stop_actions = None self._time_sp = None self._poll = None - self.max_rps = float(self.max_speed/self.count_per_rot) + self.max_rps = float(self.max_speed / self.count_per_rot) self.max_rpm = self.max_rps * 60 self.max_dps = self.max_rps * 360 self.max_dpm = self.max_rpm * 360 @@ -426,7 +438,7 @@ def address(self): @property def command(self): """ - Sends a command to the motor controller. See `commands` for a list of + Sends a command to the motor controller. See ``commands`` for a list of possible values. """ raise Exception("command is a write-only property!") @@ -439,23 +451,23 @@ def command(self, value): def commands(self): """ Returns a list of commands that are supported by the motor - controller. Possible values are `run-forever`, `run-to-abs-pos`, `run-to-rel-pos`, - `run-timed`, `run-direct`, `stop` and `reset`. Not all commands may be supported. - - - `run-forever` will cause the motor to run until another command is sent. - - `run-to-abs-pos` will run to an absolute position specified by `position_sp` - and then stop using the action specified in `stop_action`. - - `run-to-rel-pos` will run to a position relative to the current `position` value. - The new position will be current `position` + `position_sp`. When the new - position is reached, the motor will stop using the action specified by `stop_action`. - - `run-timed` will run the motor for the amount of time specified in `time_sp` - and then stop the motor using the action specified by `stop_action`. - - `run-direct` will run the motor at the duty cycle specified by `duty_cycle_sp`. - Unlike other run commands, changing `duty_cycle_sp` while running *will* + controller. Possible values are ``run-forever``, ``run-to-abs-pos``, ``run-to-rel-pos``, + ``run-timed``, ``run-direct``, ``stop`` and ``reset``. Not all commands may be supported. + + - ``run-forever`` will cause the motor to run until another command is sent. + - ``run-to-abs-pos`` will run to an absolute position specified by ``position_sp`` + and then stop using the action specified in ``stop_action``. + - ``run-to-rel-pos`` will run to a position relative to the current ``position`` value. + The new position will be current ``position`` + ``position_sp``. When the new + position is reached, the motor will stop using the action specified by ``stop_action``. + - ``run-timed`` will run the motor for the amount of time specified in ``time_sp`` + and then stop the motor using the action specified by ``stop_action``. + - ``run-direct`` will run the motor at the duty cycle specified by ``duty_cycle_sp``. + Unlike other run commands, changing ``duty_cycle_sp`` while running *will* take effect immediately. - - `stop` will stop any of the run commands before they are complete using the - action specified by `stop_action`. - - `reset` will reset all of the motor parameter attributes to their default value. + - ``stop`` will stop any of the run commands before they are complete using the + action specified by ``stop_action``. + - ``reset`` will reset all of the motor parameter attributes to their default value. This will also have the effect of stopping the motor. """ (self._commands, value) = self.get_cached_attr_set(self._commands, 'commands') @@ -516,7 +528,7 @@ def duty_cycle_sp(self, value): def full_travel_count(self): """ Returns the number of tacho counts in the full travel of the motor. When - combined with the `count_per_m` atribute, you can use this value to + combined with the ``count_per_m`` atribute, you can use this value to calculate the maximum travel distance of the motor. (linear motors only) """ (self._full_travel_count, value) = self.get_cached_attr_int(self._full_travel_count, 'full_travel_count') @@ -525,10 +537,10 @@ def full_travel_count(self): @property def polarity(self): """ - Sets the polarity of the motor. With `normal` polarity, a positive duty - cycle will cause the motor to rotate clockwise. With `inversed` polarity, + Sets the polarity of the motor. With ``normal`` polarity, a positive duty + cycle will cause the motor to rotate clockwise. With ``inversed`` polarity, a positive duty cycle will cause the motor to rotate counter-clockwise. - Valid values are `normal` and `inversed`. + Valid values are ``normal`` and ``inversed``. """ self._polarity, value = self.get_attr_string(self._polarity, 'polarity') return value @@ -591,9 +603,9 @@ def position_d(self, value): @property def position_sp(self): """ - Writing specifies the target position for the `run-to-abs-pos` and `run-to-rel-pos` + Writing specifies the target position for the ``run-to-abs-pos`` and ``run-to-rel-pos`` commands. Reading returns the current value. Units are in tacho counts. You - can use the value returned by `count_per_rot` to convert tacho counts to/from + can use the value returned by ``count_per_rot`` to convert tacho counts to/from rotations or degrees. """ self._position_sp, value = self.get_attr_int(self._position_sp, 'position_sp') @@ -606,7 +618,7 @@ def position_sp(self, value): @property def max_speed(self): """ - Returns the maximum value that is accepted by the `speed_sp` attribute. This + Returns the maximum value that is accepted by the ``speed_sp`` attribute. This may be slightly different than the maximum speed that a particular motor can reach - it's the maximum theoretical speed. """ @@ -617,7 +629,7 @@ def max_speed(self): def speed(self): """ Returns the current motor speed in tacho counts per second. Note, this is - not necessarily degrees (although it is for LEGO motors). Use the `count_per_rot` + not necessarily degrees (although it is for LEGO motors). Use the ``count_per_rot`` attribute to convert this value to RPM or deg/sec. """ self._speed, value = self.get_attr_int(self._speed, 'speed') @@ -626,11 +638,11 @@ def speed(self): @property def speed_sp(self): """ - Writing sets the target speed in tacho counts per second used for all `run-*` - commands except `run-direct`. Reading returns the current value. A negative - value causes the motor to rotate in reverse with the exception of `run-to-*-pos` - commands where the sign is ignored. Use the `count_per_rot` attribute to convert - RPM or deg/sec to tacho counts per second. Use the `count_per_m` attribute to + Writing sets the target speed in tacho counts per second used for all ``run-*`` + commands except ``run-direct``. Reading returns the current value. A negative + value causes the motor to rotate in reverse with the exception of ``run-to-*-pos`` + commands where the sign is ignored. Use the ``count_per_rot`` attribute to convert + RPM or deg/sec to tacho counts per second. Use the ``count_per_m`` attribute to convert m/s to tacho counts per second. """ self._speed_sp, value = self.get_attr_int(self._speed_sp, 'speed_sp') @@ -645,9 +657,9 @@ def ramp_up_sp(self): """ Writing sets the ramp up setpoint. Reading returns the current value. Units are in milliseconds and must be positive. When set to a non-zero value, the - motor speed will increase from 0 to 100% of `max_speed` over the span of this + motor speed will increase from 0 to 100% of ``max_speed`` over the span of this setpoint. The actual ramp time is the ratio of the difference between the - `speed_sp` and the current `speed` and max_speed multiplied by `ramp_up_sp`. + ``speed_sp`` and the current ``speed`` and max_speed multiplied by ``ramp_up_sp``. """ self._ramp_up_sp, value = self.get_attr_int(self._ramp_up_sp, 'ramp_up_sp') return value @@ -661,9 +673,9 @@ def ramp_down_sp(self): """ Writing sets the ramp down setpoint. Reading returns the current value. Units are in milliseconds and must be positive. When set to a non-zero value, the - motor speed will decrease from 0 to 100% of `max_speed` over the span of this + motor speed will decrease from 0 to 100% of ``max_speed`` over the span of this setpoint. The actual ramp time is the ratio of the difference between the - `speed_sp` and the current `speed` and max_speed multiplied by `ramp_down_sp`. + ``speed_sp`` and the current ``speed`` and max_speed multiplied by ``ramp_down_sp``. """ self._ramp_down_sp, value = self.get_attr_int(self._ramp_down_sp, 'ramp_down_sp') return value @@ -712,7 +724,7 @@ def speed_d(self, value): def state(self): """ Reading returns a list of state flags. Possible flags are - `running`, `ramping`, `holding`, `overloaded` and `stalled`. + ``running``, ``ramping``, ``holding``, ``overloaded`` and ``stalled``. """ self._state, value = self.get_attr_set(self._state, 'state') return value @@ -721,9 +733,9 @@ def state(self): def stop_action(self): """ Reading returns the current stop action. Writing sets the stop action. - The value determines the motors behavior when `command` is set to `stop`. + The value determines the motors behavior when ``command`` is set to ``stop``. Also, it determines the motors behavior when a run command completes. See - `stop_actions` for a list of possible values. + ``stop_actions`` for a list of possible values. """ self._stop_action, value = self.get_attr_string(self._stop_action, 'stop_action') return value @@ -736,12 +748,12 @@ def stop_action(self, value): def stop_actions(self): """ Returns a list of stop actions supported by the motor controller. - Possible values are `coast`, `brake` and `hold`. `coast` means that power will - be removed from the motor and it will freely coast to a stop. `brake` means + Possible values are ``coast``, ``brake`` and ``hold``. ``coast`` means that power will + be removed from the motor and it will freely coast to a stop. ``brake`` means that power will be removed from the motor and a passive electrical load will be placed on the motor. This is usually done by shorting the motor terminals together. This load will absorb the energy from the rotation of the motors and - cause the motor to stop more quickly than coasting. `hold` does not remove + cause the motor to stop more quickly than coasting. ``hold`` does not remove power from the motor. Instead it actively tries to hold the motor at the current position. If an external force tries to turn the motor, the motor will 'push back' to maintain its position. @@ -753,7 +765,7 @@ def stop_actions(self): def time_sp(self): """ Writing specifies the amount of time the motor will run when using the - `run-timed` command. Reading returns the current value. Units are in + ``run-timed`` command. Reading returns the current value. Units are in milliseconds. """ self._time_sp, value = self.get_attr_int(self._time_sp, 'time_sp') @@ -773,8 +785,8 @@ def run_forever(self, **kwargs): def run_to_abs_pos(self, **kwargs): """ - Run to an absolute position specified by `position_sp` and then - stop using the action specified in `stop_action`. + Run to an absolute position specified by ``position_sp`` and then + stop using the action specified in ``stop_action``. """ for key in kwargs: setattr(self, key, kwargs[key]) @@ -782,10 +794,10 @@ def run_to_abs_pos(self, **kwargs): def run_to_rel_pos(self, **kwargs): """ - Run to a position relative to the current `position` value. - The new position will be current `position` + `position_sp`. + Run to a position relative to the current ``position`` value. + The new position will be current ``position`` + ``position_sp``. When the new position is reached, the motor will stop using - the action specified by `stop_action`. + the action specified by ``stop_action``. """ for key in kwargs: setattr(self, key, kwargs[key]) @@ -793,8 +805,8 @@ def run_to_rel_pos(self, **kwargs): def run_timed(self, **kwargs): """ - Run the motor for the amount of time specified in `time_sp` - and then stop the motor using the action specified by `stop_action`. + Run the motor for the amount of time specified in ``time_sp`` + and then stop the motor using the action specified by ``stop_action``. """ for key in kwargs: setattr(self, key, kwargs[key]) @@ -802,8 +814,8 @@ def run_timed(self, **kwargs): def run_direct(self, **kwargs): """ - Run the motor at the duty cycle specified by `duty_cycle_sp`. - Unlike other run commands, changing `duty_cycle_sp` while running *will* + Run the motor at the duty cycle specified by ``duty_cycle_sp``. + Unlike other run commands, changing ``duty_cycle_sp`` while running *will* take effect immediately. """ for key in kwargs: @@ -813,7 +825,7 @@ def run_direct(self, **kwargs): def stop(self, **kwargs): """ Stop any of the run commands before they are complete using the - action specified by `stop_action`. + action specified by ``stop_action``. """ for key in kwargs: setattr(self, key, kwargs[key]) @@ -852,7 +864,7 @@ def is_holding(self): @property def is_overloaded(self): """ - The motor is turning, but cannot reach its `speed_sp`. + The motor is turning, but cannot reach its ``speed_sp``. """ return self.STATE_OVERLOADED in self.state @@ -946,20 +958,14 @@ def wait_while(self, s, timeout=None): return self.wait(lambda state: s not in state, timeout) def _speed_native_units(self, speed, label=None): - - # If speed is not a SpeedValue object we treat it as a percentage - if not isinstance(speed, SpeedValue): - assert -100 <= speed <= 100,\ - "{}{} is an invalid speed percentage, must be between -100 and 100 (inclusive)".format("" if label is None else (label + ": ") , speed) - speed = SpeedPercent(speed) - + speed = speed_to_speedvalue(speed, label) return speed.to_native_units(self) def _set_rel_position_degrees_and_speed_sp(self, degrees, speed): degrees = degrees if speed >= 0 else -degrees speed = abs(speed) - position_delta = int(round((degrees * self.count_per_rot)/360)) + position_delta = int(round((degrees * self.count_per_rot) / 360)) speed_sp = int(round(speed)) self.position_sp = position_delta @@ -1048,8 +1054,8 @@ def on(self, speed, brake=True, block=False): ``speed`` can be a percentage or a :class:`ev3dev2.motor.SpeedValue` object, enabling use of other units. - Note that `block` is False by default, this is different from the - other `on_for_XYZ` methods. + Note that ``block`` is False by default, this is different from the + other ``on_for_XYZ`` methods. """ speed = self._speed_native_units(speed) self.speed_sp = int(round(speed)) @@ -1089,11 +1095,10 @@ def list_motors(name_pattern=Motor.SYSTEM_DEVICE_NAME_CONVENTION, **kwargs): """ class_path = abspath(Device.DEVICE_ROOT_PATH + '/' + Motor.SYSTEM_CLASS_NAME) - return (Motor(name_pattern=name, name_exact=True) - for name in list_device_names(class_path, name_pattern, **kwargs)) + return (Motor(name_pattern=name, name_exact=True) for name in list_device_names(class_path, name_pattern, **kwargs)) -class LargeMotor(Motor): +class LargeMotor(Motor): """ EV3/NXT large servo motor. @@ -1106,11 +1111,14 @@ class LargeMotor(Motor): def __init__(self, address=None, name_pattern=SYSTEM_DEVICE_NAME_CONVENTION, name_exact=False, **kwargs): - super(LargeMotor, self).__init__(address, name_pattern, name_exact, driver_name=['lego-ev3-l-motor', 'lego-nxt-motor'], **kwargs) + super(LargeMotor, self).__init__(address, + name_pattern, + name_exact, + driver_name=['lego-ev3-l-motor', 'lego-nxt-motor'], + **kwargs) class MediumMotor(Motor): - """ EV3 medium servo motor. @@ -1127,11 +1135,11 @@ def __init__(self, address=None, name_pattern=SYSTEM_DEVICE_NAME_CONVENTION, nam class ActuonixL1250Motor(Motor): - """ Actuonix L12 50 linear servo motor. - Same as :class:`Motor`, except it will only successfully initialize if it finds an Actuonix L12 50 linear servo motor + Same as :class:`Motor`, except it will only successfully initialize if it finds an + Actuonix L12 50 linear servo motor """ SYSTEM_CLASS_NAME = Motor.SYSTEM_CLASS_NAME @@ -1140,15 +1148,19 @@ class ActuonixL1250Motor(Motor): def __init__(self, address=None, name_pattern=SYSTEM_DEVICE_NAME_CONVENTION, name_exact=False, **kwargs): - super(ActuonixL1250Motor, self).__init__(address, name_pattern, name_exact, driver_name=['act-l12-ev3-50'], **kwargs) + super(ActuonixL1250Motor, self).__init__(address, + name_pattern, + name_exact, + driver_name=['act-l12-ev3-50'], + **kwargs) class ActuonixL12100Motor(Motor): - """ Actuonix L12 100 linear servo motor. - Same as :class:`Motor`, except it will only successfully initialize if it finds an Actuonix L12 100 linear servo motor + Same as :class:`Motor`, except it will only successfully initialize if it finds an + Actuonix L12 100linear servo motor """ SYSTEM_CLASS_NAME = Motor.SYSTEM_CLASS_NAME @@ -1157,11 +1169,14 @@ class ActuonixL12100Motor(Motor): def __init__(self, address=None, name_pattern=SYSTEM_DEVICE_NAME_CONVENTION, name_exact=False, **kwargs): - super(ActuonixL12100Motor, self).__init__(address, name_pattern, name_exact, driver_name=['act-l12-ev3-100'], **kwargs) + super(ActuonixL12100Motor, self).__init__(address, + name_pattern, + name_exact, + driver_name=['act-l12-ev3-100'], + **kwargs) class DcMotor(Device): - """ The DC motor class provides a uniform interface for using regular DC motors with no fancy controls or feedback. This includes LEGO MINDSTORMS RCX motors @@ -1171,19 +1186,19 @@ class DcMotor(Device): SYSTEM_CLASS_NAME = 'dc-motor' SYSTEM_DEVICE_NAME_CONVENTION = 'motor*' __slots__ = [ - '_address', - '_command', - '_commands', - '_driver_name', - '_duty_cycle', - '_duty_cycle_sp', - '_polarity', - '_ramp_down_sp', - '_ramp_up_sp', - '_state', - '_stop_action', - '_stop_actions', - '_time_sp', + '_address', + '_command', + '_commands', + '_driver_name', + '_duty_cycle', + '_duty_cycle_sp', + '_polarity', + '_ramp_down_sp', + '_ramp_up_sp', + '_state', + '_stop_action', + '_stop_actions', + '_time_sp', ] def __init__(self, address=None, name_pattern=SYSTEM_DEVICE_NAME_CONVENTION, name_exact=False, **kwargs): @@ -1217,9 +1232,9 @@ def address(self): @property def command(self): """ - Sets the command for the motor. Possible values are `run-forever`, `run-timed` and - `stop`. Not all commands may be supported, so be sure to check the contents - of the `commands` attribute. + Sets the command for the motor. Possible values are ``run-forever``, ``run-timed`` and + ``stop``. Not all commands may be supported, so be sure to check the contents + of the ``commands`` attribute. """ raise Exception("command is a write-only property!") @@ -1271,7 +1286,7 @@ def duty_cycle_sp(self, value): @property def polarity(self): """ - Sets the polarity of the motor. Valid values are `normal` and `inversed`. + Sets the polarity of the motor. Valid values are ``normal`` and ``inversed``. """ self._polarity, value = self.get_attr_string(self._polarity, 'polarity') return value @@ -1310,9 +1325,9 @@ def ramp_up_sp(self, value): def state(self): """ Gets a list of flags indicating the motor status. Possible - flags are `running` and `ramping`. `running` indicates that the motor is - powered. `ramping` indicates that the motor has not yet reached the - `duty_cycle_sp`. + flags are ``running`` and ``ramping``. ``running`` indicates that the motor is + powered. ``ramping`` indicates that the motor has not yet reached the + ``duty_cycle_sp``. """ self._state, value = self.get_attr_set(self._state, 'state') return value @@ -1321,7 +1336,7 @@ def state(self): def stop_action(self): """ Sets the stop action that will be used when the motor stops. Read - `stop_actions` to get the list of valid values. + ``stop_actions`` to get the list of valid values. """ raise Exception("stop_action is a write-only property!") @@ -1332,8 +1347,8 @@ def stop_action(self, value): @property def stop_actions(self): """ - Gets a list of stop actions. Valid values are `coast` - and `brake`. + Gets a list of stop actions. Valid values are ``coast`` + and ``brake``. """ self._stop_actions, value = self.get_attr_set(self._stop_actions, 'stop_actions') return value @@ -1342,7 +1357,7 @@ def stop_actions(self): def time_sp(self): """ Writing specifies the amount of time the motor will run when using the - `run-timed` command. Reading returns the current value. Units are in + ``run-timed`` command. Reading returns the current value. Units are in milliseconds. """ self._time_sp, value = self.get_attr_int(self._time_sp, 'time_sp') @@ -1355,24 +1370,24 @@ def time_sp(self, value): #: Run the motor until another command is sent. COMMAND_RUN_FOREVER = 'run-forever' - #: Run the motor for the amount of time specified in `time_sp` - #: and then stop the motor using the action specified by `stop_action`. + #: Run the motor for the amount of time specified in ``time_sp`` + #: and then stop the motor using the action specified by ``stop_action``. COMMAND_RUN_TIMED = 'run-timed' - #: Run the motor at the duty cycle specified by `duty_cycle_sp`. - #: Unlike other run commands, changing `duty_cycle_sp` while running *will* + #: Run the motor at the duty cycle specified by ``duty_cycle_sp``. + #: Unlike other run commands, changing ``duty_cycle_sp`` while running *will* #: take effect immediately. COMMAND_RUN_DIRECT = 'run-direct' #: Stop any of the run commands before they are complete using the - #: action specified by `stop_action`. + #: action specified by ``stop_action``. COMMAND_STOP = 'stop' - #: With `normal` polarity, a positive duty cycle will + #: With ``normal`` polarity, a positive duty cycle will #: cause the motor to rotate clockwise. POLARITY_NORMAL = 'normal' - #: With `inversed` polarity, a positive duty cycle will + #: With ``inversed`` polarity, a positive duty cycle will #: cause the motor to rotate counter-clockwise. POLARITY_INVERSED = 'inversed' @@ -1395,8 +1410,8 @@ def run_forever(self, **kwargs): def run_timed(self, **kwargs): """ - Run the motor for the amount of time specified in `time_sp` - and then stop the motor using the action specified by `stop_action`. + Run the motor for the amount of time specified in ``time_sp`` + and then stop the motor using the action specified by ``stop_action``. """ for key in kwargs: setattr(self, key, kwargs[key]) @@ -1404,8 +1419,8 @@ def run_timed(self, **kwargs): def run_direct(self, **kwargs): """ - Run the motor at the duty cycle specified by `duty_cycle_sp`. - Unlike other run commands, changing `duty_cycle_sp` while running *will* + Run the motor at the duty cycle specified by ``duty_cycle_sp``. + Unlike other run commands, changing ``duty_cycle_sp`` while running *will* take effect immediately. """ for key in kwargs: @@ -1415,7 +1430,7 @@ def run_direct(self, **kwargs): def stop(self, **kwargs): """ Stop any of the run commands before they are complete using the - action specified by `stop_action`. + action specified by ``stop_action``. """ for key in kwargs: setattr(self, key, kwargs[key]) @@ -1423,7 +1438,6 @@ def stop(self, **kwargs): class ServoMotor(Device): - """ The servo motor class provides a uniform interface for using hobby type servo motors. @@ -1432,16 +1446,16 @@ class ServoMotor(Device): SYSTEM_CLASS_NAME = 'servo-motor' SYSTEM_DEVICE_NAME_CONVENTION = 'motor*' __slots__ = [ - '_address', - '_command', - '_driver_name', - '_max_pulse_sp', - '_mid_pulse_sp', - '_min_pulse_sp', - '_polarity', - '_position_sp', - '_rate_sp', - '_state', + '_address', + '_command', + '_driver_name', + '_max_pulse_sp', + '_mid_pulse_sp', + '_min_pulse_sp', + '_polarity', + '_position_sp', + '_rate_sp', + '_state', ] def __init__(self, address=None, name_pattern=SYSTEM_DEVICE_NAME_CONVENTION, name_exact=False, **kwargs): @@ -1472,9 +1486,9 @@ def address(self): @property def command(self): """ - Sets the command for the servo. Valid values are `run` and `float`. Setting - to `run` will cause the servo to be driven to the position_sp set in the - `position_sp` attribute. Setting to `float` will remove power from the motor. + Sets the command for the servo. Valid values are ``run`` and ``float``. Setting + to ``run`` will cause the servo to be driven to the position_sp set in the + ``position_sp`` attribute. Setting to ``float`` will remove power from the motor. """ raise Exception("command is a write-only property!") @@ -1541,10 +1555,10 @@ def min_pulse_sp(self, value): @property def polarity(self): """ - Sets the polarity of the servo. Valid values are `normal` and `inversed`. - Setting the value to `inversed` will cause the position_sp value to be - inversed. i.e `-100` will correspond to `max_pulse_sp`, and `100` will - correspond to `min_pulse_sp`. + Sets the polarity of the servo. Valid values are ``normal`` and ``inversed``. + Setting the value to ``inversed`` will cause the position_sp value to be + inversed. i.e ``-100`` will correspond to ``max_pulse_sp``, and ``100`` will + correspond to ``min_pulse_sp``. """ self._polarity, value = self.get_attr_string(self._polarity, 'polarity') return value @@ -1558,8 +1572,8 @@ def position_sp(self): """ Reading returns the current position_sp of the servo. Writing instructs the servo to move to the specified position_sp. Units are percent. Valid values - are -100 to 100 (-100% to 100%) where `-100` corresponds to `min_pulse_sp`, - `0` corresponds to `mid_pulse_sp` and `100` corresponds to `max_pulse_sp`. + are -100 to 100 (-100% to 100%) where ``-100`` corresponds to ``min_pulse_sp``, + ``0`` corresponds to ``mid_pulse_sp`` and ``100`` corresponds to ``max_pulse_sp``. """ self._position_sp, value = self.get_attr_int(self._position_sp, 'position_sp') return value @@ -1575,7 +1589,7 @@ def rate_sp(self): range of the servo). Units are in milliseconds. Example: Setting the rate_sp to 1000 means that it will take a 180 degree servo 2 second to move from 0 to 180 degrees. Note: Some servo controllers may not support this in which - case reading and writing will fail with `-EOPNOTSUPP`. In continuous rotation + case reading and writing will fail with ``-EOPNOTSUPP``. In continuous rotation servos, this value will affect the rate_sp at which the speed ramps up or down. """ self._rate_sp, value = self.get_attr_int(self._rate_sp, 'rate_sp') @@ -1590,28 +1604,28 @@ def state(self): """ Returns a list of flags indicating the state of the servo. Possible values are: - * `running`: Indicates that the motor is powered. + * ``running``: Indicates that the motor is powered. """ self._state, value = self.get_attr_set(self._state, 'state') return value - #: Drive servo to the position set in the `position_sp` attribute. + #: Drive servo to the position set in the ``position_sp`` attribute. COMMAND_RUN = 'run' #: Remove power from the motor. COMMAND_FLOAT = 'float' - #: With `normal` polarity, a positive duty cycle will + #: With ``normal`` polarity, a positive duty cycle will #: cause the motor to rotate clockwise. POLARITY_NORMAL = 'normal' - #: With `inversed` polarity, a positive duty cycle will + #: With ``inversed`` polarity, a positive duty cycle will #: cause the motor to rotate counter-clockwise. POLARITY_INVERSED = 'inversed' def run(self, **kwargs): """ - Drive servo to the position set in the `position_sp` attribute. + Drive servo to the position set in the ``position_sp`` attribute. """ for key in kwargs: setattr(self, key, kwargs[key]) @@ -1627,7 +1641,6 @@ def float(self, **kwargs): class MotorSet(object): - def __init__(self, motor_specs, desc=None): """ motor_specs is a dictionary such as @@ -1660,7 +1673,7 @@ def set_args(self, **kwargs): try: setattr(motor, key, kwargs[key]) except AttributeError as e: - #log.error("%s %s cannot set %s to %s" % (self, motor, key, kwargs[key])) + # log.error("%s %s cannot set %s to %s" % (self, motor, key, kwargs[key])) raise e def set_polarity(self, polarity, motors=None): @@ -1679,12 +1692,12 @@ def _run_command(self, **kwargs): for motor in motors: for key in kwargs: if key not in ('motors', 'commands'): - #log.debug("%s: %s set %s to %s" % (self, motor, key, kwargs[key])) + # log.debug("%s: %s set %s to %s" % (self, motor, key, kwargs[key])) setattr(motor, key, kwargs[key]) for motor in motors: motor.command = kwargs['command'] - #log.debug("%s: %s command %s" % (self, motor, kwargs['command'])) + # log.debug("%s: %s command %s" % (self, motor, kwargs['command'])) def run_forever(self, **kwargs): kwargs['command'] = LargeMotor.COMMAND_RUN_FOREVER @@ -1790,6 +1803,15 @@ def _block(self): self.wait_until_not_moving() +# follow gyro angle classes +class FollowGyroAngleErrorTooFast(Exception): + """ + Raised when a gyro following robot has been asked to follow + an angle at an unrealistic speed + """ + pass + + # line follower classes class LineFollowErrorLostLine(Exception): """ @@ -1842,29 +1864,42 @@ class MoveTank(MotorSet): # drive in a turn for 10 rotations of the outer motor tank_drive.on_for_rotations(50, 75, 10) """ - def __init__(self, left_motor_port, right_motor_port, desc=None, motor_class=LargeMotor): motor_specs = { - left_motor_port : motor_class, - right_motor_port : motor_class, + left_motor_port: motor_class, + right_motor_port: motor_class, } MotorSet.__init__(self, motor_specs, desc) self.left_motor = self.motors[left_motor_port] self.right_motor = self.motors[right_motor_port] self.max_speed = self.left_motor.max_speed + self._cs = None + self._gyro = None + + # color sensor used by follow_line() + @property + def cs(self): + return self._cs - # color sensor used by follow_line() - self.cs = None + @cs.setter + def cs(self, cs): + self._cs = cs + + # gyro sensor used by follow_gyro_angle() + @property + def gyro(self): + return self._gyro + + @gyro.setter + def gyro(self, gyro): + self._gyro = gyro def _unpack_speeds_to_native_units(self, left_speed, right_speed): left_speed = self.left_motor._speed_native_units(left_speed, "left_speed") right_speed = self.right_motor._speed_native_units(right_speed, "right_speed") - return ( - left_speed, - right_speed - ) + return (left_speed, right_speed) def on_for_degrees(self, left_speed, right_speed, degrees, brake=True, block=True): """ @@ -1876,7 +1911,8 @@ def on_for_degrees(self, left_speed, right_speed, degrees, brake=True, block=Tru ``degrees`` while the motor on the inside will have its requested distance calculated according to the expected turn. """ - (left_speed_native_units, right_speed_native_units) = self._unpack_speeds_to_native_units(left_speed, right_speed) + (left_speed_native_units, + right_speed_native_units) = self._unpack_speeds_to_native_units(left_speed, right_speed) # proof of the following distance calculation: consider the circle formed by each wheel's path # v_l = d_l/t, v_r = d_r/t @@ -1901,17 +1937,6 @@ def on_for_degrees(self, left_speed, right_speed, degrees, brake=True, block=Tru self.right_motor._set_rel_position_degrees_and_speed_sp(right_degrees, right_speed_native_units) self.right_motor._set_brake(brake) - log.debug("{}: on_for_degrees {}".format(self, degrees)) - - # These debugs involve disk I/O to pull position and position_sp so only uncomment - # if you need to troubleshoot in more detail. - # log.debug("{}: left_speed {}, left_speed_native_units {}, left_degrees {}, left-position {}->{}".format( - # self, left_speed, left_speed_native_units, left_degrees, - # self.left_motor.position, self.left_motor.position_sp)) - # log.debug("{}: right_speed {}, right_speed_native_units {}, right_degrees {}, right-position {}->{}".format( - # self, right_speed, right_speed_native_units, right_degrees, - # self.right_motor.position, self.right_motor.position_sp)) - # Start the motors self.left_motor.run_to_rel_pos() self.right_motor.run_to_rel_pos() @@ -1940,7 +1965,8 @@ def on_for_seconds(self, left_speed, right_speed, seconds, brake=True, block=Tru if seconds < 0: raise ValueError("seconds is negative ({})".format(seconds)) - (left_speed_native_units, right_speed_native_units) = self._unpack_speeds_to_native_units(left_speed, right_speed) + (left_speed_native_units, + right_speed_native_units) = self._unpack_speeds_to_native_units(left_speed, right_speed) # Set all parameters self.left_motor.speed_sp = int(round(left_speed_native_units)) @@ -1950,8 +1976,7 @@ def on_for_seconds(self, left_speed, right_speed, seconds, brake=True, block=Tru self.right_motor.time_sp = int(seconds * 1000) self.right_motor._set_brake(brake) - log.debug("%s: on_for_seconds %ss at left-speed %s, right-speed %s" % - (self, seconds, left_speed, right_speed)) + log.debug("%s: on_for_seconds %ss at left-speed %s, right-speed %s" % (self, seconds, left_speed, right_speed)) # Start the motors self.left_motor.run_timed() @@ -1965,32 +1990,29 @@ def on(self, left_speed, right_speed): Start rotating the motors according to ``left_speed`` and ``right_speed`` forever. Speeds can be percentages or any SpeedValue implementation. """ - (left_speed_native_units, right_speed_native_units) = self._unpack_speeds_to_native_units(left_speed, right_speed) + (left_speed_native_units, + right_speed_native_units) = self._unpack_speeds_to_native_units(left_speed, right_speed) # Set all parameters self.left_motor.speed_sp = int(round(left_speed_native_units)) self.right_motor.speed_sp = int(round(right_speed_native_units)) - # This debug involves disk I/O to pull speed_sp so only uncomment - # if you need to troubleshoot in more detail. - # log.debug("%s: on at left-speed %s, right-speed %s" % - # (self, self.left_motor.speed_sp, self.right_motor.speed_sp)) - # Start the motors self.left_motor.run_forever() self.right_motor.run_forever() def follow_line(self, - kp, ki, kd, - speed, - target_light_intensity=None, - follow_left_edge=True, - white=60, - off_line_count_max=20, - sleep_time=0.01, - follow_for=follow_for_forever, - **kwargs - ): + kp, + ki, + kd, + speed, + target_light_intensity=None, + follow_left_edge=True, + white=60, + off_line_count_max=20, + sleep_time=0.01, + follow_for=follow_for_forever, + **kwargs): """ PID line follower @@ -2042,24 +2064,26 @@ def follow_line(self, follow_for=follow_for_ms, ms=4500 ) - except Exception: + except LineFollowErrorTooFast: tank.stop() raise """ - assert self.cs, "ColorSensor must be defined" + if not self._cs: + raise DeviceNotDefined( + "The 'cs' variable must be defined with a ColorSensor. Example: tank.cs = ColorSensor()") if target_light_intensity is None: - target_light_intensity = self.cs.reflected_light_intensity + target_light_intensity = self._cs.reflected_light_intensity integral = 0.0 last_error = 0.0 derivative = 0.0 off_line_count = 0 + speed = speed_to_speedvalue(speed) speed_native_units = speed.to_native_units(self.left_motor) - MAX_SPEED = SpeedNativeUnits(self.max_speed) while follow_for(self, **kwargs): - reflected_light_intensity = self.cs.reflected_light_intensity + reflected_light_intensity = self._cs.reflected_light_intensity error = target_light_intensity - reflected_light_intensity integral = integral + error derivative = error - last_error @@ -2072,16 +2096,6 @@ def follow_line(self, left_speed = SpeedNativeUnits(speed_native_units - turn_native_units) right_speed = SpeedNativeUnits(speed_native_units + turn_native_units) - if left_speed > MAX_SPEED: - log.info("%s: left_speed %s is greater than MAX_SPEED %s" % (self, left_speed, MAX_SPEED)) - self.stop() - raise LineFollowErrorTooFast("The robot is moving too fast to follow the line") - - if right_speed > MAX_SPEED: - log.info("%s: right_speed %s is greater than MAX_SPEED %s" % (self, right_speed, MAX_SPEED)) - self.stop() - raise LineFollowErrorTooFast("The robot is moving too fast to follow the line") - # Have we lost the line? if reflected_light_intensity >= white: off_line_count += 1 @@ -2095,10 +2109,196 @@ def follow_line(self, if sleep_time: time.sleep(sleep_time) - self.on(left_speed, right_speed) + try: + self.on(left_speed, right_speed) + except SpeedInvalid as e: + log.exception(e) + self.stop() + raise LineFollowErrorTooFast("The robot is moving too fast to follow the line") self.stop() + def follow_gyro_angle(self, + kp, + ki, + kd, + speed, + target_angle=0, + sleep_time=0.01, + follow_for=follow_for_forever, + **kwargs): + """ + PID gyro angle follower + + ``kp``, ``ki``, and ``kd`` are the PID constants. + + ``speed`` is the desired speed of the midpoint of the robot + + ``target_angle`` is the angle we want to maintain + + ``sleep_time`` is how many seconds we sleep on each pass through + the loop. This is to give the robot a chance to react + to the new motor settings. This should be something small such + as 0.01 (10ms). + + ``follow_for`` is called to determine if we should keep following the + desired angle or stop. This function will be passed ``self`` (the current + ``MoveTank`` object). Current supported options are: + - ``follow_for_forever`` + - ``follow_for_ms`` + + ``**kwargs`` will be passed to the ``follow_for`` function + + Example: + + .. code:: python + + from ev3dev2.motor import OUTPUT_A, OUTPUT_B, MoveTank, SpeedPercent, follow_for_ms + from ev3dev2.sensor.lego import GyroSensor + + # Instantiate the MoveTank object + tank = MoveTank(OUTPUT_A, OUTPUT_B) + + # Initialize the tank's gyro sensor + tank.gyro = GyroSensor() + + # Calibrate the gyro to eliminate drift, and to initialize the current angle as 0 + tank.gyro.calibrate() + + try: + + # Follow the target_angle for 4500ms + tank.follow_gyro_angle( + kp=11.3, ki=0.05, kd=3.2, + speed=SpeedPercent(30), + target_angle=0, + follow_for=follow_for_ms, + ms=4500 + ) + except FollowGyroAngleErrorTooFast: + tank.stop() + raise + """ + if not self._gyro: + raise DeviceNotDefined( + "The 'gyro' variable must be defined with a GyroSensor. Example: tank.gyro = GyroSensor()") + + integral = 0.0 + last_error = 0.0 + derivative = 0.0 + speed = speed_to_speedvalue(speed) + speed_native_units = speed.to_native_units(self.left_motor) + + while follow_for(self, **kwargs): + current_angle = self._gyro.angle + error = current_angle - target_angle + integral = integral + error + derivative = error - last_error + last_error = error + turn_native_units = (kp * error) + (ki * integral) + (kd * derivative) + + left_speed = SpeedNativeUnits(speed_native_units - turn_native_units) + right_speed = SpeedNativeUnits(speed_native_units + turn_native_units) + + if sleep_time: + time.sleep(sleep_time) + + try: + self.on(left_speed, right_speed) + except SpeedInvalid as e: + log.exception(e) + self.stop() + raise FollowGyroAngleErrorTooFast("The robot is moving too fast to follow the angle") + + self.stop() + + def turn_degrees(self, speed, target_angle, brake=True, error_margin=2, sleep_time=0.01): + """ + Use a GyroSensor to rotate in place for ``target_angle`` + + ``speed`` is the desired speed of the midpoint of the robot + + ``target_angle`` is the number of degrees we want to rotate + + ``brake`` hit the brakes once we reach ``target_angle`` + + ``error_margin`` is the +/- angle threshold to control how accurate the turn should be + + ``sleep_time`` is how many seconds we sleep on each pass through + the loop. This is to give the robot a chance to react + to the new motor settings. This should be something small such + as 0.01 (10ms). + + Rotate in place for ``target_degrees`` at ``speed`` + + Example: + + .. code:: python + + from ev3dev2.motor import OUTPUT_A, OUTPUT_B, MoveTank, SpeedPercent + from ev3dev2.sensor.lego import GyroSensor + + # Instantiate the MoveTank object + tank = MoveTank(OUTPUT_A, OUTPUT_B) + + # Initialize the tank's gyro sensor + tank.gyro = GyroSensor() + + # Calibrate the gyro to eliminate drift, and to initialize the current angle as 0 + tank.gyro.calibrate() + + # Pivot 30 degrees + tank.turn_degrees( + speed=SpeedPercent(5), + target_angle=30 + ) + """ + + # MoveTank does not have information on wheel size and distance (that is + # MoveDifferential) so we must use a GyroSensor to control how far we rotate. + if not self._gyro: + raise DeviceNotDefined( + "The 'gyro' variable must be defined with a GyroSensor. Example: tank.gyro = GyroSensor()") + + speed = speed_to_speedvalue(speed) + speed_native_units = speed.to_native_units(self.left_motor) + target_angle = self._gyro.angle + target_angle + + while True: + current_angle = self._gyro.angle + delta = abs(target_angle - current_angle) + + if delta <= error_margin: + self.stop(brake=brake) + break + + # we are left of our target, rotate clockwise + if current_angle < target_angle: + left_speed = SpeedNativeUnits(speed_native_units) + right_speed = SpeedNativeUnits(-1 * speed_native_units) + + # we are right of our target, rotate counter-clockwise + else: + left_speed = SpeedNativeUnits(-1 * speed_native_units) + right_speed = SpeedNativeUnits(speed_native_units) + + self.on(left_speed, right_speed) + + if sleep_time: + time.sleep(sleep_time) + + def turn_right(self, speed, degrees, brake=True, error_margin=2, sleep_time=0.01): + """ + Rotate clockwise ``degrees`` in place + """ + self.turn_degrees(speed, abs(degrees), brake, error_margin, sleep_time) + + def turn_left(self, speed, degrees, brake=True, error_margin=2, sleep_time=0.01): + """ + Rotate counter-clockwise ``degrees`` in place + """ + self.turn_degrees(speed, abs(degrees) * -1, brake, error_margin, sleep_time) + class MoveSteering(MoveTank): """ @@ -2126,7 +2326,8 @@ def on_for_rotations(self, steering, speed, rotations, brake=True, block=True): The distance each motor will travel follows the rules of :meth:`MoveTank.on_for_rotations`. """ (left_speed, right_speed) = self.get_speed_steering(steering, speed) - MoveTank.on_for_rotations(self, SpeedNativeUnits(left_speed), SpeedNativeUnits(right_speed), rotations, brake, block) + MoveTank.on_for_rotations(self, SpeedNativeUnits(left_speed), SpeedNativeUnits(right_speed), rotations, brake, + block) def on_for_degrees(self, steering, speed, degrees, brake=True, block=True): """ @@ -2135,14 +2336,16 @@ def on_for_degrees(self, steering, speed, degrees, brake=True, block=True): The distance each motor will travel follows the rules of :meth:`MoveTank.on_for_degrees`. """ (left_speed, right_speed) = self.get_speed_steering(steering, speed) - MoveTank.on_for_degrees(self, SpeedNativeUnits(left_speed), SpeedNativeUnits(right_speed), degrees, brake, block) + MoveTank.on_for_degrees(self, SpeedNativeUnits(left_speed), SpeedNativeUnits(right_speed), degrees, brake, + block) def on_for_seconds(self, steering, speed, seconds, brake=True, block=True): """ Rotate the motors according to the provided ``steering`` for ``seconds``. """ (left_speed, right_speed) = self.get_speed_steering(steering, speed) - MoveTank.on_for_seconds(self, SpeedNativeUnits(left_speed), SpeedNativeUnits(right_speed), seconds, brake, block) + MoveTank.on_for_seconds(self, SpeedNativeUnits(left_speed), SpeedNativeUnits(right_speed), seconds, brake, + block) def on(self, steering, speed): """ @@ -2263,10 +2466,13 @@ class MoveDifferential(MoveTank): # Disable odometry mdiff.odometry_stop() """ - - def __init__(self, left_motor_port, right_motor_port, - wheel_class, wheel_distance_mm, - desc=None, motor_class=LargeMotor): + def __init__(self, + left_motor_port, + right_motor_port, + wheel_class, + wheel_distance_mm, + desc=None, + motor_class=LargeMotor): MoveTank.__init__(self, left_motor_port, right_motor_port, desc, motor_class) self.wheel = wheel_class() @@ -2281,12 +2487,11 @@ def __init__(self, left_motor_port, right_motor_port, self.x_pos_mm = 0.0 # robot X position in mm self.y_pos_mm = 0.0 # robot Y position in mm self.odometry_thread_run = False - self.odometry_thread_id = None self.theta = 0.0 def on_for_distance(self, speed, distance_mm, brake=True, block=True): """ - Drive distance_mm + Drive in a straight line for ``distance_mm`` """ rotations = distance_mm / self.wheel.circumference_mm log.debug("%s: on_for_rotations distance_mm %s, rotations %s, speed %s" % (self, distance_mm, rotations, speed)) @@ -2299,8 +2504,8 @@ def _on_arc(self, speed, radius_mm, distance_mm, brake, block, arc_right): """ if radius_mm < self.min_circle_radius_mm: - raise ValueError("{}: radius_mm {} is less than min_circle_radius_mm {}" .format( - self, radius_mm, self.min_circle_radius_mm)) + raise ValueError("{}: radius_mm {} is less than min_circle_radius_mm {}".format( + self, radius_mm, self.min_circle_radius_mm)) # The circle formed at the halfway point between the two wheels is the # circle that must have a radius of radius_mm @@ -2312,20 +2517,18 @@ def _on_arc(self, speed, radius_mm, distance_mm, brake, block, arc_right): # The left wheel is making the larger circle and will move at 'speed' # The right wheel is making a smaller circle so its speed will be a fraction of the left motor's speed left_speed = speed - right_speed = float(circle_inner_mm/circle_outer_mm) * left_speed + right_speed = float(circle_inner_mm / circle_outer_mm) * left_speed else: # The right wheel is making the larger circle and will move at 'speed' # The left wheel is making a smaller circle so its speed will be a fraction of the right motor's speed right_speed = speed - left_speed = float(circle_inner_mm/circle_outer_mm) * right_speed + left_speed = float(circle_inner_mm / circle_outer_mm) * right_speed - log.debug("%s: arc %s, radius %s, distance %s, left-speed %s, right-speed %s, circle_outer_mm %s, circle_middle_mm %s, circle_inner_mm %s" % - (self, "right" if arc_right else "left", - radius_mm, distance_mm, left_speed, right_speed, - circle_outer_mm, circle_middle_mm, circle_inner_mm - ) - ) + log.debug("%s: arc %s, radius %s, distance %s, left-speed %s, right-speed %s" % + (self, "right" if arc_right else "left", radius_mm, distance_mm, left_speed, right_speed)) + log.debug("%s: circle_outer_mm %s, circle_middle_mm %s, circle_inner_mm %s" % + (self, circle_outer_mm, circle_middle_mm, circle_inner_mm)) # We know we want the middle circle to be of length distance_mm so # calculate the percentage of circle_middle_mm we must travel for the @@ -2339,12 +2542,10 @@ def _on_arc(self, speed, radius_mm, distance_mm, brake, block, arc_right): outer_wheel_rotations = float(circle_outer_final_mm / self.wheel.circumference_mm) outer_wheel_degrees = outer_wheel_rotations * 360 - log.debug("%s: arc %s, circle_middle_percentage %s, circle_outer_final_mm %s, outer_wheel_rotations %s, outer_wheel_degrees %s" % - (self, "right" if arc_right else "left", - circle_middle_percentage, circle_outer_final_mm, - outer_wheel_rotations, outer_wheel_degrees - ) - ) + log.debug("%s: arc %s, circle_middle_percentage %s, circle_outer_final_mm %s, " % + (self, "right" if arc_right else "left", circle_middle_percentage, circle_outer_final_mm)) + log.debug("%s: outer_wheel_rotations %s, outer_wheel_degrees %s" % + (self, outer_wheel_rotations, outer_wheel_degrees)) MoveTank.on_for_degrees(self, left_speed, right_speed, outer_wheel_degrees, brake, block) @@ -2360,20 +2561,49 @@ def on_arc_left(self, speed, radius_mm, distance_mm, brake=True, block=True): """ self._on_arc(speed, radius_mm, distance_mm, brake, block, False) - def _turn(self, speed, degrees, brake=True, block=True): + def turn_degrees(self, speed, degrees, brake=True, block=True, error_margin=2, use_gyro=False): """ - Rotate in place 'degrees'. Both wheels must turn at the same speed for us - to rotate in place. + Rotate in place ``degrees``. Both wheels must turn at the same speed for us + to rotate in place. If the following conditions are met the GryoSensor will + be used to improve the accuracy of our turn: + - ``use_gyro``, ``brake`` and ``block`` are all True + - A GyroSensor has been defined via ``self.gyro = GyroSensor()`` """ + def final_angle(init_angle, degrees): + result = init_angle - degrees + + while result <= -360: + result += 360 + + while result >= 360: + result -= 360 + + if result < 0: + result += 360 + + return result + + # use the gyro to check that we turned the correct amount? + use_gyro = bool(use_gyro and block and brake) + if use_gyro and not self._gyro: + raise DeviceNotDefined( + "The 'gyro' variable must be defined with a GyroSensor. Example: tank.gyro = GyroSensor()") + + if use_gyro: + angle_init_degrees = self._gyro.circle_angle() + else: + angle_init_degrees = math.degrees(self.theta) + + angle_target_degrees = final_angle(angle_init_degrees, degrees) + + log.info("%s: turn_degrees() %d degrees from %s to %s" % + (self, degrees, angle_init_degrees, angle_target_degrees)) # The distance each wheel needs to travel distance_mm = (abs(degrees) / 360) * self.circumference_mm # The number of rotations to move distance_mm - rotations = distance_mm/self.wheel.circumference_mm - - log.debug("%s: turn() degrees %s, distance_mm %s, rotations %s, degrees %s" % - (self, degrees, distance_mm, rotations, degrees)) + rotations = distance_mm / self.wheel.circumference_mm # If degrees is positive rotate clockwise if degrees > 0: @@ -2381,28 +2611,91 @@ def _turn(self, speed, degrees, brake=True, block=True): # If degrees is negative rotate counter-clockwise else: - rotations = distance_mm / self.wheel.circumference_mm MoveTank.on_for_rotations(self, speed * -1, speed, rotations, brake, block) - def turn_right(self, speed, degrees, brake=True, block=True): + if use_gyro: + angle_current_degrees = self._gyro.circle_angle() + + # This can happen if we are aiming for 2 degrees and overrotate to 358 degrees + # We need to rotate counter-clockwise + if 90 >= angle_target_degrees >= 0 and 270 <= angle_current_degrees <= 360: + degrees_error = (angle_target_degrees + (360 - angle_current_degrees)) * -1 + + # This can happen if we are aiming for 358 degrees and overrotate to 2 degrees + # We need to rotate clockwise + elif 360 >= angle_target_degrees >= 270 and 0 <= angle_current_degrees <= 90: + degrees_error = angle_current_degrees + (360 - angle_target_degrees) + + # We need to rotate clockwise + elif angle_current_degrees > angle_target_degrees: + degrees_error = angle_current_degrees - angle_target_degrees + + # We need to rotate counter-clockwise + else: + degrees_error = (angle_target_degrees - angle_current_degrees) * -1 + + log.info("%s: turn_degrees() ended up at %s, error %s, error_margin %s" % + (self, angle_current_degrees, degrees_error, error_margin)) + + if abs(degrees_error) > error_margin: + self.turn_degrees(speed, degrees_error, brake, block, error_margin, use_gyro) + + def turn_right(self, speed, degrees, brake=True, block=True, error_margin=2, use_gyro=False): """ - Rotate clockwise 'degrees' in place + Rotate clockwise ``degrees`` in place """ - self._turn(speed, abs(degrees), brake, block) + self.turn_degrees(speed, abs(degrees), brake, block, error_margin, use_gyro) - def turn_left(self, speed, degrees, brake=True, block=True): + def turn_left(self, speed, degrees, brake=True, block=True, error_margin=2, use_gyro=False): + """ + Rotate counter-clockwise ``degrees`` in place """ - Rotate counter-clockwise 'degrees' in place + self.turn_degrees(speed, abs(degrees) * -1, brake, block, error_margin, use_gyro) + + def turn_to_angle(self, speed, angle_target_degrees, brake=True, block=True, error_margin=2, use_gyro=False): """ - self._turn(speed, abs(degrees) * -1, brake, block) + Rotate in place to ``angle_target_degrees`` at ``speed`` + """ + if not self.odometry_thread_run: + raise ThreadNotRunning("odometry_start() must be called to track robot coordinates") + + # Make both target and current angles positive numbers between 0 and 360 + while angle_target_degrees < 0: + angle_target_degrees += 360 + + angle_current_degrees = math.degrees(self.theta) + + while angle_current_degrees < 0: + angle_current_degrees += 360 + + # Is it shorter to rotate to the right or left + # to reach angle_target_degrees? + if angle_current_degrees > angle_target_degrees: + turn_right = True + angle_delta = angle_current_degrees - angle_target_degrees + else: + turn_right = False + angle_delta = angle_target_degrees - angle_current_degrees + + if angle_delta > 180: + angle_delta = 360 - angle_delta + turn_right = not turn_right + + log.debug("%s: turn_to_angle %s, current angle %s, delta %s, turn_right %s" % + (self, angle_target_degrees, angle_current_degrees, angle_delta, turn_right)) + self.odometry_coordinates_log() + + if turn_right: + self.turn_degrees(speed, abs(angle_delta), brake, block, error_margin, use_gyro) + else: + self.turn_degrees(speed, abs(angle_delta) * -1, brake, block, error_margin, use_gyro) + + self.odometry_coordinates_log() def odometry_coordinates_log(self): - log.debug("%s: odometry angle %s at (%d, %d)" % - (self, math.degrees(self.theta), self.x_pos_mm, self.y_pos_mm)) + log.debug("%s: odometry angle %s at (%d, %d)" % (self, math.degrees(self.theta), self.x_pos_mm, self.y_pos_mm)) - def odometry_start(self, theta_degrees_start=90.0, - x_pos_start=0.0, y_pos_start=0.0, - sleep_time=0.005): # 5ms + def odometry_start(self, theta_degrees_start=90.0, x_pos_start=0.0, y_pos_start=0.0, sleep_time=0.005): # 5ms """ Ported from: http://seattlerobotics.org/encoder/200610/Article3/IMU%20Odometry,%20by%20David%20Anderson.htm @@ -2410,7 +2703,6 @@ def odometry_start(self, theta_degrees_start=90.0, A thread is started that will run until the user calls odometry_stop() which will set odometry_thread_run to False """ - def _odometry_monitor(): left_previous = 0 right_previous = 0 @@ -2418,6 +2710,7 @@ def _odometry_monitor(): self.x_pos_mm = x_pos_start # robot X position in mm self.y_pos_mm = y_pos_start # robot Y position in mm TWO_PI = 2 * math.pi + self.odometry_thread_run = True while self.odometry_thread_run: @@ -2436,11 +2729,6 @@ def _odometry_monitor(): time.sleep(sleep_time) continue - # log.debug("%s: left_ticks %s (from %s to %s)" % - # (self, left_ticks, left_previous, left_current)) - # log.debug("%s: right_ticks %s (from %s to %s)" % - # (self, right_ticks, right_previous, right_current)) - # update _previous for next time left_previous = left_current right_previous = right_current @@ -2460,7 +2748,7 @@ def _odometry_monitor(): self.theta += (right_mm - left_mm) / self.wheel_distance_mm # and clip the rotation to plus or minus 360 degrees - self.theta -= float(int(self.theta/TWO_PI) * TWO_PI) + self.theta -= float(int(self.theta / TWO_PI) * TWO_PI) # now calculate and accumulate our position in mm self.x_pos_mm += mm * math.cos(self.theta) @@ -2469,66 +2757,26 @@ def _odometry_monitor(): if sleep_time: time.sleep(sleep_time) - self.odometry_thread_id = None + _thread.start_new_thread(_odometry_monitor, ()) - self.odometry_thread_run = True - self.odometry_thread_id = _thread.start_new_thread(_odometry_monitor, ()) + # Block until the thread has started doing work + while not self.odometry_thread_run: + pass def odometry_stop(self): """ - Signal the odometry thread to exit and wait for it to exit + Signal the odometry thread to exit """ - if self.odometry_thread_id: + if self.odometry_thread_run: self.odometry_thread_run = False - while self.odometry_thread_id: - pass - - def turn_to_angle(self, speed, angle_target_degrees, brake=True, block=True): - """ - Rotate in place to `angle_target_degrees` at `speed` - """ - assert self.odometry_thread_id, "odometry_start() must be called to track robot coordinates" - - # Make both target and current angles positive numbers between 0 and 360 - if angle_target_degrees < 0: - angle_target_degrees += 360 - - angle_current_degrees = math.degrees(self.theta) - - if angle_current_degrees < 0: - angle_current_degrees += 360 - - # Is it shorter to rotate to the right or left - # to reach angle_target_degrees? - if angle_current_degrees > angle_target_degrees: - turn_right = True - angle_delta = angle_current_degrees - angle_target_degrees - else: - turn_right = False - angle_delta = angle_target_degrees - angle_current_degrees - - if angle_delta > 180: - angle_delta = 360 - angle_delta - turn_right = not turn_right - - log.debug("%s: turn_to_angle %s, current angle %s, delta %s, turn_right %s" % - (self, angle_target_degrees, angle_current_degrees, angle_delta, turn_right)) - self.odometry_coordinates_log() - - if turn_right: - self.turn_right(speed, angle_delta, brake, block) - else: - self.turn_left(speed, angle_delta, brake, block) - - self.odometry_coordinates_log() - def on_to_coordinates(self, speed, x_target_mm, y_target_mm, brake=True, block=True): """ - Drive to (`x_target_mm`, `y_target_mm`) coordinates at `speed` + Drive to (``x_target_mm``, ``y_target_mm``) coordinates at ``speed`` """ - assert self.odometry_thread_id, "odometry_start() must be called to track robot coordinates" + if not self.odometry_thread_run: + raise ThreadNotRunning("odometry_start() must be called to track robot coordinates") # stop moving self.off(brake='hold') @@ -2549,10 +2797,9 @@ class MoveJoystick(MoveTank): """ Used to control a pair of motors via a single joystick vector. """ - def on(self, x, y, radius=100.0): """ - Convert x,y joystick coordinates to left/right motor speed percentages + Convert ``x``,``y`` joystick coordinates to left/right motor speed percentages and move the motors. This will use a classic "arcade drive" algorithm: a full-forward joystick @@ -2561,11 +2808,11 @@ def on(self, x, y, radius=100.0): Positions in the middle will control how fast the vehicle moves and how sharply it turns. - "x", "y": + ``x``, ``y``: The X and Y coordinates of the joystick's position, with (0,0) representing the center position. X is horizontal and Y is vertical. - radius (default 100): + ``radius`` (default 100): The radius of the joystick, controlling the range of the input (x, y) values. e.g. if "x" and "y" can be between -1 and 1, radius should be set to "1". """ @@ -2591,22 +2838,20 @@ def on(self, x, y, radius=100.0): left_speed_percentage = (init_left_speed_percentage * vector_length) / radius right_speed_percentage = (init_right_speed_percentage * vector_length) / radius - # log.debug(""" - # x, y : %s, %s - # radius : %s - # angle : %s - # vector length : %s - # init left_speed_percentage : %s - # init right_speed_percentage : %s - # final left_speed_percentage : %s - # final right_speed_percentage : %s - # """ % (x, y, radius, angle, vector_length, - # init_left_speed_percentage, init_right_speed_percentage, - # left_speed_percentage, right_speed_percentage)) - - MoveTank.on(self, - SpeedPercent(left_speed_percentage), - SpeedPercent(right_speed_percentage)) + # log.debug(""" + # x, y : %s, %s + # radius : %s + # angle : %s + # vector length : %s + # init left_speed_percentage : %s + # init right_speed_percentage : %s + # final left_speed_percentage : %s + # final right_speed_percentage : %s + # """ % (x, y, radius, angle, vector_length, + # init_left_speed_percentage, init_right_speed_percentage, + # left_speed_percentage, right_speed_percentage)) + + MoveTank.on(self, SpeedPercent(left_speed_percentage), SpeedPercent(right_speed_percentage)) @staticmethod def angle_to_speed_percentage(angle): @@ -2667,7 +2912,7 @@ def angle_to_speed_percentage(angle): left_speed_percentage = 1 # right motor transitions from -1 to 0 - right_speed_percentage = -1 + (angle/45.0) + right_speed_percentage = -1 + (angle / 45.0) elif 45 < angle <= 90: @@ -2757,6 +3002,7 @@ def angle_to_speed_percentage(angle): right_speed_percentage = -1 * percentage_from_315_to_360 else: - raise Exception('You created a circle with more than 360 degrees ({})...that is quite the trick'.format(angle)) + raise Exception( + 'You created a circle with more than 360 degrees ({})...that is quite the trick'.format(angle)) return (left_speed_percentage * 100, right_speed_percentage * 100) diff --git a/ev3dev2/port.py b/ev3dev2/port.py index faeccc7..264e37b 100644 --- a/ev3dev2/port.py +++ b/ev3dev2/port.py @@ -26,18 +26,21 @@ import sys from . import Device -if sys.version_info < (3,4): +if sys.version_info < (3, 4): raise SystemError('Must be using Python 3.4 or higher') class LegoPort(Device): """ - The `lego-port` class provides an interface for working with input and + The ``lego-port`` class provides an interface for working with input and output ports that are compatible with LEGO MINDSTORMS RCX/NXT/EV3, LEGO WeDo and LEGO Power Functions sensors and motors. Supported devices include the LEGO MINDSTORMS EV3 Intelligent Brick, the LEGO WeDo USB hub and various sensor multiplexers from 3rd party manufacturers. + See the following example for using this class to configure sensors: + https://github.com/ev3dev/ev3dev-lang-python-demo/blob/stretch/platform/brickpi3-motor-and-sensor.py + Some types of ports may have multiple modes of operation. For example, the input ports on the EV3 brick can communicate with sensors using UART, I2C or analog validate signals - but not all at the same time. Therefore there @@ -45,29 +48,29 @@ class LegoPort(Device): In most cases, ports are able to automatically detect what type of sensor or motor is connected. In some cases though, this must be manually specified - using the `mode` and `set_device` attributes. The `mode` attribute affects + using the ``mode`` and ``set_device`` attributes. The ``mode`` attribute affects how the port communicates with the connected device. For example the input ports on the EV3 brick can communicate using UART, I2C or analog voltages, but not all at the same time, so the mode must be set to the one that is - appropriate for the connected sensor. The `set_device` attribute is used to + appropriate for the connected sensor. The ``set_device`` attribute is used to specify the exact type of sensor that is connected. Note: the mode must be correctly set before setting the sensor type. - Ports can be found at `/sys/class/lego-port/port` where `` is + Ports can be found at ``/sys/class/lego-port/port`` where ```` is incremented each time a new port is registered. Note: The number is not - related to the actual port at all - use the `address` attribute to find + related to the actual port at all - use the ``address`` attribute to find a specific port. """ SYSTEM_CLASS_NAME = 'lego-port' SYSTEM_DEVICE_NAME_CONVENTION = '*' __slots__ = [ - '_address', - '_driver_name', - '_modes', - '_mode', - '_set_device', - '_status', + '_address', + '_driver_name', + '_modes', + '_mode', + '_set_device', + '_status', ] def __init__(self, address=None, name_pattern=SYSTEM_DEVICE_NAME_CONVENTION, name_exact=False, **kwargs): @@ -142,9 +145,9 @@ def set_device(self, value): @property def status(self): """ - In most cases, reading status will return the same value as `mode`. In - cases where there is an `auto` mode additional values may be returned, - such as `no-device` or `error`. See individual port driver documentation + In most cases, reading status will return the same value as ``mode``. In + cases where there is an ``auto`` mode additional values may be returned, + such as ``no-device`` or ``error``. See individual port driver documentation for the full list of possible values. """ self._status, value = self.get_attr_string(self._status, 'status') diff --git a/ev3dev2/power.py b/ev3dev2/power.py index afac94d..d33c38f 100644 --- a/ev3dev2/power.py +++ b/ev3dev2/power.py @@ -24,12 +24,11 @@ # ----------------------------------------------------------------------------- import sys +from ev3dev2 import Device -if sys.version_info < (3,4): +if sys.version_info < (3, 4): raise SystemError('Must be using Python 3.4 or higher') -from ev3dev2 import Device - class PowerSupply(Device): """ @@ -40,12 +39,12 @@ class PowerSupply(Device): SYSTEM_CLASS_NAME = 'power_supply' SYSTEM_DEVICE_NAME_CONVENTION = '*' __slots__ = [ - '_measured_current', - '_measured_voltage', - '_max_voltage', - '_min_voltage', - '_technology', - '_type', + '_measured_current', + '_measured_voltage', + '_max_voltage', + '_min_voltage', + '_technology', + '_type', ] def __init__(self, address=None, name_pattern=SYSTEM_DEVICE_NAME_CONVENTION, name_exact=False, **kwargs): @@ -79,29 +78,21 @@ def measured_voltage(self): @property def max_voltage(self): - """ - """ self._max_voltage, value = self.get_attr_int(self._max_voltage, 'voltage_max_design') return value @property def min_voltage(self): - """ - """ self._min_voltage, value = self.get_attr_int(self._min_voltage, 'voltage_min_design') return value @property def technology(self): - """ - """ self._technology, value = self.get_attr_string(self._technology, 'technology') return value @property def type(self): - """ - """ self._type, value = self.get_attr_string(self._type, 'type') return value diff --git a/ev3dev2/sensor/__init__.py b/ev3dev2/sensor/__init__.py index d082bc9..fc63dca 100644 --- a/ev3dev2/sensor/__init__.py +++ b/ev3dev2/sensor/__init__.py @@ -24,43 +24,39 @@ # ----------------------------------------------------------------------------- import sys - -if sys.version_info < (3,4): - raise SystemError('Must be using Python 3.4 or higher') - -import numbers from os.path import abspath from struct import unpack from ev3dev2 import get_current_platform, Device, list_device_names - # INPUT ports have platform specific values that we must import platform = get_current_platform() if platform == 'ev3': - from ev3dev2._platform.ev3 import INPUT_1, INPUT_2, INPUT_3, INPUT_4 + from ev3dev2._platform.ev3 import INPUT_1, INPUT_2, INPUT_3, INPUT_4 # noqa: F401 elif platform == 'evb': - from ev3dev2._platform.evb import INPUT_1, INPUT_2, INPUT_3, INPUT_4 + from ev3dev2._platform.evb import INPUT_1, INPUT_2, INPUT_3, INPUT_4 # noqa: F401 elif platform == 'pistorms': - from ev3dev2._platform.pistorms import INPUT_1, INPUT_2, INPUT_3, INPUT_4 + from ev3dev2._platform.pistorms import INPUT_1, INPUT_2, INPUT_3, INPUT_4 # noqa: F401 elif platform == 'brickpi': - from ev3dev2._platform.brickpi import INPUT_1, INPUT_2, INPUT_3, INPUT_4 + from ev3dev2._platform.brickpi import INPUT_1, INPUT_2, INPUT_3, INPUT_4 # noqa: F401 elif platform == 'brickpi3': - from ev3dev2._platform.brickpi3 import INPUT_1, INPUT_2, INPUT_3, INPUT_4, \ - INPUT_5, INPUT_6, INPUT_7, INPUT_8, \ - INPUT_9, INPUT_10, INPUT_11, INPUT_12, \ - INPUT_13, INPUT_14, INPUT_15, INPUT_16 + from ev3dev2._platform.brickpi3 import ( # noqa: F401 + INPUT_1, INPUT_2, INPUT_3, INPUT_4, INPUT_5, INPUT_6, INPUT_7, INPUT_8, INPUT_9, INPUT_10, INPUT_11, INPUT_12, + INPUT_13, INPUT_14, INPUT_15, INPUT_16) elif platform == 'fake': - from ev3dev2._platform.fake import INPUT_1, INPUT_2, INPUT_3, INPUT_4 + from ev3dev2._platform.fake import INPUT_1, INPUT_2, INPUT_3, INPUT_4 # noqa: F401 else: raise Exception("Unsupported platform '%s'" % platform) +if sys.version_info < (3, 4): + raise SystemError('Must be using Python 3.4 or higher') + class Sensor(Device): """ @@ -71,20 +67,8 @@ class Sensor(Device): SYSTEM_CLASS_NAME = 'lego-sensor' SYSTEM_DEVICE_NAME_CONVENTION = 'sensor*' __slots__ = [ - '_address', - '_command', - '_commands', - '_decimals', - '_driver_name', - '_mode', - '_modes', - '_num_values', - '_units', - '_value', - '_bin_data_format', - '_bin_data_size', - '_bin_data', - '_mode_scale' + '_address', '_command', '_commands', '_decimals', '_driver_name', '_mode', '_modes', '_num_values', '_units', + '_value', '_bin_data_format', '_bin_data_size', '_bin_data', '_mode_scale' ] def __init__(self, address=None, name_pattern=SYSTEM_DEVICE_NAME_CONVENTION, name_exact=False, **kwargs): @@ -102,7 +86,7 @@ def __init__(self, address=None, name_pattern=SYSTEM_DEVICE_NAME_CONVENTION, nam self._modes = None self._num_values = None self._units = None - self._value = [None,None,None,None,None,None,None,None] + self._value = [None, None, None, None, None, None, None, None] self._bin_data_format = None self._bin_data_size = None @@ -124,8 +108,8 @@ def _scale(self, mode): @property def address(self): """ - Returns the name of the port that the sensor is connected to, e.g. `ev3:in1`. - I2C sensors also include the I2C address (decimal), e.g. `ev3:in1:i2c8`. + Returns the name of the port that the sensor is connected to, e.g. ``ev3:in1``. + I2C sensors also include the I2C address (decimal), e.g. ``ev3:in1:i2c8``. """ self._address, value = self.get_attr_string(self._address, 'address') return value @@ -153,7 +137,7 @@ def commands(self): @property def decimals(self): """ - Returns the number of decimal places for the values in the `value` + Returns the number of decimal places for the values in the ``value`` attributes of the current mode. """ self._decimals, value = self.get_attr_int(self._decimals, 'decimals') @@ -171,7 +155,7 @@ def driver_name(self): @property def mode(self): """ - Returns the current mode. Writing one of the values returned by `modes` + Returns the current mode. Writing one of the values returned by ``modes`` sets the sensor to that mode. """ self._mode, value = self.get_attr_string(self._mode, 'mode') @@ -192,7 +176,7 @@ def modes(self): @property def num_values(self): """ - Returns the number of `value` attributes that will return a valid value + Returns the number of ``value`` attributes that will return a valid value for the current mode. """ self._num_values, value = self.get_attr_int(self._num_values, 'num_values') @@ -216,33 +200,33 @@ def value(self, n=0): """ n = int(n) - self._value[n], value = self.get_attr_int(self._value[n], 'value'+str(n)) + self._value[n], value = self.get_attr_int(self._value[n], 'value' + str(n)) return value @property def bin_data_format(self): """ - Returns the format of the values in `bin_data` for the current mode. + Returns the format of the values in ``bin_data`` for the current mode. Possible values are: - - `u8`: Unsigned 8-bit integer (byte) - - `s8`: Signed 8-bit integer (sbyte) - - `u16`: Unsigned 16-bit integer (ushort) - - `s16`: Signed 16-bit integer (short) - - `s16_be`: Signed 16-bit integer, big endian - - `s32`: Signed 32-bit integer (int) - - `float`: IEEE 754 32-bit floating point (float) + - ``u8``: Unsigned 8-bit integer (byte) + - ``s8``: Signed 8-bit integer (sbyte) + - ``u16``: Unsigned 16-bit integer (ushort) + - ``s16``: Signed 16-bit integer (short) + - ``s16_be``: Signed 16-bit integer, big endian + - ``s32``: Signed 32-bit integer (int) + - ``float``: IEEE 754 32-bit floating point (float) """ self._bin_data_format, value = self.get_attr_string(self._bin_data_format, 'bin_data_format') return value def bin_data(self, fmt=None): """ - Returns the unscaled raw values in the `value` attributes as raw byte - array. Use `bin_data_format`, `num_values` and the individual sensor + Returns the unscaled raw values in the ``value`` attributes as raw byte + array. Use ``bin_data_format``, ``num_values`` and the individual sensor documentation to determine how to interpret the data. - Use `fmt` to unpack the raw bytes into a struct. + Use ``fmt`` to unpack the raw bytes into a struct. Example:: @@ -254,31 +238,33 @@ def bin_data(self, fmt=None): (28,) """ - if self._bin_data_size == None: + if self._bin_data_size is None: self._bin_data_size = { - "u8": 1, - "s8": 1, - "u16": 2, - "s16": 2, - "s16_be": 2, - "s32": 4, - "float": 4 - }.get(self.bin_data_format, 1) * self.num_values - - if None == self._bin_data: - self._bin_data = self._attribute_file_open( 'bin_data' ) + "u8": 1, + "s8": 1, + "u16": 2, + "s16": 2, + "s16_be": 2, + "s32": 4, + "float": 4 + }.get(self.bin_data_format, 1) * self.num_values + + if self._bin_data is None: + self._bin_data = self._attribute_file_open('bin_data') self._bin_data.seek(0) raw = bytearray(self._bin_data.read(self._bin_data_size)) - if fmt is None: return raw + if fmt is None: + return raw return unpack(fmt, raw) - + def _ensure_mode(self, mode): if self.mode != mode: self.mode = mode + def list_sensors(name_pattern=Sensor.SYSTEM_DEVICE_NAME_CONVENTION, **kwargs): """ This is a generator function that enumerates all sensors that match the diff --git a/ev3dev2/sensor/lego.py b/ev3dev2/sensor/lego.py index 1bd1efe..281d983 100644 --- a/ev3dev2/sensor/lego.py +++ b/ev3dev2/sensor/lego.py @@ -24,14 +24,16 @@ # ----------------------------------------------------------------------------- import sys - -if sys.version_info < (3,4): - raise SystemError('Must be using Python 3.4 or higher') - +import logging import time from ev3dev2.button import ButtonBase from ev3dev2.sensor import Sensor +if sys.version_info < (3, 4): + raise SystemError('Must be using Python 3.4 or higher') + +log = logging.getLogger(__name__) + class TouchSensor(Sensor): """ @@ -43,10 +45,14 @@ class TouchSensor(Sensor): #: Button state MODE_TOUCH = 'TOUCH' - MODES = (MODE_TOUCH,) + MODES = (MODE_TOUCH, ) def __init__(self, address=None, name_pattern=SYSTEM_DEVICE_NAME_CONVENTION, name_exact=False, **kwargs): - super(TouchSensor, self).__init__(address, name_pattern, name_exact, driver_name=['lego-ev3-touch', 'lego-nxt-touch'], **kwargs) + super(TouchSensor, self).__init__(address, + name_pattern, + name_exact, + driver_name=['lego-ev3-touch', 'lego-nxt-touch'], + **kwargs) @property def is_pressed(self): @@ -65,7 +71,7 @@ def _wait(self, wait_for_press, timeout_ms, sleep_ms): tic = time.time() if sleep_ms: - sleep_ms = float(sleep_ms/1000) + sleep_ms = float(sleep_ms / 1000) # The kernel does not supoort POLLPRI or POLLIN for sensors so we have # to drop into a loop and check often @@ -156,23 +162,17 @@ class ColorSensor(Sensor): #: Brown color. COLOR_BROWN = 7 - MODES = ( - MODE_COL_REFLECT, - MODE_COL_AMBIENT, - MODE_COL_COLOR, - MODE_REF_RAW, - MODE_RGB_RAW - ) + MODES = (MODE_COL_REFLECT, MODE_COL_AMBIENT, MODE_COL_COLOR, MODE_REF_RAW, MODE_RGB_RAW) COLORS = ( - 'NoColor', - 'Black', - 'Blue', - 'Green', - 'Yellow', - 'Red', - 'White', - 'Brown', + 'NoColor', + 'Black', + 'Blue', + 'Green', + 'Yellow', + 'Red', + 'White', + 'Brown', ) def __init__(self, address=None, name_pattern=SYSTEM_DEVICE_NAME_CONVENTION, name_exact=False, **kwargs): @@ -226,7 +226,7 @@ def color_name(self): def raw(self): """ Red, green, and blue components of the detected color, as a tuple. - + Officially in the range 0-1020 but the values returned will never be that high. We do not yet know why the values returned are low, but pointing the color sensor at a well lit sheet of white paper will return @@ -265,9 +265,8 @@ def rgb(self): """ (red, green, blue) = self.raw - return (min(int((red * 255) / self.red_max), 255), - min(int((green * 255) / self.green_max), 255), - min(int((blue * 255) / self.blue_max), 255)) + return (min(int((red * 255) / self.red_max), 255), min(int((green * 255) / self.green_max), + 255), min(int((blue * 255) / self.blue_max), 255)) @property def lab(self): @@ -294,8 +293,8 @@ def lab(self): Y = (RGB[0] * 0.2126729) + (RGB[1] * 0.7151522) + (RGB[2] * 0.0721750) Z = (RGB[0] * 0.0193339) + (RGB[1] * 0.1191920) + (RGB[2] * 0.9503041) - XYZ[0] = X / 95.047 # ref_X = 95.047 - XYZ[1] = Y / 100.0 # ref_Y = 100.000 + XYZ[0] = X / 95.047 # ref_X = 95.047 + XYZ[1] = Y / 100.0 # ref_Y = 100.000 XYZ[2] = Z / 108.883 # ref_Z = 108.883 for (num, value) in enumerate(XYZ): @@ -332,19 +331,19 @@ def hsv(self): if minc == maxc: return 0.0, 0.0, v - s = (maxc-minc) / maxc - rc = (maxc-r) / (maxc-minc) - gc = (maxc-g) / (maxc-minc) - bc = (maxc-b) / (maxc-minc) + s = (maxc - minc) / maxc + rc = (maxc - r) / (maxc - minc) + gc = (maxc - g) / (maxc - minc) + bc = (maxc - b) / (maxc - minc) if r == maxc: - h = bc-gc + h = bc - gc elif g == maxc: - h = 2.0+rc-bc + h = 2.0 + rc - bc else: - h = 4.0+gc-rc + h = 4.0 + gc - rc - h = (h/6.0) % 1.0 + h = (h / 6.0) % 1.0 return (h, s, v) @@ -356,36 +355,36 @@ def hls(self): L: color lightness S: color saturation """ - (r, g, b) = self.rgb - maxc = max(r, g, b) - minc = min(r, g, b) - l = (minc+maxc)/2.0 + (red, green, blue) = self.rgb + maxc = max(red, green, blue) + minc = min(red, green, blue) + luminance = (minc + maxc) / 2.0 if minc == maxc: - return 0.0, l, 0.0 + return 0.0, luminance, 0.0 - if l <= 0.5: - s = (maxc-minc) / (maxc+minc) + if luminance <= 0.5: + saturation = (maxc - minc) / (maxc + minc) else: - if 2.0-maxc-minc == 0: - s = 0 + if 2.0 - maxc - minc == 0: + saturation = 0 else: - s = (maxc-minc) / (2.0-maxc-minc) + saturation = (maxc - minc) / (2.0 - maxc - minc) - rc = (maxc-r) / (maxc-minc) - gc = (maxc-g) / (maxc-minc) - bc = (maxc-b) / (maxc-minc) + rc = (maxc - red) / (maxc - minc) + gc = (maxc - green) / (maxc - minc) + bc = (maxc - blue) / (maxc - minc) - if r == maxc: - h = bc-gc - elif g == maxc: - h = 2.0+rc-bc + if red == maxc: + hue = bc - gc + elif green == maxc: + hue = 2.0 + rc - bc else: - h = 4.0+gc-rc + hue = 4.0 + gc - rc - h = (h/6.0) % 1.0 + hue = (hue / 6.0) % 1.0 - return (h, l, s) + return (hue, luminance, saturation) @property def red(self): @@ -444,7 +443,11 @@ class UltrasonicSensor(Sensor): ) def __init__(self, address=None, name_pattern=SYSTEM_DEVICE_NAME_CONVENTION, name_exact=False, **kwargs): - super(UltrasonicSensor, self).__init__(address, name_pattern, name_exact, driver_name=['lego-ev3-us', 'lego-nxt-us'], **kwargs) + super(UltrasonicSensor, self).__init__(address, + name_pattern, + name_exact, + driver_name=['lego-ev3-us', 'lego-nxt-us'], + **kwargs) @property def distance_centimeters_continuous(self): @@ -586,6 +589,7 @@ class GyroSensor(Sensor): def __init__(self, address=None, name_pattern=SYSTEM_DEVICE_NAME_CONVENTION, name_exact=False, **kwargs): super(GyroSensor, self).__init__(address, name_pattern, name_exact, driver_name='lego-ev3-gyro', **kwargs) self._direct = None + self._init_angle = self.angle @property def angle(self): @@ -622,6 +626,15 @@ def tilt_rate(self): self._ensure_mode(self.MODE_TILT_RATE) return self.value(0) + def calibrate(self): + """ + The robot should be perfectly still when you call this + """ + current_mode = self.mode + self._ensure_mode(self.MODE_GYRO_CAL) + time.sleep(2) + self._ensure_mode(current_mode) + def reset(self): """Resets the angle to 0. @@ -631,7 +644,8 @@ def reset(self): PiStorms, or with any sensor multiplexors. """ # 17 comes from inspecting the .vix file of the Gyro sensor block in EV3-G - self._direct = self.set_attr_raw(self._direct, 'direct', bytes(17,)) + self._direct = self.set_attr_raw(self._direct, 'direct', b'\x11') + self._init_angle = self.angle def wait_until_angle_changed_by(self, delta, direction_sensitive=False): """ @@ -660,6 +674,61 @@ def wait_until_angle_changed_by(self, delta, direction_sensitive=False): while abs(start_angle - self.value(0)) < delta: time.sleep(0.01) + def circle_angle(self): + """ + As the gryo rotates clockwise the angle increases, it will increase + by 360 for each full rotation. As the gyro rotates counter-clockwise + the gyro angle will decrease. + + The angles on a circle have the opposite behavior though, they start + at 0 and increase as you move counter-clockwise around the circle. + + Convert the gyro angle to the angle on a circle. We consider the initial + position of the gyro to be at 90 degrees on the cirlce. + """ + current_angle = self.angle + delta = abs(current_angle - self._init_angle) % 360 + + if delta == 0: + result = 90 + + # the gyro has turned clockwise relative to where we started + elif current_angle > self._init_angle: + + if delta <= 90: + result = 90 - delta + + elif delta <= 180: + result = 360 - (delta - 90) + + elif delta <= 270: + result = 270 - (delta - 180) + + else: + result = 180 - (delta - 270) + + # This can be chatty (but helpful) so save it for a rainy day + # log.info("%s moved clockwise %s degrees to %s" % (self, delta, result)) + + # the gyro has turned counter-clockwise relative to where we started + else: + if delta <= 90: + result = 90 + delta + + elif delta <= 180: + result = 180 + (delta - 90) + + elif delta <= 270: + result = 270 + (delta - 180) + + else: + result = delta - 270 + + # This can be chatty (but helpful) so save it for a rainy day + # log.info("%s moved counter-clockwise %s degrees to %s" % (self, delta, result)) + + return result + class InfraredSensor(Sensor, ButtonBase): """ @@ -684,32 +753,26 @@ class InfraredSensor(Sensor, ButtonBase): #: Calibration ??? MODE_IR_CAL = 'IR-CAL' - MODES = ( - MODE_IR_PROX, - MODE_IR_SEEK, - MODE_IR_REMOTE, - MODE_IR_REM_A, - MODE_IR_CAL - ) + MODES = (MODE_IR_PROX, MODE_IR_SEEK, MODE_IR_REMOTE, MODE_IR_REM_A, MODE_IR_CAL) # The following are all of the various combinations of button presses for # the remote control. The key/index is the number that will be written in # the attribute file to indicate what combination of buttons are currently # pressed. _BUTTON_VALUES = { - 0: [], - 1: ['top_left'], - 2: ['bottom_left'], - 3: ['top_right'], - 4: ['bottom_right'], - 5: ['top_left', 'top_right'], - 6: ['top_left', 'bottom_right'], - 7: ['bottom_left', 'top_right'], - 8: ['bottom_left', 'bottom_right'], - 9: ['beacon'], - 10: ['top_left', 'bottom_left'], - 11: ['top_right', 'bottom_right'] - } + 0: [], + 1: ['top_left'], + 2: ['bottom_left'], + 3: ['top_right'], + 4: ['bottom_right'], + 5: ['top_left', 'top_right'], + 6: ['top_left', 'bottom_right'], + 7: ['bottom_left', 'top_right'], + 8: ['bottom_left', 'bottom_right'], + 9: ['beacon'], + 10: ['top_left', 'bottom_left'], + 11: ['top_right', 'bottom_right'] + } _BUTTONS = ('top_left', 'bottom_left', 'top_right', 'bottom_right', 'beacon') @@ -861,7 +924,7 @@ def process(self): old state, call the appropriate button event handlers. To use the on_channel1_top_left, etc handlers your program would do something like: - + .. code:: python def top_left_channel_1_action(state): @@ -877,12 +940,12 @@ def bottom_right_channel_4_action(state): while True: ir.process() time.sleep(0.01) - + """ new_state = [] state_diff = [] - for channel in range(1,5): + for channel in range(1, 5): for button in self.buttons_pressed(channel): new_state.append((button, channel)) @@ -896,11 +959,10 @@ def bottom_right_channel_4_action(state): if (button, channel) not in new_state and (button, channel) in self._state: state_diff.append((button, channel)) - old_state = self._state self._state = new_state for (button, channel) in state_diff: - handler = getattr(self, 'on_channel' + str(channel) + '_' + button ) + handler = getattr(self, 'on_channel' + str(channel) + '_' + button) if handler is not None: handler((button, channel) in new_state) @@ -924,8 +986,8 @@ class SoundSensor(Sensor): MODE_DBA = 'DBA' MODES = ( - MODE_DB, - MODE_DBA, + MODE_DB, + MODE_DBA, ) def __init__(self, address=None, name_pattern=SYSTEM_DEVICE_NAME_CONVENTION, name_exact=False, **kwargs): @@ -965,8 +1027,8 @@ class LightSensor(Sensor): MODE_AMBIENT = 'AMBIENT' MODES = ( - MODE_REFLECT, - MODE_AMBIENT, + MODE_REFLECT, + MODE_AMBIENT, ) def __init__(self, address=None, name_pattern=SYSTEM_DEVICE_NAME_CONVENTION, name_exact=False, **kwargs): diff --git a/ev3dev2/sound.py b/ev3dev2/sound.py index cd2f4e4..350d82f 100644 --- a/ev3dev2/sound.py +++ b/ev3dev2/sound.py @@ -24,14 +24,14 @@ # ----------------------------------------------------------------------------- import sys +import os +import re +from time import sleep +from ev3dev2 import is_micropython if sys.version_info < (3, 4): raise SystemError('Must be using Python 3.4 or higher') -from ev3dev2 import is_micropython -import os -import re - if not is_micropython(): import shlex from subprocess import Popen, PIPE @@ -43,11 +43,10 @@ def _make_scales(notes): for note, freq in notes: freq = round(freq) for n in note.split('/'): - res[n] = freq + res[n.upper()] = freq return res - def get_command_processes(command): """ :param string command: a string of command(s) to run that may include pipes @@ -77,14 +76,18 @@ class Sound(object): Examples:: + from ev3dev2.sound import Sound + + spkr = Sound() + # Play 'bark.wav': - Sound.play_file('bark.wav') + spkr.play_file('bark.wav') # Introduce yourself: - Sound.speak('Hello, I am Robot') + spkr.speak('Hello, I am Robot') # Play a small song - Sound.play_song(( + spkr.play_song(( ('D4', 'e3'), ('D4', 'e3'), ('D4', 'e3'), @@ -100,15 +103,11 @@ class Sound(object): channel = None # play_types - PLAY_WAIT_FOR_COMPLETE = 0 #: Play the sound and block until it is complete - PLAY_NO_WAIT_FOR_COMPLETE = 1 #: Start playing the sound but return immediately - PLAY_LOOP = 2 #: Never return; start the sound immediately after it completes, until the program is killed + PLAY_WAIT_FOR_COMPLETE = 0 #: Play the sound and block until it is complete + PLAY_NO_WAIT_FOR_COMPLETE = 1 #: Start playing the sound but return immediately + PLAY_LOOP = 2 #: Never return; start the sound immediately after it completes, until the program is killed - PLAY_TYPES = ( - PLAY_WAIT_FOR_COMPLETE, - PLAY_NO_WAIT_FOR_COMPLETE, - PLAY_LOOP - ) + PLAY_TYPES = (PLAY_WAIT_FOR_COMPLETE, PLAY_NO_WAIT_FOR_COMPLETE, PLAY_LOOP) def _validate_play_type(self, play_type): assert play_type in self.PLAY_TYPES, \ @@ -133,7 +132,7 @@ def _audio_command(self, command, play_type): return None else: - with open(os.devnull, 'w') as n: + with open(os.devnull, 'w'): if play_type == Sound.PLAY_WAIT_FOR_COMPLETE: processes = get_command_processes(command) @@ -164,7 +163,8 @@ def beep(self, args='', play_type=PLAY_WAIT_FOR_COMPLETE): :param play_type: The behavior of ``beep`` once playback has been initiated :type play_type: ``Sound.PLAY_WAIT_FOR_COMPLETE`` or ``Sound.PLAY_NO_WAIT_FOR_COMPLETE`` - :return: When python3 is used and ``Sound.PLAY_NO_WAIT_FOR_COMPLETE`` is specified, returns the returns the spawn subprocess from ``subprocess.Popen``; ``None`` otherwise + :return: When python3 is used and ``Sound.PLAY_NO_WAIT_FOR_COMPLETE`` is specified, returns the + spawn subprocess from ``subprocess.Popen``; ``None`` otherwise .. _`beep man page`: https://linux.die.net/man/1/beep .. _`linux beep music`: https://www.google.com/search?q=linux+beep+music @@ -204,12 +204,15 @@ def tone(self, *args, play_type=PLAY_WAIT_FOR_COMPLETE): Have also a look at :py:meth:`play_song` for a more musician-friendly way of doing, which uses the conventional notation for notes and durations. - :param list[tuple(float,float,float)] tone_sequence: The sequence of tones to play. The first number of each tuple is frequency in Hz, the second is duration in milliseconds, and the third is delay in milliseconds between this and the next tone in the sequence. + :param list[tuple(float,float,float)] tone_sequence: The sequence of tones to play. The first + number of each tuple is frequency in Hz, the second is duration in milliseconds, and the + third is delay in milliseconds between this and the next tone in the sequence. :param play_type: The behavior of ``tone`` once playback has been initiated :type play_type: ``Sound.PLAY_WAIT_FOR_COMPLETE`` or ``Sound.PLAY_NO_WAIT_FOR_COMPLETE`` - :return: When python3 is used and ``Sound.PLAY_NO_WAIT_FOR_COMPLETE`` is specified, returns the returns the spawn subprocess from ``subprocess.Popen``; ``None`` otherwise + :return: When python3 is used and ``Sound.PLAY_NO_WAIT_FOR_COMPLETE`` is specified, returns the + spawn subprocess from ``subprocess.Popen``; ``None`` otherwise .. rubric:: tone(frequency, duration) @@ -221,16 +224,17 @@ def tone(self, *args, play_type=PLAY_WAIT_FOR_COMPLETE): :param play_type: The behavior of ``tone`` once playback has been initiated :type play_type: ``Sound.PLAY_WAIT_FOR_COMPLETE`` or ``Sound.PLAY_NO_WAIT_FOR_COMPLETE`` - :return: When python3 is used and ``Sound.PLAY_NO_WAIT_FOR_COMPLETE`` is specified, returns the returns the spawn subprocess from ``subprocess.Popen``; ``None`` otherwise + :return: When python3 is used and ``Sound.PLAY_NO_WAIT_FOR_COMPLETE`` is specified, returns the + spawn subprocess from ``subprocess.Popen``; ``None`` otherwise """ def play_tone_sequence(tone_sequence): def beep_args(frequency=None, duration=None, delay=None): args = '' if frequency is not None: args += '-f %s ' % frequency - if duration is not None: + if duration is not None: args += '-l %s ' % duration - if delay is not None: + if delay is not None: args += '-D %s ' % delay return args @@ -244,8 +248,7 @@ def beep_args(frequency=None, duration=None, delay=None): else: raise Exception("Unsupported number of parameters in Sound.tone(): expected 1 or 2, got " + str(len(args))) - def play_tone(self, frequency, duration, delay=0.0, volume=100, - play_type=PLAY_WAIT_FOR_COMPLETE): + def play_tone(self, frequency, duration, delay=0.0, volume=100, play_type=PLAY_WAIT_FOR_COMPLETE): """ Play a single tone, specified by its frequency, duration, volume and final delay. :param int frequency: the tone frequency, in Hertz @@ -256,7 +259,8 @@ def play_tone(self, frequency, duration, delay=0.0, volume=100, :param play_type: The behavior of ``play_tone`` once playback has been initiated :type play_type: ``Sound.PLAY_WAIT_FOR_COMPLETE``, ``Sound.PLAY_NO_WAIT_FOR_COMPLETE`` or ``Sound.PLAY_LOOP`` - :return: When python3 is used and ``Sound.PLAY_NO_WAIT_FOR_COMPLETE`` is specified, returns the PID of the underlying beep command; ``None`` otherwise + :return: When python3 is used and ``Sound.PLAY_NO_WAIT_FOR_COMPLETE`` is specified, returns + the PID of the underlying beep command; ``None`` otherwise :raises ValueError: if invalid parameter """ @@ -286,7 +290,8 @@ def play_note(self, note, duration, volume=100, play_type=PLAY_WAIT_FOR_COMPLETE :param play_type: The behavior of ``play_note`` once playback has been initiated :type play_type: ``Sound.PLAY_WAIT_FOR_COMPLETE``, ``Sound.PLAY_NO_WAIT_FOR_COMPLETE`` or ``Sound.PLAY_LOOP`` - :return: When python3 is used and ``Sound.PLAY_NO_WAIT_FOR_COMPLETE`` is specified, returns the PID of the underlying beep command; ``None`` otherwise + :return: When python3 is used and ``Sound.PLAY_NO_WAIT_FOR_COMPLETE`` is specified, returns + the PID of the underlying beep command; ``None`` otherwise :raises ValueError: is invalid parameter (note, duration,...) """ @@ -304,7 +309,8 @@ def play_note(self, note, duration, volume=100, play_type=PLAY_WAIT_FOR_COMPLETE return self.play_tone(freq, duration=duration, volume=volume, play_type=play_type) def play_file(self, wav_file, volume=100, play_type=PLAY_WAIT_FOR_COMPLETE): - """ Play a sound file (wav format) at a given volume. + """ Play a sound file (wav format) at a given volume. The EV3 audio subsystem will work best if + the file is encoded as 16-bit, mono, 22050Hz. :param string wav_file: The sound file path :param int volume: The play volume, in percent of maximum volume @@ -312,7 +318,8 @@ def play_file(self, wav_file, volume=100, play_type=PLAY_WAIT_FOR_COMPLETE): :param play_type: The behavior of ``play_file`` once playback has been initiated :type play_type: ``Sound.PLAY_WAIT_FOR_COMPLETE``, ``Sound.PLAY_NO_WAIT_FOR_COMPLETE`` or ``Sound.PLAY_LOOP`` - :return: When python3 is used and ``Sound.PLAY_NO_WAIT_FOR_COMPLETE`` is specified, returns the spawn subprocess from ``subprocess.Popen``; ``None`` otherwise + :return: When python3 is used and ``Sound.PLAY_NO_WAIT_FOR_COMPLETE`` is specified, returns the + spawn subprocess from ``subprocess.Popen``; ``None`` otherwise """ if not 0 < volume <= 100: raise ValueError('invalid volume (%s)' % volume) @@ -339,7 +346,8 @@ def speak(self, text, espeak_opts='-a 200 -s 130', volume=100, play_type=PLAY_WA :param play_type: The behavior of ``speak`` once playback has been initiated :type play_type: ``Sound.PLAY_WAIT_FOR_COMPLETE``, ``Sound.PLAY_NO_WAIT_FOR_COMPLETE`` or ``Sound.PLAY_LOOP`` - :return: When python3 is used and ``Sound.PLAY_NO_WAIT_FOR_COMPLETE`` is specified, returns the spawn subprocess from ``subprocess.Popen``; ``None`` otherwise + :return: When python3 is used and ``Sound.PLAY_NO_WAIT_FOR_COMPLETE`` is specified, returns the + spawn subprocess from ``subprocess.Popen``; ``None`` otherwise """ self._validate_play_type(play_type) self.set_volume(volume) @@ -398,7 +406,7 @@ def get_volume(self, channel=None): if m: return int(m.group(1)) else: - raise Exception('Failed to parse output of `amixer get {}`'.format(channel)) + raise Exception('Failed to parse output of ``amixer get {}``'.format(channel)) def play_song(self, song, tempo=120, delay=0.05): """ Plays a song provided as a list of tuples containing the note name and its @@ -406,6 +414,7 @@ def play_song(self, song, tempo=120, delay=0.05): and duration. It supports symbolic notes (e.g. ``A4``, ``D#3``, ``Gb5``) and durations (e.g. ``q``, ``h``). + You can also specify rests by using ``R`` instead of note pitch. For an exhaustive list of accepted note symbols and values, have a look at the ``_NOTE_FREQUENCIES`` and ``_NOTE_VALUES`` private dictionaries in the source code. @@ -418,7 +427,7 @@ def play_song(self, song, tempo=120, delay=0.05): Shortcuts exist for common modifiers: - - ``3`` produces a triplet member note. For instance `e3` gives a triplet of eight notes, + - ``3`` produces a triplet member note. For instance ``e3`` gives a triplet of eight notes, i.e. 3 eight notes in the duration of a single quarter. You must ensure that 3 triplets notes are defined in sequence to match the count, otherwise the result will not be the expected one. @@ -429,7 +438,9 @@ def play_song(self, song, tempo=120, delay=0.05): >>> # A long time ago in a galaxy far, >>> # far away... - >>> Sound.play_song(( + >>> from ev3dev2.sound import Sound + >>> spkr = Sound() + >>> spkr.play_song(( >>> ('D4', 'e3'), # intro anacrouse >>> ('D4', 'e3'), >>> ('D4', 'e3'), @@ -469,43 +480,41 @@ def play_song(self, song, tempo=120, delay=0.05): raise ValueError('invalid delay (%s)' % delay) delay_ms = int(delay * 1000) - meas_duration_ms = 60000 / tempo * 4 # we only support 4/4 bars, hence "* 4" - - def beep_args(note, value): - """ Builds the arguments string for producing a beep matching - the requested note and value. - - Args: - note (str): the note note and octave - value (str): the note value expression - Returns: - str: the arguments to be passed to the beep command - """ - freq = self._NOTE_FREQUENCIES.get(note.upper(), self._NOTE_FREQUENCIES[note]) + meas_duration_ms = 60000 / tempo * 4 # we only support 4/4 bars, hence "* 4" + + for (note, value) in song: + value = value.lower() if '/' in value: base, factor = value.split('/') - duration_ms = meas_duration_ms * self._NOTE_VALUES[base] / float(factor) + factor = float(factor) + elif '*' in value: base, factor = value.split('*') - duration_ms = meas_duration_ms * self._NOTE_VALUES[base] * float(factor) + factor = float(factor) + elif value.endswith('.'): base = value[:-1] - duration_ms = meas_duration_ms * self._NOTE_VALUES[base] * 1.5 + factor = 1.5 + elif value.endswith('3'): base = value[:-1] - duration_ms = meas_duration_ms * self._NOTE_VALUES[base] * 2 / 3 + factor = float(2 / 3) + else: - duration_ms = meas_duration_ms * self._NOTE_VALUES[value] + base = value + factor = 1.0 - return '-f %d -l %d -D %d' % (freq, duration_ms, delay_ms) + try: + duration_ms = meas_duration_ms * self._NOTE_VALUES[base] * factor + except KeyError: + raise ValueError('invalid note (%s)' % base) - try: - return self.beep(' -n '.join( - [beep_args(note, value) for (note, value) in song] - )) - except KeyError as e: - raise ValueError('invalid note (%s)' % e) + if note == "R": + sleep(duration_ms / 1000 + delay) + else: + freq = self._NOTE_FREQUENCIES[note.upper()] + self.beep('-f %d -l %d -D %d' % (freq, duration_ms, delay_ms)) #: Note frequencies. #: @@ -517,7 +526,7 @@ def beep_args(note, value): ('C0', 16.35), ('C#0/Db0', 17.32), ('D0', 18.35), - ('D#0/Eb0', 19.45), # expanded in one entry per symbol by _make_scales + ('D#0/Eb0', 19.45), # expanded in one entry per symbol by _make_scales ('E0', 20.60), ('F0', 21.83), ('F#0/Gb0', 23.12), @@ -621,8 +630,7 @@ def beep_args(note, value): ('G#8/Ab8', 6644.88), ('A8', 7040.00), ('A#8/Bb8', 7458.62), - ('B8', 7902.13) - )) + ('B8', 7902.13))) #: Common note values. #: @@ -646,8 +654,8 @@ def beep_args(note, value): #: :py:meth:`Sound.play_song` method. _NOTE_VALUES = { 'w': 1., - 'h': 1./2, - 'q': 1./4, - 'e': 1./8, - 's': 1./16, + 'h': 1. / 2, + 'q': 1. / 4, + 'e': 1. / 8, + 's': 1. / 16, } diff --git a/ev3dev2/stopwatch.py b/ev3dev2/stopwatch.py index 3029b84..60fba91 100644 --- a/ev3dev2/stopwatch.py +++ b/ev3dev2/stopwatch.py @@ -17,68 +17,125 @@ def get_ticks_ms(): return int(dt.datetime.timestamp(dt.datetime.now()) * 1000) -class StopWatch(object): +class StopWatchAlreadyStartedException(Exception): + """ + Exception raised when start() is called on a StopWatch which was already start()ed and not yet + stopped. + """ + pass + +class StopWatch(object): + """ + A timer class which lets you start timing and then check the amount of time + elapsed. + """ def __init__(self, desc=None): + """ + Initializes the StopWatch but does not start it. + + desc: + A string description to print when stringifying. + """ self.desc = desc - self._value = 0 - self.start_time = None - self.prev_update_time = None + self._start_time = None + self._stopped_total_time = None def __str__(self): - if self.desc is not None: - return self.desc - else: - return self.__class__.__name__ + name = self.desc if self.desc is not None else self.__class__.__name__ + return "{}: {}".format(name, self.hms_str) def start(self): - assert self.start_time is None, "%s is already running" % self - self.start_time = get_ticks_ms() - - def update(self): - - if self.start_time is None: - return - - current_time = get_ticks_ms() + """ + Starts the timer. If the timer is already running, resets it. - if self.prev_update_time is None: - delta = current_time - self.start_time - else: - delta = current_time - self.prev_update_time + Raises a :py:class:`ev3dev2.stopwatch.StopWatchAlreadyStartedException` if already started. + """ + if self.is_started: + raise StopWatchAlreadyStartedException() - self._value += delta - self.prev_update_time = current_time + self._stopped_total_time = None + self._start_time = get_ticks_ms() def stop(self): - - if self.start_time is None: + """ + Stops the timer. The time value of this Stopwatch is paused and will not continue increasing. + """ + if self._start_time is None: return - self.update() - self.start_time = None - self.prev_update_time = None + self._stopped_total_time = get_ticks_ms() - self._start_time + self._start_time = None def reset(self): - self.stop() - self._value = 0 + """ + Resets the timer and leaves it stopped. + """ + self._start_time = None + self._stopped_total_time = None + + def restart(self): + """ + Resets and then starts the timer. + """ + self.reset() + self.start() + + @property + def is_started(self): + """ + True if the StopWatch has been started but not stoped (i.e., it's currently running), false otherwise. + """ + return self._start_time is not None @property def value_ms(self): """ Returns the value of the stopwatch in milliseconds """ - self.update() - return self._value + if self._stopped_total_time is not None: + return self._stopped_total_time + + return get_ticks_ms() - self._start_time if self._start_time is not None else 0 + + @property + def value_secs(self): + """ + Returns the value of the stopwatch in seconds + """ + return self.value_ms / 1000 @property def value_hms(self): """ - Returns the value of the stopwatch in HH:MM:SS.msec format + Returns this StopWatch's elapsed time as a tuple + ``(hours, minutes, seconds, milliseconds)``. """ - self.update() - (hours, x) = divmod(int(self._value), 3600000) + (hours, x) = divmod(int(self.value_ms), 3600000) (mins, x) = divmod(x, 60000) (secs, x) = divmod(x, 1000) + return hours, mins, secs, x + + @property + def hms_str(self): + """ + Returns the stringified value of the stopwatch in HH:MM:SS.msec format + """ + return '%02d:%02d:%02d.%03d' % self.value_hms + + def is_elapsed_ms(self, duration_ms): + """ + Returns True if this timer has measured at least ``duration_ms`` + milliseconds. + Otherwise, returns False. If ``duration_ms`` is None, returns False. + """ + + return duration_ms is not None and self.value_ms >= duration_ms + + def is_elapsed_secs(self, duration_secs): + """ + Returns True if this timer has measured at least ``duration_secs`` seconds. + Otherwise, returns False. If ``duration_secs`` is None, returns False. + """ - return '%02d:%02d:%02d.%03d' % (hours, mins, secs, x) + return duration_secs is not None and self.value_secs >= duration_secs diff --git a/ev3dev2/unit.py b/ev3dev2/unit.py index 2a15abb..d7bbf0f 100644 --- a/ev3dev2/unit.py +++ b/ev3dev2/unit.py @@ -20,10 +20,9 @@ # THE SOFTWARE. # ----------------------------------------------------------------------------- - import sys -if sys.version_info < (3,4): +if sys.version_info < (3, 4): raise SystemError('Must be using Python 3.4 or higher') CENTIMETER_MM = 10 @@ -38,7 +37,7 @@ class DistanceValue(object): """ A base class for other unit types. Don't use this directly; instead, see - :class:`DistanceMillimeters`, :class:`DistanceCentimeters`, :class:`DistanceDecimeters`, :class:`DistanceMeters`, + :class:`DistanceMillimeters`, :class:`DistanceCentimeters`, :class:`DistanceDecimeters``, :class:`DistanceMeters`, :class:`DistanceInches`, :class:`DistanceFeet`, :class:`DistanceYards` and :class:`DistanceStuds`. """ @@ -54,7 +53,6 @@ class DistanceMillimeters(DistanceValue): """ Distance in millimeters """ - def __init__(self, millimeters): self.millimeters = millimeters @@ -74,7 +72,6 @@ class DistanceCentimeters(DistanceValue): """ Distance in centimeters """ - def __init__(self, centimeters): self.centimeters = centimeters @@ -94,7 +91,6 @@ class DistanceDecimeters(DistanceValue): """ Distance in decimeters """ - def __init__(self, decimeters): self.decimeters = decimeters @@ -114,7 +110,6 @@ class DistanceMeters(DistanceValue): """ Distance in meters """ - def __init__(self, meters): self.meters = meters @@ -134,7 +129,6 @@ class DistanceInches(DistanceValue): """ Distance in inches """ - def __init__(self, inches): self.inches = inches @@ -154,7 +148,6 @@ class DistanceFeet(DistanceValue): """ Distance in feet """ - def __init__(self, feet): self.feet = feet @@ -174,7 +167,6 @@ class DistanceYards(DistanceValue): """ Distance in yards """ - def __init__(self, yards): self.yards = yards @@ -194,7 +186,6 @@ class DistanceStuds(DistanceValue): """ Distance in studs """ - def __init__(self, studs): self.studs = studs diff --git a/ev3dev2/wheel.py b/ev3dev2/wheel.py index 0f01828..16ccf43 100644 --- a/ev3dev2/wheel.py +++ b/ev3dev2/wheel.py @@ -1,5 +1,4 @@ #!/usr/bin/env python3 - """ Wheel and Rim classes @@ -28,7 +27,6 @@ class Wheel(object): # calculate the number of rotations needed to travel forward 500 mm rotations_for_500mm = 500 / tire.circumference_mm """ - def __init__(self, diameter_mm, width_mm): self.diameter_mm = float(diameter_mm) self.width_mm = float(width_mm) @@ -53,7 +51,6 @@ class EV3Tire(Wheel): part number 44309 comes in set 31313 """ - def __init__(self): Wheel.__init__(self, 43.2, 21) @@ -63,7 +60,6 @@ class EV3EducationSetRim(Wheel): part number 56908 comes in set 45544 """ - def __init__(self): Wheel.__init__(self, 43, 26) @@ -73,6 +69,5 @@ class EV3EducationSetTire(Wheel): part number 41897 comes in set 45544 """ - def __init__(self): Wheel.__init__(self, 56, 28) diff --git a/git_version.py b/git_version.py index 63703c0..2744309 100644 --- a/git_version.py +++ b/git_version.py @@ -1,9 +1,11 @@ from subprocess import Popen, PIPE -import os, sys +import os +import sys pyver = sys.version_info -#---------------------------------------------------------------------------- + +# ---------------------------------------------------------------------------- # Get version string from git # # Author: Douglas Creager @@ -11,16 +13,15 @@ # # PEP 386 adaptation from # https://gist.github.com/ilogue/2567778/f6661ea2c12c070851b2dfb4da8840a6641914bc -#---------------------------------------------------------------------------- +# ---------------------------------------------------------------------------- def call_git_describe(abbrev=4): try: - p = Popen(['git', 'describe', '--abbrev=%d' % abbrev], - stdout=PIPE, stderr=PIPE) + p = Popen(['git', 'describe', '--exclude', 'ev3dev-*', '--abbrev=%d' % abbrev], stdout=PIPE, stderr=PIPE) p.stderr.close() line = p.stdout.readlines()[0] return line.strip().decode('utf8') - except: + except Exception: return None @@ -29,7 +30,7 @@ def read_release_version(): with open('{}/RELEASE-VERSION'.format(os.path.dirname(__file__)), 'r') as f: version = f.readlines()[0] return version.strip() - except: + except Exception: return None @@ -42,7 +43,7 @@ def pep386adapt(version): # adapt git-describe version to be in line with PEP 386 parts = version.split('-') if len(parts) > 1: - parts[-2] = 'post'+parts[-2] + parts[-2] = 'post' + parts[-2] version = '.'.join(parts[:-1]) return version @@ -59,7 +60,7 @@ def git_version(abbrev=4): if version is None: version = release_version else: - #adapt to PEP 386 compatible versioning scheme + # adapt to PEP 386 compatible versioning scheme version = pep386adapt(version) # If we still don't have anything, that's an error. @@ -77,4 +78,3 @@ def git_version(abbrev=4): # Finally, return the current version. return version - diff --git a/setup.py b/setup.py index 9c697d6..6decb29 100644 --- a/setup.py +++ b/setup.py @@ -6,22 +6,16 @@ with open(path.join(this_directory, 'README.rst'), encoding='utf-8') as f: long_description = f.read() -setup( - name='python-ev3dev2', - version=git_version(), - description='v2.x Python language bindings for ev3dev', - author='ev3dev Python team', - author_email='python-team@ev3dev.org', - license='MIT', - url='https://github.com/ev3dev/ev3dev-lang-python', - include_package_data=True, - long_description=long_description, - long_description_content_type='text/x-rst', - packages=['ev3dev2', - 'ev3dev2.fonts', - 'ev3dev2.sensor', - 'ev3dev2.control', - 'ev3dev2._platform'], - package_data={'': ['*.pil', '*.pbm']}, - install_requires=['Pillow'] - ) +setup(name='python-ev3dev2', + version=git_version(), + description='v2.x Python language bindings for ev3dev', + author='ev3dev Python team', + author_email='python-team@ev3dev.org', + license='MIT', + url='https://github.com/ev3dev/ev3dev-lang-python', + include_package_data=True, + long_description=long_description, + long_description_content_type='text/x-rst', + packages=['ev3dev2', 'ev3dev2.fonts', 'ev3dev2.sensor', 'ev3dev2.control', 'ev3dev2._platform'], + package_data={'': ['*.pil', '*.pbm']}, + install_requires=['Pillow']) diff --git a/tests/api_tests.py b/tests/api_tests.py index e979742..7ff4533 100755 --- a/tests/api_tests.py +++ b/tests/api_tests.py @@ -1,5 +1,7 @@ #!/usr/bin/env python3 -import unittest, sys, os.path +import unittest +import sys +import os.path import os FAKE_SYS = os.path.join(os.path.dirname(__file__), 'fake-sys') @@ -8,31 +10,27 @@ sys.path.append(FAKE_SYS) sys.path.append(os.path.join(os.path.dirname(__file__), '..')) -from populate_arena import populate_arena -from clean_arena import clean_arena +from populate_arena import populate_arena # noqa: E402 +from clean_arena import clean_arena # noqa: E402 -import ev3dev2 -from ev3dev2.sensor.lego import InfraredSensor +import ev3dev2 # noqa: E402 +import ev3dev2.stopwatch # noqa: E402 from ev3dev2.motor import \ OUTPUT_A, OUTPUT_B, \ Motor, MediumMotor, LargeMotor, \ MoveTank, MoveSteering, MoveJoystick, \ - SpeedPercent, SpeedDPM, SpeedDPS, SpeedRPM, SpeedRPS, SpeedNativeUnits - -from ev3dev2.unit import ( - DistanceMillimeters, - DistanceCentimeters, - DistanceDecimeters, - DistanceMeters, - DistanceInches, - DistanceFeet, - DistanceYards, - DistanceStuds -) + SpeedPercent, SpeedDPM, SpeedDPS, SpeedRPM, SpeedRPS, SpeedNativeUnits # noqa: E402 +from ev3dev2.sensor.lego import InfraredSensor # noqa: E402 +from ev3dev2.stopwatch import StopWatch, StopWatchAlreadyStartedException # noqa: E402 +from ev3dev2.unit import ( # noqa: E402 + DistanceMillimeters, DistanceCentimeters, DistanceDecimeters, DistanceMeters, DistanceInches, DistanceFeet, + DistanceYards, DistanceStuds) ev3dev2.Device.DEVICE_ROOT_PATH = os.path.join(FAKE_SYS, 'arena') _internal_set_attribute = ev3dev2.Device._set_attribute + + def _set_attribute(self, attribute, name, value): # Follow the text with a newline to separate new content from stuff that # already existed in the buffer. On the real device we're writing to sysfs @@ -41,51 +39,77 @@ def _set_attribute(self, attribute, name, value): attribute = _internal_set_attribute(self, attribute, name, value) attribute.write(b'\n') return attribute + + ev3dev2.Device._set_attribute = _set_attribute _internal_get_attribute = ev3dev2.Device._get_attribute + + def _get_attribute(self, attribute, name): # Split on newline delimiter; see _set_attribute above attribute, value = _internal_get_attribute(self, attribute, name) return attribute, value.split('\n', 1)[0] + + ev3dev2.Device._get_attribute = _get_attribute + def dummy_wait(self, cond, timeout=None): pass + Motor.wait = dummy_wait -class TestAPI(unittest.TestCase): +# for StopWatch +mock_ticks_ms = 0 + + +def _mock_get_ticks_ms(): + return mock_ticks_ms + +ev3dev2.stopwatch.get_ticks_ms = _mock_get_ticks_ms + + +def set_mock_ticks_ms(value): + global mock_ticks_ms + mock_ticks_ms = value + + +class TestAPI(unittest.TestCase): def setUp(self): # micropython does not have _testMethodName try: - print("\n\n{}\n{}".format(self._testMethodName, "=" * len(self._testMethodName,))) + print("\n\n{}\n{}".format(self._testMethodName, "=" * len(self._testMethodName, ))) except AttributeError: pass + # ensure tests don't depend on order based on StopWatch tick state + set_mock_ticks_ms(0) + def test_device(self): clean_arena() populate_arena([('medium_motor', 0, 'outA'), ('infrared_sensor', 0, 'in1')]) - d = ev3dev2.Device('tacho-motor', 'motor*') + ev3dev2.Device('tacho-motor', 'motor*') - d = ev3dev2.Device('tacho-motor', 'motor0') + ev3dev2.Device('tacho-motor', 'motor0') - d = ev3dev2.Device('tacho-motor', 'motor*', driver_name='lego-ev3-m-motor') + ev3dev2.Device('tacho-motor', 'motor*', driver_name='lego-ev3-m-motor') - d = ev3dev2.Device('tacho-motor', 'motor*', address='outA') + ev3dev2.Device('tacho-motor', 'motor*', address='outA') with self.assertRaises(ev3dev2.DeviceNotFound): - d = ev3dev2.Device('tacho-motor', 'motor*', address='outA', driver_name='not-valid') + ev3dev2.Device('tacho-motor', 'motor*', address='outA', driver_name='not-valid') with self.assertRaises(ev3dev2.DeviceNotFound): - d = ev3dev2.Device('tacho-motor', 'motor*', address='this-does-not-exist') + ev3dev2.Device('tacho-motor', 'motor*', address='this-does-not-exist') - d = ev3dev2.Device('lego-sensor', 'sensor*') + ev3dev2.Device('lego-sensor', 'sensor*') with self.assertRaises(ev3dev2.DeviceNotFound): - d = ev3dev2.Device('this-does-not-exist') + ev3dev2.Device('this-does-not-exist') def test_medium_motor(self): def dummy(self): @@ -105,24 +129,25 @@ def dummy(self): self.assertEqual(m.driver_name, 'lego-ev3-m-motor') self.assertEqual(m.driver_name, 'lego-ev3-m-motor') - self.assertEqual(m.count_per_rot, 360) - self.assertEqual(m.commands, ['run-forever', 'run-to-abs-pos', 'run-to-rel-pos', 'run-timed', 'run-direct', 'stop', 'reset']) - self.assertEqual(m.duty_cycle, 0) - self.assertEqual(m.duty_cycle_sp, 42) - self.assertEqual(m.polarity, 'normal') - self.assertEqual(m.address, 'outA') - self.assertEqual(m.position, 42) - self.assertEqual(m.position_sp, 42) - self.assertEqual(m.ramp_down_sp, 0) - self.assertEqual(m.ramp_up_sp, 0) - self.assertEqual(m.speed, 0) - self.assertEqual(m.speed_sp, 0) - self.assertEqual(m.state, ['running']) - self.assertEqual(m.stop_action, 'coast') - self.assertEqual(m.time_sp, 1000) + self.assertEqual(m.count_per_rot, 360) + self.assertEqual( + m.commands, ['run-forever', 'run-to-abs-pos', 'run-to-rel-pos', 'run-timed', 'run-direct', 'stop', 'reset']) + self.assertEqual(m.duty_cycle, 0) + self.assertEqual(m.duty_cycle_sp, 42) + self.assertEqual(m.polarity, 'normal') + self.assertEqual(m.address, 'outA') + self.assertEqual(m.position, 42) + self.assertEqual(m.position_sp, 42) + self.assertEqual(m.ramp_down_sp, 0) + self.assertEqual(m.ramp_up_sp, 0) + self.assertEqual(m.speed, 0) + self.assertEqual(m.speed_sp, 0) + self.assertEqual(m.state, ['running']) + self.assertEqual(m.stop_action, 'coast') + self.assertEqual(m.time_sp, 1000) with self.assertRaises(Exception): - c = m.command + m.command def test_infrared_sensor(self): clean_arena() @@ -130,24 +155,24 @@ def test_infrared_sensor(self): s = InfraredSensor() - self.assertEqual(s.device_index, 0) + self.assertEqual(s.device_index, 0) self.assertEqual(s.bin_data_format, 's8') - self.assertEqual(s.bin_data(' toc - time.time(): toc += self.interval @@ -42,38 +43,39 @@ def join(self, timeout=None): super(LogThread, self).join(timeout) -test = json.loads( open( args.infile ).read() ) +test = json.loads(open(args.infile).read()) + def execute_actions(actions): - for p,c in actions['ports'].items(): + for p, c in actions['ports'].items(): for b in c: - for k,v in b.items(): - setattr( device[p], k, v ) - + for k, v in b.items(): + setattr(device[p], k, v) + + device = {} logs = {} -for p,v in test['meta']['ports'].items(): - device[p] = getattr( ev3, v['device_class'] )( p ) +for p, v in test['meta']['ports'].items(): + device[p] = getattr(ev3, v['device_class'])(p) if test['actions'][0]['time'] < 0: execute_actions(test['actions'][0]) -for p,v in test['meta']['ports'].items(): - device[p] = getattr( ev3, v['device_class'] )( p ) +for p, v in test['meta']['ports'].items(): + device[p] = getattr(ev3, v['device_class'])(p) - logs[p] = LogThread(test['meta']['interval'] * 1e-3, - device[p], - v['log_attributes'] ) + logs[p] = LogThread(test['meta']['interval'] * 1e-3, device[p], v['log_attributes']) logs[p].start() start = time.time() -end = start + test['meta']['max_time'] * 1e-3 +end = start + test['meta']['max_time'] * 1e-3 for a in test['actions']: if a['time'] >= 0: then = start + a['time'] * 1e-3 - while time.time() < then: pass + while time.time() < then: + pass execute_actions(a) while time.time() < end: @@ -81,9 +83,9 @@ def execute_actions(actions): test['data'] = {} -for p,v in test['meta']['ports'].items(): - logs[p].join() - test['data'][p] = logs[p].results +for p, v in test['meta']['ports'].items(): + logs[p].join() + test['data'][p] = logs[p].results # Add a nice JSON formatter here - maybe? -print (json.dumps( test, indent = 4 )) +print(json.dumps(test, indent=4)) diff --git a/tests/motor/motor_info.py b/tests/motor/motor_info.py index 82b69e2..6c9bacc 100644 --- a/tests/motor/motor_info.py +++ b/tests/motor/motor_info.py @@ -1,56 +1,64 @@ motor_info = { - 'lego-ev3-l-motor': {'motion_type': 'rotation', - 'count_per_rot': 360, - 'max_speed': 1050, - 'position_p': 80000, - 'position_i': 0, - 'position_d': 0, - 'polarity': 'normal', - 'speed_p': 1000, - 'speed_i': 60, - 'speed_d': 0 }, - 'lego-ev3-m-motor': {'motion_type': 'rotation', - 'count_per_rot': 360, - 'max_speed': 1560, - 'position_p': 160000, - 'position_i': 0, - 'position_d': 0, - 'polarity': 'normal', - 'speed_p': 1000, - 'speed_i': 60, - 'speed_d': 0 }, - 'lego-nxt-motor': {'motion_type': 'rotation', - 'count_per_rot': 360, - 'max_speed': 1020, - 'position_p': 80000, - 'position_i': 0, - 'position_d': 0, - 'polarity': 'normal', - 'speed_p': 1000, - 'speed_i': 60, - 'speed_d': 0 }, - 'fi-l12-ev3-50': {'motion_type': 'linear', - 'count_per_m': 2000, - 'full_travel_count': 100, - 'max_speed': 24, - 'position_p': 40000, - 'position_i': 0, - 'position_d': 0, - 'polarity': 'normal', - 'speed_p': 1000, - 'speed_i': 60, - 'speed_d': 0, - }, - 'fi-l12-ev3-100': {'motion_type': 'linear', - 'count_per_m': 2000, - 'full_travel_count': 200, - 'max_speed': 24, - 'position_p': 40000, - 'position_i': 0, - 'position_d': 0, - 'polarity': 'normal', - 'speed_p': 1000, - 'speed_i': 60, - 'speed_d': 0, - } + 'lego-ev3-l-motor': { + 'motion_type': 'rotation', + 'count_per_rot': 360, + 'max_speed': 1050, + 'position_p': 80000, + 'position_i': 0, + 'position_d': 0, + 'polarity': 'normal', + 'speed_p': 1000, + 'speed_i': 60, + 'speed_d': 0 + }, + 'lego-ev3-m-motor': { + 'motion_type': 'rotation', + 'count_per_rot': 360, + 'max_speed': 1560, + 'position_p': 160000, + 'position_i': 0, + 'position_d': 0, + 'polarity': 'normal', + 'speed_p': 1000, + 'speed_i': 60, + 'speed_d': 0 + }, + 'lego-nxt-motor': { + 'motion_type': 'rotation', + 'count_per_rot': 360, + 'max_speed': 1020, + 'position_p': 80000, + 'position_i': 0, + 'position_d': 0, + 'polarity': 'normal', + 'speed_p': 1000, + 'speed_i': 60, + 'speed_d': 0 + }, + 'fi-l12-ev3-50': { + 'motion_type': 'linear', + 'count_per_m': 2000, + 'full_travel_count': 100, + 'max_speed': 24, + 'position_p': 40000, + 'position_i': 0, + 'position_d': 0, + 'polarity': 'normal', + 'speed_p': 1000, + 'speed_i': 60, + 'speed_d': 0, + }, + 'fi-l12-ev3-100': { + 'motion_type': 'linear', + 'count_per_m': 2000, + 'full_travel_count': 200, + 'max_speed': 24, + 'position_p': 40000, + 'position_i': 0, + 'position_d': 0, + 'polarity': 'normal', + 'speed_p': 1000, + 'speed_i': 60, + 'speed_d': 0, + } } diff --git a/tests/motor/motor_motion_unittest.py b/tests/motor/motor_motion_unittest.py index 8bfac60..9acece4 100644 --- a/tests/motor/motor_motion_unittest.py +++ b/tests/motor/motor_motion_unittest.py @@ -7,13 +7,10 @@ import unittest import time import ev3dev.ev3 as ev3 - import parameterizedtestcase as ptc -from motor_info import motor_info class TestMotorMotion(ptc.ParameterizedTestCase): - @classmethod def setUpClass(cls): pass @@ -25,7 +22,7 @@ def tearDownClass(cls): def initialize_motor(self): self._param['motor'].command = 'reset' - def run_to_positions(self,stop_action,command,speed_sp,positions,tolerance): + def run_to_positions(self, stop_action, command, speed_sp, positions, tolerance): self._param['motor'].stop_action = stop_action self._param['motor'].speed_sp = speed_sp @@ -37,11 +34,11 @@ def run_to_positions(self,stop_action,command,speed_sp,positions,tolerance): target += i else: target = i - print( "PRE position = {0} i = {1} target = {2}".format(self._param['motor'].position, i, target)) + print("PRE position = {0} i = {1} target = {2}".format(self._param['motor'].position, i, target)) self._param['motor'].command = command while 'running' in self._param['motor'].state: pass - print( "POS position = {0} i = {1} target = {2}".format(self._param['motor'].position, i, target)) + print("POS position = {0} i = {1} target = {2}".format(self._param['motor'].position, i, target)) self.assertGreaterEqual(tolerance, abs(self._param['motor'].position - target)) time.sleep(0.2) @@ -51,67 +48,76 @@ def test_stop_brake_no_ramp_med_speed_relative(self): if not self._param['has_brake']: self.skipTest('brake not supported by this motor controller') self.initialize_motor() - self.run_to_positions('brake','run-to-rel-pos',400,[0,90,180,360,720,-720,-360,-180,-90,0],20) + self.run_to_positions('brake', 'run-to-rel-pos', 400, [0, 90, 180, 360, 720, -720, -360, -180, -90, 0], 20) def test_stop_hold_no_ramp_med_speed_relative(self): self.initialize_motor() - self.run_to_positions('hold','run-to-rel-pos',400,[0,90,180,360,720,-720,-360,-180,-90,0],5) + self.run_to_positions('hold', 'run-to-rel-pos', 400, [0, 90, 180, 360, 720, -720, -360, -180, -90, 0], 5) def test_stop_brake_no_ramp_low_speed_relative(self): if not self._param['has_brake']: self.skipTest('brake not supported by this motor controller') self.initialize_motor() - self.run_to_positions('brake','run-to-rel-pos',100,[0,90,180,360,720,-720,-360,-180,-90,0],20) + self.run_to_positions('brake', 'run-to-rel-pos', 100, [0, 90, 180, 360, 720, -720, -360, -180, -90, 0], 20) def test_stop_hold_no_ramp_low_speed_relative(self): self.initialize_motor() - self.run_to_positions('hold','run-to-rel-pos',100,[0,90,180,360,720,-720,-360,-180,-90,0],5) + self.run_to_positions('hold', 'run-to-rel-pos', 100, [0, 90, 180, 360, 720, -720, -360, -180, -90, 0], 5) def test_stop_brake_no_ramp_high_speed_relative(self): if not self._param['has_brake']: self.skipTest('brake not supported by this motor controller') self.initialize_motor() - self.run_to_positions('brake','run-to-rel-pos',900,[0,90,180,360,720,-720,-360,-180,-90,0],50) + self.run_to_positions('brake', 'run-to-rel-pos', 900, [0, 90, 180, 360, 720, -720, -360, -180, -90, 0], 50) def test_stop_hold_no_ramp_high_speed_relative(self): self.initialize_motor() - self.run_to_positions('hold','run-to-rel-pos',100,[0,90,180,360,720,-720,-360,-180,-90,0],5) + self.run_to_positions('hold', 'run-to-rel-pos', 100, [0, 90, 180, 360, 720, -720, -360, -180, -90, 0], 5) def test_stop_brake_no_ramp_med_speed_absolute(self): if not self._param['has_brake']: self.skipTest('brake not supported by this motor controller') self.initialize_motor() - self.run_to_positions('brake','run-to-abs-pos',400,[0,90,180,360,180,90,0,-90,-180,-360,-180,-90,0],20) + self.run_to_positions('brake', 'run-to-abs-pos', 400, + [0, 90, 180, 360, 180, 90, 0, -90, -180, -360, -180, -90, 0], 20) def test_stop_hold_no_ramp_med_speed_absolute(self): self.initialize_motor() - self.run_to_positions('hold','run-to-abs-pos',400,[0,90,180,360,180,90,0,-90,-180,-360,-180,-90,0],5) + self.run_to_positions('hold', 'run-to-abs-pos', 400, + [0, 90, 180, 360, 180, 90, 0, -90, -180, -360, -180, -90, 0], 5) def test_stop_brake_no_ramp_low_speed_absolute(self): if not self._param['has_brake']: self.skipTest('brake not supported by this motor controller') self.initialize_motor() - self.run_to_positions('brake','run-to-abs-pos',100,[0,90,180,360,180,90,0,-90,-180,-360,-180,-90,0],20) + self.run_to_positions('brake', 'run-to-abs-pos', 100, + [0, 90, 180, 360, 180, 90, 0, -90, -180, -360, -180, -90, 0], 20) def test_stop_hold_no_ramp_low_speed_absolute(self): self.initialize_motor() - self.run_to_positions('hold','run-to-abs-pos',100,[0,90,180,360,180,90,0,-90,-180,-360,-180,-90,0],5) + self.run_to_positions('hold', 'run-to-abs-pos', 100, + [0, 90, 180, 360, 180, 90, 0, -90, -180, -360, -180, -90, 0], 5) def test_stop_brake_no_ramp_high_speed_absolute(self): if not self._param['has_brake']: self.skipTest('brake not supported by this motor controller') self.initialize_motor() - self.run_to_positions('brake','run-to-abs-pos',900,[0,90,180,360,180,90,0,-90,-180,-360,-180,-90,0],50) + self.run_to_positions('brake', 'run-to-abs-pos', 900, + [0, 90, 180, 360, 180, 90, 0, -90, -180, -360, -180, -90, 0], 50) def test_stop_hold_no_ramp_high_speed_absolute(self): self.initialize_motor() - self.run_to_positions('hold','run-to-abs-pos',100,[0,90,180,360,180,90,0,-90,-180,-360,-180,-90,0],5) + self.run_to_positions('hold', 'run-to-abs-pos', 100, + [0, 90, 180, 360, 180, 90, 0, -90, -180, -360, -180, -90, 0], 5) + # Add all the tests to the suite - some tests apply only to certain drivers! + def AddTachoMotorMotionTestsToSuite(suite, params): suite.addTest(ptc.ParameterizedTestCase.parameterize(TestMotorMotion, param=params)) + if __name__ == '__main__': ev3_params = { 'motor': ev3.Motor('outA'), @@ -136,4 +142,4 @@ def AddTachoMotorMotionTestsToSuite(suite, params): AddTachoMotorMotionTestsToSuite(suite, ev3_params) - unittest.TextTestRunner(verbosity=1,buffer=True ).run(suite) + unittest.TextTestRunner(verbosity=1, buffer=True).run(suite) diff --git a/tests/motor/motor_param_unittest.py b/tests/motor/motor_param_unittest.py index ad2c3b4..1c721b9 100644 --- a/tests/motor/motor_param_unittest.py +++ b/tests/motor/motor_param_unittest.py @@ -5,16 +5,13 @@ # http://eli.thegreenplace.net/2011/08/02/python-unit-testing-parametrized-test-cases import unittest -import time -import sys import ev3dev.ev3 as ev3 - import parameterizedtestcase as ptc from motor_info import motor_info -class TestTachoMotorAddressValue(ptc.ParameterizedTestCase): +class TestTachoMotorAddressValue(ptc.ParameterizedTestCase): def test_address_value(self): self.assertEqual(self._param['motor'].address, self._param['port']) @@ -22,8 +19,8 @@ def test_address_value_is_read_only(self): with self.assertRaises(AttributeError): self._param['motor'].address = "ThisShouldNotWork" -class TestTachoMotorCommandsValue(ptc.ParameterizedTestCase): +class TestTachoMotorCommandsValue(ptc.ParameterizedTestCase): def test_commands_value(self): self.assertTrue(self._param['motor'].commands == self._param['commands']) @@ -31,17 +28,18 @@ def test_commands_value_is_read_only(self): with self.assertRaises(AttributeError): self._param['motor'].commands = "ThisShouldNotWork" -class TestTachoMotorCountPerRotValue(ptc.ParameterizedTestCase): +class TestTachoMotorCountPerRotValue(ptc.ParameterizedTestCase): def test_count_per_rot_value(self): - self.assertEqual(self._param['motor'].count_per_rot, motor_info[self._param['motor'].driver_name]['count_per_rot']) + self.assertEqual(self._param['motor'].count_per_rot, + motor_info[self._param['motor'].driver_name]['count_per_rot']) def test_count_per_rot_value_is_read_only(self): with self.assertRaises(AttributeError): self._param['motor'].count_per_rot = "ThisShouldNotWork" -class TestTachoMotorCountPerMValue(ptc.ParameterizedTestCase): +class TestTachoMotorCountPerMValue(ptc.ParameterizedTestCase): def test_count_per_m_value(self): self.assertEqual(self._param['motor'].count_per_m, motor_info[self._param['motor'].driver_name]['count_per_m']) @@ -49,17 +47,18 @@ def test_count_per_m_value_is_read_only(self): with self.assertRaises(AttributeError): self._param['motor'].count_per_m = "ThisShouldNotWork" -class TestTachoMotorFullTravelCountValue(ptc.ParameterizedTestCase): +class TestTachoMotorFullTravelCountValue(ptc.ParameterizedTestCase): def test_full_travel_count_value(self): - self.assertEqual(self._param['motor'].full_travel_count, motor_info[self._param['motor'].driver_name]['full_travel_count']) + self.assertEqual(self._param['motor'].full_travel_count, + motor_info[self._param['motor'].driver_name]['full_travel_count']) def test_full_travel_count_value_is_read_only(self): with self.assertRaises(AttributeError): self._param['motor'].count_per_m = "ThisShouldNotWork" -class TestTachoMotorDriverNameValue(ptc.ParameterizedTestCase): +class TestTachoMotorDriverNameValue(ptc.ParameterizedTestCase): def test_driver_name_value(self): self.assertEqual(self._param['motor'].driver_name, self._param['driver_name']) @@ -67,8 +66,8 @@ def test_driver_name_value_is_read_only(self): with self.assertRaises(AttributeError): self._param['motor'].driver_name = "ThisShouldNotWork" -class TestTachoMotorDutyCycleValue(ptc.ParameterizedTestCase): +class TestTachoMotorDutyCycleValue(ptc.ParameterizedTestCase): def test_duty_cycle_value_is_read_only(self): with self.assertRaises(AttributeError): self._param['motor'].duty_cycle = "ThisShouldNotWork" @@ -77,8 +76,8 @@ def test_duty_cycle_value_after_reset(self): self._param['motor'].command = 'reset' self.assertEqual(self._param['motor'].duty_cycle, 0) -class TestTachoMotorDutyCycleSpValue(ptc.ParameterizedTestCase): +class TestTachoMotorDutyCycleSpValue(ptc.ParameterizedTestCase): def test_duty_cycle_sp_large_negative(self): with self.assertRaises(IOError): self._param['motor'].duty_cycle_sp = -101 @@ -115,7 +114,6 @@ def test_duty_cycle_sp_after_reset(self): class TestTachoMotorMaxSpeedValue(ptc.ParameterizedTestCase): - def test_max_speed_value(self): self.assertEqual(self._param['motor'].max_speed, motor_info[self._param['motor'].driver_name]['max_speed']) @@ -123,8 +121,8 @@ def test_max_speed_value_is_read_only(self): with self.assertRaises(AttributeError): self._param['motor'].max_speed = "ThisShouldNotWork" -class TestTachoMotorPositionPValue(ptc.ParameterizedTestCase): +class TestTachoMotorPositionPValue(ptc.ParameterizedTestCase): def test_position_p_negative(self): with self.assertRaises(IOError): self._param['motor'].position_p = -1 @@ -144,11 +142,11 @@ def test_position_p_after_reset(self): if self._param['hold_pid']: expected = self._param['hold_pid']['kP'] else: - expected = motor_info[self._param['motor'].driver_name]['position_p'] + expected = motor_info[self._param['motor'].driver_name]['position_p'] self.assertEqual(self._param['motor'].position_p, expected) -class TestTachoMotorPositionIValue(ptc.ParameterizedTestCase): +class TestTachoMotorPositionIValue(ptc.ParameterizedTestCase): def test_position_i_negative(self): with self.assertRaises(IOError): self._param['motor'].position_i = -1 @@ -168,11 +166,11 @@ def test_position_i_after_reset(self): if self._param['hold_pid']: expected = self._param['hold_pid']['kI'] else: - expected = motor_info[self._param['motor'].driver_name]['position_i'] + expected = motor_info[self._param['motor'].driver_name]['position_i'] self.assertEqual(self._param['motor'].position_i, expected) -class TestTachoMotorPositionDValue(ptc.ParameterizedTestCase): +class TestTachoMotorPositionDValue(ptc.ParameterizedTestCase): def test_position_d_negative(self): with self.assertRaises(IOError): self._param['motor'].position_d = -1 @@ -192,11 +190,11 @@ def test_position_d_after_reset(self): if self._param['hold_pid']: expected = self._param['hold_pid']['kD'] else: - expected = motor_info[self._param['motor'].driver_name]['position_d'] + expected = motor_info[self._param['motor'].driver_name]['position_d'] self.assertEqual(self._param['motor'].position_d, expected) -class TestTachoMotorPolarityValue(ptc.ParameterizedTestCase): +class TestTachoMotorPolarityValue(ptc.ParameterizedTestCase): def test_polarity_normal_value(self): self._param['motor'].polarity = 'normal' self.assertEqual(self._param['motor'].polarity, 'normal') @@ -221,8 +219,8 @@ def test_polarity_after_reset(self): else: self.assertEqual(self._param['motor'].polarity, 'inversed') -class TestTachoMotorPositionValue(ptc.ParameterizedTestCase): +class TestTachoMotorPositionValue(ptc.ParameterizedTestCase): def test_position_large_negative(self): self._param['motor'].position = -1000000 self.assertEqual(self._param['motor'].position, -1000000) @@ -249,8 +247,8 @@ def test_position_after_reset(self): self._param['motor'].command = 'reset' self.assertEqual(self._param['motor'].position, 0) -class TestTachoMotorPositionSpValue(ptc.ParameterizedTestCase): +class TestTachoMotorPositionSpValue(ptc.ParameterizedTestCase): def test_position_sp_large_negative(self): self._param['motor'].position_sp = -1000000 self.assertEqual(self._param['motor'].position_sp, -1000000) @@ -277,8 +275,8 @@ def test_position_sp_after_reset(self): self._param['motor'].command = 'reset' self.assertEqual(self._param['motor'].position_sp, 0) -class TestTachoMotorRampDownSpValue(ptc.ParameterizedTestCase): +class TestTachoMotorRampDownSpValue(ptc.ParameterizedTestCase): def test_ramp_down_sp_negative_value(self): with self.assertRaises(IOError): self._param['motor'].ramp_down_sp = -1 @@ -305,8 +303,8 @@ def test_ramp_down_sp_after_reset(self): self._param['motor'].command = 'reset' self.assertEqual(self._param['motor'].ramp_down_sp, 0) -class TestTachoMotorRampUpSpValue(ptc.ParameterizedTestCase): +class TestTachoMotorRampUpSpValue(ptc.ParameterizedTestCase): def test_ramp_up_negative_value(self): with self.assertRaises(IOError): self._param['motor'].ramp_up_sp = -1 @@ -333,8 +331,8 @@ def test_ramp_up_sp_after_reset(self): self._param['motor'].command = 'reset' self.assertEqual(self._param['motor'].ramp_up_sp, 0) -class TestTachoMotorSpeedValue(ptc.ParameterizedTestCase): +class TestTachoMotorSpeedValue(ptc.ParameterizedTestCase): def test_speed_value_is_read_only(self): with self.assertRaises(AttributeError): self._param['motor'].speed = 1 @@ -343,44 +341,44 @@ def test_speed_value_after_reset(self): self._param['motor'].command = 'reset' self.assertEqual(self._param['motor'].speed, 0) -class TestTachoMotorSpeedSpValue(ptc.ParameterizedTestCase): - def test_speed_sp_large_negative(self): +class TestTachoMotorSpeedSpValue(ptc.ParameterizedTestCase): + def test_speed_sp_large_negative(self): with self.assertRaises(IOError): - self._param['motor'].speed_sp = -(motor_info[self._param['motor'].driver_name]['max_speed']+1) + self._param['motor'].speed_sp = -(motor_info[self._param['motor'].driver_name]['max_speed'] + 1) - def test_speed_sp_max_negative(self): + def test_speed_sp_max_negative(self): self._param['motor'].speed_sp = -motor_info[self._param['motor'].driver_name]['max_speed'] self.assertEqual(self._param['motor'].speed_sp, -motor_info[self._param['motor'].driver_name]['max_speed']) - def test_speed_sp_min_negative(self): + def test_speed_sp_min_negative(self): self._param['motor'].speed_sp = -1 self.assertEqual(self._param['motor'].speed_sp, -1) - def test_speed_sp_zero(self): + def test_speed_sp_zero(self): self._param['motor'].speed_sp = 0 self.assertEqual(self._param['motor'].speed_sp, 0) - def test_speed_sp_min_positive(self): + def test_speed_sp_min_positive(self): self._param['motor'].speed_sp = 1 self.assertEqual(self._param['motor'].speed_sp, 1) - def test_speed_sp_max_positive(self): + def test_speed_sp_max_positive(self): self._param['motor'].speed_sp = (motor_info[self._param['motor'].driver_name]['max_speed']) self.assertEqual(self._param['motor'].speed_sp, motor_info[self._param['motor'].driver_name]['max_speed']) - def test_speed_sp_large_positive(self): + def test_speed_sp_large_positive(self): with self.assertRaises(IOError): self._param['motor'].speed_sp = motor_info[self._param['motor'].driver_name]['max_speed'] + 1 - def test_speed_sp_after_reset(self): - self._param['motor'].speed_sp = motor_info[self._param['motor'].driver_name]['max_speed']/2 - self.assertEqual(self._param['motor'].speed_sp, motor_info[self._param['motor'].driver_name]['max_speed']/2) + def test_speed_sp_after_reset(self): + self._param['motor'].speed_sp = motor_info[self._param['motor'].driver_name]['max_speed'] / 2 + self.assertEqual(self._param['motor'].speed_sp, motor_info[self._param['motor'].driver_name]['max_speed'] / 2) self._param['motor'].command = 'reset' self.assertEqual(self._param['motor'].speed_sp, 0) -class TestTachoMotorSpeedPValue(ptc.ParameterizedTestCase): +class TestTachoMotorSpeedPValue(ptc.ParameterizedTestCase): def test_speed_i_negative(self): with self.assertRaises(IOError): self._param['motor'].speed_p = -1 @@ -400,11 +398,11 @@ def test_speed_p_after_reset(self): if self._param['speed_pid']: expected = self._param['speed_pid']['kP'] else: - expected = motor_info[self._param['motor'].driver_name]['speed_p'] + expected = motor_info[self._param['motor'].driver_name]['speed_p'] self.assertEqual(self._param['motor'].speed_p, expected) -class TestTachoMotorSpeedIValue(ptc.ParameterizedTestCase): +class TestTachoMotorSpeedIValue(ptc.ParameterizedTestCase): def test_speed_i_negative(self): with self.assertRaises(IOError): self._param['motor'].speed_i = -1 @@ -424,11 +422,11 @@ def test_speed_i_after_reset(self): if self._param['speed_pid']: expected = self._param['speed_pid']['kI'] else: - expected = motor_info[self._param['motor'].driver_name]['speed_i'] + expected = motor_info[self._param['motor'].driver_name]['speed_i'] self.assertEqual(self._param['motor'].speed_i, expected) -class TestTachoMotorSpeedDValue(ptc.ParameterizedTestCase): +class TestTachoMotorSpeedDValue(ptc.ParameterizedTestCase): def test_speed_d_negative(self): with self.assertRaises(IOError): self._param['motor'].speed_d = -1 @@ -448,11 +446,11 @@ def test_speed_d_after_reset(self): if self._param['speed_pid']: expected = self._param['speed_pid']['kD'] else: - expected = motor_info[self._param['motor'].driver_name]['speed_d'] + expected = motor_info[self._param['motor'].driver_name]['speed_d'] self.assertEqual(self._param['motor'].speed_d, expected) -class TestTachoMotorStateValue(ptc.ParameterizedTestCase): +class TestTachoMotorStateValue(ptc.ParameterizedTestCase): def test_state_value_is_read_only(self): with self.assertRaises(AttributeError): self._param['motor'].state = 'ThisShouldNotWork' @@ -461,8 +459,8 @@ def test_state_value_after_reset(self): self._param['motor'].command = 'reset' self.assertEqual(self._param['motor'].state, []) -class TestTachoMotorStopActionValue(ptc.ParameterizedTestCase): +class TestTachoMotorStopActionValue(ptc.ParameterizedTestCase): def test_stop_action_illegal(self): with self.assertRaises(IOError): self._param['motor'].stop_action = 'ThisShouldNotWork' @@ -500,8 +498,8 @@ def test_stop_action_after_reset(self): self._param['motor'].action = 'reset' self.assertEqual(self._param['motor'].stop_action, self._param['stop_actions'][0]) -class TestTachoMotorStopActionsValue(ptc.ParameterizedTestCase): +class TestTachoMotorStopActionsValue(ptc.ParameterizedTestCase): def test_stop_actions_value(self): self.assertTrue(self._param['motor'].stop_actions == self._param['stop_actions']) @@ -509,8 +507,8 @@ def test_stop_actions_value_is_read_only(self): with self.assertRaises(AttributeError): self._param['motor'].stop_actions = "ThisShouldNotWork" -class TestTachoMotorTimeSpValue(ptc.ParameterizedTestCase): +class TestTachoMotorTimeSpValue(ptc.ParameterizedTestCase): def test_time_sp_negative(self): with self.assertRaises(IOError): self._param['motor'].time_sp = -1 @@ -532,6 +530,7 @@ def test_time_sp_after_reset(self): self._param['motor'].command = 'reset' self.assertEqual(self._param['motor'].time_sp, 0) + ev3_params = { 'motor': ev3.Motor('outA'), 'port': 'outA', @@ -552,8 +551,16 @@ def test_time_sp_after_reset(self): 'driver_name': 'lego-nxt-motor', 'commands': ['run-forever', 'run-to-abs-pos', 'run-to-rel-pos', 'run-timed', 'run-direct', 'stop', 'reset'], 'stop_actions': ['coast', 'hold'], - 'speed_pid': { 'kP': 1000, 'kI': 60, 'kD': 0 }, - 'hold_pid': { 'kP': 20000, 'kI': 0, 'kD': 0 }, + 'speed_pid': { + 'kP': 1000, + 'kI': 60, + 'kD': 0 + }, + 'hold_pid': { + 'kP': 20000, + 'kI': 0, + 'kD': 0 + }, } pistorms_params = { 'motor': ev3.Motor('pistorms:BAM1'), @@ -561,8 +568,16 @@ def test_time_sp_after_reset(self): 'driver_name': 'lego-nxt-motor', 'commands': ['run-forever', 'run-to-abs-pos', 'run-to-rel-pos', 'run-timed', 'stop', 'reset'], 'stop_actions': ['coast', 'brake', 'hold'], - 'speed_pid': { 'kP': 1000, 'kI': 60, 'kD': 0 }, - 'hold_pid': { 'kP': 20000, 'kI': 0, 'kD': 0 }, + 'speed_pid': { + 'kP': 1000, + 'kI': 60, + 'kD': 0 + }, + 'hold_pid': { + 'kP': 20000, + 'kI': 0, + 'kD': 0 + }, } paramsA = pistorms_params paramsA['motor'].command = 'reset' @@ -589,10 +604,9 @@ def test_time_sp_after_reset(self): suite.addTest(ptc.ParameterizedTestCase.parameterize(TestTachoMotorSpeedIValue, param=paramsA)) suite.addTest(ptc.ParameterizedTestCase.parameterize(TestTachoMotorSpeedDValue, param=paramsA)) suite.addTest(ptc.ParameterizedTestCase.parameterize(TestTachoMotorStateValue, param=paramsA)) -suite.addTest(ptc.ParameterizedTestCase.parameterize(TestTachoMotorStopCommandValue, param=paramsA)) -suite.addTest(ptc.ParameterizedTestCase.parameterize(TestTachoMotorStopCommandsValue, param=paramsA)) +suite.addTest(ptc.ParameterizedTestCase.parameterize(TestTachoMotorStopActionValue, param=paramsA)) +suite.addTest(ptc.ParameterizedTestCase.parameterize(TestTachoMotorStopActionsValue, param=paramsA)) suite.addTest(ptc.ParameterizedTestCase.parameterize(TestTachoMotorTimeSpValue, param=paramsA)) - if __name__ == '__main__': - unittest.main(verbosity=2,buffer=True ).run(suite) + unittest.main(verbosity=2, buffer=True).run(suite) diff --git a/tests/motor/motor_run_direct_unittest.py b/tests/motor/motor_run_direct_unittest.py index c0d63de..4ae42a9 100755 --- a/tests/motor/motor_run_direct_unittest.py +++ b/tests/motor/motor_run_direct_unittest.py @@ -7,13 +7,10 @@ import unittest import time import ev3dev.ev3 as ev3 - import parameterizedtestcase as ptc -from motor_info import motor_info class TestMotorRunDirect(ptc.ParameterizedTestCase): - @classmethod def setUpClass(cls): pass @@ -25,30 +22,33 @@ def tearDownClass(cls): def initialize_motor(self): self._param['motor'].command = 'reset' - def run_direct_duty_cycles(self,stop_action,duty_cycles): + def run_direct_duty_cycles(self, stop_action, duty_cycles): self._param['motor'].stop_action = stop_action self._param['motor'].command = 'run-direct' for i in duty_cycles: - self._param['motor'].duty_cycle_sp = i + self._param['motor'].duty_cycle_sp = i time.sleep(0.5) self._param['motor'].command = 'stop' def test_stop_coast_duty_cycles(self): self.initialize_motor() - self.run_direct_duty_cycles('coast',[0,20,40,60,80,100,66,33,0,-20,-40,-60,-80,-100,-66,-33,0]) + self.run_direct_duty_cycles('coast', [0, 20, 40, 60, 80, 100, 66, 33, 0, -20, -40, -60, -80, -100, -66, -33, 0]) + # Add all the tests to the suite - some tests apply only to certain drivers! -def AddTachoMotorRunDirectTestsToSuite( suite, driver_name, params ): + +def AddTachoMotorRunDirectTestsToSuite(suite, driver_name, params): suite.addTest(ptc.ParameterizedTestCase.parameterize(TestMotorRunDirect, param=params)) + if __name__ == '__main__': - params = { 'motor': ev3.Motor('outA'), 'port': 'outA', 'driver_name': 'lego-ev3-l-motor' } + params = {'motor': ev3.Motor('outA'), 'port': 'outA', 'driver_name': 'lego-ev3-l-motor'} suite = unittest.TestSuite() - AddTachoMotorRunDirectTestsToSuite( suite, 'lego-ev3-l-motor', params ) + AddTachoMotorRunDirectTestsToSuite(suite, 'lego-ev3-l-motor', params) - unittest.TextTestRunner(verbosity=1,buffer=True ).run(suite) + unittest.TextTestRunner(verbosity=1, buffer=True).run(suite) diff --git a/tests/motor/motor_unittest.py b/tests/motor/motor_unittest.py index 5933ed3..090f537 100644 --- a/tests/motor/motor_unittest.py +++ b/tests/motor/motor_unittest.py @@ -10,8 +10,8 @@ import parameterizedtestcase as ptc from motor_info import motor_info -class TestTachoMotorAddressValue(ptc.ParameterizedTestCase): +class TestTachoMotorAddressValue(ptc.ParameterizedTestCase): def test_address_value(self): # Use the class variable self.assertEqual(self._param['motor'].address, self._param['port']) @@ -21,8 +21,8 @@ def test_address_value_is_read_only(self): with self.assertRaises(AttributeError): self._param['motor'].address = "ThisShouldNotWork" -class TestTachoMotorCommandsValue(ptc.ParameterizedTestCase): +class TestTachoMotorCommandsValue(ptc.ParameterizedTestCase): def test_commands_value(self): self.assertTrue(self._param['motor'].commands == self._param['commands']) @@ -31,8 +31,8 @@ def test_commands_value_is_read_only(self): with self.assertRaises(AttributeError): self._param['motor'].commands = "ThisShouldNotWork" -class TestTachoMotorCountPerRotValue(ptc.ParameterizedTestCase): +class TestTachoMotorCountPerRotValue(ptc.ParameterizedTestCase): def test_count_per_rot_value(self): # This is not available for linear motors - move to driver specific tests? self.assertEqual(self._param['motor'].count_per_rot, 360) @@ -42,8 +42,8 @@ def test_count_per_rot_value_is_read_only(self): with self.assertRaises(AttributeError): self._param['motor'].count_per_rot = "ThisShouldNotWork" -class TestTachoMotorDriverNameValue(ptc.ParameterizedTestCase): +class TestTachoMotorDriverNameValue(ptc.ParameterizedTestCase): def test_driver_name_value(self): # move to driver specific tests? self.assertEqual(self._param['motor'].driver_name, self._param['driver_name']) @@ -53,8 +53,8 @@ def test_driver_name_value_is_read_only(self): with self.assertRaises(AttributeError): self._param['motor'].driver_name = "ThisShouldNotWork" -class TestTachoMotorDutyCycleValue(ptc.ParameterizedTestCase): +class TestTachoMotorDutyCycleValue(ptc.ParameterizedTestCase): def test_duty_cycle_value_is_read_only(self): # Use the class variable with self.assertRaises(AttributeError): @@ -64,8 +64,8 @@ def test_duty_cycle_value_after_reset(self): self._param['motor'].command = 'reset' self.assertEqual(self._param['motor'].duty_cycle, 0) -class TestTachoMotorDutyCycleSpValue(ptc.ParameterizedTestCase): +class TestTachoMotorDutyCycleSpValue(ptc.ParameterizedTestCase): def test_duty_cycle_sp_large_negative(self): with self.assertRaises(IOError): self._param['motor'].duty_cycle_sp = -101 @@ -102,7 +102,6 @@ def test_duty_cycle_sp_after_reset(self): class TestTachoMotorMaxSpeedValue(ptc.ParameterizedTestCase): - def test_max_speed_value(self): # This is not available for linear motors - move to driver specific tests? self.assertEqual(self._param['motor'].max_speed, motor_info[self._param['motor'].driver_name]['max_speed']) @@ -112,8 +111,8 @@ def test_max_speed_value_is_read_only(self): with self.assertRaises(AttributeError): self._param['motor'].max_speed = "ThisShouldNotWork" -class TestTachoMotorPositionPValue(ptc.ParameterizedTestCase): +class TestTachoMotorPositionPValue(ptc.ParameterizedTestCase): def test_position_p_negative(self): with self.assertRaises(IOError): self._param['motor'].position_p = -1 @@ -134,11 +133,11 @@ def test_position_p_after_reset(self): if 'hold_pid' in self._param: expected = self._param['hold_pid']['kP'] else: - expected = motor_info[self._param['motor'].driver_name]['position_p'] + expected = motor_info[self._param['motor'].driver_name]['position_p'] self.assertEqual(self._param['motor'].position_p, expected) -class TestTachoMotorPositionIValue(ptc.ParameterizedTestCase): +class TestTachoMotorPositionIValue(ptc.ParameterizedTestCase): def test_position_i_negative(self): with self.assertRaises(IOError): self._param['motor'].position_i = -1 @@ -159,11 +158,11 @@ def test_position_i_after_reset(self): if 'hold_pid' in self._param: expected = self._param['hold_pid']['kI'] else: - expected = motor_info[self._param['motor'].driver_name]['position_i'] + expected = motor_info[self._param['motor'].driver_name]['position_i'] self.assertEqual(self._param['motor'].position_i, expected) -class TestTachoMotorPositionDValue(ptc.ParameterizedTestCase): +class TestTachoMotorPositionDValue(ptc.ParameterizedTestCase): def test_position_d_negative(self): with self.assertRaises(IOError): self._param['motor'].position_d = -1 @@ -184,11 +183,11 @@ def test_position_d_after_reset(self): if 'hold_pid' in self._param: expected = self._param['hold_pid']['kD'] else: - expected = motor_info[self._param['motor'].driver_name]['position_d'] + expected = motor_info[self._param['motor'].driver_name]['position_d'] self.assertEqual(self._param['motor'].position_d, expected) -class TestTachoMotorPolarityValue(ptc.ParameterizedTestCase): +class TestTachoMotorPolarityValue(ptc.ParameterizedTestCase): def test_polarity_normal_value(self): self._param['motor'].polarity = 'normal' self.assertEqual(self._param['motor'].polarity, 'normal') @@ -214,8 +213,8 @@ def test_polarity_after_reset(self): else: self.assertEqual(self._param['motor'].polarity, 'inversed') -class TestTachoMotorPositionValue(ptc.ParameterizedTestCase): +class TestTachoMotorPositionValue(ptc.ParameterizedTestCase): def test_position_large_negative(self): self._param['motor'].position = -1000000 self.assertEqual(self._param['motor'].position, -1000000) @@ -244,7 +243,6 @@ def test_position_after_reset(self): class TestTachoMotorPositionSpValue(ptc.ParameterizedTestCase): - def test_position_sp_large_negative(self): self._param['motor'].position_sp = -1000000 self.assertEqual(self._param['motor'].position_sp, -1000000) @@ -271,8 +269,8 @@ def test_position_sp_after_reset(self): self._param['motor'].command = 'reset' self.assertEqual(self._param['motor'].position_sp, 0) -class TestTachoMotorRampDownSpValue(ptc.ParameterizedTestCase): +class TestTachoMotorRampDownSpValue(ptc.ParameterizedTestCase): def test_ramp_down_sp_negative_value(self): with self.assertRaises(IOError): self._param['motor'].ramp_down_sp = -1 @@ -298,9 +296,9 @@ def test_ramp_down_sp_after_reset(self): self.assertEqual(self._param['motor'].ramp_down_sp, 100) self._param['motor'].command = 'reset' self.assertEqual(self._param['motor'].ramp_down_sp, 0) - -class TestTachoMotorRampUpSpValue(ptc.ParameterizedTestCase): + +class TestTachoMotorRampUpSpValue(ptc.ParameterizedTestCase): def test_ramp_up_negative_value(self): with self.assertRaises(IOError): self._param['motor'].ramp_up_sp = -1 @@ -326,9 +324,9 @@ def test_ramp_up_sp_after_reset(self): self.assertEqual(self._param['motor'].ramp_up_sp, 100) self._param['motor'].command = 'reset' self.assertEqual(self._param['motor'].ramp_up_sp, 0) - -class TestTachoMotorSpeedValue(ptc.ParameterizedTestCase): + +class TestTachoMotorSpeedValue(ptc.ParameterizedTestCase): def test_speed_value_is_read_only(self): # Use the class variable with self.assertRaises(AttributeError): @@ -337,45 +335,45 @@ def test_speed_value_is_read_only(self): def test_speed_value_after_reset(self): self._param['motor'].command = 'reset' self.assertEqual(self._param['motor'].speed, 0) - -class TestTachoMotorSpeedSpValue(ptc.ParameterizedTestCase): - def test_speed_sp_large_negative(self): + +class TestTachoMotorSpeedSpValue(ptc.ParameterizedTestCase): + def test_speed_sp_large_negative(self): with self.assertRaises(IOError): - self._param['motor'].speed_sp = -(motor_info[self._param['motor'].driver_name]['max_speed']+1) + self._param['motor'].speed_sp = -(motor_info[self._param['motor'].driver_name]['max_speed'] + 1) - def test_speed_sp_max_negative(self): + def test_speed_sp_max_negative(self): self._param['motor'].speed_sp = -motor_info[self._param['motor'].driver_name]['max_speed'] self.assertEqual(self._param['motor'].speed_sp, -motor_info[self._param['motor'].driver_name]['max_speed']) - def test_speed_sp_min_negative(self): + def test_speed_sp_min_negative(self): self._param['motor'].speed_sp = -1 self.assertEqual(self._param['motor'].speed_sp, -1) - def test_speed_sp_zero(self): + def test_speed_sp_zero(self): self._param['motor'].speed_sp = 0 self.assertEqual(self._param['motor'].speed_sp, 0) - def test_speed_sp_min_positive(self): + def test_speed_sp_min_positive(self): self._param['motor'].speed_sp = 1 self.assertEqual(self._param['motor'].speed_sp, 1) - def test_speed_sp_max_positive(self): + def test_speed_sp_max_positive(self): self._param['motor'].speed_sp = (motor_info[self._param['motor'].driver_name]['max_speed']) self.assertEqual(self._param['motor'].speed_sp, motor_info[self._param['motor'].driver_name]['max_speed']) - def test_speed_sp_large_positive(self): + def test_speed_sp_large_positive(self): with self.assertRaises(IOError): self._param['motor'].speed_sp = motor_info[self._param['motor'].driver_name]['max_speed'] + 1 - def test_speed_sp_after_reset(self): + def test_speed_sp_after_reset(self): self._param['motor'].speed_sp = 100 self.assertEqual(self._param['motor'].speed_sp, 100) self._param['motor'].command = 'reset' self.assertEqual(self._param['motor'].speed_sp, 0) - -class TestTachoMotorSpeedPValue(ptc.ParameterizedTestCase): + +class TestTachoMotorSpeedPValue(ptc.ParameterizedTestCase): def test_speed_i_negative(self): with self.assertRaises(IOError): self._param['motor'].speed_p = -1 @@ -396,11 +394,11 @@ def test_speed_p_after_reset(self): if 'speed_pid' in self._param: expected = self._param['speed_pid']['kP'] else: - expected = motor_info[self._param['motor'].driver_name]['speed_p'] + expected = motor_info[self._param['motor'].driver_name]['speed_p'] self.assertEqual(self._param['motor'].speed_p, expected) -class TestTachoMotorSpeedIValue(ptc.ParameterizedTestCase): +class TestTachoMotorSpeedIValue(ptc.ParameterizedTestCase): def test_speed_i_negative(self): with self.assertRaises(IOError): self._param['motor'].speed_i = -1 @@ -421,11 +419,11 @@ def test_speed_i_after_reset(self): if 'speed_pid' in self._param: expected = self._param['speed_pid']['kI'] else: - expected = motor_info[self._param['motor'].driver_name]['speed_i'] + expected = motor_info[self._param['motor'].driver_name]['speed_i'] self.assertEqual(self._param['motor'].speed_i, expected) -class TestTachoMotorSpeedDValue(ptc.ParameterizedTestCase): +class TestTachoMotorSpeedDValue(ptc.ParameterizedTestCase): def test_speed_d_negative(self): with self.assertRaises(IOError): self._param['motor'].speed_d = -1 @@ -446,11 +444,11 @@ def test_speed_d_after_reset(self): if 'speed_pid' in self._param: expected = self._param['speed_pid']['kD'] else: - expected = motor_info[self._param['motor'].driver_name]['speed_d'] + expected = motor_info[self._param['motor'].driver_name]['speed_d'] self.assertEqual(self._param['motor'].speed_d, expected) -class TestTachoMotorStateValue(ptc.ParameterizedTestCase): +class TestTachoMotorStateValue(ptc.ParameterizedTestCase): def test_state_value_is_read_only(self): # Use the class variable with self.assertRaises(AttributeError): @@ -460,10 +458,10 @@ def test_state_value_after_reset(self): self._param['motor'].command = 'reset' self.assertEqual(self._param['motor'].state, []) + # def test_stop_action_value(self): # self.assertEqual(self._param['motor'].stop_action, 'coast') class TestTachoMotorStopCommandValue(ptc.ParameterizedTestCase): - def test_stop_action_illegal(self): with self.assertRaises(IOError): self._param['motor'].stop_action = 'ThisShouldNotWork' @@ -501,8 +499,8 @@ def test_stop_action_after_reset(self): self._param['motor'].command = 'reset' self.assertEqual(self._param['motor'].stop_action, self._param['stop_actions'][0]) -class TestTachoMotorStopCommandsValue(ptc.ParameterizedTestCase): +class TestTachoMotorStopCommandsValue(ptc.ParameterizedTestCase): def test_stop_actions_value(self): self.assertTrue(self._param['motor'].stop_actions == self._param['stop_actions']) @@ -511,8 +509,8 @@ def test_stop_actions_value_is_read_only(self): with self.assertRaises(AttributeError): self._param['motor'].stop_actions = "ThisShouldNotWork" -class TestTachoMotorTimeSpValue(ptc.ParameterizedTestCase): +class TestTachoMotorTimeSpValue(ptc.ParameterizedTestCase): def test_time_sp_negative(self): with self.assertRaises(IOError): self._param['motor'].time_sp = -1 @@ -534,6 +532,7 @@ def test_time_sp_after_reset(self): self._param['motor'].command = 'reset' self.assertEqual(self._param['motor'].time_sp, 0) + ev3_params = { 'motor': ev3.Motor('outA'), 'port': 'outA', @@ -554,8 +553,16 @@ def test_time_sp_after_reset(self): 'driver_name': 'lego-nxt-motor', 'commands': ['run-forever', 'run-to-abs-pos', 'run-to-rel-pos', 'run-timed', 'run-direct', 'stop', 'reset'], 'stop_actions': ['coast', 'hold'], - 'speed_pid': { 'kP': 1000, 'kI': 60, 'kD': 0 }, - 'hold_pid': { 'kP': 20000, 'kI': 0, 'kD': 0 }, + 'speed_pid': { + 'kP': 1000, + 'kI': 60, + 'kD': 0 + }, + 'hold_pid': { + 'kP': 20000, + 'kI': 0, + 'kD': 0 + }, } pistorms_params = { 'motor': ev3.Motor('pistorms:BAM1'), @@ -563,8 +570,16 @@ def test_time_sp_after_reset(self): 'driver_name': 'lego-nxt-motor', 'commands': ['run-forever', 'run-to-abs-pos', 'run-to-rel-pos', 'run-timed', 'stop', 'reset'], 'stop_actions': ['coast', 'brake', 'hold'], - 'speed_pid': { 'kP': 1000, 'kI': 60, 'kD': 0 }, - 'hold_pid': { 'kP': 20000, 'kI': 0, 'kD': 0 }, + 'speed_pid': { + 'kP': 1000, + 'kI': 60, + 'kD': 0 + }, + 'hold_pid': { + 'kP': 20000, + 'kI': 0, + 'kD': 0 + }, } paramsA = ev3_params paramsA['motor'].command = 'reset' @@ -595,16 +610,15 @@ def test_time_sp_after_reset(self): suite.addTest(ptc.ParameterizedTestCase.parameterize(TestTachoMotorStopCommandsValue, param=paramsA)) suite.addTest(ptc.ParameterizedTestCase.parameterize(TestTachoMotorTimeSpValue, param=paramsA)) - if __name__ == '__main__': - unittest.TextTestRunner(verbosity=1,buffer=True ).run(suite) + unittest.TextTestRunner(verbosity=1, buffer=True).run(suite) exit() # Move these up later -class TestMotorRelativePosition(unittest.TestCase): +class TestMotorRelativePosition(unittest.TestCase): @classmethod def setUpClass(cls): cls._motor = ev3.Motor('outA') @@ -630,7 +644,7 @@ def test_stop_brake(self): self._motor.stop_action = 'brake' self._motor.position = 0 - for i in range(1,5): + for i in range(1, 5): self._motor.command = 'run-to-rel-pos' time.sleep(1) print(self._motor.position) @@ -640,7 +654,7 @@ def test_stop_hold(self): self._motor.stop_action = 'hold' self._motor.position = 0 - for i in range(1,5): + for i in range(1, 5): self._motor.command = 'run-to-rel-pos' time.sleep(1) print(self._motor.position) @@ -648,4 +662,4 @@ def test_stop_hold(self): if __name__ == '__main__': - unittest.main(verbosity=2,buffer=True ).run(suite) + unittest.main(verbosity=2, buffer=True).run(suite) diff --git a/tests/motor/parameterizedtestcase.py b/tests/motor/parameterizedtestcase.py index 3c59cd5..bdca528 100644 --- a/tests/motor/parameterizedtestcase.py +++ b/tests/motor/parameterizedtestcase.py @@ -1,5 +1,6 @@ import unittest + class ParameterizedTestCase(unittest.TestCase): """ TestCase classes that want to be parametrized should inherit from this class. @@ -17,5 +18,5 @@ def parameterize(testcase_class, param=None): testnames = testloader.getTestCaseNames(testcase_class) suite = unittest.TestSuite() for name in testnames: - suite.addTest(testcase_class(name,param=param)) + suite.addTest(testcase_class(name, param=param)) return suite diff --git a/tests/motor/plot_matplotlib.py b/tests/motor/plot_matplotlib.py index b6f7d62..c7a3725 100644 --- a/tests/motor/plot_matplotlib.py +++ b/tests/motor/plot_matplotlib.py @@ -3,8 +3,7 @@ import argparse parser = argparse.ArgumentParser(description='Plot ev3dev datalogs.') -parser.add_argument('infile', - help='the input file to be logged') +parser.add_argument('infile', help='the input file to be logged') args = parser.parse_args() @@ -14,11 +13,10 @@ # http://matplotlib.org/examples/pylab_examples/subplots_demo.html # These are the "Tableau 20" colors as RGB. -tableau20 = [(31, 119, 180), (174, 199, 232), (255, 127, 14), (255, 187, 120), - (44, 160, 44), (152, 223, 138), (214, 39, 40), (255, 152, 150), - (148, 103, 189), (197, 176, 213), (140, 86, 75), (196, 156, 148), - (227, 119, 194), (247, 182, 210), (127, 127, 127), (199, 199, 199), - (188, 189, 34), (219, 219, 141), (23, 190, 207), (158, 218, 229)] +tableau20 = [(31, 119, 180), (174, 199, 232), (255, 127, 14), (255, 187, 120), (44, 160, 44), (152, 223, 138), + (214, 39, 40), (255, 152, 150), (148, 103, 189), (197, 176, 213), (140, 86, 75), (196, 156, 148), + (227, 119, 194), (247, 182, 210), (127, 127, 127), (199, 199, 199), (188, 189, 34), (219, 219, 141), + (23, 190, 207), (158, 218, 229)] # Scale the RGB values to the [0, 1] range, which is the format matplotlib accepts. for i in range(len(tableau20)): @@ -27,46 +25,49 @@ plt.style.use(['dark_background']) -test = json.loads( open( args.infile ).read() ) +test = json.loads(open(args.infile).read()) values = {} # Extract the data from the log in a format that's useful for plotting -for k,d in test['data'].items(): +for k, d in test['data'].items(): values['k'] = {} values['k']['x'] = [row[0] for row in d] values['k']['y'] = [] - for i,a in enumerate(test['meta']['ports'][k]['log_attributes']): - values['k']['y'].append( {'name': a, 'values': [row[1][i] for row in d]}) + for i, a in enumerate(test['meta']['ports'][k]['log_attributes']): + values['k']['y'].append({'name': a, 'values': [row[1][i] for row in d]}) f, axarr = plt.subplots(3, sharex=True) axarr[2].set_xlabel('Time (seconds)') - f.text(.95,0, args.infile, - fontsize=10, - horizontalalignment='left', - verticalalignment='center' ) - - f.text(.5,1, "{0} - {1}".format( test['meta']['title'], k), - fontsize=14, - horizontalalignment='center', - verticalalignment='center' ) - - f.text(.5,.96, "{0}".format( test['meta']['subtitle']), - fontsize=10, - horizontalalignment='center', - verticalalignment='center' ) - - f.text(.92,.5, "{0}".format( test['meta']['notes']), - fontsize=10, - horizontalalignment='left', - verticalalignment='center' ) + f.text(.95, 0, args.infile, fontsize=10, horizontalalignment='left', verticalalignment='center') + + f.text(.5, + 1, + "{0} - {1}".format(test['meta']['title'], k), + fontsize=14, + horizontalalignment='center', + verticalalignment='center') + + f.text(.5, + .96, + "{0}".format(test['meta']['subtitle']), + fontsize=10, + horizontalalignment='center', + verticalalignment='center') + + f.text(.92, + .5, + "{0}".format(test['meta']['notes']), + fontsize=10, + horizontalalignment='left', + verticalalignment='center') # Clean up the chartjunk - for i,ax in enumerate(axarr): + for i, ax in enumerate(axarr): print(i, ax) # Remove the plot frame lines. They are unnecessary chartjunk. ax.spines["top"].set_visible(False) @@ -76,12 +77,14 @@ ax.get_xaxis().tick_bottom() ax.get_yaxis().tick_left() - axarr[i].plot(values['k']['x'],values['k']['y'][i]['values'], lw=1.5, color=tableau20[i] ) - axarr[i].text(.95,1, "{0}".format( values['k']['y'][i]['name'] ), + axarr[i].plot(values['k']['x'], values['k']['y'][i]['values'], lw=1.5, color=tableau20[i]) + axarr[i].text(.95, + 1, + "{0}".format(values['k']['y'][i]['name']), fontsize=14, color=tableau20[i], horizontalalignment='right', verticalalignment='center', - transform = axarr[i].transAxes) + transform=axarr[i].transAxes) - plt.savefig("{0}-{1}.png".format(args.infile,k), bbox_inches="tight") + plt.savefig("{0}-{1}.png".format(args.infile, k), bbox_inches="tight") diff --git a/utils/console_fonts.py b/utils/console_fonts.py index f2d79a2..ace86b0 100644 --- a/utils/console_fonts.py +++ b/utils/console_fonts.py @@ -3,7 +3,6 @@ from sys import stderr from os import listdir from ev3dev2.console import Console - """ Used to iterate over the system console fonts (in /usr/share/consolefonts) and show the max row/col. @@ -20,8 +19,8 @@ def show_fonts(): """ Iterate through all the Latin "1 & 5" fonts, and see how many rows/columns the EV3 LCD console can accommodate for each font. - Note: `Terminus` fonts are "thinner"; `TerminusBold` and `VGA` offer more contrast on the LCD console - and are thus more readable; the `TomThumb` font is waaaaay too small to read! + Note: ``Terminus`` fonts are "thinner"; ``TerminusBold`` and ``VGA`` offer more contrast on the LCD console + and are thus more readable; the ``TomThumb`` font is waaaaay too small to read! """ console = Console() files = [f for f in listdir("/usr/share/consolefonts/") if f.startswith("Lat15") and f.endswith(".psf.gz")] @@ -32,7 +31,10 @@ def show_fonts(): console.text_at(font, 1, 1, False, True) console.clear_to_eol() console.text_at("{}, {}".format(console.columns, console.rows), - column=2, row=4, reset_console=False, inverse=False) + column=2, + row=4, + reset_console=False, + inverse=False) print("{}, {}, \"{}\"".format(console.columns, console.rows, font), file=stderr) fonts.append((console.columns, console.rows, font)) @@ -42,13 +44,14 @@ def show_fonts(): for cols, rows, font in fonts: print(cols, rows, font, file=stderr) console.set_font(font, True) - for row in range(1, rows+1): - for col in range(1, cols+1): + for row in range(1, rows + 1): + for col in range(1, cols + 1): console.text_at("{}".format(col % 10), col, row, False, (row % 2 == 0)) console.text_at(font.split(".")[0], 1, 1, False, True) console.clear_to_eol() -# Show the fonts; you may want to adjust the `startswith` filter to show other codesets. + +# Show the fonts; you may want to adjust the ``startswith`` filter to show other codesets. show_fonts() sleep(5) diff --git a/utils/line-follower-find-kp-ki-kd.py b/utils/line-follower-find-kp-ki-kd.py index ed3bbfc..bf97d7f 100755 --- a/utils/line-follower-find-kp-ki-kd.py +++ b/utils/line-follower-find-kp-ki-kd.py @@ -1,7 +1,6 @@ - """ This program is used to find the kp, ki, kd PID values for -`MoveTank.follow_line()`. These values vary from robot to robot, the best way +``MoveTank.follow_line()``. These values vary from robot to robot, the best way to find them for your robot is to have it follow a line, tweak the values a little, repeat. @@ -16,9 +15,7 @@ from ev3dev2.motor import OUTPUT_A, OUTPUT_B, MoveTank, SpeedPercent, LineFollowError, follow_for_ms from ev3dev2.sensor.lego import ColorSensor -from time import sleep import logging -import sys def frange(start, end, increment): @@ -37,8 +34,8 @@ def frange(start, end, increment): def find_kp_ki_kd(tank, start, end, increment, speed, kx_to_tweak, kp, ki, kd): """ - Return the optimal `kx_to_tweak` value where `kx_to_tweak` must be "kp", "ki" or "kd" - This will test values from `start` to `end` in steps of `increment`. The value + Return the optimal ``kx_to_tweak`` value where ``kx_to_tweak`` must be "kp", "ki" or "kd" + This will test values from ``start`` to ``end`` in steps of ``increment``. The value that results in the robot moving the least total distance is the optimal value that is returned by this function. """ @@ -53,7 +50,9 @@ def find_kp_ki_kd(tank, start, end, increment, speed, kx_to_tweak, kp, ki, kd): try: if kx_to_tweak == "kp": tank.follow_line( - kp=kx, ki=ki, kd=kd, + kp=kx, + ki=ki, + kd=kd, speed=speed, follow_for=follow_for_ms, ms=10000, @@ -61,7 +60,9 @@ def find_kp_ki_kd(tank, start, end, increment, speed, kx_to_tweak, kp, ki, kd): elif kx_to_tweak == "ki": tank.follow_line( - kp=kp, ki=kx, kd=kd, + kp=kp, + ki=kx, + kd=kd, speed=speed, follow_for=follow_for_ms, ms=10000, @@ -69,7 +70,9 @@ def find_kp_ki_kd(tank, start, end, increment, speed, kx_to_tweak, kp, ki, kd): elif kx_to_tweak == "kd": tank.follow_line( - kp=kp, ki=ki, kd=kx, + kp=kp, + ki=ki, + kd=kx, speed=speed, follow_for=follow_for_ms, ms=10000, @@ -81,7 +84,7 @@ def find_kp_ki_kd(tank, start, end, increment, speed, kx_to_tweak, kp, ki, kd): except LineFollowError: continue - except: + except Exception: tank.stop() raise @@ -102,8 +105,7 @@ def find_kp_ki_kd(tank, start, end, increment, speed, kx_to_tweak, kp, ki, kd): if __name__ == "__main__": # logging - logging.basicConfig(level=logging.DEBUG, - format="%(asctime)s %(levelname)5s: %(message)s") + logging.basicConfig(level=logging.DEBUG, format="%(asctime)s %(levelname)5s: %(message)s") log = logging.getLogger(__name__) tank = MoveTank(OUTPUT_A, OUTPUT_B) diff --git a/utils/move_differential.py b/utils/move_differential.py index 0466a30..34d406d 100755 --- a/utils/move_differential.py +++ b/utils/move_differential.py @@ -1,19 +1,15 @@ #!/usr/bin/env python3 - """ Used to experiment with the MoveDifferential class """ from ev3dev2.motor import OUTPUT_A, OUTPUT_B, MoveDifferential, SpeedRPM from ev3dev2.wheel import EV3Tire -from ev3dev2.unit import DistanceFeet from math import pi import logging -import sys # logging -logging.basicConfig(level=logging.DEBUG, - format="%(asctime)s %(levelname)5s: %(message)s") +logging.basicConfig(level=logging.DEBUG, format="%(asctime)s %(levelname)5s: %(message)s") log = logging.getLogger(__name__) STUD_MM = 8 @@ -34,53 +30,52 @@ mdiff = MoveDifferential(OUTPUT_A, OUTPUT_B, EV3Tire, 16.3 * STUD_MM) # This goes crazy on brickpi3, does it do the same on ev3? -#mdiff.on_for_distance(SpeedRPM(-40), 720, brake=False) -#mdiff.on_for_distance(SpeedRPM(40), 720, brake=False) +# mdiff.on_for_distance(SpeedRPM(-40), 720, brake=False) +# mdiff.on_for_distance(SpeedRPM(40), 720, brake=False) # Test arc left/right turns -#mdiff.on_arc_right(SpeedRPM(80), ONE_FOOT_CICLE_RADIUS_MM, ONE_FOOT_CICLE_CIRCUMFERENCE_MM / 4) +# mdiff.on_arc_right(SpeedRPM(80), ONE_FOOT_CICLE_RADIUS_MM, ONE_FOOT_CICLE_CIRCUMFERENCE_MM / 4) mdiff.on_arc_left(SpeedRPM(80), ONE_FOOT_CICLE_RADIUS_MM, ONE_FOOT_CICLE_CIRCUMFERENCE_MM) # Test turning in place -#mdiff.turn_right(SpeedRPM(40), 180) -#mdiff.turn_left(SpeedRPM(40), 180) +# mdiff.turn_right(SpeedRPM(40), 180) +# mdiff.turn_left(SpeedRPM(40), 180) # Test odometry -#mdiff.odometry_start() -#mdiff.odometry_coordinates_log() +# mdiff.odometry_start() +# mdiff.odometry_coordinates_log() -#mdiff.turn_to_angle(SpeedRPM(40), 0) -#mdiff.on_for_distance(SpeedRPM(40), DistanceFeet(2).mm) -#mdiff.turn_right(SpeedRPM(40), 180) -#mdiff.turn_left(SpeedRPM(30), 90) -#mdiff.on_arc_left(SpeedRPM(80), ONE_FOOT_CICLE_RADIUS_MM, ONE_FOOT_CICLE_CIRCUMFERENCE_MM) +# from ev3dev2.unit import DistanceFeet +# mdiff.turn_to_angle(SpeedRPM(40), 0) +# mdiff.on_for_distance(SpeedRPM(40), DistanceFeet(2).mm) +# mdiff.turn_right(SpeedRPM(40), 180) +# mdiff.turn_left(SpeedRPM(30), 90) +# mdiff.on_arc_left(SpeedRPM(80), ONE_FOOT_CICLE_RADIUS_MM, ONE_FOOT_CICLE_CIRCUMFERENCE_MM) # Drive in a quarter arc to the right then go back to where you started -#log.info("turn on arc to the right") -#mdiff.on_arc_right(SpeedRPM(40), ONE_FOOT_CICLE_RADIUS_MM, ONE_FOOT_CICLE_CIRCUMFERENCE_MM / 4) -#mdiff.odometry_coordinates_log() -#log.info("\n\n\n\n") -#log.info("go back to (0, 0)") -#mdiff.odometry_coordinates_log() -#mdiff.on_to_coordinates(SpeedRPM(40), 0, 0) -#mdiff.turn_to_angle(SpeedRPM(40), 90) +# log.info("turn on arc to the right") +# mdiff.on_arc_right(SpeedRPM(40), ONE_FOOT_CICLE_RADIUS_MM, ONE_FOOT_CICLE_CIRCUMFERENCE_MM / 4) +# mdiff.odometry_coordinates_log() +# log.info("\n\n\n\n") +# log.info("go back to (0, 0)") +# mdiff.odometry_coordinates_log() +# mdiff.on_to_coordinates(SpeedRPM(40), 0, 0) +# mdiff.turn_to_angle(SpeedRPM(40), 90) # Drive in a rectangle -#mdiff.turn_to_angle(SpeedRPM(40), 120) -#mdiff.on_to_coordinates(SpeedRPM(40), 0, DistanceFeet(1).mm) -#mdiff.on_to_coordinates(SpeedRPM(40), DistanceFeet(2).mm, DistanceFeet(1).mm) -#mdiff.on_to_coordinates(SpeedRPM(40), DistanceFeet(2).mm, 0) -#mdiff.on_to_coordinates(SpeedRPM(40), 0, 0) -#mdiff.turn_to_angle(SpeedRPM(40), 90) - +# mdiff.turn_to_angle(SpeedRPM(40), 120) +# mdiff.on_to_coordinates(SpeedRPM(40), 0, DistanceFeet(1).mm) +# mdiff.on_to_coordinates(SpeedRPM(40), DistanceFeet(2).mm, DistanceFeet(1).mm) +# mdiff.on_to_coordinates(SpeedRPM(40), DistanceFeet(2).mm, 0) +# mdiff.on_to_coordinates(SpeedRPM(40), 0, 0) +# mdiff.turn_to_angle(SpeedRPM(40), 90) # Use odometry to drive to specific coordinates -#mdiff.on_to_coordinates(SpeedRPM(40), 600, 300) +# mdiff.on_to_coordinates(SpeedRPM(40), 600, 300) # Now go back to where we started and rotate in place to 90 degrees -#mdiff.on_to_coordinates(SpeedRPM(40), 0, 0) -#mdiff.turn_to_angle(SpeedRPM(40), 90) - +# mdiff.on_to_coordinates(SpeedRPM(40), 0, 0) +# mdiff.turn_to_angle(SpeedRPM(40), 90) -#mdiff.odometry_coordinates_log() -#mdiff.odometry_stop() +# mdiff.odometry_coordinates_log() +# mdiff.odometry_stop() diff --git a/utils/move_motor.py b/utils/move_motor.py index 4c0aee8..bfafd45 100755 --- a/utils/move_motor.py +++ b/utils/move_motor.py @@ -1,5 +1,4 @@ #!/usr/bin/env python3 - """ Used to adjust the position of a motor in an already assembled robot where you can"t move the motor by hand. @@ -8,7 +7,6 @@ from ev3dev2.motor import OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D, Motor import argparse import logging -import sys # command line args parser = argparse.ArgumentParser(description="Used to adjust the position of a motor in an already assembled robot") @@ -18,8 +16,7 @@ args = parser.parse_args() # logging -logging.basicConfig(level=logging.INFO, - format="%(asctime)s %(levelname)5s: %(message)s") +logging.basicConfig(level=logging.INFO, format="%(asctime)s %(levelname)5s: %(message)s") log = logging.getLogger(__name__) if args.motor == "A": @@ -36,6 +33,4 @@ if args.degrees: log.info("Motor %s, current position %d, move to position %d, max speed %d" % (args.motor, motor.position, args.degrees, motor.max_speed)) - motor.run_to_rel_pos(speed_sp=args.speed, - position_sp=args.degrees, - stop_action='hold') + motor.run_to_rel_pos(speed_sp=args.speed, position_sp=args.degrees, stop_action='hold') diff --git a/utils/stop_all_motors.py b/utils/stop_all_motors.py index 1f39a27..e040146 100755 --- a/utils/stop_all_motors.py +++ b/utils/stop_all_motors.py @@ -1,5 +1,4 @@ #!/usr/bin/env micropython - """ Stop all motors """