Updated robotcontainer.py and constants.py for the maxswerve example …

examples/maxswerve/constants.py

@@ -13,7 +13,6 @@
 from wpimath import units
 from wpimath.geometry import Translation2d
 from wpimath.kinematics import SwerveDrive4Kinematics
-from wpimath.trajectory import TrapezoidProfileRadians
 
 from rev import CANSparkMax
 
@@ -130,12 +129,3 @@ class AutoConstants:
     kMaxAccelerationMetersPerSecondSquared = 3
     kMaxAngularSpeedRadiansPerSecond = math.pi
     kMaxAngularSpeedRadiansPerSecondSquared = math.pi
-
-    kPXController = 1
-    kPYController = 1
-    kPThetaController = 1
-
-    # Constraint for the motion profiled robot angle controller
-    kThetaControllerConstraints = TrapezoidProfileRadians.Constraints(
-        kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared
-    )

examples/maxswerve/robotcontainer.py

@@ -7,7 +7,16 @@
 from commands2 import cmd
 from wpimath.controller import PIDController, ProfiledPIDControllerRadians
 from wpimath.geometry import Pose2d, Rotation2d, Translation2d
-from wpimath.trajectory import TrajectoryConfig, TrajectoryGenerator
+from wpimath.trajectory import (
+    TrajectoryConfig,
+    TrajectoryGenerator,
+    TrapezoidProfileRadians,
+)
+from wpimath.controller import (
+    HolonomicDriveController,
+    PIDController,
+    ProfiledPIDControllerRadians,
+)
 
 from constants import AutoConstants, DriveConstants, OIConstants
 from subsystems.drivesubsystem import DriveSubsystem
@@ -88,22 +97,29 @@ def getAutonomousCommand(self) -> commands2.Command:
             config,
         )
 
-        thetaController = ProfiledPIDControllerRadians(
-            AutoConstants.kPThetaController,
-            0,
-            0,
-            AutoConstants.kThetaControllerConstraints,
+        # Constraint for the motion profiled robot angle controller
+        kThetaControllerConstraints = TrapezoidProfileRadians.Constraints(
+            AutoConstants.kMaxAngularSpeedRadiansPerSecond,
+            AutoConstants.kMaxAngularSpeedRadiansPerSecondSquared,
+        )
+
+        kPXController = PIDController(1.0, 0.0, 0.0)
+        kPYController = PIDController(1.0, 0.0, 0.0)
+        kPThetaController = ProfiledPIDControllerRadians(
+            1.0, 0.0, 0.0, kThetaControllerConstraints
+        )
+        kPThetaController.enableContinuousInput(-math.pi, math.pi)
+
+        kPIDController = HolonomicDriveController(
+            kPXController, kPYController, kPThetaController
         )
-        thetaController.enableContinuousInput(-math.pi, math.pi)
 
         swerveControllerCommand = commands2.SwerveControllerCommand(
             exampleTrajectory,
             self.robotDrive.getPose,  # Functional interface to feed supplier
             DriveConstants.kDriveKinematics,
             # Position controllers
-            PIDController(AutoConstants.kPXController, 0, 0),
-            PIDController(AutoConstants.kPYController, 0, 0),
-            thetaController,
+            kPIDController,
             self.robotDrive.setModuleStates,
             (self.robotDrive,),
         )