Update advanced controllers section with Python examples (#2575)

source/docs/software/advanced-controls/controllers/bang-bang.rst

@@ -7,7 +7,7 @@ This may initially seem like a poor control strategy, as PID loops are known to
 
 However, when controlling the velocity of high-inertia mechanisms under varying loads (like a shooter flywheel), a bang-bang controller can yield faster recovery time and thus better/more consistent performance than a proportional controller.  Unlike an ordinary P loop, a bang-bang controller is *asymmetric* - that is, the controller turns on when the process variable is below the setpoint, and does nothing otherwise.  This allows the control effort in the forward direction to be made as large as possible without risking destructive oscillations as the control loop tries to correct a resulting overshoot.
 
-Asymmetric bang-bang control is provided in WPILib by the BangBangController class (`Java <https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/math/controller/BangBangController.html>`__, `C++ <https://github.wpilib.org/allwpilib/docs/release/cpp/classfrc_1_1_bang_bang_controller.html>`__).
+Asymmetric bang-bang control is provided in WPILib by the BangBangController class (`Java <https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/math/controller/BangBangController.html>`__, `C++ <https://github.wpilib.org/allwpilib/docs/release/cpp/classfrc_1_1_bang_bang_controller.html>`__, :external:py:class:`Python <wpimath.controller.BangBangController>`).
 
 Constructing a BangBangController
 ---------------------------------
@@ -28,8 +28,10 @@ Since a bang-bang controller does not have any gains, it does not need any const
 
   .. code-block:: python
 
-     # Creates a BangBangController
-     controller = wpimath.BangBangController()
+    from wpimath.controller import BangBangController
+
+    # Creates a BangBangController
+    controller = BangBangController()
 
 Using a BangBangController
 --------------------------

source/docs/software/advanced-controls/controllers/combining-feedforward-feedback.rst

@@ -28,15 +28,15 @@ Users may add any feedforward they like to the output of the controller before s
 
   .. code-block:: python
 
-     // Adds a feedforward to the loop output before sending it to the motor
+     # Adds a feedforward to the loop output before sending it to the motor
      motor.setVoltage(pid.calculate(encoder.getDistance(), setpoint) + feedforward)
 
 Moreover, feedforward is a separate feature entirely from feedback, and thus has no reason to be handled in the same controller object, as this violates separation of concerns.  WPILib comes with several helper classes to compute accurate feedforward voltages for common FRC\ |reg| mechanisms - for more information, see :ref:`docs/software/advanced-controls/controllers/feedforward:Feedforward Control in WPILib`.
 
 Using Feedforward Components with PID
 -------------------------------------
 
-.. note:: Since feedforward voltages are physically meaningful, it is best to use the ``setVoltage()`` (`Java <https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/wpilibj/motorcontrol/MotorController.html#setVoltage(double)>`__, `C++ <https://github.wpilib.org/allwpilib/docs/release/cpp/classfrc_1_1_motor_controller.html#a613c23a3336e103876e433bcb8b5ad3e>`__) method when applying them to motors to compensate for "voltage sag" from the battery.
+.. note:: Since feedforward voltages are physically meaningful, it is best to use the ``setVoltage()`` (`Java <https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/wpilibj/motorcontrol/MotorController.html#setVoltage(double)>`__, `C++ <https://github.wpilib.org/allwpilib/docs/release/cpp/classfrc_1_1_motor_controller.html#a613c23a3336e103876e433bcb8b5ad3e>`__, :external:py:meth:`Python <wpilib.interfaces.MotorController.setVoltage>`) method when applying them to motors to compensate for "voltage sag" from the battery.
 
 What might a more complete example of combined feedforward/PID control look like?  Consider the :ref:`drive example <docs/software/advanced-controls/controllers/feedforward:Using Feedforward to Control Mechanisms>` from the feedforward page.  We can easily modify this to include feedback control (with a ``SimpleMotorFeedforward`` component):
 

source/docs/software/advanced-controls/controllers/feedforward.rst

@@ -18,9 +18,9 @@ The WPILib feedforward classes closely match the available mechanism characteriz
 
 WPILib currently provides the following three helper classes for feedforward control:
 
-* `SimpleMotorFeedforward`_ (`Java <https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.html>`__, `C++ <https://github.wpilib.org/allwpilib/docs/release/cpp/classfrc_1_1_simple_motor_feedforward.html>`__)
-* `ArmFeedforward`_ (`Java <https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/math/controller/ArmFeedforward.html>`__, `C++ <https://github.wpilib.org/allwpilib/docs/release/cpp/classfrc_1_1_arm_feedforward.html>`__)
-* `ElevatorFeedforward`_ (`Java <https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/math/controller/ElevatorFeedforward.html>`__, `C++ <https://github.wpilib.org/allwpilib/docs/release/cpp/classfrc_1_1_elevator_feedforward.html>`__)
+* `SimpleMotorFeedforward`_ (`Java <https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.html>`__, `C++ <https://github.wpilib.org/allwpilib/docs/release/cpp/classfrc_1_1_simple_motor_feedforward.html>`__, :external:py:class:`Python <wpimath.controller.SimpleMotorFeedforwardMeters>`)
+* `ArmFeedforward`_ (`Java <https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/math/controller/ArmFeedforward.html>`__, `C++ <https://github.wpilib.org/allwpilib/docs/release/cpp/classfrc_1_1_arm_feedforward.html>`__, :external:py:class:`Python <wpimath.controller.ArmFeedforward>`)
+* `ElevatorFeedforward`_ (`Java <https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/math/controller/ElevatorFeedforward.html>`__, `C++ <https://github.wpilib.org/allwpilib/docs/release/cpp/classfrc_1_1_elevator_feedforward.html>`__, :external:py:class:`Python <wpimath.controller.ElevatorFeedforward>`)
 
 SimpleMotorFeedforward
 ----------------------
@@ -29,6 +29,8 @@ SimpleMotorFeedforward
 
 .. note:: The Java feedforward components will calculate outputs in units determined by the units of the user-provided feedforward gains.  Users *must* take care to keep units consistent, as WPILibJ does not have a type-safe unit system.
 
+.. note:: The API documentation for Python feedforward components indicate which unit is being used as ``wpimath.units.NAME``. Users *must* take care to use correct units, as Python does not have a type-safe unit system.
+
 The ``SimpleMotorFeedforward`` class calculates feedforwards for mechanisms that consist of permanent-magnet DC motors with no external loading other than friction and inertia, such as flywheels and robot drives.
 
 To create a ``SimpleMotorFeedforward``, simply construct it with the required gains:
@@ -48,6 +50,14 @@ To create a ``SimpleMotorFeedforward``, simply construct it with the required ga
     // Distance is measured in meters
     frc::SimpleMotorFeedforward<units::meters> feedforward(kS, kV, kA);
 
+  .. code-block:: python
+
+    from wpimath.controller import SimpleMotorFeedforwardMeters
+
+    # Create a new SimpleMotorFeedforward with gains kS, kV, and kA
+    # Distance is measured in meters
+    feedforward = SimpleMotorFeedforwardMeters(kS, kV, kA)
+
 To calculate the feedforward, simply call the ``calculate()`` method with the desired motor velocity and acceleration:
 
 .. note:: The acceleration argument may be omitted from the ``calculate()`` call, and if it is, will default to a value of zero.  This should be done whenever there is not a clearly-defined acceleration setpoint.
@@ -66,13 +76,21 @@ To calculate the feedforward, simply call the ``calculate()`` method with the de
     // Output is in volts
     feedforward.Calculate(10_mps, 20_mps_sq);
 
+  .. code-block:: python
+
+    # Calculates the feedforward for a velocity of 10 meters/second and an acceleration of 20 meters/second^2
+    # Output is in volts
+    feedforward.calculate(10, 20)
+
 ArmFeedforward
 --------------
 
 .. note:: In C++, the ``ArmFeedforward`` class assumes distances are angular, not linear.  The passed-in gains *must* have units consistent with the angular unit, or a compile-time error will be thrown.  ``kS`` and ``kG`` should have units of ``volts``, ``kV`` should have units of ``volts * seconds / radians``, and ``kA`` should have units of ``volts * seconds^2 / radians``.  For more information on C++ units, see :ref:`docs/software/basic-programming/cpp-units:The C++ Units Library`.
 
 .. note:: The Java feedforward components will calculate outputs in units determined by the units of the user-provided feedforward gains.  Users *must* take care to keep units consistent, as WPILibJ does not have a type-safe unit system.
 
+.. note:: The API documentation for Python feedforward components indicate which unit is being used as ``wpimath.units.NAME``. Users *must* take care to use correct units, as Python does not have a type-safe unit system.
+
 The ``ArmFeedforward`` class calculates feedforwards for arms that are controlled directly by a permanent-magnet DC motor, with external loading of friction, inertia, and mass of the arm.  This is an accurate model of most arms in FRC.
 
 To create an ``ArmFeedforward``, simply construct it with the required gains:
@@ -91,6 +109,13 @@ To create an ``ArmFeedforward``, simply construct it with the required gains:
     // Create a new ArmFeedforward with gains kS, kG, kV, and kA
     frc::ArmFeedforward feedforward(kS, kG, kV, kA);
 
+  .. code-block:: python
+
+    from wpimath.controller import ArmFeedforward
+
+    # Create a new ArmFeedforward with gains kS, kG, kV, and kA
+    feedforward = ArmFeedforward(kS, kG, kV, kA)
+
 To calculate the feedforward, simply call the ``calculate()`` method with the desired arm position, velocity, and acceleration:
 
 .. note:: The acceleration argument may be omitted from the ``calculate()`` call, and if it is, will default to a value of zero.  This should be done whenever there is not a clearly-defined acceleration setpoint.
@@ -111,13 +136,22 @@ To calculate the feedforward, simply call the ``calculate()`` method with the de
     // Output is in volts
     feedforward.Calculate(1_rad, 2_rad_per_s, 3_rad/(1_s * 1_s));
 
+  .. code-block:: python
+
+    # Calculates the feedforward for a position of 1 radians, a velocity of 2 radians/second, and
+    # an acceleration of 3 radians/second^2
+    # Output is in volts
+    feedforward.calculate(1, 2, 3)
+
 ElevatorFeedforward
 -------------------
 
 .. note:: In C++, the passed-in gains *must* have units consistent with the distance units, or a compile-time error will be thrown.  ``kS`` and ``kG`` should have units of ``volts``, ``kV`` should have units of ``volts * seconds / distance``, and ``kA`` should have units of ``volts * seconds^2 / distance``.  For more information on C++ units, see :ref:`docs/software/basic-programming/cpp-units:The C++ Units Library`.
 
 .. note:: The Java feedforward components will calculate outputs in units determined by the units of the user-provided feedforward gains.  Users *must* take care to keep units consistent, as WPILibJ does not have a type-safe unit system.
 
+.. note:: The API documentation for Python feedforward components indicate which unit is being used as ``wpimath.units.NAME``. Users *must* take care to use correct units, as Python does not have a type-safe unit system.
+
 The ``ElevatorFeedforward`` class calculates feedforwards for elevators that consist of permanent-magnet DC motors loaded by friction, inertia, and the mass of the elevator.  This is an accurate model of most elevators in FRC.
 
 To create a ``ElevatorFeedforward``, simply construct it with the required gains:
@@ -137,6 +171,14 @@ To create a ``ElevatorFeedforward``, simply construct it with the required gains
     // Distance is measured in meters
     frc::ElevatorFeedforward feedforward(kS, kG, kV, kA);
 
+  .. code-block:: python
+
+    from wpimath.controller import ElevatorFeedforward
+
+    # Create a new ElevatorFeedforward with gains kS, kV, and kA
+    # Distance is measured in meters
+    feedforward = ElevatorFeedforward(kS, kG, kV, kA)
+
 To calculate the feedforward, simply call the ``calculate()`` method with the desired motor velocity and acceleration:
 
 .. note:: The acceleration argument may be omitted from the ``calculate()`` call, and if it is, will default to a value of zero.  This should be done whenever there is not a clearly-defined acceleration setpoint.
@@ -157,10 +199,17 @@ To calculate the feedforward, simply call the ``calculate()`` method with the de
     // Output is in volts
     feedforward.Calculate(20_mps, 30_mps_sq);
 
+  .. code-block:: python
+
+    # Calculates the feedforward for a velocity of 20 meters/second
+    # and an acceleration of 30 meters/second^2
+    # Output is in volts
+    feedforward.calculate(20, 30)
+
 Using Feedforward to Control Mechanisms
 ---------------------------------------
 
-.. note:: Since feedforward voltages are physically meaningful, it is best to use the ``setVoltage()`` (`Java <https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/wpilibj/motorcontrol/MotorController.html#setVoltage(double)>`__, `C++ <https://github.wpilib.org/allwpilib/docs/release/cpp/classfrc_1_1_motor_controller.html#a613c23a3336e103876e433bcb8b5ad3e>`__) method when applying them to motors to compensate for "voltage sag" from the battery.
+.. note:: Since feedforward voltages are physically meaningful, it is best to use the ``setVoltage()`` (`Java <https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/wpilibj/motorcontrol/MotorController.html#setVoltage(double)>`__, `C++ <https://github.wpilib.org/allwpilib/docs/release/cpp/classfrc_1_1_motor_controller.html#a613c23a3336e103876e433bcb8b5ad3e>`__, :external:py:meth:`Python <wpilib.interfaces.MotorController.setVoltage>`) method when applying them to motors to compensate for "voltage sag" from the battery.
 
 Feedforward control can be used entirely on its own, without a feedback controller.  This is known as "open-loop" control, and for many mechanisms (especially robot drives) can be perfectly satisfactory.  A ``SimpleMotorFeedforward`` might be employed to control a robot drive as follows:
 
@@ -180,3 +229,9 @@ Feedforward control can be used entirely on its own, without a feedback controll
       leftMotor.SetVoltage(feedforward.Calculate(leftVelocity));
       rightMotor.SetVoltage(feedforward.Calculate(rightVelocity));
     }
