2
2
# Open Source Software; you can modify and/or share it under the terms of
3
3
# the WPILib BSD license file in the root directory of this project.
4
4
5
- from typing import Union
5
+ from typing import Union , cast
6
6
7
- from wpimath .controller import ProfiledPIDController
7
+ from wpimath .controller import (ProfiledPIDController ,
8
+ ProfiledPIDControllerRadians )
8
9
from wpimath .trajectory import TrapezoidProfile
9
10
10
11
from .subsystem import Subsystem
11
12
12
13
13
14
class ProfiledPIDSubsystem (Subsystem ):
14
15
"""
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.
17
19
"""
18
20
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
+ ):
20
26
"""Creates a new PIDSubsystem."""
21
27
super ().__init__ ()
22
28
self ._controller = controller
@@ -31,28 +37,39 @@ def periodic(self):
31
37
self ._controller .getSetpoint (),
32
38
)
33
39
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."""
36
44
return self ._controller
37
45
38
46
def setGoal (self , goal : Union [TrapezoidProfile .State , float ]):
39
47
"""
40
48
Sets the goal state for the subsystem.
41
49
"""
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 ))
44
58
else :
45
- self ._controller .setGoal (TrapezoidProfile .State (goal , 0 ))
59
+ # ProfiledPIDController.setGoals() accepts both State objects and floats
60
+ self ._controller .setGoal (goal )
46
61
47
62
def _useOutput (self , output : float , setpoint : TrapezoidProfile .State ):
48
63
"""
49
- Uses the output from the ProfiledPIDController.
64
+ Uses the output from the ProfiledPIDController or ProfiledPIDControllerRadians
65
+ object.
50
66
"""
51
67
raise NotImplementedError ("Subclasses must implement this method" )
52
68
53
69
def _getMeasurement (self ) -> float :
54
70
"""
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.
56
73
"""
57
74
raise NotImplementedError ("Subclasses must implement this method" )
58
75
0 commit comments