Transfered commands from constants.py to robotcontainer.py

Author: GitCloneHenry

examples/maxswerve/constants.py

@@ -13,9 +13,6 @@
 from wpimath import units
 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
 
@@ -132,15 +129,3 @@ class AutoConstants:
     kMaxAccelerationMetersPerSecondSquared = 3
     kMaxAngularSpeedRadiansPerSecond = math.pi
     kMaxAngularSpeedRadiansPerSecondSquared = math.pi
-
-    # 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)
-    kPThetaController.enableContinuousInput(-math.pi, math.pi)
-    
-    kPIDController = HolonomicDriveController(kPXController, kPYController, kPThetaController)

examples/maxswerve/robotcontainer.py

@@ -7,8 +7,8 @@
 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.controller import HolonomicDriveController
+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
@@ -89,12 +89,27 @@ def getAutonomousCommand(self) -> commands2.Command:
             config,
         )
 
+        # 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)
+
         swerveControllerCommand = commands2.SwerveControllerCommand(
             exampleTrajectory,
             self.robotDrive.getPose,  # Functional interface to feed supplier
             DriveConstants.kDriveKinematics,
             # Position controllers
-            AutoConstants.kPIDController,
+            kPIDController,
             self.robotDrive.setModuleStates,
             (self.robotDrive,)
         )