SwerveRobotContainer.java

package frc.robot;

import java.util.List;

import com.pathplanner.lib.PathConstraints;
import com.pathplanner.lib.PathPlanner;
import com.pathplanner.lib.PathPlannerTrajectory;
import com.pathplanner.lib.commands.PPSwerveControllerCommand;

import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.XboxController;
import frc.robot.Constants.AutoConstants;
import frc.robot.Constants.DriveConstants;
import frc.robot.Constants.OIConstants;
import frc.robot.commands.SwerveTeleOpCmd;
import frc.robot.subsystems.DriveSubsystem;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;

/**
 * This class is where the bulk of the robot should be declared. Since Command-based is a
 * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
 * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
 * subsystems, commands, and button mappings) should be declared here.
 */
public class RobotContainer {
	// The robot's subsystems and commands are defined here...
	private final DriveSubsystem driveSubsystem = new DriveSubsystem();

	private final XboxController joystick_1 = new XboxController(OIConstants.kDriverControllerPort);

	/** The container for the robot. Contains subsystems, OI devices, and commands. */
	public RobotContainer() {
		driveSubsystem.setDefaultCommand(new SwerveTeleOpCmd(
			driveSubsystem, 
			() -> -joystick_1.getLeftY(), 
			() -> -joystick_1.getLeftX(),
			() -> joystick_1.getRightX(), 
			() -> joystick_1.getRightBumper()));
		
		configureButtonBindings();
	}

	/**
	 * Use this method to define your button->command mappings. Buttons can be created by
	 * instantiating a {@link GenericHID} or one of its subclasses ({@link
	 * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
	 * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
	 */
	private void configureButtonBindings() {
		new JoystickButton(joystick_1, XboxController.Button.kA.value).whenPressed(() -> driveSubsystem.zeroHeading());
	}

	/**
	 * Use this to pass the autonomous command to the main {@link Robot} class.
	 *
	 * @return the command to run in autonomous
	 */
	public Command getAutonomousCommand() {
		TrajectoryConfig trajectoryConfig = new TrajectoryConfig(
				AutoConstants.kMaxSpeedMetersPerSecond, 
				AutoConstants.kMaxAccelerationMetersPerSecondSquared)
						.setKinematics(DriveConstants.kDriveKinematics);

		Trajectory trajectory = TrajectoryGenerator.generateTrajectory(
				new Pose2d(0, 0, new Rotation2d(0)),
				List.of(
					new Translation2d(1, 0.75),
					new Translation2d(1.5, 0),
					new Translation2d(1, -0.75),
					new Translation2d(0, 0),
					new Translation2d(-1, 0.75),
					new Translation2d(-1.5, 0),
					new Translation2d(-1, -0.75)
				),
				new Pose2d(0, 0, Rotation2d.fromDegrees(180)), 
				trajectoryConfig);


		PathPlannerTrajectory PPtrajectory = PathPlanner.loadPath("New Path", new PathConstraints(1.2, 3));

		PIDController xController = new PIDController(AutoConstants.kPXController, 0, 0);
		PIDController yController = new PIDController(AutoConstants.kPYController, 0, 0);
		PIDController thetaController = new PIDController(AutoConstants.kPThetaController, 0, 0);
		thetaController.enableContinuousInput(-Math.PI, Math.PI);
		ProfiledPIDController profiledThetaController = new ProfiledPIDController(
				AutoConstants.kPThetaController, 0, 0, AutoConstants.kThetaControllerConstraints);
		profiledThetaController.enableContinuousInput(-Math.PI, Math.PI);
	

		PPSwerveControllerCommand PPswerveControllerCommand = new PPSwerveControllerCommand(
				PPtrajectory, 
				driveSubsystem::getPose2d,
				DriveConstants.kDriveKinematics,
				xController, 
				yController, 
				thetaController, 
				driveSubsystem::setModuleStates, 
				driveSubsystem);

		SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand(
				trajectory, 
				driveSubsystem::getPose2d, 
				DriveConstants.kDriveKinematics, 
				xController, 
				yController, 
				profiledThetaController, 
				driveSubsystem::setModuleStates, 
				driveSubsystem);
		
		return new SequentialCommandGroup(
				new InstantCommand(() -> driveSubsystem.resetOdometry(PPtrajectory.getInitialPose())),
				PPswerveControllerCommand,
				new InstantCommand(() -> driveSubsystem.stopModules())
		);
	}
}