From dce0325a8744ff3d5e189b98d89b7c59ae7c8121 Mon Sep 17 00:00:00 2001 From: Ethan Torres Date: Mon, 9 Feb 2026 14:03:21 -0500 Subject: [PATCH 1/3] Added current logging for indexer --- .../java/frc/robot/subsystems/Spindexer.java | 21 ++++++++++++++----- 1 file changed, 16 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Spindexer.java b/src/main/java/frc/robot/subsystems/Spindexer.java index b14b184..619398e 100644 --- a/src/main/java/frc/robot/subsystems/Spindexer.java +++ b/src/main/java/frc/robot/subsystems/Spindexer.java @@ -8,6 +8,7 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.SpindexerConstants; @@ -16,6 +17,7 @@ public class Spindexer extends SubsystemBase { private TalonFX SpindexerMotor; private LaserCan SpindexerLaser; private Debouncer SpindexDebouncer; + private Debouncer currentEmptyDebouncer = new Debouncer(0.4); /** Creates a new Spindexer. */ public Spindexer() { @@ -43,6 +45,14 @@ public double getSpeed() { return SpindexerMotor.get(); } + public double getCurrent() { + return SpindexerMotor.getStatorCurrent().getValueAsDouble(); + } + + public boolean currentSaysEmpty() { + return currentEmptyDebouncer.calculate(getCurrent() < 9.0); // random number need to test + } + public Command runSpindexer() { return run(this::setSpeed); } @@ -65,10 +75,6 @@ public Command downSpeed(double speed) { }); } - // new command runUntilEMpty - // empty means laser can cant see anything anymore - // look up debouncer - public Command runUntilEmptyCommand() { return (runSpindexer()).until(() -> SpindexDebouncer.calculate(!beamBroken())); } @@ -88,8 +94,13 @@ public boolean beamBroken() { } } + public boolean isEmpty() { + return !beamBroken() && currentSaysEmpty(); + } + @Override public void periodic() { - // This method will be called once per scheduler run + SmartDashboard.putNumber("Spindexer Current", getCurrent()); + SmartDashboard.putBoolean("Spindexer Beam Broken", beamBroken()); } } From 068158c5d601c5169e6328f1409d9b2dca166249 Mon Sep 17 00:00:00 2001 From: Ethan Torres Date: Mon, 9 Feb 2026 14:04:11 -0500 Subject: [PATCH 2/3] Added dotProduct math to guidedswerve to ensure you are only locked when necessary --- simgui-ds.json | 5 ----- .../java/frc/robot/commands/GuidedTeleopSwerve.java | 10 +++++----- 2 files changed, 5 insertions(+), 10 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index be50a26..f178b77 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,9 +1,4 @@ { - "System Joysticks": { - "window": { - "enabled": false - } - }, "keyboardJoysticks": [ { "axisConfig": [ diff --git a/src/main/java/frc/robot/commands/GuidedTeleopSwerve.java b/src/main/java/frc/robot/commands/GuidedTeleopSwerve.java index e9bd813..ca41d0a 100644 --- a/src/main/java/frc/robot/commands/GuidedTeleopSwerve.java +++ b/src/main/java/frc/robot/commands/GuidedTeleopSwerve.java @@ -109,9 +109,8 @@ private double getDotProduct() { commandedVelocity.set(1, 0, getStrafeSpeed()); Vector toTarget = new Vector<>(Nat.N2()); - - toTarget.set(0, 0, robotPose.getX() - targetPose.getX()); - toTarget.set(1, 0, robotPose.getY() - targetPose.getY()); + toTarget.set(0, 0, (flipFactor) * (targetPose.getX() - robotPose.getX())); + toTarget.set(1, 0, (flipFactor) * (targetPose.getY() - robotPose.getY())); double dotProduct = commandedVelocity.unit().dot(toTarget.unit()); @@ -185,7 +184,7 @@ private void driveBump(double forward, double strafe) { swerve.setControl( fieldOriented .withVelocityX(forward) - .withVelocityY(strafe) + .withVelocityY(strafe) .withRotationalRate(rotDiagonalSpeed)); } @@ -204,7 +203,8 @@ public void execute() { double dotProduct = currentDriveMode == DriveMode.NormalDrive ? 0 : getDotProduct(); DriveMode effectiveDriveMode = - (manualOverrideSupplier.getAsBoolean() || (dotProduct < .353)) + (manualOverrideSupplier.getAsBoolean() + || (dotProduct < 0.05)) // only lock if in trying to drive in that direction ? DriveMode.NormalDrive : currentDriveMode; From d81b85db86beee39b5128387f7278f1ca81a07da Mon Sep 17 00:00:00 2001 From: Ethan Torres Date: Mon, 9 Feb 2026 14:04:30 -0500 Subject: [PATCH 3/3] Added new trigger to restrict firing when we are too close to the hub --- src/main/java/frc/robot/Constants.java | 1 + src/main/java/frc/robot/RobotContainer.java | 55 ++++++------------- .../frc/robot/commands/ShootOnTheMove.java | 7 ++- .../java/frc/robot/subsystems/Swerve.java | 52 ++++++++++++------ .../java/frc/robot/util/SOTMCalculator.java | 11 +++- 5 files changed, 66 insertions(+), 60 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b3063a5..d4f00a9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -69,6 +69,7 @@ public static class SimConstants { public static final int maxCapacity = 30; public static final double fuelsPerSecond = 6.7; public static final double loopPeriodSecs = 0.020; + public static final Distance closestPossibleShotDistance = Meters.of(1.5); } public static class SOTMConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fd10150..7263a62 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,7 +19,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.Constants.FieldConstants; @@ -43,14 +42,7 @@ import frc.robot.util.SwerveTelemetry; import java.util.function.Supplier; -/** - * This class is where the bulk of the robot should be declared. Since Command-based is a - * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} - * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including - * subsystems, commands, and trigger mappings) should be declared here. - */ public class RobotContainer { - // Replace with CommandPS4Controller or CommandJoystick if needed private final ExtendedCommandXboxController driverController = new ExtendedCommandXboxController(OperatorConstants.kDriverControllerPort); private final ExtendedCommandXboxController operatorController = @@ -77,7 +69,6 @@ public class RobotContainer { private Pose2d goalShotTarget; private final Supplier goalShotTargetSupplier = () -> goalShotTarget; - private boolean toggleAutomatedShooting = true; private boolean canPreShoot = false; @Logged(name = "Swerve") @@ -108,7 +99,10 @@ public class RobotContainer { Trigger inAllianceZoneTrigger = new Trigger(() -> swerve.inAllianceZone()); Trigger activeHubTrigger = new Trigger(HubTracker::isActive).or(() -> HubTracker.getMatchTime() < 0); - Trigger automatedShootingTrigger = new Trigger(() -> toggleAutomatedShooting); + Trigger automatedShootingTrigger = + new Trigger(() -> SmartDashboard.getBoolean("Automated Shooting Toggle", false)); + + Trigger tooCloseToHubTrigger = new Trigger(() -> swerve.tooCloseToHub()); Trigger preShiftShoot = new Trigger( @@ -124,16 +118,15 @@ public class RobotContainer { double timeOfFlight = SOTMConstants.timeOfFlightMap.get(distanceToHub); - return (timeUntilActive) <= timeOfFlight; // 0.5s buffer + return (timeUntilActive) <= timeOfFlight; }) - .and(activeHubTrigger.negate()) - .and(inAllianceZoneTrigger); + .and(activeHubTrigger.negate()) // only need to preshoot if not already active + .and(inAllianceZoneTrigger) // only preshoot if in alliance zone + .and(tooCloseToHubTrigger.negate()); // only shoot if we are far enough away - /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - NamedCommands.registerCommand("Drive Over Bump To Middle", swerve.driveOverBump("To Middle")); - NamedCommands.registerCommand( - "Drive Over Bump To Alliance", swerve.driveOverBump("To Alliance")); + NamedCommands.registerCommand("Drive Over Bump To Middle", swerve.driveOverBump(true)); + NamedCommands.registerCommand("Drive Over Bump To Alliance", swerve.driveOverBump(false)); NamedCommands.registerCommand("Move To Fuel", new MoveToFuel(swerve).withTimeout(2)); NamedCommands.registerCommand( "Shoot On The Move", @@ -161,9 +154,9 @@ public RobotContainer() { NamedCommands.registerCommand( "Shoot", Commands.run(() -> shooter.setSpeed(MetersPerSecond.of(1))).withTimeout(1)); - // Configure the trigger bindings configureDriverBindings(); // configureOperatorBindings(); + swerve.configureAutoBuilder(); configureAutoChooser(); @@ -196,15 +189,6 @@ public RobotContainer() { activeHubTrigger.onFalse(Commands.runOnce(() -> canPreShoot = false)); } - /** - * Use this method to define your trigger->command mappings. Triggers can be created via the - * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary - * predicate, or via the named factories in {@link - * edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses for {@link - * CommandXboxController Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller - * PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight - * joysticks}. - */ private void configureFuelSim() { fuelInstance.spawnStartingFuel(); fuelInstance.registerRobot( @@ -237,6 +221,7 @@ private void configureFuelSim() { SmartDashboard.putBoolean("Air Resistance Toggle", false); SmartDashboard.putBoolean("Only Score while Active", false); + SmartDashboard.putBoolean("Automated Shooting Toggle", false); } private void configureDriverBindings() { @@ -247,6 +232,8 @@ private void configureDriverBindings() { (activeHubTrigger.or(() -> canPreShoot)) .and(automatedShootingTrigger) .and(inAllianceZoneTrigger) + .and(shootButton.negate()) + .and(tooCloseToHubTrigger.negate()) .whileTrue( new ShootOnTheMove( swerve, turret, hood, shooter, goalShotTargetSupplier, robotVisualization)); @@ -268,16 +255,7 @@ private void configureDriverBindings() { turret.setDefaultCommand(turret.faceTarget(goalShotTargetSupplier, swerve::getRobotPose)); hood.setDefaultCommand(hood.aimForTarget(goalShotTargetSupplier, swerve::getRobotPose)); - // hood.setDefaultCommand( - // new PhysicsStationaryShoot( - // shooter, - // hood, - // robotVisualization, - // swerve::getRobotPose, - // AllianceUtil::getHubPose, - // () -> FieldConstants.mainHubHeight)); - - // shooter.setDefaulyCommand(); + shootButton.whileTrue( new ShootOnTheMove( swerve, turret, hood, shooter, goalShotTargetSupplier, robotVisualization)); @@ -287,6 +265,7 @@ private void configureOperatorBindings() { (activeHubTrigger.or(() -> canPreShoot)) .and(automatedShootingTrigger) .and(inAllianceZoneTrigger) + // .and(shootButton.negate()) .whileTrue( new ShootOnTheMove( swerve, turret, hood, shooter, goalShotTargetSupplier, robotVisualization)); @@ -332,7 +311,7 @@ private void configureAutoChooser() { autoChooser.addOption( "[SysID] Quasistatic Steer Forward", swerve.sysIdQuasistaticSteer(Direction.kForward)); autoChooser.addOption( - "[SysID] Quasistatic Steer Reverse", swerve.sysIdQuasistaticSteer(Direction.kForward)); + "[SysID] Quasistatic Steer Reverse", swerve.sysIdQuasistaticSteer(Direction.kReverse)); autoChooser.addOption( "[SysID] Dynamic Steer Forward", swerve.sysIdDynamicSteer(Direction.kForward)); autoChooser.addOption( diff --git a/src/main/java/frc/robot/commands/ShootOnTheMove.java b/src/main/java/frc/robot/commands/ShootOnTheMove.java index 98ff3f2..b62d286 100644 --- a/src/main/java/frc/robot/commands/ShootOnTheMove.java +++ b/src/main/java/frc/robot/commands/ShootOnTheMove.java @@ -95,29 +95,31 @@ public void execute() { swerve, turret, targetPoseSupplier.get(), fieldAccelX, fieldAccelY, fieldSpeeds); turret.setTargetAngle(shootingParameters.turretAngle()); - hood.setTargetAngle(shootingParameters.hoodAngle()); shooter.setGoalSpeed(shootingParameters.shooterSpeed()); + swerve.setLookAheadPose(shootingParameters.lookAheadPosition()); double turretErrorDeg = turret.getTurretAngle().in(Degrees) - shootingParameters.turretAngle().in(Degrees); double hoodErrorDeg = hood.getHoodAngle().in(Degrees) - shootingParameters.hoodAngle().in(Degrees); + if (turretSetPointDebouncer.calculate(Math.abs(turretErrorDeg) <= turretTolerance) && hoodSetPointDebouncer.calculate(Math.abs(hoodErrorDeg) <= hoodTolerance) && shooterDebouncer.calculate(shooterAtSetPoint)) { if (isFirstShot || ((Timer.getFPGATimestamp() - startTime) > 1 / SimConstants.fuelsPerSecond)) { robotVisualization.shootFuel(shootingParameters); + startTime = Timer.getFPGATimestamp(); isFirstShot = false; } + } else { // indexer.stop(); } } - // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { // shooter.stop(); @@ -126,7 +128,6 @@ public void end(boolean interrupted) { hood.stopHood(); } - // Returns true when the command should end. @Override public boolean isFinished() { return false; diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 3dd23b1..561de77 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -49,6 +49,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants.AutoConstants; import frc.robot.Constants.FieldConstants; +import frc.robot.Constants.SimConstants; import frc.robot.Constants.TurretConstants; import frc.robot.Constants.VisionConstants; import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain; @@ -63,6 +64,7 @@ import java.util.Optional; import java.util.Set; import java.util.function.Supplier; +import lombok.Setter; import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; @@ -108,8 +110,6 @@ public int compareTo(PoseEstimate other) { .withForwardPerspective(ForwardPerspectiveValue.OperatorPerspective) .withSteerRequestType(SteerRequestType.Position); - private Timer timer = new Timer(); - private double targetFuelYaw = 0; public Rotation2d desiredFuelRotation = Rotation2d.k180deg; @@ -146,6 +146,8 @@ public int compareTo(PoseEstimate other) { public SwerveDriveState stateCache = getState(); + @Setter private Translation2d lookAheadPose = new Translation2d(); + private ExtendedVisionSystemSim visionSim; private PhotonCameraSim arducamLeftSim; @@ -348,20 +350,23 @@ public Command pathFindToPose(Pose2d targetPose) { Set.of(this)); } - public Command driveOverBump(String direction) { - return Commands.runOnce(timer::restart) - .andThen( - Commands.run( - () -> { - if (direction == "To Middle") { - this.setControl( - fieldOriented.withVelocityX(0.1).withVelocityY(0).withRotationalRate(0)); - } else { - this.setControl( - fieldOriented.withVelocityX(-0.1).withVelocityY(0).withRotationalRate(0)); - } - })) - .until(() -> timer.hasElapsed(0.5) && getPigeon2().getPitch().getValueAsDouble() == 0); + public Command driveOverBump(boolean toMiddle) { + double startTime = Timer.getFPGATimestamp(); + return Commands.run( + () -> { + if (toMiddle) { + this.setControl( + fieldOriented.withVelocityX(0.1).withVelocityY(0).withRotationalRate(0)); + } else { + this.setControl( + fieldOriented.withVelocityX(-0.1).withVelocityY(0).withRotationalRate(0)); + } + }) + .until( + () -> + (Timer.getFPGATimestamp() - startTime) > 0.5 + && Math.abs(getPigeon2().getPitch().getValueAsDouble()) <= 0.353) + .withTimeout(Seconds.of(3.53)); } public Command pathFindThroughTrench() { @@ -396,6 +401,21 @@ public Command pathFindThroughTrench() { .withName("PathFindThroughTrench"); } + public boolean tooCloseToHub() { + + Translation2d turretPose = + stateCache.Pose + .getTranslation() + .plus( + TurretConstants.robotToTurretTransform + .getTranslation() + .rotateBy(stateCache.Pose.getRotation())); + + double turretToHubDistance = lookAheadPose.getDistance(turretPose); + + return turretToHubDistance < SimConstants.closestPossibleShotDistance.in(Meters); + } + public boolean onLeftSide() { Pose2d pose = stateCache.Pose; return !AllianceUtil.isRedAlliance() && pose.getMeasureY().gt(FieldConstants.fieldWidth.div(2)) diff --git a/src/main/java/frc/robot/util/SOTMCalculator.java b/src/main/java/frc/robot/util/SOTMCalculator.java index 2217429..6745c38 100644 --- a/src/main/java/frc/robot/util/SOTMCalculator.java +++ b/src/main/java/frc/robot/util/SOTMCalculator.java @@ -27,7 +27,10 @@ public class SOTMCalculator { public static Time accelTime = Seconds.of(0.1353); public record ShootingParameters( - LinearVelocity shooterSpeed, Angle turretAngle, Angle hoodAngle) {} + LinearVelocity shooterSpeed, + Angle turretAngle, + Angle hoodAngle, + Translation2d lookAheadPosition) {} public static ShootingParameters getParameters( Swerve swerve, @@ -44,6 +47,7 @@ public static ShootingParameters getParameters( robotPose.getTranslation().plus(robotToTurret2d.rotateBy(robotPose.getRotation())); double robotAngle = robotPose.getRotation().getRadians(); + double turretVelocityX = fieldChassisSpeeds.vxMetersPerSecond + fieldChassisSpeeds.omegaRadiansPerSecond @@ -62,7 +66,7 @@ public static ShootingParameters getParameters( double timeOfFlight = timeOfFlightMap.get(turretToTargetDistance); - SmartDashboard.putNumber("SOTM/distance", turretToTargetDistance); + SmartDashboard.putNumber("SOTM/turretToLookAheadDistance", turretToTargetDistance); for (int i = 0; i < 20; i++) { double offsetX = @@ -95,6 +99,7 @@ public static ShootingParameters getParameters( Rotation2d hoodAngle = hoodAngleMap.get(turretToTargetDistance); LinearVelocity shooterSpeed = MetersPerSecond.of(shooterSpeedMap.get(turretToTargetDistance)); - return new ShootingParameters(shooterSpeed, turretAngle, hoodAngle.getMeasure()); + return new ShootingParameters( + shooterSpeed, turretAngle, hoodAngle.getMeasure(), lookAheadPosition); } }