Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Updated TrapezoidProfileSubsystem to include dimensionless and radians #41

Open
wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Updated TrapezoidProfileSubsystem to include dimensionless and radians #41

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
65 changes: 53 additions & 12 deletions commands2/trapezoidprofilesubsystem.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,10 +3,10 @@
# the WPILib BSD license file in the root directory of this project.
from __future__ import annotations

from typing import Union
from typing import Union, overload

from .subsystem import Subsystem
from wpimath.trajectory import TrapezoidProfile
from wpimath.trajectory import TrapezoidProfile, TrapezoidProfileRadians


class TrapezoidProfileSubsystem(Subsystem):
Expand All @@ -15,9 +15,41 @@ class TrapezoidProfileSubsystem(Subsystem):
how to use the current state of the motion profile by overriding the `useState` method.
"""

@overload
def __init__(
self,
constraints: TrapezoidProfile.Constraints,
constraints: Union[
TrapezoidProfile.Constraints, TrapezoidProfileRadians.Constraints
],
):
"""
Creates a new TrapezoidProfileSubsystem.

:param constraints: The constraints (maximum velocity and acceleration) for the profiles.
"""
...

@overload
def __init__(
self,
constraints: Union[
TrapezoidProfile.Constraints, TrapezoidProfileRadians.Constraints
],
initial_position: float = 0.0,
):
"""
Creates a new TrapezoidProfileSubsystem.

:param constraints: The constraints (maximum velocity and acceleration) for the profiles.
:param initial_position: The initial position of the controlled mechanism when the subsystem is constructed.
"""
...

def __init__(
self,
constraints: Union[
TrapezoidProfile.Constraints, TrapezoidProfileRadians.Constraints
],
initial_position: float = 0.0,
period: float = 0.02,
):
Expand All @@ -28,9 +60,18 @@ def __init__(
:param initial_position: The initial position of the controlled mechanism when the subsystem is constructed.
:param period: The period of the main robot loop, in seconds.
"""
self._profile = TrapezoidProfile(constraints)
self._state = TrapezoidProfile.State(initial_position, 0)
self.setGoal(initial_position)
if isinstance(constraints, TrapezoidProfile.Constraints):
self._state: TrapezoidProfile.State = TrapezoidProfile.State(
initial_position
)
self._profile: TrapezoidProfile = TrapezoidProfile(constraints)
else: # TrapezoidProfileradians
self._state: TrapezoidProfileRadians.State = TrapezoidProfileRadians.State( # type: ignore
initial_position
)
self._profile: TrapezoidProfileRadians = TrapezoidProfileRadians(constraints) # type: ignore

self.setGoal(self._state)
self._period = period
self._enabled = True

Expand All @@ -44,18 +85,16 @@ def periodic(self):
if self._enabled:
self.useState(self._state)

def setGoal(self, goal: Union[TrapezoidProfile.State, float]):
def setGoal(
self, goal: Union[TrapezoidProfile.State, TrapezoidProfileRadians.State]
):
"""
Sets the goal state for the subsystem. Goal velocity assumed to be zero.

:param goal: The goal position for the subsystem's motion profile. The goal
can either be a `TrapezoidProfile.State` or `float`. If float is provided,
the assumed velocity for the goal will be 0.
"""
# If we got a float, instantiate the state
if isinstance(goal, (float, int)):
goal = TrapezoidProfile.State(goal, 0)

self._goal = goal

def enable(self):
Expand All @@ -66,7 +105,9 @@ def disable(self):
"""Disable the TrapezoidProfileSubsystem's output."""
self._enabled = False

def useState(self, state: TrapezoidProfile.State):
def useState(
self, state: Union[TrapezoidProfile.State, TrapezoidProfileRadians.State]
):
"""
Users should override this to consume the current state of the motion profile.

Expand Down