mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
40fb9ff562
Resync with `mostrobotpy` This mostly involves the big "ignore almost everything in the HAL project" and some fixups for the Addressable LED classes. Required two small hand fixes to get it building over here with bazel, and with more compiler warnings on. I also manually zeroed out the `repo_url` field in the toml files to avoid unnecessary churn whenever it goes from a release build to a development build. I already did this with `version` field in there, and will do a follow up PR that updates the copybara script to do it automatically. --------- Co-authored-by: Default email <default@default.com>
81 lines
2.8 KiB
Python
81 lines
2.8 KiB
Python
# validated: 2024-01-24 DS f29a7d2e501b ProfiledPIDCommand.java
|
|
#
|
|
# Copyright (c) FIRST and other WPILib contributors.
|
|
# Open Source Software; you can modify and/or share it under the terms of
|
|
# the WPILib BSD license file in the root directory of this project.
|
|
#
|
|
|
|
from typing import Any, Generic
|
|
|
|
from wpimath import ProfiledPIDController, ProfiledPIDControllerRadians, TrapezoidProfile, TrapezoidProfileRadians
|
|
|
|
from .command import Command
|
|
from .subsystem import Subsystem
|
|
from .typing import (
|
|
FloatOrFloatSupplier,
|
|
FloatSupplier,
|
|
TProfiledPIDController,
|
|
UseOutputFunction,
|
|
)
|
|
|
|
|
|
class ProfiledPIDCommand(Command, Generic[TProfiledPIDController]):
|
|
"""A command that controls an output with a :class:`.ProfiledPIDController`. Runs forever by default -
|
|
to add exit conditions and/or other behavior, subclass this class. The controller calculation and
|
|
output are performed synchronously in the command's execute() method.
|
|
"""
|
|
|
|
_stateCls: Any
|
|
|
|
def __init__(
|
|
self,
|
|
controller: TProfiledPIDController,
|
|
measurementSource: FloatSupplier,
|
|
goalSource: FloatOrFloatSupplier,
|
|
useOutput: UseOutputFunction,
|
|
*requirements: Subsystem,
|
|
):
|
|
"""Creates a new ProfiledPIDCommand, which controls the given output with a ProfiledPIDController. Goal
|
|
velocity is specified.
|
|
|
|
:param controller: the controller that controls the output.
|
|
:param measurementSource: the measurement of the process variable
|
|
:param goalSource: the controller's goal
|
|
:param useOutput: the controller's output
|
|
:param requirements: the subsystems required by this command
|
|
"""
|
|
|
|
super().__init__()
|
|
if isinstance(controller, ProfiledPIDController):
|
|
self._stateCls = TrapezoidProfile.State
|
|
elif isinstance(controller, ProfiledPIDControllerRadians):
|
|
self._stateCls = TrapezoidProfileRadians.State
|
|
else:
|
|
raise ValueError(f"unknown controller type {controller!r}")
|
|
|
|
self._controller: TProfiledPIDController = controller
|
|
self._useOutput = useOutput
|
|
self._measurement = measurementSource
|
|
if isinstance(goalSource, (float, int)):
|
|
self._goal = lambda: float(goalSource)
|
|
else:
|
|
self._goal = goalSource
|
|
|
|
self.addRequirements(*requirements)
|
|
|
|
def initialize(self):
|
|
self._controller.reset(self._measurement())
|
|
|
|
def execute(self):
|
|
self._useOutput.accept(
|
|
self._controller.calculate(self._measurement(), self._goal()),
|
|
self._controller.getSetpoint(),
|
|
)
|
|
|
|
def end(self, interrupted: bool):
|
|
self._useOutput(0.0, self._stateCls())
|
|
|
|
def getController(self):
|
|
"""Gets the controller used by the command"""
|
|
return self._controller
|