+
+  .. code-block:: python
+
+    def tankDriveWithFeedforward(self, leftVelocity: float, rightVelocity: float):
+        self.leftMotor.setVoltage(feedForward.calculate(leftVelocity))
+        self.rightMotor.setVoltage(feedForward.calculate(rightVelocity))

source/docs/software/advanced-controls/controllers/pidcontroller.rst

@@ -5,7 +5,7 @@ PID Control in WPILib
 
 .. note:: For a guide on implementing PID control through the :ref:`command-based framework <docs/software/commandbased/what-is-command-based:What Is "Command-Based" Programming?>`, see :ref:`docs/software/commandbased/pid-subsystems-commands:PID Control through PIDSubsystems and PIDCommands`.
 
-WPILib supports PID control of mechanisms through the ``PIDController`` class (`Java <https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/math/controller/PIDController.html>`__, `C++ <https://github.wpilib.org/allwpilib/docs/release/cpp/classfrc_1_1_p_i_d_controller.html>`__).  This class handles the feedback loop calculation for the user, as well as offering methods for returning the error, setting tolerances, and checking if the control loop has reached its setpoint within the specified tolerances.
+WPILib supports PID control of mechanisms through the ``PIDController`` class (`Java <https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/math/controller/PIDController.html>`__, `C++ <https://github.wpilib.org/allwpilib/docs/release/cpp/classfrc_1_1_p_i_d_controller.html>`__, :external:py:class:`Python <wpimath.controller.PIDController>`).  This class handles the feedback loop calculation for the user, as well as offering methods for returning the error, setting tolerances, and checking if the control loop has reached its setpoint within the specified tolerances.
 
 Using the PIDController Class
 -----------------------------
