Set brake mode, add auton, add reset angle and bindings

src/main/java/com/stuypulse/robot/Robot.java

@@ -6,6 +6,7 @@
 package com.stuypulse.robot;
 
 import com.stuypulse.robot.commands.TeleopInit;
+import com.stuypulse.robot.commands.swerve.SwerveDriveResetHeading;
 
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
@@ -72,6 +73,8 @@ public void disabledPeriodic() {}
     public void autonomousInit() {
         auto = robot.getAutonomousCommand();
 
+        new SwerveDriveResetHeading().schedule();
+
         if (auto != null) {
             auto.schedule();
         }

src/main/java/com/stuypulse/robot/RobotContainer.java

@@ -7,15 +7,18 @@
 
 import com.stuypulse.robot.commands.auton.DoNothingAuton;
 import com.stuypulse.robot.commands.auton.DriveAndTurnBump;
+import com.stuypulse.robot.commands.auton.EightFootAuton;
 import com.stuypulse.robot.commands.auton.Mobility;
 import com.stuypulse.robot.commands.auton.MobilityBump;
 import com.stuypulse.robot.commands.swerve.SwerveDriveDrive;
+import com.stuypulse.robot.commands.swerve.SwerveDriveResetHeading;
 import com.stuypulse.robot.constants.Ports;
 import com.stuypulse.robot.subsystems.swerve.Odometry;
 import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
 import com.stuypulse.robot.util.BootlegXbox;
 import com.stuypulse.stuylib.input.Gamepad;
 
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.DutyCycleEncoder;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
@@ -58,15 +61,21 @@ private void configureDefaultCommands() {
     /*** BUTTONS ***/
     /***************/
 
-    private void configureButtonBindings() {}
+    private void configureButtonBindings() {
+        driver.getDPadUp().onTrue(new SwerveDriveResetHeading(Rotation2d.fromDegrees(180)));
+        driver.getDPadDown().onTrue(new SwerveDriveResetHeading(Rotation2d.fromDegrees(0)));
+        driver.getDPadLeft().onTrue(new SwerveDriveResetHeading(Rotation2d.fromDegrees(270)));
+        driver.getDPadRight().onTrue(new SwerveDriveResetHeading(Rotation2d.fromDegrees(90)));
+    }
 
     /**************/
     /*** AUTONS ***/
     /**************/
 
     public void configureAutons() {
-        autonChooser.setDefaultOption("Do Nothing", new DoNothingAuton());
-        autonChooser.addOption("Mobility", new Mobility());
+        autonChooser.addOption("Do Nothing", new DoNothingAuton());
+        autonChooser.addOption("8 Feet", new EightFootAuton());
+        autonChooser.setDefaultOption("Mobility", new Mobility());
         autonChooser.addOption("Mobility Bump", new MobilityBump());
         autonChooser.addOption("Drive And Turn Bump", new DriveAndTurnBump());
 

src/main/java/com/stuypulse/robot/commands/auton/EightFootAuton.java

@@ -0,0 +1,37 @@
+/************************ PROJECT PHIL ************************/
+/* Copyright (c) 2023 StuyPulse Robotics. All rights reserved.*/
+/* This work is licensed under the terms of the MIT license.  */
+/**************************************************************/
+
+package com.stuypulse.robot.commands.auton;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+import java.util.Arrays;
+
+import com.pathplanner.lib.PathConstraints;
+import com.pathplanner.lib.PathPlanner;
+import com.pathplanner.lib.PathPlannerTrajectory;
+import com.pathplanner.lib.PathPoint;
+import com.stuypulse.robot.subsystems.swerve.SwerveDriveFollowTrajectory;
+
+public class EightFootAuton extends SequentialCommandGroup {
+    private static final PathConstraints CONSTRAINTS = new PathConstraints(0.01, 0.01);
+    private static final PathPlannerTrajectory PATH = PathPlanner.generatePath(CONSTRAINTS, Arrays.asList(
+        new PathPoint(new Translation2d(), new Rotation2d(), new Rotation2d()),
+        new PathPoint(new Translation2d(0, 0), new Rotation2d(), new Rotation2d())
+    ));
+
+    public EightFootAuton() {
+        addCommands(
+            new SwerveDriveFollowTrajectory(PATH)
+                .robotRelative()
+                .withStop()
+        );
+    }
+
+
+}

src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveResetHeading.java

@@ -0,0 +1,22 @@
+package com.stuypulse.robot.commands.swerve;
+
+import com.stuypulse.robot.subsystems.swerve.Odometry;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+public class SwerveDriveResetHeading extends InstantCommand {
+
+    public SwerveDriveResetHeading(Rotation2d heading) {
+        super(() -> {
+            Odometry o = Odometry.getInstance();
+            o.reset(new Pose2d(o.getTranslation(), heading));
+        });
+    }
+
+    public SwerveDriveResetHeading() {
+        this(new Rotation2d());
+    }
+
+}

src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveModuleImpl.java

@@ -4,6 +4,7 @@
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 import com.revrobotics.RelativeEncoder;
+import com.revrobotics.CANSparkMax.IdleMode;
 import com.stuypulse.robot.Robot;
 import com.stuypulse.robot.Robot.MatchState;
 import com.stuypulse.robot.constants.Settings.Swerve;
@@ -52,6 +53,9 @@ public SwerveModuleImpl(String id, Translation2d translationOffset, Rotation2d a
         turnMotor = new CANSparkMax(turnID, MotorType.kBrushless);
         driveMotor = new CANSparkMax(driveID, MotorType.kBrushless);
 
+        turnMotor.setIdleMode(IdleMode.kBrake);
+        driveMotor.setIdleMode(IdleMode.kBrake);
+
         turnEncoder = new CANCoder(encoderID);
         driveEncoder = driveMotor.getEncoder();