Fixed Mypy errors and added PR comments.

commands2/pidsubsystem.py

@@ -15,20 +15,20 @@ def __init__(self, controller: PIDController, initial_position: float = 0.0):
         :param controller: The PIDController to use.
         :param initial_position: The initial setpoint of the subsystem.
         """
-        self.m_controller: PIDController = controller
+        self._controller = controller
         self.setSetpoint(initial_position)
-        self.addChild("PID Controller", self.m_controller)
-        self.m_enabled = False
+        self.addChild("PID Controller", self._controller)
+        self._enabled = False
 
     def periodic(self):
         """
         Executes the PID control logic during each periodic update.
 
         This method is called synchronously from the subsystem's periodic() method.
         """
-        if self.m_enabled:
+        if self._enabled:
             self.useOutput(
-                self.m_controller.calculate(self.getMeasurement()), self.getSetpoint()
+                self._controller.calculate(self.getMeasurement()), self.getSetpoint()
             )
 
     def getController(self) -> PIDController:
@@ -37,23 +37,23 @@ def getController(self) -> PIDController:
 
         :return: The PIDController.
         """
-        return self.m_controller
+        return self._controller
 
     def setSetpoint(self, setpoint: float):
         """
         Sets the setpoint for the subsystem.
 
         :param setpoint: The setpoint for the subsystem.
         """
-        self.m_controller.setSetpoint(setpoint)
+        self._controller.setSetpoint(setpoint)
 
     def getSetpoint(self) -> float:
         """
         Returns the current setpoint of the subsystem.
 
         :return: The current setpoint.
         """
-        return self.m_controller.getSetpoint()
+        return self._controller.getSetpoint()
 
     def useOutput(self, output: float, setpoint: float):
         """
@@ -74,12 +74,12 @@ def getMeasurement(self) -> float:
 
     def enable(self):
         """Enables the PID control. Resets the controller."""
-        self.m_enabled = True
-        self.m_controller.reset()
+        self._enabled = True
+        self._controller.reset()
 
     def disable(self):
         """Disables the PID control. Sets output to zero."""
-        self.m_enabled = False
+        self._enabled = False
         self.useOutput(0, 0)
 
     def isEnabled(self) -> bool:
@@ -88,4 +88,4 @@ def isEnabled(self) -> bool:
 
         :return: Whether the controller is enabled.
         """
-        return self.m_enabled
+        return self._enabled

commands2/trapezoidprofilesubsystem.py

@@ -2,67 +2,77 @@
 # 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 typing import Union
 from .subsystem import Subsystem
 from wpimath.trajectory import TrapezoidProfile
 
+
 class TrapezoidProfileSubsystem(Subsystem):
     """A subsystem that generates and runs trapezoidal motion profiles automatically. The user specifies
     how to use the current state of the motion profile by overriding the `useState` method.
 
     This class is provided by the NewCommands VendorDep
     """
+
     def __init__(
-            self, constraints: TrapezoidProfile.Constraints, 
-            initial_position: float, 
-            period: float = 0.02
-        ):
+        self,
+        constraints: TrapezoidProfile.Constraints,
+        initial_position: float,
+        period: float = 0.02,
+    ):
         """
         Creates a new TrapezoidProfileSubsystem.
 
         :param constraints: The constraints (maximum velocity and acceleration) for the profiles.
         :param initial_position: The initial position of the controlled mechanism when the subsystem is constructed.
         :param period: The period of the main robot loop, in seconds.
         """
-        self.profile = TrapezoidProfile(constraints)
-        self.state = TrapezoidProfile.State(initial_position, 0)
+        self._profile = TrapezoidProfile(constraints)
+        self._state = TrapezoidProfile.State(initial_position, 0)
         self.setGoal(initial_position)
-        self.period = period
-        self.enabled = True
+        self._period = period
+        self._enabled = True
 
     def periodic(self):
         """
         Executes the TrapezoidProfileSubsystem logic during each periodic update.
 
         This method is called synchronously from the subsystem's periodic() method.
         """
-        self.state = self.profile.calculate(self.period, self.goal, self.state)
-        if self.enabled:
-            self.useState(self.state)
+        self._state = self._profile.calculate(self._period, self.goal, self._state)
+        if self._enabled:
+            self.useState(self._state)
 
-    def setGoal(self, goal: TrapezoidProfile.State):
+    def __setGoal(self, goal: TrapezoidProfile.State):
         """
         Sets the goal state for the subsystem.
 
         :param goal: The goal state for the subsystem's motion profile.
         """
         self.goal = goal
 
-    def setGoal(self, goal: float):
+    def setGoal(self, goal: Union[TrapezoidProfile.State, float]):
         """
         Sets the goal state for the subsystem. Goal velocity assumed to be zero.
 
-        :param goal: The goal position for the subsystem's motion profile.
+        :param goal: The goal position for the subsystem's motion profile. The goal
+        can either be a `TrapezoidProfile.State` or `float`. If float is provided,
+        the assumed velocity for the goal will be 0.
         """
-        self.setGoal(TrapezoidProfile.State(goal, 0))
+        newGoal: TrapezoidProfile.State = goal
+        # If we got a float, instantiate the state
+        if isinstance(goal, float):
+            newGoal = TrapezoidProfile.State(goal, 0)
+
+        self.__setGoal(newGoal)
 
     def enable(self):
         """Enable the TrapezoidProfileSubsystem's output."""
-        self.enabled = True
+        self._enabled = True
 
     def disable(self):
         """Disable the TrapezoidProfileSubsystem's output."""
-        self.enabled = False
+        self._enabled = False
 
     def useState(self, state: TrapezoidProfile.State):
         """