@@ -29,6 +29,13 @@ In order to use WPILib's PID control functionality, users must first construct a
     // Creates a PIDController with gains kP, kI, and kD
     frc::PIDController pid{kP, kI, kD};
 
+  .. code-block:: python
+
+    from wpimath.controller import PIDController
+
+    # Creates a PIDController with gains kP, kI, and kD
+    pid = PIDController(kP, kI, kD)
+
 An optional fourth parameter can be provided to the constructor, specifying the period at which the controller will be run.  The ``PIDController`` object is intended primarily for synchronous use from the main robot loop, and so this value is defaulted to 20ms.
 
 Using the Feedback Loop Output
@@ -52,6 +59,12 @@ Using the constructed ``PIDController`` is simple: simply call the ``calculate()
     // and sends it to a motor
     motor.Set(pid.Calculate(encoder.GetDistance(), setpoint));
 
+  .. code-block:: python
+
+    # Calculates the output of the PID algorithm based on the sensor reading
+    # and sends it to a motor
+    motor.set(pid.calculate(encoder.getDistance(), setpoint))
+
 Checking Errors
 ^^^^^^^^^^^^^^^
 
@@ -92,6 +105,15 @@ To do this, we first must specify the tolerances with the ``setTolerance()`` met
     // error derivative is less than 10 units
     pid.AtSetpoint();
 
