auto (not looked over)

src/main/java/frc4388/robot/Constants.java

@@ -51,14 +51,16 @@ public final class Constants {
     }
 
     public static final class AutoConstants {
-      public static final PIDController X_CONTROLLER = new PIDController(0.0, 0.0, 0.0);
-      public static final PIDController Y_CONTROLLER = new PIDController(0.0, 0.0, 0.0);
-      public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(0.0, 0.0, 0.0, 
-        new TrapezoidProfile.Constraints(0.0, 0.0)
-      );
+      public static final Gains X_CONTROLLER = new Gains(0.0, 0.0, 0.0);
+      public static final Gains Y_CONTROLLER = new Gains(0.0, 0.0, 0.0);
+      public static final Gains THETA_CONTROLLER = new Gains(0.0, 0.0, 0.0);
+      public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(0.0, 0.0);
 
       public static final double PATH_MAX_VEL = SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND / 4; // TODO: find the actual value
       public static final double PATH_MAX_ACC = 3; // TODO: find the actual value
+
+      public static final double kPX_CONTROLLER = 1.5; //TODO: find actual value
+      public static final double kPY_CONTROLLER = 1.5; //TODO: find actual value
     }
 
     public static final class Conversions {

src/main/java/frc4388/robot/RobotContainer.java

@@ -7,13 +7,26 @@
 
 package frc4388.robot;
 
+import java.util.List;
+
+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.Joystick;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc4388.robot.Constants.*;
+import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
+import frc4388.robot.Constants.SwerveDriveConstants.PIDConstants;
 import frc4388.robot.commands.AutoBalance;
 import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.utility.controller.DeadbandedXboxController;
@@ -92,9 +105,38 @@ public class RobotContainer {
      * @return the command to run in autonomous
      */
     public Command getAutonomousCommand() {
-        TrajectoryConfig trajectoryConfig = new TrajectoryConfig(SwerveDriveConstants.AutoConstants.PATH_MAX_VEL, SwerveDriveConstants.AutoConstants.PATH_MAX_ACC).setKinematics(m_robotSwerveDrive.getKinematics());
+        //Create Trajectory Settings
+        TrajectoryConfig trajectoryConfig = new TrajectoryConfig(SwerveDriveConstants.AutoConstants.PATH_MAX_VEL, 
+            SwerveDriveConstants.AutoConstants.PATH_MAX_ACC).setKinematics(m_robotSwerveDrive.getKinematics());
+        //Generate Trajactory
+        Trajectory trajectory = TrajectoryGenerator.generateTrajectory(new Pose2d(0, 0, new Rotation2d(0)), 
+            List.of(new Translation2d(1, 0), new Translation2d(1, -1)), 
+            new Pose2d(0, 0, Rotation2d.fromDegrees(180)), 
+            trajectoryConfig);
 
-        return new InstantCommand();
+        //Defining PID Controller for tracking trajectory
+        PIDController xController = new PIDController(SwerveDriveConstants.AutoConstants.kPX_CONTROLLER, 0, 0);
+        PIDController yController = new PIDController(SwerveDriveConstants.AutoConstants.kPY_CONTROLLER, 0, 0);
+        ProfiledPIDController thetaController = new ProfiledPIDController(AutoConstants.THETA_CONTROLLER.kP,
+        AutoConstants.THETA_CONTROLLER.kI, AutoConstants.THETA_CONTROLLER.kD, AutoConstants.THETA_CONSTRAINTS, 0);
+        thetaController.enableContinuousInput(-Math.PI, Math.PI);
+
+        //Command to follow trajectory
+        SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand(
+            trajectory,
+            m_robotSwerveDrive::getOdometry,
+            m_robotSwerveDrive.getKinematics(),
+            xController,
+            yController,
+            thetaController,
+            m_robotSwerveDrive::setModuleStates,
+            m_robotSwerveDrive);
+
+        //Init and wrap-up, return everything
+        return new SequentialCommandGroup(
+                new InstantCommand(() -> m_robotSwerveDrive.setOdometry(trajectory.getInitialPose())),
+                swerveControllerCommand,
+                new InstantCommand(() -> m_robotSwerveDrive.stopModules()));
     }
 
     /**

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -68,14 +68,12 @@ public class SwerveDrive extends SubsystemBase {
 
   public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
     if (fieldRelative) {
-
       if (rightStick.getNorm() > 0.1) {
         rotTarget = new Rotation2d(rightStick.getX(), -rightStick.getY()).minus(new Rotation2d(0, 1));
       }
 
       double rot = rotTarget.minus(gyro.getRotation2d()).getRadians();
 
-
       // Use the left joystick to set speed. Apply a quadratic curve and the set max speed.
       Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
 
@@ -105,6 +103,13 @@ public class SwerveDrive extends SubsystemBase {
     }
   }
 
+  public void stopModules() {
+    leftFront.stop();
+    rightFront.stop();
+    leftBack.stop();
+    rightBack.stop();
+  }
+
   public double getGyroAngle() {
     return gyro.getAngle();
   }

src/main/java/frc4388/utility/Gains.java

@@ -54,6 +54,12 @@ public class Gains {
         this.kMinOutput = -1.0;
     }
 
+    public Gains(double kP, double kI, double kD) {
+        this.kP = kP;
+        this.kI = kI;
+        this.kD = kD;
+    }
+
     /**
      * Creates Gains object for PIDs
      * @param kP The P value.