diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5aaea62..bf0ebdd 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,5 +1,7 @@ package frc.robot; +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; @@ -12,8 +14,10 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc.robot.Constants.AutoConstants; +import frc.robot.Constants.DriveConstants; import frc.robot.Constants.DriveConstants.SwerveModuleConstants; import frc.robot.Constants.IntakeConstants; import frc.robot.Constants.ShooterConstants; @@ -84,6 +88,27 @@ public Command getAutonomousCommand() { new Pose2d(3, 0, new Rotation2d(0)), config); - return Commands.print("No autonomous command configured"); + var thetaController = + new ProfiledPIDController( + AutoConstants.kPThetaController, 0, 0, AutoConstants.kThetaControllerConstraints); + thetaController.enableContinuousInput(-Math.PI, Math.PI); + + SwerveControllerCommand swerveControllerCommand = + new SwerveControllerCommand( + exampleTrajectory, + driveSubsystem::getPose, // Functional interface to feed supplier + DriveConstants.SwerveModuleConstants.kDriveKinematics, + + // Position controllers + new PIDController(AutoConstants.kPXController, 0, 0), + new PIDController(AutoConstants.kPYController, 0, 0), + thetaController, + driveSubsystem::setModuleStates, + driveSubsystem); + + // Reset odometry to the starting pose of the trajectory. + driveSubsystem.resetOdometry(exampleTrajectory.getInitialPose()); + + return swerveControllerCommand.andThen(() -> driveSubsystem.drive(0, 0, 0)); } }