From 47df42c0f38c7d255785de8e3d723edd05f71bb9 Mon Sep 17 00:00:00 2001
From: Henry W <164814454+GitCloneHenry@users.noreply.github.com>
Date: Sat, 26 Oct 2024 23:25:41 -0600
Subject: [PATCH] =?UTF-8?q?Updated=20robotcontainer.py=20and=20constants.p?=
 =?UTF-8?q?y=20for=20the=20maxswerve=20example=20=E2=80=A6=20(#65)?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

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

* Update constants.py

Added the line to enable continuous inputs on the ProfiledPIDController.

* Update constants.py

Oops, changed the wrong PID controller.

* Transfered commands from constants.py to robotcontainer.py

* Added new line to end of robotcontainer.py

* Fixed Formatting

* Fixed some formatting issues to comply with ci.

* Fixed some spacing
---
 examples/maxswerve/constants.py      | 10 --------
 examples/maxswerve/robotcontainer.py | 36 ++++++++++++++++++++--------
 2 files changed, 26 insertions(+), 20 deletions(-)

diff --git a/examples/maxswerve/constants.py b/examples/maxswerve/constants.py
index f43472a..0c02876 100644
--- a/examples/maxswerve/constants.py
+++ b/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
-    )
diff --git a/examples/maxswerve/robotcontainer.py b/examples/maxswerve/robotcontainer.py
index cd8a1ea..1271e2c 100644
--- a/examples/maxswerve/robotcontainer.py
+++ b/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,),
         )