def __init__(self, *args, **kwargs):
"""
Arguments can be structured as follows:
- deviceNumber
- talonSrx
:param deviceNumber:
CAN Device Id of Pigeon [0,62]
:param talonSrx:
Object for the TalonSRX connected via ribbon cable.
"""
super().__init__()
deviceNumber_arg = ("deviceNumber", [int])
talonSrx_arg = ("talonSrx", [TalonSRX])
templates = [[deviceNumber_arg], [talonSrx_arg]]
index, results = match_arglist('PigeonIMU.__init__', args, kwargs,
templates)
self.generalStatus = [0] * 10
self.fusionStatus = [0] * 10
if index == 0:
self.deviceNumber = results['deviceNumber']
self._create1(self.deviceNumber)
elif index == 1:
self.deviceNumber = results['talonSrx'].getDeviceID()
self._create2(self.deviceNumber)
hal.report(64, self.deviceNumber + 1)
hal.report(hal.UsageReporting.kResourceType_PigeonIMU,
self.deviceNumber + 1)
def __init__(self, *args, **kwargs):
"""
Arguments can be structured as follows:
- deviceNumber
- talonSrx
:param deviceNumber:
CAN Device Id of Pigeon [0,62]
:param talonSrx:
Object for the TalonSRX connected via ribbon cable.
.. note:: To use this in simulation, you can initialize the device
gyro support via one of the following in your physics.py::
physics_controller.add_device_gyro_channel('pigeon_device_X')
physics_controller.add_device_gyro_channel('pigeon_srx_X')
"""
super().__init__()
deviceNumber_arg = ("deviceNumber", [int])
talonSrx_arg = ("talonSrx", [TalonSRX])
templates = [[deviceNumber_arg], [talonSrx_arg]]
index, results = match_arglist("PigeonIMU.__init__", args, kwargs, templates)
self.generalStatus = [0] * 10
self.fusionStatus = [0] * 10
if index == 0:
self.deviceNumber = results["deviceNumber"]
self._create1(self.deviceNumber)
elif index == 1:
self.deviceNumber = results["talonSrx"].getDeviceID()
self._create2(self.deviceNumber)
def putData(cls, *args, **kwargs):
"""
Maps the specified key (name of the :class:`.Sendable` if not provided)
to the specified value in this table.
The value can be retrieved by calling the get method with a key that
is equal to the original key.
Two argument formats are supported:
- key, data
- value
:param key: the key (cannot be None)
:type key: str
:param data: the value
:type data: :class:`.Sendable`
:param value: the value
:type value: :class:`.Sendable`
"""
with cls.mutex:
key_arg = ("key", [str])
data_arg = ("data", [HasAttribute("initSendable")])
value_arg = ("value", [HasAttribute("initSendable")])
templates = [[key_arg, data_arg],
[value_arg],]
index, results = match_arglist('SmartDashboard.putData',
args, kwargs, templates)
if index == 0:
key = results['key']
data = results["data"]
elif index == 1:
data = results["value"]
key = data.getName()
else:
raise ValueError("only (key, data) or (value) accepted")
sddata = cls.tablesToData.get(key, None)
if sddata is None or sddata.sendable != data:
if sddata is not None:
sddata.builder.stopListeners()
sddata = Data(data)
cls.tablesToData[key] = sddata
dataTable = cls.getTable().getSubTable(key)
sddata.builder.setTable(dataTable)
data.initSendable(sddata.builder)
sddata.builder.updateTable()
sddata.builder.startListeners()
dataTable.getEntry('.name').setString(key)
if data is None:
raise KeyError("SmartDashboard data does not exist: '%s'" % key)
return data.sendable
def __init__(self, Kp, Ki, Kd, *args, **kwargs):
f_arg = ("Kf", [float, int])
source_arg = ("source",
[HasAttribute("pidGet"),
HasAttribute("__call__")])
output_arg = ("output",
[HasAttribute("pidWrite"),
HasAttribute("__call__")])
period_arg = ("period", [float, int])
templates = [[f_arg, source_arg, output_arg, period_arg],
[source_arg, output_arg, period_arg],
[source_arg, output_arg], [f_arg, source_arg, output_arg]]
_, results = match_arglist('PIDController.__init__', args, kwargs,
templates)
self.P = Kp # factor for "proportional" control
self.I = Ki # factor for "integral" control
self.D = Kd # factor for "derivative" control
self.F = results.pop("Kf", 0.0) # factor for feedforward term
self.pidOutput = results.pop("output")
self.origSource = results.pop("source")
self.period = results.pop("period", self.kDefaultPeriod)
self.filter = LinearDigitalFilter.movingAverage(self.origSource, 1)
self.pidInput = self.filter
self.pidInput = PIDSource.from_obj_or_callable(self.pidInput)
if hasattr(self.pidOutput, 'pidWrite'):
self.pidOutput = self.pidOutput.pidWrite
self.maximumOutput = 1.0 # |maximum output|
self.minimumOutput = -1.0 # |minimum output|
self.maximumInput = 0.0 # maximum input - limit setpoint to this
self.minimumInput = 0.0 # minimum input - limit setpoint to this
self.continuous = False # do the endpoints wrap around? eg. Absolute encoder
self.enabled = False # is the pid controller enabled
self.prevError = 0.0 # the prior error (used to compute velocity)
self.totalError = 0.0 #the sum of the errors for use in the integral calc
self.buf = deque(maxlen=1)
self.setpoint = 0.0
self.prevSetpoint = 0.0
self.error = 0.0
self.result = 0.0
self.mutex = WithStub()
self.pidWriteMutex = WithStub()
self.setpointTimer = Timer()
self.setpointTimer.start()
def set(self, *args, **kwargs):
"""
See :meth:`.BaseMotorController.set`
Can be called three ways:
- speed
- mode, value
- mode, demand0, demand1
largely a wrapper around :meth:`.BaseMotorController.set`
:param value:
:type value: float
:param mode: ControlMode.PercentOutput if not provided
:type mode: ControlMode
:param speed:
:type speed: float
:param demand0:
:type demand0: float
:param demand1:
:type demand1: float
"""
speed_arg = ("speed", [float, int])
value_arg = ("value", [float, int])
mode_arg = ("mode", [ControlMode])
demand0_arg = ("demand0", [float, int])
demand1_arg = ("demand1", [float, int])
templates = [[speed_arg], [mode_arg, value_arg],
[mode_arg, demand0_arg, demand1_arg]]
index, results = match_arglist('WPI_VictorSPX.__init__', args, kwargs,
templates)
if index == 2:
super().set(results['mode'], results['demand0'],
results['demand1'])
else:
if index == 0:
self.speed = value = results['speed']
mode = ControlMode.PercentOutput
elif index == 1:
value = results['value']
mode = results['mode']
super().set(mode, value)
self.feed()
def __init__(self, output: float, encoder, P=1, I=0.0, D=0.0):
k4X = 2
self.Kp = P
self.Ki = I
self.Kd = D
self.encoder = wpilib.encoder(0, 6, True, k4X)
self.set(output)
self.output = 0
self.sample_time = 0.00
self.current_time = time.time()
self.last_time = self.current_time
self.results = self.period
self.integral = 0
self.previous_error = 0
self.mutex = threading.RLock()
self.rcw = 0
f_arg = ("Kf", [0.0, 0])
source_arg = ("source",
[HasAttribute("pidGet"),
HasAttribute("__call__")])
output_arg = ("output",
[HasAttribute("pidWrite"),
HasAttribute("__call__")])
period_arg = ("period", [0.0, 0])
templates = [
[f_arg, source_arg, output_arg, period_arg],
[source_arg, output_arg, period_arg],
[source_arg, output_arg],
[f_arg, source_arg, output_arg],
]
_, results = match_arglist("PIDController.__init__", args, kwargs,
templates)
Kf = results.pop("Kf", 0.0) # factor for feedforward term
output = results.pop("output")
source = results.pop("source")
super().__init__(Kp, Ki, Kd, Kf, source, output)
self.period = results.pop("period", self.kDefaultPeriod)
self.controlLoop = Notifier(self._calculate)
self.controlLoop.startPeriodic(self.period)
def __init__(self, Kp, Ki, Kd, *args, **kwargs, output):
"""Allocate a PID object with the given constants for P, I, D, and F
Arguments can be structured as follows:
- Kp, Ki, Kd, Kf, source, output, period
- Kp, Ki, Kd, source, output, period
- Kp, Ki, Kd, source, output
- Kp, Ki, Kd, Kf, source, output
:param Kp: the proportional coefficient
:param Ki: the integral coefficient
:param Kd: the derivative coefficient
:param Kf: the feed forward term
:param source: Called to get values
:param output: Receives the output percentage
:param period: the loop time for doing calculations. This particularly
effects calculations of the integral and differential terms.
The default is 0.05 (50ms).
"""
f_arg = ("Kf", [0.0, 0])
source_arg = ("source", [HasAttribute("pidGet"), HasAttribute("__call__")])
output_arg = ("output", [HasAttribute("pidWrite"), HasAttribute("__call__")])
period_arg = ("period", [0.0, 0])
templates = [
[f_arg, source_arg, output_arg, period_arg],
[source_arg, output_arg, period_arg],
[source_arg, output_arg],
[f_arg, source_arg, output_arg],
]
_, results = match_arglist("PIDController.__init__", args, kwargs, templates)
Kf = results.pop("Kf", 0.0) # factor for feedforward term
output = results.pop("output")
source = results.pop("source")
super().__init__(Kp, Ki, Kd, Kf, source, output)
self.period = results.pop("period", self.kDefaultPeriod)
self.controlLoop = Notifier(self._calculate)
self.controlLoop.startPeriodic(self.period)
def __init__(self, *args, **kwargs):
self.encoder = wpilib.encoder(0, 6, True, k4X)
"""Encoder constructor. Construct a Encoder given a and b channels
and optionally an index channel.
The encoder will start counting immediately.
The a, b, and optional index channel arguments may be either channel
numbers or :class:`.DigitalSource` sources. There may also be a boolean
reverseDirection, and an encodingType according to the following
list.
- aSource, bSource
- aSource, bSource, reverseDirection
- aSource, bSource, reverseDirection, encodingType
- aSource, bSource, indexSource, reverseDirection
- aSource, bSource, indexSource
- aChannel, bChannel
- aChannel, bChannel, reverseDirection
- aChannel, bChannel, reverseDirection, encodingType
- aChannel, bChannel, indexChannel, reverseDirection
- aChannel, bChannel, indexChannel
For positional arguments, if the passed object has a
`getPortHandleForRouting` function, it is assumed to be a DigitalSource.
Alternatively, sources and/or channels may be passed as keyword
arguments. The behavior of specifying both a source and a number
for the same channel is undefined, as is passing both a positional
and a keyword argument for the same channel.
In addition, keyword parameters may be provided for reverseDirection
and inputType.
:param aSource: The source that should be used for the a channel.
:type aSource: :class:`.DigitalSource`
:param bSource: The source that should be used for the b channel.
:type bSource: :class:`.DigitalSource`
:param indexSource: The source that should be used for the index
channel.
:type indexSource: :class:`.DigitalSource`
:param aChannel: The digital input index that should be used for
the a channel.
:type aChannel: int
:param bChannel: The digital input index that should be used for
the b channel.
:type bChannel: int
:param indexChannel: The digital input index that should be used
for the index channel.
:type indexChannel: int
:param reverseDirection:
Represents the orientation of the encoder and inverts the
output values if necessary so forward represents positive
values. Defaults to False if unspecified.
:type reverseDirection: bool
:param encodingType:
Either k1X, k2X, or k4X to indicate 1X, 2X or 4X decoding. If
4X is selected, then an encoder FPGA object is used and the
returned counts will be 4x the encoder spec'd value since all
rising and falling edges are counted. If 1X or 2X are selected
then a counter object will be used and the returned value will
either exactly match the spec'd count or be double (2x) the
spec'd count. Defaults to k4X if unspecified.
:type encodingType: :class:`Encoder.EncodingType`
"""
super().__init__()
a_source_arg = ("aSource", HasAttribute("getPortHandleForRouting"))
b_source_arg = ("bSource", HasAttribute("getPortHandleForRouting"))
index_source_arg = ("indexSource",
HasAttribute("getPortHandleForRouting"))
a_channel_arg = ("aChannel", int)
b_channel_arg = ("bChannel", int)
index_channel_arg = ("indexChannel", int)
# fmt: off
argument_templates = [
[a_source_arg, b_source_arg],
[a_source_arg, b_source_arg, ("reverseDirection", bool)],
[
a_source_arg, b_source_arg, ("reverseDirection", bool),
("encodingType", int)
], [a_source_arg, b_source_arg, index_source_arg],
[
a_source_arg, b_source_arg, index_source_arg,
("reverseDirection", bool)
], [a_channel_arg, b_channel_arg],
[a_channel_arg, b_channel_arg, ("reverseDirection", bool)],
[
a_channel_arg, b_channel_arg, ("reverseDirection", bool),
("encodingType", int)
], [a_channel_arg, b_channel_arg, index_channel_arg],
[
a_channel_arg, b_channel_arg, index_channel_arg,
("reverseDirection", bool)
]
]
# fmt: on
_, results = match_arglist("Encoder.__init__", args, kwargs,
argument_templates)
# keyword arguments
aSource = results.pop("aSource", None)
bSource = results.pop("bSource", None)
indexSource = results.pop("indexSource", None)
aChannel = results.pop("aChannel", None)
bChannel = results.pop("bChannel", None)
indexChannel = results.pop("indexChannel", None)
reverseDirection = results.pop("reverseDirection", False)
encodingType = results.pop("encodingType", self.EncodingType.k4X)
# convert channels into sources
self.allocatedA = False
self.allocatedB = False
self.allocatedIndex = False
if aSource is None:
if aChannel is None:
raise ValueError("didn't specify A channel")
aSource = DigitalInput(aChannel)
self.allocatedA = True
self.addChild(aSource)
if bSource is None:
if bChannel is None:
raise ValueError("didn't specify B channel")
bSource = DigitalInput(bChannel)
self.allocatedB = True
self.addChild(bSource)
if indexSource is None and indexChannel is not None:
indexSource = DigitalInput(indexChannel)
self.allocatedIndex = True
self.addChild(indexSource)
# save to instance variables
self.aSource = aSource
self.bSource = bSource
self.indexSource = indexSource
self.encodingType = encodingType
self.pidSource = self.PIDSourceType.kDisplacement
self._encoder = None
self.counter = None
self.speedEntry = None
self.distanceEntry = None
self.distancePerTickEntry = None
self._encoder = hal.initializeEncoder(
aSource.getPortHandleForRouting(),
aSource.getAnalogTriggerTypeForRouting(),
bSource.getPortHandleForRouting(),
bSource.getAnalogTriggerTypeForRouting(),
reverseDirection,
encodingType,
)
self.__finalizer = weakref.finalize(self, _freeEncoder, self._encoder)
# Need this to free on unit test wpilib reset
Resource._add_global_resource(self)
if self.indexSource is not None:
self.setIndexSource(self.indexSource)
self.index = self.getFPGAIndex()
hal.report(hal.UsageReporting.kResourceType_Encoder, self.index,
encodingType)
self.setName("Encoder", self.index)
def __init__(self, Kp, Ki, Kd, *args, **kwargs):
"""Allocate a PID object with the given constants for P, I, D, and F
Arguments can be structured as follows:
- Kp, Ki, Kd, Kf, source, output, period
- Kp, Ki, Kd, source, output, period
- Kp, Ki, Kd, source, output
- Kp, Ki, Kd, Kf, source, output
:param Kp: the proportional coefficient
:type Kp: float or int
:param Ki: the integral coefficient
:type Ki: float or int
:param Kd: the derivative coefficient
:type Kd: float or int
:param Kf: the feed forward term
:type Kf: float or int
:param source: Called to get values
:type source: A function, or an object that implements :class:`.PIDSource`
:param output: Receives the output percentage
:type output: A function, or an object that implements :class:`.PIDOutput`
:param period: the loop time for doing calculations. This particularly
effects calculations of the integral and differential terms.
The default is 50ms.
:type period: float or int
"""
super().__init__(False)
f_arg = ("Kf", [float, int])
source_arg = ("source",
[HasAttribute("pidGet"),
HasAttribute("__call__")])
output_arg = ("output",
[HasAttribute("pidWrite"),
HasAttribute("__call__")])
period_arg = ("period", [float, int])
templates = [[f_arg, source_arg, output_arg, period_arg],
[source_arg, output_arg, period_arg],
[source_arg, output_arg], [f_arg, source_arg, output_arg]]
_, results = match_arglist('PIDController.__init__', args, kwargs,
templates)
self.P = Kp # factor for "proportional" control
self.I = Ki # factor for "integral" control
self.D = Kd # factor for "derivative" control
self.F = results.pop("Kf", 0.0) # factor for feedforward term
self.pidOutput = results.pop("output")
self.origSource = results.pop("source")
self.period = results.pop("period", self.kDefaultPeriod)
self.filter = LinearDigitalFilter.movingAverage(self.origSource, 1)
self.pidInput = self.filter
self.pidInput = PIDSource.from_obj_or_callable(self.pidInput)
if hasattr(self.pidOutput, 'pidWrite'):
self.pidOutput = self.pidOutput.pidWrite
self.maximumOutput = 1.0 # |maximum output|
self.minimumOutput = -1.0 # |minimum output|
self.maximumInput = 0.0 # maximum input - limit setpoint to this
self.minimumInput = 0.0 # minimum input - limit setpoint to this
self.inputRange = 0.0 # input range - difference between maximum and minimum
self.continuous = False # do the endpoints wrap around? eg. Absolute encoder
self.enabled = False # is the pid controller enabled
self.prevError = 0.0 # the prior error (used to compute velocity)
self.totalError = 0.0 #the sum of the errors for use in the integral calc
self.setpoint = 0.0
self.prevSetpoint = 0.0
self.error = 0.0
self.result = 0.0
self.mutex = threading.RLock()
self.pidWriteMutex = threading.RLock()
self.pid_task = Notifier(self._calculate)
self.pid_task.startPeriodic(self.period)
self.setpointTimer = Timer()
self.setpointTimer.start()
# Need this to free on unit test wpilib reset
Resource._add_global_resource(self)
PIDController.instances += 1
hal.report(hal.UsageReporting.kResourceType_PIDController,
PIDController.instances)
self.setName("PIDController", PIDController.instances)
