From bb206d5db1628bf2151d94ef33101cd9aa33cabf Mon Sep 17 00:00:00 2001 From: Kennan Date: Fri, 22 Dec 2023 22:15:26 +0000 Subject: [PATCH 1/6] Add gravity-compensated kinematic equations and gravity feedforward control --- README.md | 1 + source/docs/contributing/contributors.rst | 1 + .../docs/software/concepts/control-loops.rst | 15 ++++++ .../images/kinematics/reference_frame.svg | 51 +++++++++++++++++++ source/docs/software/concepts/kinematics.rst | 35 +++++++++++++ 5 files changed, 103 insertions(+) create mode 100644 source/docs/software/concepts/images/kinematics/reference_frame.svg diff --git a/README.md b/README.md index 2b29644b..c3b71cd4 100644 --- a/README.md +++ b/README.md @@ -38,5 +38,6 @@ How to see the options for building: - `make help` How to develop the website: +- Download sphinx autobuild with `pip install sphinx-autobuild`or poetry - Run `make autobuild` - This will set up a file watcher and build on file changes. A development server is served at `http://127.0.0.1:8000` by default. Go to your preferred browser and open that URL to view your local development version of gm0. diff --git a/source/docs/contributing/contributors.rst b/source/docs/contributing/contributors.rst index 63a1a533..0322b6c6 100644 --- a/source/docs/contributing/contributors.rst +++ b/source/docs/contributing/contributors.rst @@ -41,6 +41,7 @@ Other Contributors - Justin - FTC 9656 Omega - Karter - FTC 5975 Cybots - Kelvin - FTC 731 Wannabee Strange +- Kennan - FTC 11329 I.C.E Robotics - Keval - FTC 731 Wannabee Strange/FTC 10195 Night Owls - Kevin - FTC 9048 Philobots - Nate - FTC 12897 Newton's Law of Mass diff --git a/source/docs/software/concepts/control-loops.rst b/source/docs/software/concepts/control-loops.rst index 5ef2cca6..648c0395 100644 --- a/source/docs/software/concepts/control-loops.rst +++ b/source/docs/software/concepts/control-loops.rst @@ -202,6 +202,21 @@ In every system there is bound to be some amount of static Friction. This means sign = signum(error) # sign of error, will be -1, 0, or 1 output = sign * staticFriction + PID(error); # PID Controller + Friction Feedforward +.. _gravity-compensated-feedforward: + +Gravity Compensated Feedforward +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +In :ref:`gravity-compensation` we derive the effect of gravity upon an arm as :math:`F_g = g * \sin{\theta}`. Here we can use that with the following logic. + +.. code-block:: python + + while True: + error = desire_position - current_position; # no effect on gravity compensation + current_angle = (TICKS_AT_ZERO - current_tick) * DEGREE_PER_TICK + output = PID(error) + sin(radians(current_angle)) * kF + +This code uses a kF constant to approximate how much power the arm needs to counteract gravity. In theory, this is something related to gravity multiplied by the rotational moment of inertia of your arm. Calculating this is impractical and kF is instead found emperically. This can be done by setting the gains on the PID to 0, and increasing kF until the arm can hold itself up at any position. If your arm is still falling, increase kF, and if your arm is moving upwards, decrease kF. FTC team 16379 Kookybotz has `an excellent video on Arm programming `_, where they demonstrate how to increase kF. Motion Profiles --------------- diff --git a/source/docs/software/concepts/images/kinematics/reference_frame.svg b/source/docs/software/concepts/images/kinematics/reference_frame.svg new file mode 100644 index 00000000..1e4fb450 --- /dev/null +++ b/source/docs/software/concepts/images/kinematics/reference_frame.svg @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/source/docs/software/concepts/kinematics.rst b/source/docs/software/concepts/kinematics.rst index f21d2fc8..2e79c715 100644 --- a/source/docs/software/concepts/kinematics.rst +++ b/source/docs/software/concepts/kinematics.rst @@ -107,3 +107,38 @@ The inverse kinematics of a mecanum drive relate the desired velocity of the rob v_{br} = v_f - v_s + (2r_b \cdot \omega) v_{fr} = v_f + v_s + (2r_b \cdot \omega) + +Manipulators +-------------- + +.. _gravity-compensation: + +Gravity Compensation +^^^^^^^^^^^^^^^^^^^^ + +Often in FTC, teams have arms that swing out around an axis. Controlling these arms requires a bit of thought as depending on the angle they're at, the effect of gravity drastically changes. + +Our first step is defining a reference frame. Our recommendation is to define zero as straight up and down, as this is when gravity doesn't have an effect on your arm. Other reference frames will still work, but the trigonometry won't be as straightforward. + +.. figure:: ./images/kinematics/reference_frame.svg + :alt: A diagram of a robot with a long arm, with 0 degrees marked up and down + :align: center + :width: 400px + + Image courtesy of 11329 ICE Robotics + +In a reference frame like this, the relative force of gravity will be equal to the sin of the angle. This makes sense as when the sin function is evaluated at zero degrees, it returns 0, while at 90 degrees where the effect of gravity is at its greatest, it returns 1. + +.. math:: + + F_g = g * \sin{\theta} + +Assuming you have an encoder on your arm, you can find :math:`\theta` pretty easily using the following code. + +.. note:: DEGREE_PER_TICK can be found by taking 360 divided by your encoder resolution all multiplied by your gear ratio. A GoBuilda 5202 motor with a gear ratio of 1:1 will have a TICK_PER_REV of 751.8, and a DEGREE_PER_TICK of 0.47885075818. + +.. code:: java + + current_angle = (TICKS_AT_ZERO - current_tick) * DEGREE_PER_TICK + +The typical way to utilize the effect of gravity you just found, is to pass it in as a feedforward parameter to a PID controller. We cover this in :ref:`gravity-compensated-feedforward`. \ No newline at end of file From 756136c33bb6a4d8c33f0e121c4cd9951c94b621 Mon Sep 17 00:00:00 2001 From: Kennan Date: Fri, 22 Dec 2023 17:22:40 -0500 Subject: [PATCH 2/6] Fix linter errors --- source/docs/software/concepts/control-loops.rst | 2 +- source/docs/software/concepts/kinematics.rst | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/source/docs/software/concepts/control-loops.rst b/source/docs/software/concepts/control-loops.rst index 648c0395..bd85e270 100644 --- a/source/docs/software/concepts/control-loops.rst +++ b/source/docs/software/concepts/control-loops.rst @@ -205,7 +205,7 @@ In every system there is bound to be some amount of static Friction. This means .. _gravity-compensated-feedforward: Gravity Compensated Feedforward -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ In :ref:`gravity-compensation` we derive the effect of gravity upon an arm as :math:`F_g = g * \sin{\theta}`. Here we can use that with the following logic. diff --git a/source/docs/software/concepts/kinematics.rst b/source/docs/software/concepts/kinematics.rst index 2e79c715..ea27ef36 100644 --- a/source/docs/software/concepts/kinematics.rst +++ b/source/docs/software/concepts/kinematics.rst @@ -133,7 +133,7 @@ In a reference frame like this, the relative force of gravity will be equal to t F_g = g * \sin{\theta} -Assuming you have an encoder on your arm, you can find :math:`\theta` pretty easily using the following code. +Assuming you have an encoder on your arm, you can find :math:`\theta` pretty easily using the following code. .. note:: DEGREE_PER_TICK can be found by taking 360 divided by your encoder resolution all multiplied by your gear ratio. A GoBuilda 5202 motor with a gear ratio of 1:1 will have a TICK_PER_REV of 751.8, and a DEGREE_PER_TICK of 0.47885075818. @@ -141,4 +141,4 @@ Assuming you have an encoder on your arm, you can find :math:`\theta` pretty eas current_angle = (TICKS_AT_ZERO - current_tick) * DEGREE_PER_TICK -The typical way to utilize the effect of gravity you just found, is to pass it in as a feedforward parameter to a PID controller. We cover this in :ref:`gravity-compensated-feedforward`. \ No newline at end of file +The typical way to utilize the effect of gravity you just found, is to pass it in as a feedforward parameter to a PID controller. We cover this in :ref:`gravity-compensated-feedforward`. From 76bbb3d149e73725c3702feb759154a058804d5c Mon Sep 17 00:00:00 2001 From: Kennan Date: Fri, 22 Dec 2023 17:24:18 -0500 Subject: [PATCH 3/6] readme tweaks --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index c3b71cd4..fb8f897f 100644 --- a/README.md +++ b/README.md @@ -38,6 +38,6 @@ How to see the options for building: - `make help` How to develop the website: -- Download sphinx autobuild with `pip install sphinx-autobuild`or poetry +- Download sphinx autobuild with `pip install sphinx-autobuild` or `poetry install` - Run `make autobuild` - This will set up a file watcher and build on file changes. A development server is served at `http://127.0.0.1:8000` by default. Go to your preferred browser and open that URL to view your local development version of gm0. From 5d12daf38a380ffcbd07d3f6eada4ff0ff410ce1 Mon Sep 17 00:00:00 2001 From: Kennan Date: Fri, 22 Dec 2023 17:39:26 -0500 Subject: [PATCH 4/6] remove readme tweaks --- README.md | 1 - 1 file changed, 1 deletion(-) diff --git a/README.md b/README.md index fb8f897f..2b29644b 100644 --- a/README.md +++ b/README.md @@ -38,6 +38,5 @@ How to see the options for building: - `make help` How to develop the website: -- Download sphinx autobuild with `pip install sphinx-autobuild` or `poetry install` - Run `make autobuild` - This will set up a file watcher and build on file changes. A development server is served at `http://127.0.0.1:8000` by default. Go to your preferred browser and open that URL to view your local development version of gm0. From 87a0c747440064db6e1a59d6792a53f4bd9df70e Mon Sep 17 00:00:00 2001 From: Kennan Date: Mon, 25 Dec 2023 00:36:45 -0500 Subject: [PATCH 5/6] Apply wording and content changes from pull request review Co-authored-by: abidingabi --- source/docs/software/concepts/control-loops.rst | 2 +- source/docs/software/concepts/kinematics.rst | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/source/docs/software/concepts/control-loops.rst b/source/docs/software/concepts/control-loops.rst index bd85e270..cf7aaac8 100644 --- a/source/docs/software/concepts/control-loops.rst +++ b/source/docs/software/concepts/control-loops.rst @@ -207,7 +207,7 @@ In every system there is bound to be some amount of static Friction. This means Gravity Compensated Feedforward ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -In :ref:`gravity-compensation` we derive the effect of gravity upon an arm as :math:`F_g = g * \sin{\theta}`. Here we can use that with the following logic. +In :ref:`gravity-compensation` we derive the effect of gravity upon an arm as :math:`F_g = g\sin{\theta}`. Here we can use that with the following logic. .. code-block:: python diff --git a/source/docs/software/concepts/kinematics.rst b/source/docs/software/concepts/kinematics.rst index ea27ef36..b755fccb 100644 --- a/source/docs/software/concepts/kinematics.rst +++ b/source/docs/software/concepts/kinematics.rst @@ -125,17 +125,17 @@ Our first step is defining a reference frame. Our recommendation is to define ze :align: center :width: 400px - Image courtesy of 11329 ICE Robotics + 11329 I.C.E. Robotics In a reference frame like this, the relative force of gravity will be equal to the sin of the angle. This makes sense as when the sin function is evaluated at zero degrees, it returns 0, while at 90 degrees where the effect of gravity is at its greatest, it returns 1. .. math:: - F_g = g * \sin{\theta} + F_g = g\sin{\theta} -Assuming you have an encoder on your arm, you can find :math:`\theta` pretty easily using the following code. +Assuming you have an encoder on your arm, you can find :math:`\theta` (the angle of the arm relative to the vertical) using something similar to the following pseudocode. -.. note:: DEGREE_PER_TICK can be found by taking 360 divided by your encoder resolution all multiplied by your gear ratio. A GoBuilda 5202 motor with a gear ratio of 1:1 will have a TICK_PER_REV of 751.8, and a DEGREE_PER_TICK of 0.47885075818. +.. note:: DEGREE_PER_TICK can be found by taking 360 divided by your encoder resolution all multiplied by your gear ratio. For example, for a `19.2:1 goBILDA Yellow Jacket `_, there are 537.7 ticks per revolution, and so :math:`\frac{360}{537.7}` degrees per tick at the gearbox output shaft. If there was a 2:1 gear ratio after that, then the ticks per revolution would instead be :math:`\frac{360}{2 \times 537.7}` .. code:: java From d2699eb24db223dd8cbd2925cd727ea5fae53691 Mon Sep 17 00:00:00 2001 From: Abigail Date: Mon, 25 Dec 2023 00:47:10 -0500 Subject: [PATCH 6/6] Fix image width issue --- source/conf.py | 6 +++--- source/docs/software/concepts/kinematics.rst | 2 -- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/source/conf.py b/source/conf.py index 18a25c1f..095f05e4 100644 --- a/source/conf.py +++ b/source/conf.py @@ -119,10 +119,10 @@ # Required accuracy for redirect writer rediraffe_auto_redirect_perc = 80 -# Set the default image width. +# Set the default image width (in pixels). # This only applies to images without an explicit width set. -default_image_width_html = "25em" -default_image_width_latex = "20em" +default_image_width_html = "400" +default_image_width_latex = "320" # Center images by default. default_image_centered = True diff --git a/source/docs/software/concepts/kinematics.rst b/source/docs/software/concepts/kinematics.rst index b755fccb..25d94e4e 100644 --- a/source/docs/software/concepts/kinematics.rst +++ b/source/docs/software/concepts/kinematics.rst @@ -122,8 +122,6 @@ Our first step is defining a reference frame. Our recommendation is to define ze .. figure:: ./images/kinematics/reference_frame.svg :alt: A diagram of a robot with a long arm, with 0 degrees marked up and down - :align: center - :width: 400px 11329 I.C.E. Robotics