Skip to content

Commit

Permalink
Pinned version of experimental libraries and renamed class variables
Browse files Browse the repository at this point in the history
  • Loading branch information
NewtonCrosby committed Dec 30, 2023
1 parent 5517b58 commit 9739b5f
Show file tree
Hide file tree
Showing 2 changed files with 37 additions and 37 deletions.
70 changes: 35 additions & 35 deletions commands2/ramsetecommand.py
Original file line number Diff line number Diff line change
Expand Up @@ -69,12 +69,12 @@ def __init__(
super().__init__()

self._timer = Timer()
self.trajectory = trajectory
self.pose = pose
self.follower = controller
self.kinematics = kinematics
self.output = output
self.usePID = False
self._trajectory = trajectory
self._pose = pose
self._follower = controller
self._kinematics = kinematics
self._output = output
self._usePID = False
# All the parameter checks pass, inform scheduler. Type ignoring is set explictitly for the linter because this
# implementation declares the tuple explicitly, whereas the general implementations use the unpack operator (*)
# to pass the requirements to the scheduler.
Expand Down Expand Up @@ -119,18 +119,18 @@ def __init__(
self._timer = Timer()

# Store all the requirements that are required
self.trajectory = trajectory
self.pose = pose
self.follower = controller
self.kinematics = kinematics
self.output = output
self.leftController = leftController
self.rightController = rightController
self.wheelspeeds = wheelSpeeds
self.feedforward = feedforward
self._trajectory = trajectory
self._pose = pose
self._follower = controller
self._kinematics = kinematics
self._output = output
self._leftController = leftController
self._rightController = rightController
self._wheelspeeds = wheelSpeeds
self._feedforward = feedforward
self._prevSpeeds = DifferentialDriveWheelSpeeds()
self._prevTime = -1.0
self.usePID = True
self._usePID = True
# All the parameter checks pass, inform scheduler. Type ignoring is set explictitly for the linter because this
# implementation declares the tuple explicitly, whereas the general implementations use the unpack operator (*)
# to pass the requirements to the scheduler.
Expand All @@ -155,57 +155,57 @@ def __init__(

def initialize(self):
self._prevTime = -1
initial_state = self.trajectory.sample(0)
self._prevSpeeds = self.kinematics.toWheelSpeeds(
initial_state = self._trajectory.sample(0)
self._prevSpeeds = self._kinematics.toWheelSpeeds(
ChassisSpeeds(
initial_state.velocity,
0,
initial_state.curvature * initial_state.velocity,
)
)
self._timer.restart()
if self.usePID:
self.leftController.reset()
self.rightController.reset()
if self._usePID:
self._leftController.reset()
self._rightController.reset()

def execute(self):
curTime = self._timer.get()
dt = curTime - self._prevTime

if self._prevTime < 0:
self.output(0.0, 0.0)
self._output(0.0, 0.0)
self._prevTime = curTime
return

targetWheelSpeeds = self.kinematics.toWheelSpeeds(
self.follower.calculate(self.pose(), self.trajectory.sample(curTime))
targetWheelSpeeds = self._kinematics.toWheelSpeeds(
self._follower.calculate(self._pose(), self._trajectory.sample(curTime))
)

leftSpeedSetpoint = targetWheelSpeeds.left
rightSpeedSetpoint = targetWheelSpeeds.right

if self.usePID:
leftFeedforward = self.feedforward.calculate(
if self._usePID:
leftFeedforward = self._feedforward.calculate(
leftSpeedSetpoint, (leftSpeedSetpoint - self._prevSpeeds.left) / dt
)

rightFeedforward = self.feedforward.calculate(
rightFeedforward = self._feedforward.calculate(
rightSpeedSetpoint,
(rightSpeedSetpoint - self._prevSpeeds.right) / dt,
)

leftOutput = leftFeedforward + self.leftController.calculate(
self.wheelspeeds().left, leftSpeedSetpoint
leftOutput = leftFeedforward + self._leftController.calculate(
self._wheelspeeds().left, leftSpeedSetpoint
)

rightOutput = rightFeedforward + self.rightController.calculate(
self.wheelspeeds().right, rightSpeedSetpoint
rightOutput = rightFeedforward + self._rightController.calculate(
self._wheelspeeds().right, rightSpeedSetpoint
)
self.output(leftOutput, rightOutput)
self._output(leftOutput, rightOutput)
else:
leftOutput = leftSpeedSetpoint
rightOutput = rightSpeedSetpoint
self.output(leftOutput, rightOutput)
self._output(leftOutput, rightOutput)

self._prevSpeeds = targetWheelSpeeds
self._prevTime = curTime
Expand All @@ -214,10 +214,10 @@ def end(self, interrupted: bool):
self._timer.stop()

if interrupted:
self.output(0.0, 0.0)
self._output(0.0, 0.0)

def isFinished(self):
return self._timer.hasElapsed(self.trajectory.totalTime())
return self._timer.hasElapsed(self._trajectory.totalTime())

def initSendable(self, builder: SendableBuilder):
super().initSendable(builder)
Expand Down
4 changes: 2 additions & 2 deletions setup.py
Original file line number Diff line number Diff line change
Expand Up @@ -14,8 +14,8 @@
install_requires=[
"wpilib<2025,>=2024.0.0b2",
"typing_extensions>=4.1.0,<5",
"overtake>=0.4.0",
"beartype>=0.16.4",
"overtake~=0.4.0",
"beartype~=0.16.4",
],
license="BSD-3-Clause",
python_requires=">=3.8",
Expand Down

0 comments on commit 9739b5f

Please sign in to comment.