Add gravity compensation

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

source/docs/software/concepts/control-loops.rst

@@ -202,6 +202,22 @@ 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\cos{\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) + cos(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 empirically. 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 <https://youtu.be/E6H6Nqe6qJo?si=AztzVtAqShFEA4EP&t=440>`_, where they demonstrate how to increase kF.
+
 
 Motion Profiles
 ---------------

source/docs/software/concepts/kinematics.rst

@@ -107,3 +107,36 @@ 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 to the side, as this is when gravity has the greatest effect on your arm. Other reference frames will still work, but the exact trigonometry will change.
+
+.. figure:: ./images/kinematics/reference-frame.svg
+   :alt: A diagram of a robot with a long arm, with 0 degrees marked to the side
+
+   11329 I.C.E. Robotics
+
+In a reference frame like this, the relative force of gravity will be equal to the cos of the angle. This makes sense as when the cos function is evaluated at zero degrees, it returns 1, while at 90 degrees where there is no effect of gravity, it returns 0.
+
+.. math::
+   F_g = g\cos{\theta}
+
+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. For example, for a `19.2:1 goBILDA Yellow Jacket <https://www.gobilda.com/5203-series-yellow-jacket-planetary-gear-motor-19-2-1-ratio-24mm-length-8mm-rex-shaft-312-rpm-3-3-5v-encoder/>`_, 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
+
+   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`.