+  .. code-block:: python
+
+    # Sets the error tolerance to 5, and the error derivative tolerance to 10 per second
+    pid.setTolerance(5, 10)
+
+    # Returns true if the error is less than 5 units, and the
+    # error derivative is less than 10 units
+    pid.atSetpoint()
+
 Resetting the Controller
 ^^^^^^^^^^^^^^^^^^^^^^^^
 
@@ -122,6 +144,12 @@ The range limits may be increased or decreased using the ``setIntegratorRange()`
     // the total loop output
     pid.SetIntegratorRange(-0.5, 0.5);
 
+  .. code-block:: python
+
+    # The integral gain term will never add or subtract more than 0.5 from
+    # the total loop output
+    pid.setIntegratorRange(-0.5, 0.5)
+
 Disabling Integral Gain if the Error is Too High
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
@@ -151,6 +179,15 @@ By default, ``IZone`` is disabled.
     // more than 2
     pid.SetIZone(2);
 
+  .. code-block:: python
+
+    # Disable IZone
+    pid.setIZone(math.inf)
+
+    # Integral gain will not be applied if the absolute value of the error is
+    # more than 2
+    pid.setIZone(2)
+
 Setting Continuous Input
 ^^^^^^^^^^^^^^^^^^^^^^^^
 
@@ -174,6 +211,11 @@ To configure a ``PIDController`` to automatically do this, use the ``enableConti
     // Enables continuous input on a range from -180 to 180
     pid.EnableContinuousInput(-180, 180);
 
+  .. code-block:: python
+
+    # Enables continuous input on a range from -180 to 180
+    pid.enableContinuousInput(-180, 180)
+
 Clamping Controller Output
 --------------------------
 
@@ -188,3 +230,12 @@ Clamping Controller Output
 
     // Clamps the controller output to between -0.5 and 0.5
     std::clamp(pid.Calculate(encoder.GetDistance(), setpoint), -0.5, 0.5);
+
+  .. code-block:: python
+
+    # Python doesn't have a builtin clamp function
+    def clamp(v, minval, maxval):
+        return max(min(v, maxval), minval)
+
+    # Clamps the controller output to between -0.5 and 0.5
+    clamp(pid.calculate(encoder.getDistance(), setpoint), -0.5, 0.5)