From 1e9c9e0197519a4dc0af3551e18df7aa46c0615d Mon Sep 17 00:00:00 2001 From: Henry W <164814454+GitCloneHenry@users.noreply.github.com> Date: Thu, 24 Oct 2024 08:05:11 -0600 Subject: [PATCH 1/8] Updated robotcontainer.py and constants.py for the maxswerve example to match updates to the commands2 class. --- examples/maxswerve/constants.py | 12 ++++++++---- examples/maxswerve/robotcontainer.py | 17 ++++------------- 2 files changed, 12 insertions(+), 17 deletions(-) diff --git a/examples/maxswerve/constants.py b/examples/maxswerve/constants.py index f43472a..d470838 100644 --- a/examples/maxswerve/constants.py +++ b/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) \ No newline at end of file diff --git a/examples/maxswerve/robotcontainer.py b/examples/maxswerve/robotcontainer.py index cd8a1ea..1a52a18 100644 --- a/examples/maxswerve/robotcontainer.py +++ b/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, ) - ) + ) \ No newline at end of file From b07f20b7ecb4d5de2b8b4a9056115abeeebb120d Mon Sep 17 00:00:00 2001 From: Henry W <164814454+GitCloneHenry@users.noreply.github.com> Date: Thu, 24 Oct 2024 08:19:01 -0600 Subject: [PATCH 2/8] Update constants.py Added the line to enable continuous inputs on the ProfiledPIDController. --- examples/maxswerve/constants.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/examples/maxswerve/constants.py b/examples/maxswerve/constants.py index d470838..d7b44f3 100644 --- a/examples/maxswerve/constants.py +++ b/examples/maxswerve/constants.py @@ -142,4 +142,5 @@ class AutoConstants: kPYController = PIDController(1.0, 0.0, 0.0) kPThetaController = ProfiledPIDControllerRadians(1.0, 0.0, 0.0, kThetaControllerConstraints) - kPIDController = HolonomicDriveController(kPXController, kPYController, kPThetaController) \ No newline at end of file + kPIDController = HolonomicDriveController(kPXController, kPYController, kPThetaController) + kPIDController.enableContinuousInput(-math.pi, math.pi) From d39f75555bb5a95527940242ef42a3c56939de1e Mon Sep 17 00:00:00 2001 From: Henry W <164814454+GitCloneHenry@users.noreply.github.com> Date: Thu, 24 Oct 2024 08:20:04 -0600 Subject: [PATCH 3/8] Update constants.py Oops, changed the wrong PID controller. --- examples/maxswerve/constants.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/maxswerve/constants.py b/examples/maxswerve/constants.py index d7b44f3..7b478af 100644 --- a/examples/maxswerve/constants.py +++ b/examples/maxswerve/constants.py @@ -141,6 +141,6 @@ class AutoConstants: 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) - kPIDController.enableContinuousInput(-math.pi, math.pi) From 3a295159b92ff43b10daed4612d43449ab42b6fc Mon Sep 17 00:00:00 2001 From: Henry W <164814454+GitCloneHenry@users.noreply.github.com> Date: Thu, 24 Oct 2024 09:06:14 -0600 Subject: [PATCH 4/8] Transfered commands from constants.py to robotcontainer.py --- examples/maxswerve/constants.py | 15 --------------- examples/maxswerve/robotcontainer.py | 21 ++++++++++++++++++--- 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/examples/maxswerve/constants.py b/examples/maxswerve/constants.py index 7b478af..0c02876 100644 --- a/examples/maxswerve/constants.py +++ b/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) diff --git a/examples/maxswerve/robotcontainer.py b/examples/maxswerve/robotcontainer.py index 1a52a18..569396c 100644 --- a/examples/maxswerve/robotcontainer.py +++ b/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,) ) From 1b4ab4be02457d059ae6c5b188bf983ee3919584 Mon Sep 17 00:00:00 2001 From: Henry W <164814454+GitCloneHenry@users.noreply.github.com> Date: Thu, 24 Oct 2024 13:40:53 -0600 Subject: [PATCH 5/8] Added new line to end of robotcontainer.py --- examples/maxswerve/robotcontainer.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/maxswerve/robotcontainer.py b/examples/maxswerve/robotcontainer.py index 569396c..f03dd1b 100644 --- a/examples/maxswerve/robotcontainer.py +++ b/examples/maxswerve/robotcontainer.py @@ -123,4 +123,4 @@ def getAutonomousCommand(self) -> commands2.Command: lambda: self.robotDrive.drive(0, 0, 0, False, False), self.robotDrive, ) - ) \ No newline at end of file + ) From 284707bc9a0517e6f6081971fdc6664edcd304c6 Mon Sep 17 00:00:00 2001 From: Henry W <164814454+GitCloneHenry@users.noreply.github.com> Date: Thu, 24 Oct 2024 15:29:52 -0600 Subject: [PATCH 6/8] Fixed Formatting --- examples/maxswerve/robotcontainer.py | 26 ++++++++++++++++++-------- 1 file changed, 18 insertions(+), 8 deletions(-) diff --git a/examples/maxswerve/robotcontainer.py b/examples/maxswerve/robotcontainer.py index f03dd1b..36caf05 100644 --- a/examples/maxswerve/robotcontainer.py +++ b/examples/maxswerve/robotcontainer.py @@ -7,8 +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, TrapezoidProfileRadians -from wpimath.controller import HolonomicDriveController, PIDController, ProfiledPIDControllerRadians +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 @@ -91,18 +99,20 @@ def getAutonomousCommand(self) -> commands2.Command: # Constraint for the motion profiled robot angle controller kThetaControllerConstraints = TrapezoidProfileRadians.Constraints( - AutoConstants.kMaxAngularSpeedRadiansPerSecond, AutoConstants.kMaxAngularSpeedRadiansPerSecondSquared + 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 = ProfiledPIDControllerRadians( + 1.0, 0.0, 0.0, kThetaControllerConstraints + ) kPThetaController.enableContinuousInput(-math.pi, math.pi) kPIDController = HolonomicDriveController( - kPXController, - kPYController, - kPThetaController) + kPXController, kPYController, kPThetaController + ) swerveControllerCommand = commands2.SwerveControllerCommand( exampleTrajectory, @@ -111,7 +121,7 @@ def getAutonomousCommand(self) -> commands2.Command: # Position controllers kPIDController, self.robotDrive.setModuleStates, - (self.robotDrive,) + (self.robotDrive,), ) # Reset odometry to the starting pose of the trajectory. From 13b02374f6452927c8b703bb1a918ce52d4f5315 Mon Sep 17 00:00:00 2001 From: Henry W <164814454+GitCloneHenry@users.noreply.github.com> Date: Fri, 25 Oct 2024 08:13:32 -0600 Subject: [PATCH 7/8] Fixed some formatting issues to comply with ci. --- examples/maxswerve/robotcontainer.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/examples/maxswerve/robotcontainer.py b/examples/maxswerve/robotcontainer.py index 36caf05..5a00af1 100644 --- a/examples/maxswerve/robotcontainer.py +++ b/examples/maxswerve/robotcontainer.py @@ -10,12 +10,12 @@ from wpimath.trajectory import ( TrajectoryConfig, TrajectoryGenerator, - TrapezoidProfileRadians + TrapezoidProfileRadians, ) from wpimath.controller import ( HolonomicDriveController, PIDController, - ProfiledPIDControllerRadians + ProfiledPIDControllerRadians, ) from constants import AutoConstants, DriveConstants, OIConstants @@ -100,7 +100,7 @@ def getAutonomousCommand(self) -> commands2.Command: # Constraint for the motion profiled robot angle controller kThetaControllerConstraints = TrapezoidProfileRadians.Constraints( AutoConstants.kMaxAngularSpeedRadiansPerSecond, - AutoConstants.kMaxAngularSpeedRadiansPerSecondSquared + AutoConstants.kMaxAngularSpeedRadiansPerSecondSquared, ) kPXController = PIDController(1.0, 0.0, 0.0) @@ -113,7 +113,7 @@ def getAutonomousCommand(self) -> commands2.Command: kPIDController = HolonomicDriveController( kPXController, kPYController, kPThetaController ) - + swerveControllerCommand = commands2.SwerveControllerCommand( exampleTrajectory, self.robotDrive.getPose, # Functional interface to feed supplier From f45537afbfe15aa9128bc7435e728fd67149e87f Mon Sep 17 00:00:00 2001 From: Henry W <164814454+GitCloneHenry@users.noreply.github.com> Date: Fri, 25 Oct 2024 18:23:39 -0600 Subject: [PATCH 8/8] Fixed some spacing --- examples/maxswerve/robotcontainer.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/examples/maxswerve/robotcontainer.py b/examples/maxswerve/robotcontainer.py index 5a00af1..1271e2c 100644 --- a/examples/maxswerve/robotcontainer.py +++ b/examples/maxswerve/robotcontainer.py @@ -8,12 +8,12 @@ from wpimath.controller import PIDController, ProfiledPIDControllerRadians from wpimath.geometry import Pose2d, Rotation2d, Translation2d from wpimath.trajectory import ( - TrajectoryConfig, - TrajectoryGenerator, + TrajectoryConfig, + TrajectoryGenerator, TrapezoidProfileRadians, ) from wpimath.controller import ( - HolonomicDriveController, + HolonomicDriveController, PIDController, ProfiledPIDControllerRadians, ) @@ -99,7 +99,7 @@ def getAutonomousCommand(self) -> commands2.Command: # Constraint for the motion profiled robot angle controller kThetaControllerConstraints = TrapezoidProfileRadians.Constraints( - AutoConstants.kMaxAngularSpeedRadiansPerSecond, + AutoConstants.kMaxAngularSpeedRadiansPerSecond, AutoConstants.kMaxAngularSpeedRadiansPerSecondSquared, ) @@ -109,11 +109,11 @@ def getAutonomousCommand(self) -> commands2.Command: 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