From 928c488fdef171d6756f34e837e5a23326b61df7 Mon Sep 17 00:00:00 2001 From: Museum25 <91571536+mvog2501@users.noreply.github.com> Date: Sun, 10 Mar 2024 21:11:55 -0500 Subject: [PATCH 1/3] [Robot] Added feature to make robot switch to coast mode at the end of the match This lets us clutch up the park point by sliding in during disabled --- .../robot/subsystems/swerve/SwerveDrive.java | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveDrive.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveDrive.java index 0e984e2..eff43b2 100644 --- a/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveDrive.java +++ b/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; @@ -240,6 +242,13 @@ 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); + } + // Update estimator // Do refresh here, so we get the most up-to-date data SwerveModulePosition[] positions = getCurrentModulePositions(true); @@ -317,6 +326,12 @@ public TalonFX getTurnMotor(int module) { return modules[module].getSteerMotor(); } + public void setNeutralMode(NeutralModeValue neutralMode) { + for (SwerveModule module : modules) { + module.configNeutralMode(neutralMode); + } + } + public void setEstimatorIgnoreVision(boolean ignoreVision) { estimator.setIgnoreVision(ignoreVision); } From f414e8cd3466fb1c48483238065afcc1ca101289 Mon Sep 17 00:00:00 2001 From: rmheuer <63077980+rmheuer@users.noreply.github.com> Date: Thu, 14 Mar 2024 10:42:58 -0500 Subject: [PATCH 2/3] [Robot] Only update neutral mode if it changed --- .../swrobotics/robot/subsystems/swerve/SwerveDrive.java | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveDrive.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveDrive.java index eff43b2..4a21fe7 100644 --- a/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveDrive.java +++ b/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveDrive.java @@ -89,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; @@ -118,6 +119,7 @@ public SwerveDrive(FieldInfo fieldInfo, MessengerClient msg) { prevPositions = null; currentDriveRequest = NULL_DRIVE; currentTurnRequest = NULL_TURN; + currentNeutralMode = NeutralModeValue.Brake; // Configure pathing AutoBuilder.configureHolonomic( @@ -247,6 +249,8 @@ public void periodic() { double time = DriverStation.getMatchTime(); if (DriverStation.isTeleopEnabled() && time < 3) { setNeutralMode(NeutralModeValue.Coast); + } else { + setNeutralMode(NeutralModeValue.Brake); } // Update estimator @@ -327,6 +331,10 @@ public TalonFX getTurnMotor(int module) { } public void setNeutralMode(NeutralModeValue neutralMode) { + if (neutralMode == currentNeutralMode) + return; + + currentNeutralMode = neutralMode; for (SwerveModule module : modules) { module.configNeutralMode(neutralMode); } From 35cad0ea90ce24036d12d55d3e9bb8968c510700 Mon Sep 17 00:00:00 2001 From: rmheuer <63077980+rmheuer@users.noreply.github.com> Date: Thu, 14 Mar 2024 10:46:57 -0500 Subject: [PATCH 3/3] [Robot] Add LED coast mode indicator --- .../robot/subsystems/lights/LightsSubsystem.java | 9 ++++++++- .../swrobotics/robot/subsystems/swerve/SwerveDrive.java | 4 ++++ 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/lights/LightsSubsystem.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/lights/LightsSubsystem.java index 15dd98e..c0e28b0 100644 --- a/Robot/src/main/java/com/swrobotics/robot/subsystems/lights/LightsSubsystem.java +++ b/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(); diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveDrive.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveDrive.java index 4a21fe7..e600805 100644 --- a/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveDrive.java +++ b/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveDrive.java @@ -343,4 +343,8 @@ public void setNeutralMode(NeutralModeValue neutralMode) { public void setEstimatorIgnoreVision(boolean ignoreVision) { estimator.setIgnoreVision(ignoreVision); } + + public NeutralModeValue getCurrentNeutralMode() { + return currentNeutralMode; + } }