77from commands2 import cmd
88from wpimath .controller import PIDController , ProfiledPIDControllerRadians
99from wpimath .geometry import Pose2d , Rotation2d , Translation2d
10- from wpimath .trajectory import TrajectoryConfig , TrajectoryGenerator , TrapezoidProfileRadians
11- from wpimath .controller import HolonomicDriveController , PIDController , ProfiledPIDControllerRadians
10+ from wpimath .trajectory import (
11+ TrajectoryConfig ,
12+ TrajectoryGenerator ,
13+ TrapezoidProfileRadians
14+ )
15+ from wpimath .controller import (
16+ HolonomicDriveController ,
17+ PIDController ,
18+ ProfiledPIDControllerRadians
19+ )
1220
1321from constants import AutoConstants , DriveConstants , OIConstants
1422from subsystems .drivesubsystem import DriveSubsystem
@@ -91,18 +99,20 @@ def getAutonomousCommand(self) -> commands2.Command:
9199
92100 # Constraint for the motion profiled robot angle controller
93101 kThetaControllerConstraints = TrapezoidProfileRadians .Constraints (
94- AutoConstants .kMaxAngularSpeedRadiansPerSecond , AutoConstants .kMaxAngularSpeedRadiansPerSecondSquared
102+ AutoConstants .kMaxAngularSpeedRadiansPerSecond ,
103+ AutoConstants .kMaxAngularSpeedRadiansPerSecondSquared
95104 )
96105
97106 kPXController = PIDController (1.0 , 0.0 , 0.0 )
98107 kPYController = PIDController (1.0 , 0.0 , 0.0 )
99- kPThetaController = ProfiledPIDControllerRadians (1.0 , 0.0 , 0.0 , kThetaControllerConstraints )
108+ kPThetaController = ProfiledPIDControllerRadians (
109+ 1.0 , 0.0 , 0.0 , kThetaControllerConstraints
110+ )
100111 kPThetaController .enableContinuousInput (- math .pi , math .pi )
101112
102113 kPIDController = HolonomicDriveController (
103- kPXController ,
104- kPYController ,
105- kPThetaController )
114+ kPXController , kPYController , kPThetaController
115+ )
106116
107117 swerveControllerCommand = commands2 .SwerveControllerCommand (
108118 exampleTrajectory ,
@@ -111,7 +121,7 @@ def getAutonomousCommand(self) -> commands2.Command:
111121 # Position controllers
112122 kPIDController ,
113123 self .robotDrive .setModuleStates ,
114- (self .robotDrive ,)
124+ (self .robotDrive ,),
115125 )
116126
117127 # Reset odometry to the starting pose of the trajectory.
0 commit comments