diff --git a/src/main/java/org/littletonrobotics/frc2024/Robot.java b/src/main/java/org/littletonrobotics/frc2024/Robot.java index 15359dc3..a2efc591 100644 --- a/src/main/java/org/littletonrobotics/frc2024/Robot.java +++ b/src/main/java/org/littletonrobotics/frc2024/Robot.java @@ -83,6 +83,7 @@ public class Robot extends LoggedRobot { AlertType.WARNING); private final Alert sameBatteryAlert = new Alert("The battery has not been changed since the last match.", AlertType.WARNING); + private final Alert gcAlert = new Alert("Please wait 30 seconds to enable.", AlertType.WARNING); public static Trigger createTeleopTimeTrigger(DoubleSupplier teleElapsedTime) { return new Trigger( @@ -262,6 +263,9 @@ public void robotPeriodic() { Leds.getInstance().lowBatteryAlert = true; } + // GC alert + gcAlert.set(Timer.getFPGATimestamp() < 30.0); + // Update battery alert String batteryName = batteryNameSubscriber.get(); Logger.recordOutput("BatteryName", batteryName); diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index 970729c7..befa18f5 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -312,6 +312,7 @@ public RobotContainer() { && rollers.getGamepieceState() == GamepieceState.SHOOTER_STAGED && superstructure.getCurrentGoal() != Superstructure.Goal.PREPARE_CLIMB && superstructure.getCurrentGoal() != Superstructure.Goal.PREPARE_PREPARE_TRAP_CLIMB + && superstructure.getCurrentGoal() != Superstructure.Goal.POST_PREPARE_TRAP_CLIMB && superstructure.getCurrentGoal() != Superstructure.Goal.CLIMB && superstructure.getCurrentGoal() != Superstructure.Goal.TRAP && superstructure.getCurrentGoal() != Superstructure.Goal.UNTRAP); @@ -524,6 +525,9 @@ private void configureButtonBindings() { // Intake Floor driver .leftTrigger() + .and( + DriverStation + ::isEnabled) // Must be enabled, allowing driver to hold button as soon as auto ends .whileTrue( superstructure .setGoalCommand(Superstructure.Goal.INTAKE) diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotState.java b/src/main/java/org/littletonrobotics/frc2024/RobotState.java index cd9862aa..9b52608e 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotState.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotState.java @@ -70,6 +70,7 @@ public record AimingParameters( armAngleMap.put(4.0, 27.75); armAngleMap.put(4.25, 26.8); armAngleMap.put(4.5, 25.6); + armAngleMap.put(8.0, 8.8); // Added in with slope of previous two points to make a best guess } @AutoLogOutput @Getter @Setter private double shotCompensationDegrees = 0.0; diff --git a/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java b/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java index f8d91bc8..74b25220 100644 --- a/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java +++ b/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java @@ -71,6 +71,9 @@ public static Command trapSequence( Arm.prepareClimbProfileConstraints.get()), rollers.setGoalCommand(Rollers.Goal.SHUFFLE_BACKPACK)), + // Pre-move arm to climb position to help with alignment and prevent wedging + superstructure.setGoalCommand(Superstructure.Goal.POST_PREPARE_TRAP_CLIMB), + // Allow driver to line up and climb superstructure.setGoalCommand(Superstructure.Goal.CLIMB), diff --git a/src/main/java/org/littletonrobotics/frc2024/commands/WheelRadiusCharacterization.java b/src/main/java/org/littletonrobotics/frc2024/commands/WheelRadiusCharacterization.java index 6f4bda6a..0d859d81 100644 --- a/src/main/java/org/littletonrobotics/frc2024/commands/WheelRadiusCharacterization.java +++ b/src/main/java/org/littletonrobotics/frc2024/commands/WheelRadiusCharacterization.java @@ -88,6 +88,7 @@ public void execute() { @Override public void end(boolean interrupted) { + drive.endCharacterization(); if (accumGyroYawRads <= Math.PI * 2.0) { System.out.println("Not enough data for characterization"); } else { diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java index 6fc483c1..bc6eb66e 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java @@ -24,7 +24,7 @@ import java.util.concurrent.locks.ReentrantLock; import java.util.function.Supplier; import java.util.stream.IntStream; -import lombok.Getter; +import lombok.Setter; import lombok.experimental.ExtensionMethod; import org.littletonrobotics.frc2024.Constants; import org.littletonrobotics.frc2024.RobotState; @@ -51,7 +51,7 @@ public class Drive extends SubsystemBase { private static final LoggedTunableNumber coastMetersPerSecThreshold = new LoggedTunableNumber("Drive/CoastMetersPerSecThreshold", 0.05); - public enum DriveMode { + public static enum DriveMode { /** Driving with input from driver joysticks. (Default) */ TELEOP, @@ -68,6 +68,12 @@ public enum DriveMode { WHEEL_RADIUS_CHARACTERIZATION } + public static enum CoastRequest { + AUTOMATIC, + ALWAYS_BRAKE, + ALWAYS_COAST + } + @AutoLog public static class OdometryTimestampInputs { public double[] timestamps = new double[] {}; @@ -93,10 +99,15 @@ public static class OdometryTimestampInputs { private boolean modulesOrienting = false; private final Timer lastMovementTimer = new Timer(); - @Getter @AutoLogOutput(key = "Drive/BrakeModeEnabled") private boolean brakeModeEnabled = true; + @Setter + @AutoLogOutput(key = "Drive/CoastRequest") + private CoastRequest coastRequest = CoastRequest.AUTOMATIC; + + private boolean lastEnabled = false; + private ChassisSpeeds desiredSpeeds = new ChassisSpeeds(); private SwerveSetpoint currentSetpoint = @@ -122,6 +133,7 @@ public Drive(GyroIO gyroIO, ModuleIO fl, ModuleIO fr, ModuleIO bl, ModuleIO br) modules[2] = new Module(bl, 2); modules[3] = new Module(br, 3); lastMovementTimer.start(); + setBrakeMode(true); setpointGenerator = SwerveSetpointGenerator.builder() @@ -220,6 +232,26 @@ public void periodic() { .anyMatch(module -> module.getVelocityMetersPerSec() > coastMetersPerSecThreshold.get())) { lastMovementTimer.reset(); } + if (DriverStation.isEnabled() && !lastEnabled) { + coastRequest = CoastRequest.AUTOMATIC; + } + + lastEnabled = DriverStation.isEnabled(); + switch (coastRequest) { + case AUTOMATIC -> { + if (DriverStation.isEnabled()) { + setBrakeMode(true); + } else if (lastMovementTimer.hasElapsed(coastWaitTime.get())) { + setBrakeMode(false); + } + } + case ALWAYS_BRAKE -> { + setBrakeMode(true); + } + case ALWAYS_COAST -> { + setBrakeMode(false); + } + } // Run drive based on current mode ChassisSpeeds teleopSpeeds = teleopDriveController.update(); @@ -254,16 +286,16 @@ public void periodic() { default -> {} } - // Run robot at desiredSpeeds - // Generate feasible next setpoint - currentSetpoint = - setpointGenerator.generateSetpoint( - currentModuleLimits, currentSetpoint, desiredSpeeds, Constants.loopPeriodSecs); - - // run modules + // Run modules if (currentDriveMode != DriveMode.CHARACTERIZATION && !modulesOrienting) { + // Run robot at desiredSpeeds + // Generate feasible next setpoint + currentSetpoint = + setpointGenerator.generateSetpoint( + currentModuleLimits, currentSetpoint, desiredSpeeds, Constants.loopPeriodSecs); SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4]; SwerveModuleState[] optimizedSetpointTorques = new SwerveModuleState[4]; + for (int i = 0; i < modules.length; i++) { // Optimize setpoints optimizedSetpointStates[i] = @@ -401,6 +433,14 @@ public double[] getWheelRadiusCharacterizationPosition() { return Arrays.stream(modules).mapToDouble(Module::getPositionRads).toArray(); } + /** Set brake mode to {@code enabled} doesn't change brake mode if already set. */ + private void setBrakeMode(boolean enabled) { + if (brakeModeEnabled != enabled) { + Arrays.stream(modules).forEach(module -> module.setBrakeMode(enabled)); + } + brakeModeEnabled = enabled; + } + /** * Returns command that orients all modules to start of {@link HolonomicTrajectory}, ending when * the modules have rotated. @@ -433,12 +473,15 @@ public Command orientModules(Rotation2d orientation) { */ public Command orientModules(Rotation2d[] orientations) { return run(() -> { + SwerveModuleState[] states = new SwerveModuleState[4]; for (int i = 0; i < orientations.length; i++) { modules[i].runSetpoint( SwerveModuleState.optimize( new SwerveModuleState(0.0, orientations[i]), modules[i].getAngle()), new SwerveModuleState(0.0, new Rotation2d())); + states[i] = new SwerveModuleState(0.0, modules[i].getAngle()); } + currentSetpoint = new SwerveSetpoint(new ChassisSpeeds(), states); }) .until( () -> diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Module.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Module.java index c30a27fc..19efe2ae 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Module.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Module.java @@ -94,6 +94,12 @@ public void runCharacterization(double turnSetpointRads, double input) { io.runCharacterization(input); } + /** Sets brake mode to {@code enabled}. */ + public void setBrakeMode(boolean enabled) { + io.setDriveBrakeMode(enabled); + io.setTurnBrakeMode(enabled); + } + /** Stops motors. */ public void stop() { io.stop(); diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOKrakenFOC.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOKrakenFOC.java index 96d9467d..3f283478 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOKrakenFOC.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOKrakenFOC.java @@ -22,6 +22,8 @@ import edu.wpi.first.wpilibj.AnalogInput; import edu.wpi.first.wpilibj.RobotController; import java.util.Queue; +import java.util.concurrent.Executor; +import java.util.concurrent.Executors; import java.util.function.Supplier; public class ModuleIOKrakenFOC implements ModuleIO { @@ -52,6 +54,7 @@ public class ModuleIOKrakenFOC implements ModuleIO { // Controller Configs private final TalonFXConfiguration driveTalonConfig = new TalonFXConfiguration(); private final TalonFXConfiguration turnTalonConfig = new TalonFXConfiguration(); + private static final Executor brakeModeExecutor = Executors.newFixedThreadPool(8); // Control private final VoltageOut voltageControl = new VoltageOut(0).withUpdateFreqHz(0); @@ -227,16 +230,26 @@ public void setTurnPID(double kP, double kI, double kD) { @Override public void setDriveBrakeMode(boolean enable) { - driveTalonConfig.MotorOutput.NeutralMode = - enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; - driveTalon.getConfigurator().apply(driveTalonConfig); + brakeModeExecutor.execute( + () -> { + synchronized (driveTalonConfig) { + driveTalonConfig.MotorOutput.NeutralMode = + enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; + driveTalon.getConfigurator().apply(driveTalonConfig, 0.25); + } + }); } @Override public void setTurnBrakeMode(boolean enable) { - turnTalonConfig.MotorOutput.NeutralMode = - enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; - turnTalon.getConfigurator().apply(turnTalonConfig); + brakeModeExecutor.execute( + () -> { + synchronized (turnTalonConfig) { + turnTalonConfig.MotorOutput.NeutralMode = + enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; + turnTalon.getConfigurator().apply(turnTalonConfig, 0.25); + } + }); } @Override diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSim.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSim.java index 66e307af..1a365857 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSim.java @@ -11,11 +11,13 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import org.littletonrobotics.frc2024.Constants; +import org.littletonrobotics.frc2024.subsystems.drive.DriveConstants.ModuleConfig; public class ModuleIOSim implements ModuleIO { private final DCMotorSim driveSim = @@ -32,6 +34,9 @@ public class ModuleIOSim implements ModuleIO { private double turnAppliedVolts = 0.0; private final Rotation2d turnAbsoluteInitPosition; + private boolean driveCoast = false; + private SlewRateLimiter driveVoltsLimiter = new SlewRateLimiter(4.0); + public ModuleIOSim(ModuleConfig config) { turnAbsoluteInitPosition = config.absoluteEncoderOffset(); turnFeedback.enableContinuousInput(-Math.PI, Math.PI); @@ -43,6 +48,12 @@ public void updateInputs(ModuleIOInputs inputs) { stop(); } + if (driveCoast && DriverStation.isDisabled()) { + runDriveVolts(driveVoltsLimiter.calculate(driveAppliedVolts)); + } else { + driveVoltsLimiter.reset(driveAppliedVolts); + } + driveSim.update(Constants.loopPeriodSecs); turnSim.update(Constants.loopPeriodSecs); @@ -101,6 +112,11 @@ public void setTurnPID(double kP, double kI, double kD) { turnFeedback.setPID(kP, kI, kD); } + @Override + public void setDriveBrakeMode(boolean enable) { + driveCoast = !enable; + } + @Override public void stop() { runDriveVolts(0.0); diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/leds/Leds.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/leds/Leds.java index 0a65d913..2d8502c9 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/leds/Leds.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/leds/Leds.java @@ -170,7 +170,7 @@ public synchronized void periodic() { // Same battery alert if (sameBattery) { - strobe(Color.kRed, strobeFastDuration); + breath(Color.kRed, Color.kBlack); } } else if (DriverStation.isAutonomous()) { wave(Color.kGold, Color.kDarkBlue, waveFastCycleLength, waveFastDuration); diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/RollersSensorsIOCompbot.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/RollersSensorsIOCompbot.java index 88f4c2f3..30c85215 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/RollersSensorsIOCompbot.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/RollersSensorsIOCompbot.java @@ -17,7 +17,7 @@ public class RollersSensorsIOCompbot implements RollersSensorsIO { private final DigitalGlitchFilter glitchFilter = new DigitalGlitchFilter(); public RollersSensorsIOCompbot() { - glitchFilter.setPeriodNanoSeconds(Duration.ofMillis(5).toNanos()); + glitchFilter.setPeriodNanoSeconds(Duration.ofMillis(1).toNanos()); glitchFilter.add(shooterStagedSensor); glitchFilter.add(backpackStagedSensor); } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java index d5719f3b..28c083a1 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java @@ -35,6 +35,7 @@ public enum Goal { RESET_CLIMB, PREPARE_PREPARE_TRAP_CLIMB, PREPARE_CLIMB, + POST_PREPARE_TRAP_CLIMB, CLIMB, TRAP, UNTRAP, @@ -72,6 +73,7 @@ public void periodic() { if (!climber.retracted() && desiredGoal != Goal.PREPARE_PREPARE_TRAP_CLIMB && desiredGoal != Goal.PREPARE_CLIMB + && desiredGoal != Goal.POST_PREPARE_TRAP_CLIMB && desiredGoal != Goal.CLIMB && desiredGoal != Goal.TRAP && desiredGoal != Goal.UNTRAP @@ -159,6 +161,11 @@ public void periodic() { climber.setGoal(Climber.Goal.EXTEND); backpackActuator.setGoal(BackpackActuator.Goal.RETRACT); } + case POST_PREPARE_TRAP_CLIMB -> { + arm.setGoal(Arm.Goal.CLIMB); + climber.setGoal(Climber.Goal.EXTEND); + backpackActuator.setGoal(BackpackActuator.Goal.RETRACT); + } case CLIMB -> { arm.setGoal(Arm.Goal.CLIMB); climber.setGoal(Climber.Goal.RETRACT); diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmConstants.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmConstants.java index 49996bb3..5f5c3eff 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmConstants.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmConstants.java @@ -46,7 +46,8 @@ public class ArmConstants { /** The offset of the arm encoder in radians. */ public static final double armEncoderOffsetRads = switch (Constants.getRobot()) { - default -> 1.1980389953386859; // 1.0753205323078345 rad as measured at hardstop on 3/12/24, + default -> + 1.1980389953386859 + .006; // 1.0753205323078345 rad as measured at hardstop on 3/12/24, // corresponding to an arm position of 0.11965050145508001 rad case DEVBOT -> -1.233 - Math.PI / 2.0; }; diff --git a/trajectory_native/src/trajectory_service.cpp b/trajectory_native/src/trajectory_service.cpp index f028bd20..3a42fd3d 100644 --- a/trajectory_native/src/trajectory_service.cpp +++ b/trajectory_native/src/trajectory_service.cpp @@ -19,7 +19,7 @@ namespace vts = org::littletonrobotics::vehicletrajectoryservice; // We take the tradeoff of increased computation time since we cache paths anyway. static const double CONTROL_INTERVAL_GUESS_SCALAR = 1.0; -static const int MINIMUM_CONTROL_INTERVAL_COUNT = 32; +static const int MINIMUM_CONTROL_INTERVAL_COUNT = 20; static std::unique_ptr server;