Updated robotcontainer.py and constants.py for the maxswerve example to match updates to the commands2 class.

Author: GitCloneHenry

examples/maxswerve/constants.py

@@ -14,6 +14,8 @@
 from wpimath.geometry import Translation2d
 from wpimath.kinematics import SwerveDrive4Kinematics
 from wpimath.trajectory import TrapezoidProfileRadians
+from wpimath.controller import HolonomicDriveController, PIDController, ProfiledPIDControllerRadians
+from wpimath.trajectory import TrapezoidProfileRadians
 
 from rev import CANSparkMax
 
@@ -131,11 +133,13 @@ class AutoConstants:
     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
     )
+
+    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)
+
+    kPIDController = HolonomicDriveController(kPXController, kPYController, kPThetaController)

examples/maxswerve/robotcontainer.py

@@ -8,6 +8,7 @@
 from wpimath.controller import PIDController, ProfiledPIDControllerRadians
 from wpimath.geometry import Pose2d, Rotation2d, Translation2d
 from wpimath.trajectory import TrajectoryConfig, TrajectoryGenerator
+from wpimath.controller import HolonomicDriveController
 
 from constants import AutoConstants, DriveConstants, OIConstants
 from subsystems.drivesubsystem import DriveSubsystem
@@ -88,24 +89,14 @@ def getAutonomousCommand(self) -> commands2.Command:
             config,
         )
 
-        thetaController = ProfiledPIDControllerRadians(
-            AutoConstants.kPThetaController,
-            0,
-            0,
-            AutoConstants.kThetaControllerConstraints,
-        )
-        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,
+            AutoConstants.kPIDController,
             self.robotDrive.setModuleStates,
-            (self.robotDrive,),
+            (self.robotDrive,)
         )
 
         # Reset odometry to the starting pose of the trajectory.
@@ -117,4 +108,4 @@ def getAutonomousCommand(self) -> commands2.Command:
                 lambda: self.robotDrive.drive(0, 0, 0, False, False),
                 self.robotDrive,
             )
-        )
+        )