Fixed formatting with Black.

Author: NewtonCrosby

commands2/ramsetecommand.py

@@ -4,9 +4,17 @@
 from __future__ import annotations
 
 from typing import Callable, Union
-from wpimath.controller import PIDController, RamseteController, SimpleMotorFeedforwardMeters
+from wpimath.controller import (
+    PIDController,
+    RamseteController,
+    SimpleMotorFeedforwardMeters,
+)
 from wpimath.geometry import Pose2d
-from wpimath.kinematics import ChassisSpeeds, DifferentialDriveKinematics, DifferentialDriveWheelSpeeds
+from wpimath.kinematics import (
+    ChassisSpeeds,
+    DifferentialDriveKinematics,
+    DifferentialDriveWheelSpeeds,
+)
 from wpimath.trajectory import Trajectory
 from wpiutil import SendableBuilder
 from wpilib import Timer
@@ -15,6 +23,7 @@
 from .command import Command
 from .subsystem import Subsystem
 
+
 class RamseteCommand(Command):
     """
     A command that uses a RAMSETE controller (RamseteController) to follow a trajectory
@@ -62,7 +71,7 @@ def __init__(
 
         OPTIONAL PARAMETERS
         When the following optional parameters are provided, when executed, the RamseteCommand will follow
-        provided trajectory. PID control and feedforward are handled internally, and the outputs are scaled 
+        provided trajectory. PID control and feedforward are handled internally, and the outputs are scaled
         from -12 to 12 representing units of volts. If any one of the optional parameters are provided, each
         other optional parameter becomes required.
         :param feedforward      The feedforward to use for the drive
@@ -90,11 +99,11 @@ def __init__(
         # requirements become required.
         if feedforward or leftController or rightController or wheelSpeeds is not None:
             if feedforward or leftController or rightController or wheelSpeeds is None:
-                raise RuntimeError(f'Failed to instantiate RamseteCommand, not all optional arguments were provided.\n \
-                                    Feedforward - {feedforward}, LeftController - {leftController}, RightController - {rightController}, WheelSpeeds - {wheelSpeeds} '
+                raise RuntimeError(
+                    f"Failed to instantiate RamseteCommand, not all optional arguments were provided.\n \
+                                    Feedforward - {feedforward}, LeftController - {leftController}, RightController - {rightController}, WheelSpeeds - {wheelSpeeds} "
                 )
-            
-        
+
             self.leftController: PIDController = leftController
             self.rightController: PIDController = rightController
             self.wheelspeeds: Callable[[], DifferentialDriveWheelSpeeds] = wheelSpeeds
@@ -109,7 +118,11 @@ def initialize(self):
         self.prevTime = -1
         initial_state = self.trajectory.sample(0)
         initial_speeds = self.kinematics.toWheelSpeeds(
-            ChassisSpeeds( initial_state.velocity, 0, initial_state.curvature * initial_state.velocity )
+            ChassisSpeeds(
+                initial_state.velocity,
+                0,
+                initial_state.curvature * initial_state.velocity,
+            )
         )
         self.prevSpeeds = initial_speeds
         self.timer.restart()
@@ -135,29 +148,20 @@ def execute(self):
 
         if self.usePID:
             left_feedforward = self.feedforward.calculate(
-                left_speed_setpoint,
-                (left_speed_setpoint - self.prevSpeeds.left) / dt
+                left_speed_setpoint, (left_speed_setpoint - self.prevSpeeds.left) / dt
             )
 
             right_feedforward = self.feedforward.calculate(
                 right_speed_setpoint,
-                (right_speed_setpoint - self.prevSpeeds.right) / dt
+                (right_speed_setpoint - self.prevSpeeds.right) / dt,
             )
 
-            left_output = (
-                left_feedforward
-                + self.leftController.calculate(
-                    self.wheelspeeds().left,
-                    left_speed_setpoint
-                )
+            left_output = left_feedforward + self.leftController.calculate(
+                self.wheelspeeds().left, left_speed_setpoint
             )
 
-            right_output = (
-                right_feedforward
-                + self.rightController.calculate(
-                    self.wheelspeeds().right,
-                    right_speed_setpoint
-                )
+            right_output = right_feedforward + self.rightController.calculate(
+                self.wheelspeeds().right, right_speed_setpoint
             )
         else:
             left_output = left_speed_setpoint

tests/test_ramsetecommand.py

@@ -48,27 +48,31 @@ def __init__(self):
         self.kMaxMetersPerSecond = 3.0
         self.kMaxAccelerationMetersPerSecondSquared = 1.0
         self.kEncoderCPR = 1024
-        self.kEncoderDistancePerPulse = (self.kWheelDiameterMeters * math.pi) / self.kEncoderCPR
+        self.kEncoderDistancePerPulse = (
+            self.kWheelDiameterMeters * math.pi
+        ) / self.kEncoderCPR
 
         self.command_kinematics: kinematics.DifferentialDriveKinematics = (
             kinematics.DifferentialDriveKinematics(self.kTrackWidth)
         )
 
         self.command_voltage_constraint: constraints.DifferentialDriveVoltageConstraint = constraints.DifferentialDriveVoltageConstraint(
             controller.SimpleMotorFeedforwardMeters(
-                self.ksVolts, 
+                self.ksVolts,
                 self.kvVoltSecondsPerMeter,
-                self.kaVoltSecondsSquaredPerMeter
-            ), 
-            self.command_kinematics, 
-            10
+                self.kaVoltSecondsSquaredPerMeter,
+            ),
+            self.command_kinematics,
+            10,
         )
 
-        self.command_odometry: kinematics.DifferentialDriveOdometry = kinematics.DifferentialDriveOdometry(
-            self.angle,
-            self.leftDistance,
-            self.rightDistance,
-            geometry.Pose2d(0, 0, geometry.Rotation2d(0))
+        self.command_odometry: kinematics.DifferentialDriveOdometry = (
+            kinematics.DifferentialDriveOdometry(
+                self.angle,
+                self.leftDistance,
+                self.rightDistance,
+                geometry.Pose2d(0, 0, geometry.Rotation2d(0)),
+            )
         )
 
     def setWheelSpeeds(self, leftspeed: float, rightspeed: float) -> None:
@@ -84,7 +88,7 @@ def getCurrentWheelDistances(self) -> kinematics.DifferentialDriveWheelPositions
 
     def getRobotPose(self) -> geometry.Pose2d:
         positions = self.getCurrentWheelDistances()
-        self.command_odometry.update(self.angle, positions.left,positions.right)
+        self.command_odometry.update(self.angle, positions.left, positions.right)
         return self.command_odometry.getPose()
 
 
@@ -111,12 +115,14 @@ def test_ramseteCommand(
         end_state = new_trajectory.sample(new_trajectory.totalTime())
 
         command = commands2.RamseteCommand(
-            new_trajectory, 
+            new_trajectory,
             fixture_data.getRobotPose,
-            controller.RamseteController(fixture_data.kRamseteB, fixture_data.kRamseteZeta),
+            controller.RamseteController(
+                fixture_data.kRamseteB, fixture_data.kRamseteZeta
+            ),
             fixture_data.command_kinematics,
             fixture_data.setWheelSpeeds,
-            subsystem
+            subsystem,
         )
 
         fixture_data.timer.restart()
@@ -139,4 +145,4 @@ def test_ramseteCommand(
         )
         assert end_state.pose.Y() == pytest.approx(
             fixture_data.getRobotPose().Y(), fixture_data.kyTolerance
-        )
+        )