3
3
# the WPILib BSD license file in the root directory of this project.
4
4
from __future__ import annotations
5
5
6
- from typing import Any , Callable
6
+ from typing import Any , Callable , Union
7
7
8
8
from .command import Command
9
9
from .subsystem import Subsystem
@@ -16,15 +16,13 @@ class PIDCommand(Command):
16
16
A command that controls an output with a PIDController. Runs forever by default - to add
17
17
exit conditions and/or other behavior, subclass this class. The controller calculation and output
18
18
are performed synchronously in the command's execute() method.
19
-
20
- This class is provided by the NewCommands VendorDep
21
19
"""
22
20
23
21
def __init__ (
24
22
self ,
25
23
controller : PIDController ,
26
24
measurementSource : Callable [[], float ],
27
- setpoint : float ,
25
+ setpoint : Union [ Callable [[], float ], float ] ,
28
26
useOutput : Callable [[float ], Any ],
29
27
* requirements : Subsystem ,
30
28
):
@@ -38,34 +36,32 @@ def __init__(
38
36
:param requirements: the subsystems required by this command
39
37
"""
40
38
super ().__init__ ()
41
- if controller is None :
42
- raise ValueError ("controller must not be None" )
43
- if measurementSource is None :
44
- raise ValueError ("measurementSource must not be None" )
45
- if setpoint is None :
46
- raise ValueError ("setpointSource must not be None" )
47
- if useOutput is None :
48
- raise ValueError ("useOutput must not be None" )
49
39
50
- self .controller = controller
51
- self .useOutput = useOutput
52
- self .measurement = measurementSource
53
- self .setpoint = setpoint
54
- self .requirements .addAll (set (requirements ))
40
+ self ._controller = controller
41
+ self ._useOutput = useOutput
42
+ self ._measurement = measurementSource
43
+
44
+ if isinstance (setpoint , (float , int )):
45
+ setpoint = lambda : setpoint
46
+
47
+ self ._setpoint = setpoint
48
+ self .addRequirements (* requirements )
55
49
56
50
def initialize (self ):
57
- self .controller .reset ()
51
+ self ._controller .reset ()
58
52
59
53
def execute (self ):
60
- self .useOutput (self .controller .calculate (self .measurement (), self .setpoint ))
54
+ self ._useOutput (
55
+ self ._controller .calculate (self ._measurement (), self ._setpoint ())
56
+ )
61
57
62
58
def end (self , interrupted ):
63
- self .useOutput (0 )
59
+ self ._useOutput (0 )
64
60
65
61
def getController (self ):
66
62
"""
67
63
Returns the PIDController used by the command.
68
64
69
65
:return: The PIDController
70
66
"""
71
- return self .controller
67
+ return self ._controller
0 commit comments