7
7
from commands2 import cmd
8
8
from wpimath .controller import PIDController , ProfiledPIDControllerRadians
9
9
from 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
+ )
12
20
13
21
from constants import AutoConstants , DriveConstants , OIConstants
14
22
from subsystems .drivesubsystem import DriveSubsystem
@@ -91,18 +99,20 @@ def getAutonomousCommand(self) -> commands2.Command:
91
99
92
100
# Constraint for the motion profiled robot angle controller
93
101
kThetaControllerConstraints = TrapezoidProfileRadians .Constraints (
94
- AutoConstants .kMaxAngularSpeedRadiansPerSecond , AutoConstants .kMaxAngularSpeedRadiansPerSecondSquared
102
+ AutoConstants .kMaxAngularSpeedRadiansPerSecond ,
103
+ AutoConstants .kMaxAngularSpeedRadiansPerSecondSquared
95
104
)
96
105
97
106
kPXController = PIDController (1.0 , 0.0 , 0.0 )
98
107
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
+ )
100
111
kPThetaController .enableContinuousInput (- math .pi , math .pi )
101
112
102
113
kPIDController = HolonomicDriveController (
103
- kPXController ,
104
- kPYController ,
105
- kPThetaController )
114
+ kPXController , kPYController , kPThetaController
115
+ )
106
116
107
117
swerveControllerCommand = commands2 .SwerveControllerCommand (
108
118
exampleTrajectory ,
@@ -111,7 +121,7 @@ def getAutonomousCommand(self) -> commands2.Command:
111
121
# Position controllers
112
122
kPIDController ,
113
123
self .robotDrive .setModuleStates ,
114
- (self .robotDrive ,)
124
+ (self .robotDrive ,),
115
125
)
116
126
117
127
# Reset odometry to the starting pose of the trajectory.
0 commit comments