Adds PIDCommand to the Commands2 framework. robotpy#28

commands2/__init__.py

@@ -30,6 +30,7 @@
 from .paralleldeadlinegroup import ParallelDeadlineGroup
 from .parallelracegroup import ParallelRaceGroup
 from .perpetualcommand import PerpetualCommand
+from .pidcommand import PIDCommand
 from .printcommand import PrintCommand
 from .proxycommand import ProxyCommand
 from .proxyschedulecommand import ProxyScheduleCommand
@@ -60,6 +61,7 @@
     "ParallelDeadlineGroup",
     "ParallelRaceGroup",
     "PerpetualCommand",
+    "PIDCommand",
     "PrintCommand",
     "ProxyCommand",
     "ProxyScheduleCommand",

commands2/pidcommand.py

@@ -0,0 +1,62 @@
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+from __future__ import annotations
+
+from wpimath.controller import PIDController
+from .subsystem import Subsystem
+from .command import Command
+from typing import Set, Callable, Union
+
+
+class PIDCommand(Command):
+    """
+    A command that controls an output with a :class:`.PIDController`. Runs forever by default - to add
+    exit conditions and/or other behavior, subclass this class. The controller calculation and output
+    are performed synchronously in the command's execute() method.
+
+    This class is provided by the NewCommands VendorDep
+    """
+
+    def __init__(
+        self,
+        controller: PIDController,
+        measurement_source: Callable[[], float],
+        setpoint_source: Union[float, Callable[[], float]],
+        use_output: Callable[[float], None],
+        *requirements: Subsystem,
+    ):
+        """Creates a new PIDCommand, which controls the given output with a PIDController.
+
+        :param controller:        the controller that controls the output.
+        :param measurementSource: the measurement of the process variable
+        :param setpointSource:    the controller's setpoint
+        :param useOutput:         the controller's output
+        :param requirements:      the subsystems required by this command
+        """
+        super().__init__()
+        self.controller = controller
+        self.use_output = use_output
+        self.measurement = measurement_source
+        self.setpoint = setpoint_source
+        self.requirements: Set[Subsystem] = set(requirements)
+
+    def initialize(self):
+        self.controller.reset()
+
+    def execute(self):
+        set_point = (
+            self.setpoint() if isinstance(self.setpoint, Callable) else self.setpoint
+        )
+
+        self.use_output(self.controller.calculate(self.measurement(), set_point))
+
+    def end(self, interrupted: bool):
+        self.use_output(0)
+
+    def get_controller(self) -> PIDController:
+        """Returns the PIDController used by the command.
+
+        :returns: The PIDController
+        """
+        return self.controller

tests/test_pidcommand.py

@@ -0,0 +1,96 @@
+from typing import TYPE_CHECKING
+
+from util import *  # type: ignore
+import wpimath.controller as controller
+import commands2
+
+if TYPE_CHECKING:
+    from .util import *
+
+import pytest
+
+
+def test_pidCommandSupplier(scheduler: commands2.CommandScheduler):
+    with ManualSimTime() as sim:
+        output_float = OOFloat(0.0)
+        measurement_source = OOFloat(5.0)
+        setpoint_source = OOFloat(2.0)
+        pid_controller = controller.PIDController(.1, .01, .001)
+        system = commands2.Subsystem()
+        pidCommand = commands2.PIDCommand(pid_controller, measurement_source, setpoint_source, output_float.set, system)
+        start_spying_on(pidCommand)
+        scheduler.schedule(pidCommand)
+        scheduler.run()
+        sim.step(1)
+        scheduler.run()
+
+        assert scheduler.isScheduled(pidCommand)
+
+        assert not pidCommand.controller.atSetpoint()
+
+        # Tell the pid command we're at our setpoint through the controller
+        measurement_source.set(setpoint_source())
+
+        sim.step(2)
+
+        scheduler.run()
+
+        # Should be measuring error of 0 now
+        assert pidCommand.controller.atSetpoint()
+
+
+def test_pidCommandScalar(scheduler: commands2.CommandScheduler):
+    with ManualSimTime() as sim:
+        output_float = OOFloat(0.0)
+        measurement_source = OOFloat(5.0)
+        setpoint_source = 2.0
+        pid_controller = controller.PIDController(.1, .01, .001)
+        system = commands2.Subsystem()
+        pidCommand = commands2.PIDCommand(pid_controller, measurement_source, setpoint_source, output_float.set, system)
+        start_spying_on(pidCommand)
+        scheduler.schedule(pidCommand)
+        scheduler.run()
+        sim.step(1)
+        scheduler.run()
+
+        assert scheduler.isScheduled(pidCommand)
+
+        assert not pidCommand.controller.atSetpoint()
+
+        # Tell the pid command we're at our setpoint through the controller
+        measurement_source.set(setpoint_source)
+
+        sim.step(2)
+
+        scheduler.run()
+
+        # Should be measuring error of 0 now
+        assert pidCommand.controller.atSetpoint()
+
+
+def test_withTimeout(scheduler: commands2.CommandScheduler):
+    with ManualSimTime() as sim:
+        output_float = OOFloat(0.0)
+        measurement_source = OOFloat(5.0)
+        setpoint_source = OOFloat(2.0)
+        pid_controller = controller.PIDController(.1, .01, .001)
+        system = commands2.Subsystem()
+        command1 = commands2.PIDCommand(pid_controller, measurement_source, setpoint_source, output_float.set, system)
+        start_spying_on(command1)
+
+        timeout = command1.withTimeout(2)
+
+        scheduler.schedule(timeout)
+        scheduler.run()
+
+        verify(command1).initialize()
+        verify(command1).execute()
+        assert not scheduler.isScheduled(command1)
+        assert scheduler.isScheduled(timeout)
+
+        sim.step(3)
+        scheduler.run()
+
+        verify(command1).end(True)
+        verify(command1, never()).end(False)
+        assert not scheduler.isScheduled(timeout)

tests/util.py

@@ -141,6 +141,35 @@ def __call__(self) -> bool:
         return self.pressed
 
 
+class OOFloat:
+    def __init__(self, value: float = 0.0) -> None:
+        self.value = value
+
+    def get(self) -> float:
+        return self.value
+
+    def set(self, value: float):
+        self.value = value
+
+    def incrementAndGet(self) -> float:
+        self.value += 1
+        return self.value
+
+    def addAndGet(self, value: float) -> float:
+        self.value += value
+        return self.value
+
+    def __eq__(self, value: float) -> bool:
+        return self.value == value
+
+    def __lt__(self, value: float) -> bool:
+        return self.value < value
+
+    def __call__(self) -> float:
+        return self.value
+
+    def __name__(self) -> str:
+        return "OOFloat"
 ##########################################
 # Fakito Framework