Add gravity-compensated kinematic equations and feedback control

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,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 <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,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`.