Skip to content

Commit 2921008

Browse files
committed
ProfiledPIDSubsystem: Added support for ProfiledPIDControllerRadians controller objects.
1 parent f193901 commit 2921008

File tree

1 file changed

+29
-12
lines changed

1 file changed

+29
-12
lines changed

commands2/profiledpidsubsystem.py

+29-12
Original file line numberDiff line numberDiff line change
@@ -2,21 +2,27 @@
22
# Open Source Software; you can modify and/or share it under the terms of
33
# the WPILib BSD license file in the root directory of this project.
44

5-
from typing import Union
5+
from typing import Union, cast
66

7-
from wpimath.controller import ProfiledPIDController
7+
from wpimath.controller import (ProfiledPIDController,
8+
ProfiledPIDControllerRadians)
89
from wpimath.trajectory import TrapezoidProfile
910

1011
from .subsystem import Subsystem
1112

1213

1314
class ProfiledPIDSubsystem(Subsystem):
1415
"""
15-
A subsystem that uses a ProfiledPIDController to control an output. The controller
16-
is run synchronously from the subsystem's periodic() method.
16+
A subsystem that uses a ProfiledPIDController or ProfiledPIDControllerRadians to
17+
control an output. The controller is run synchronously from the subsystem's
18+
periodic() method.
1719
"""
1820

19-
def __init__(self, controller: ProfiledPIDController, initial_position: float = 0):
21+
def __init__(
22+
self,
23+
controller: Union[ProfiledPIDController, ProfiledPIDControllerRadians],
24+
initial_position: float = 0,
25+
):
2026
"""Creates a new PIDSubsystem."""
2127
super().__init__()
2228
self._controller = controller
@@ -31,28 +37,39 @@ def periodic(self):
3137
self._controller.getSetpoint(),
3238
)
3339

34-
def getController(self) -> ProfiledPIDController:
35-
"""Returns the ProfiledPIDController."""
40+
def getController(
41+
self,
42+
) -> Union[ProfiledPIDController, ProfiledPIDControllerRadians]:
43+
"""Returns the ProfiledPIDController or ProfiledPIDControllerRadians object."""
3644
return self._controller
3745

3846
def setGoal(self, goal: Union[TrapezoidProfile.State, float]):
3947
"""
4048
Sets the goal state for the subsystem.
4149
"""
42-
if isinstance(goal, TrapezoidProfile.State):
43-
self._controller.setGoal(goal)
50+
if isinstance(self._controller, ProfiledPIDControllerRadians):
51+
if isinstance(goal, TrapezoidProfile.State):
52+
# ProfiledPIDControllerRadians.setGoals() does not accept a State
53+
# object, so just use the state's position (presumably in radians)
54+
self._controller.setGoal(goal.position)
55+
else:
56+
# cast goal to a float so mypy does not complain
57+
self._controller.setGoal(cast(float, goal))
4458
else:
45-
self._controller.setGoal(TrapezoidProfile.State(goal, 0))
59+
# ProfiledPIDController.setGoals() accepts both State objects and floats
60+
self._controller.setGoal(goal)
4661

4762
def _useOutput(self, output: float, setpoint: TrapezoidProfile.State):
4863
"""
49-
Uses the output from the ProfiledPIDController.
64+
Uses the output from the ProfiledPIDController or ProfiledPIDControllerRadians
65+
object.
5066
"""
5167
raise NotImplementedError("Subclasses must implement this method")
5268

5369
def _getMeasurement(self) -> float:
5470
"""
55-
Returns the measurement of the process variable used by the ProfiledPIDController.
71+
Returns the measurement of the process variable used by the
72+
ProfiledPIDController or ProfiledPIDControllerRadians object.
5673
"""
5774
raise NotImplementedError("Subclasses must implement this method")
5875

0 commit comments

Comments
 (0)