Add a new autonomous doing a S curve path

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));
   }
 }