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)