88from wpimath .controller import PIDController , ProfiledPIDControllerRadians
99from wpimath .geometry import Pose2d , Rotation2d , Translation2d
1010from wpimath .trajectory import (
11- TrajectoryConfig ,
12- TrajectoryGenerator ,
11+ TrajectoryConfig ,
12+ TrajectoryGenerator ,
1313 TrapezoidProfileRadians ,
1414)
1515from wpimath .controller import (
16- HolonomicDriveController ,
16+ HolonomicDriveController ,
1717 PIDController ,
1818 ProfiledPIDControllerRadians ,
1919)
@@ -99,7 +99,7 @@ def getAutonomousCommand(self) -> commands2.Command:
9999
100100 # Constraint for the motion profiled robot angle controller
101101 kThetaControllerConstraints = TrapezoidProfileRadians .Constraints (
102- AutoConstants .kMaxAngularSpeedRadiansPerSecond ,
102+ AutoConstants .kMaxAngularSpeedRadiansPerSecond ,
103103 AutoConstants .kMaxAngularSpeedRadiansPerSecondSquared ,
104104 )
105105
@@ -109,11 +109,11 @@ def getAutonomousCommand(self) -> commands2.Command:
109109 1.0 , 0.0 , 0.0 , kThetaControllerConstraints
110110 )
111111 kPThetaController .enableContinuousInput (- math .pi , math .pi )
112-
112+
113113 kPIDController = HolonomicDriveController (
114114 kPXController , kPYController , kPThetaController
115115 )
116-
116+
117117 swerveControllerCommand = commands2 .SwerveControllerCommand (
118118 exampleTrajectory ,
119119 self .robotDrive .getPose , # Functional interface to feed supplier
0 commit comments