[Robot] Added feature to make robot switch to coast mode at the end o…

Robot/src/main/java/com/swrobotics/robot/subsystems/lights/LightsSubsystem.java

@@ -1,5 +1,6 @@
 package com.swrobotics.robot.subsystems.lights;
 
+import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.swrobotics.mathlib.MathUtil;
 import com.swrobotics.robot.RobotContainer;
 import com.swrobotics.robot.config.IOAllocation;
@@ -80,6 +81,10 @@ private void showShooterStatus() {
         }
     }
 
+    private void showCoastDriving() {
+        applySolid(Color.kMagenta);
+    }
+
     private void showAutoDriving() {
         // Rainbow
         applyStripes(5f,
@@ -104,13 +109,15 @@ private void showIdle() {
     @Override
     public void periodic() {
         // Special indicator to swap battery
-        boolean batteryLow = RobotController.getBatteryVoltage() < 10;
+        boolean batteryLow = RobotController.getBatteryVoltage() < 11;
 
         boolean resetShooterBlink = true;
         if (batteryLowDebounce.calculate(batteryLow)) {
             showLowBattery();
         } else if (DriverStation.isDisabled()) {
             prideSequencer.apply(this);
+        } else if (robot.drive.getCurrentNeutralMode() == NeutralModeValue.Coast) {
+            showCoastDriving();
         } else if (robot.shooter.isPreparing()) {
             resetShooterBlink = false;
             showShooterStatus();

Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveDrive.java

@@ -16,6 +16,7 @@
 import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants;
 import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType;
 import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.SteerRequestType;
+import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.kauailabs.navx.frc.AHRS;
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.pathfinding.Pathfinding;
@@ -36,6 +37,7 @@
 import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.SPI;
+import edu.wpi.first.wpilibj.DriverStation.MatchType;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 import com.swrobotics.lib.net.NTEntry;
@@ -87,6 +89,7 @@ public static record TurnRequest(int priority, Rotation2d turn) {
     private DriveRequest currentDriveRequest;
     private TurnRequest currentTurnRequest;
     private int lastSelectedPriority;
+    private NeutralModeValue currentNeutralMode;
 
     public SwerveDrive(FieldInfo fieldInfo, MessengerClient msg) {
         this.fieldInfo = fieldInfo;
@@ -116,6 +119,7 @@ public SwerveDrive(FieldInfo fieldInfo, MessengerClient msg) {
         prevPositions = null;
         currentDriveRequest = NULL_DRIVE;
         currentTurnRequest = NULL_TURN;
+        currentNeutralMode = NeutralModeValue.Brake;
 
         // Configure pathing
         AutoBuilder.configureHolonomic(
@@ -240,6 +244,15 @@ public void periodic() {
             }
         }
 
+        // Coast if we are really close to the end of the match
+        // (lets us slide into the stage at the end to clutch up the 1 pt)
+        double time = DriverStation.getMatchTime();
+        if (DriverStation.isTeleopEnabled() && time < 3) {
+            setNeutralMode(NeutralModeValue.Coast);
+        } else {
+            setNeutralMode(NeutralModeValue.Brake);
+        }
+
         // Update estimator
         // Do refresh here, so we get the most up-to-date data
         SwerveModulePosition[] positions = getCurrentModulePositions(true);
@@ -317,7 +330,21 @@ public TalonFX getTurnMotor(int module) {
         return modules[module].getSteerMotor();
     }
 
+    public void setNeutralMode(NeutralModeValue neutralMode) {
+        if (neutralMode == currentNeutralMode)
+            return;
+
+        currentNeutralMode = neutralMode;
+        for (SwerveModule module : modules) {
+            module.configNeutralMode(neutralMode);
+        }
+    }
+
     public void setEstimatorIgnoreVision(boolean ignoreVision) {
         estimator.setIgnoreVision(ignoreVision);
     }
+
+    public NeutralModeValue getCurrentNeutralMode() {
+        return currentNeutralMode;
+    }
 }