8
8
9
9
10
10
class ProfiledPIDSubsystem (Subsystem ):
11
+ """
12
+ A subsystem that uses a ProfiledPIDController to control an output. The controller
13
+ is run synchronously from the subsystem's periodic() method.
14
+ """
15
+
11
16
def __init__ (self , controller : ProfiledPIDController , initial_position : float = 0 ):
17
+ """Creates a new PIDSubsystem."""
12
18
super ().__init__ ()
13
- self ._m_controller = controller
14
- self ._m_enabled = False
19
+ self ._controller = controller
20
+ self ._enabled = False
15
21
self .setGoal (initial_position )
16
22
17
23
def periodic (self ):
18
24
"""Updates the output of the controller."""
19
- if self ._m_enabled :
20
- self ._useOutput (self ._m_controller .calculate (self ._getMeasurement ()),
21
- self ._m_controller .getSetpoint ())
25
+ if self ._enabled :
26
+ self ._useOutput (
27
+ self ._controller .calculate (self ._getMeasurement ()),
28
+ self ._controller .getSetpoint (),
29
+ )
22
30
23
- def get_controller (self ) -> ProfiledPIDController :
31
+ def getController (self ) -> ProfiledPIDController :
24
32
"""Returns the ProfiledPIDController."""
25
- return self ._m_controller
33
+ return self ._controller
26
34
27
35
def setGoal (self , goal : TrapezoidProfile .State | float ):
28
36
"""
29
37
Sets the goal state for the subsystem.
30
38
"""
31
- if isinstance (goal , TrapezoidProfile ):
32
- self ._m_controller .setGoal (goal )
39
+ if isinstance (goal , TrapezoidProfile . State ):
40
+ self ._controller .setGoal (goal )
33
41
else :
34
- self ._m_controller .setGoal (TrapezoidProfile .State (goal , 0 ))
42
+ self ._controller .setGoal (TrapezoidProfile .State (goal , 0 ))
35
43
36
44
def _useOutput (self , output : float , setpoint : TrapezoidProfile .State ):
37
45
"""
@@ -47,16 +55,16 @@ def _getMeasurement(self) -> float:
47
55
48
56
def enable (self ):
49
57
"""Enables the PID control. Resets the controller."""
50
- self ._m_enabled = True
51
- self ._m_controller .reset (self ._getMeasurement ())
58
+ self ._enabled = True
59
+ self ._controller .reset (self ._getMeasurement ())
52
60
53
61
def disable (self ):
54
62
"""Disables the PID control. Sets output to zero."""
55
- self ._m_enabled = False
63
+ self ._enabled = False
56
64
self ._useOutput (0 , TrapezoidProfile .State ())
57
65
58
66
def isEnabled (self ) -> bool :
59
67
"""
60
68
Returns whether the controller is enabled.
61
69
"""
62
- return self ._m_enabled
70
+ return self ._enabled
0 commit comments