From 22d1bda23306057a8fcf19e6276b1bb6ee6b01b9 Mon Sep 17 00:00:00 2001 From: WesleyWong-972 Date: Mon, 20 Apr 2026 17:31:31 -0700 Subject: [PATCH] stuff --- .../deploy/pathplanner/paths/Left Swipe1.path | 27 +---- .../pathplanner/paths/Right Swipe1.path | 29 +---- src/main/java/frc/robot/RobotContainer.java | 15 +++ .../java/frc/robot/commands/LogCommand.java | 5 +- .../auto_comm/DynamicAutoBuilder.java | 7 +- .../frc/robot/commands/gpm/RunSpindexer.java | 5 + .../commands/gpm/RunSpindexerWithStop.java | 113 ++++++++++++++++++ .../robot/subsystems/spindexer/Spindexer.java | 15 +++ .../spindexer/SpindexerConstants.java | 2 +- 9 files changed, 161 insertions(+), 57 deletions(-) create mode 100644 src/main/java/frc/robot/commands/gpm/RunSpindexerWithStop.java diff --git a/src/main/deploy/pathplanner/paths/Left Swipe1.path b/src/main/deploy/pathplanner/paths/Left Swipe1.path index 33bf814..7213bd0 100644 --- a/src/main/deploy/pathplanner/paths/Left Swipe1.path +++ b/src/main/deploy/pathplanner/paths/Left Swipe1.path @@ -110,7 +110,7 @@ { "name": "Constraints Zone", "minWaypointRelativePos": 0.7761529808773961, - "maxWaypointRelativePos": 1.642294713160863, + "maxWaypointRelativePos": 2.2491730981256937, "constraints": { "maxVelocity": 1.0, "maxAcceleration": 2.0, @@ -122,30 +122,7 @@ } ], "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Stop Hood Down", - "waypointRelativePos": 4.968503937007888, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "Stop Hood Down" - } - } - }, - { - "name": "Start Spindexer", - "waypointRelativePos": 5.0, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "Start Spindexer" - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 2.5, diff --git a/src/main/deploy/pathplanner/paths/Right Swipe1.path b/src/main/deploy/pathplanner/paths/Right Swipe1.path index 3bd63f6..44a9f20 100644 --- a/src/main/deploy/pathplanner/paths/Right Swipe1.path +++ b/src/main/deploy/pathplanner/paths/Right Swipe1.path @@ -21,7 +21,7 @@ }, "prevControl": { "x": 8.155370348167532, - "y": 0.8386737269319345 + "y": 0.8386737269319344 }, "nextControl": { "x": 8.45722898958081, @@ -110,7 +110,7 @@ { "name": "Constraints Zone", "minWaypointRelativePos": 0.7761529808773961, - "maxWaypointRelativePos": 1.642294713160863, + "maxWaypointRelativePos": 2.2491730981256826, "constraints": { "maxVelocity": 1, "maxAcceleration": 2, @@ -122,30 +122,7 @@ } ], "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Stop Hood Down", - "waypointRelativePos": 4.968503937007888, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "Stop Hood Down" - } - } - }, - { - "name": "Start Spindexer", - "waypointRelativePos": 5, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "Start Spindexer" - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 3, "maxAcceleration": 2.5, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1018db1..fcda69f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -21,6 +21,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import frc.robot.commands.DoNothing; import frc.robot.commands.LogCommand; +import frc.robot.commands.auto_comm.DynamicAutoBuilder; import frc.robot.commands.drive_comm.DefaultDriveCommand; import frc.robot.commands.gpm.IntakeMovementCommand; import frc.robot.commands.gpm.LockedShoot; @@ -255,6 +256,15 @@ public class RobotContainer { } } + public void addAuto(String name, Command auto) { + try { + autoChooser.addOption(name, auto); + } catch (AutoBuilderException e){ + e.printStackTrace(); + System.out.println("HELLOOOO AUTO \"" + name + "\" NOT FOUND"); + } + } + /** * Initialize the SendableChooser on the SmartDashboard. * Fill the SendableChooser with available Commands. @@ -284,6 +294,11 @@ public class RobotContainer { addAuto(rightDoNothing); addAuto(centerDoNothing); + DynamicAutoBuilder dynamicAutoBuilder = new DynamicAutoBuilder(spindexer, turret, hood, intake, breaker); + + String leftDynamicDoubleSwipe = "LeftDynamicDoubleLiberalSwipe"; + addAuto(leftDynamicDoubleSwipe, dynamicAutoBuilder.getLeftDynamicDoubleLiberalSwipe()); + // put the Chooser on the SmartDashboard SmartDashboard.putData("Auto chooser", autoChooser); } diff --git a/src/main/java/frc/robot/commands/LogCommand.java b/src/main/java/frc/robot/commands/LogCommand.java index 06b346d..6392304 100644 --- a/src/main/java/frc/robot/commands/LogCommand.java +++ b/src/main/java/frc/robot/commands/LogCommand.java @@ -4,6 +4,7 @@ import org.littletonrobotics.junction.Logger; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.constants.Constants; +import frc.robot.subsystems.spindexer.Spindexer; import frc.robot.util.Elastic; import frc.robot.util.HubActive; import frc.robot.util.Elastic.Notification; @@ -19,9 +20,7 @@ public class LogCommand extends Command { @Override public void execute() { boolean current = HubActive.isHubActive(); - if (!Constants.DISABLE_LOGGING) { - Logger.recordOutput("HubActive", current); - } + Logger.recordOutput("HubActive", current); if (current && !hubActive) { Elastic.sendNotification(new Notification(NotificationLevel.INFO, "HUB ACTIVE", "")); diff --git a/src/main/java/frc/robot/commands/auto_comm/DynamicAutoBuilder.java b/src/main/java/frc/robot/commands/auto_comm/DynamicAutoBuilder.java index f3430b8..98426b0 100644 --- a/src/main/java/frc/robot/commands/auto_comm/DynamicAutoBuilder.java +++ b/src/main/java/frc/robot/commands/auto_comm/DynamicAutoBuilder.java @@ -86,8 +86,11 @@ public class DynamicAutoBuilder { var timer = new Timer(); timer.start(); - return new RunSpindexer(spindexer, turret, hood, intake).raceWith( - new WaitUntilCommand(() -> breaker.getAverageCurrentDraw(BreakerConstants.SPINDEXER_PORTS) < 5.0 && timer.hasElapsed(3.0))); + // return new RunSpindexer(spindexer, turret, hood, intake).raceWith( + // new WaitUntilCommand(() -> spindexer.spinningAir() && timer.hasElapsed(3.0))); + // return new RunSpindexer(spindexer, turret, hood, intake).raceWith(new WaitCommand(3.0)); + return new RunSpindexer(spindexer, turret, hood, intake).until(() -> spindexer.spinningAir() && timer.hasElapsed(3.0)); + // return new ParallelDeadlineGroup(new WaitUntilCommand(() -> // spindexer.spinningAir()), new RunSpindexer(spindexer, turret, hood, intake)); diff --git a/src/main/java/frc/robot/commands/gpm/RunSpindexer.java b/src/main/java/frc/robot/commands/gpm/RunSpindexer.java index bac1123..3dbbbf1 100644 --- a/src/main/java/frc/robot/commands/gpm/RunSpindexer.java +++ b/src/main/java/frc/robot/commands/gpm/RunSpindexer.java @@ -1,5 +1,7 @@ package frc.robot.commands.gpm; +import org.littletonrobotics.junction.Logger; + import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.wpilibj.Timer; @@ -63,7 +65,10 @@ public class RunSpindexer extends Command { return; // this is so the balls don't fly out when unaligned } boolean jammed = spindexer.getSubsystemStatorCurrent() / 2 > SpindexerConstants.JAM_CURRENT_THRESHOLD; + Logger.recordOutput("SpindexerJammed", jammed); if (jam_debouncer.calculate(jammed)) { + Logger.recordOutput("SpindexerJammedDebounced", jammed); + reversing = true; reverseTimer.reset(); reverseTimer.start(); diff --git a/src/main/java/frc/robot/commands/gpm/RunSpindexerWithStop.java b/src/main/java/frc/robot/commands/gpm/RunSpindexerWithStop.java new file mode 100644 index 0000000..86b45d2 --- /dev/null +++ b/src/main/java/frc/robot/commands/gpm/RunSpindexerWithStop.java @@ -0,0 +1,113 @@ +package frc.robot.commands.gpm; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.filter.Debouncer.DebounceType; +import edu.wpi.first.units.measure.Time; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.Constants; +import frc.robot.subsystems.Intake.Intake; +import frc.robot.subsystems.Intake.IntakeConstants; +import frc.robot.subsystems.hood.Hood; +import frc.robot.subsystems.spindexer.Spindexer; +import frc.robot.subsystems.spindexer.SpindexerConstants; +import frc.robot.subsystems.turret.Turret; + +public class RunSpindexerWithStop extends Command { + private Spindexer spindexer; + private Turret turret; + private Hood hood; + private Intake intake; + + private Debouncer jam_debouncer = new Debouncer(SpindexerConstants.JAM_DEBOUNCE_TIME, DebounceType.kRising); // if there is jam I would think this is 0 -> 1 + + private boolean reversing = false; + private boolean wasHoodForcedDown = false; + + private Timer reverseTimer = new Timer(); + + private double storedIntakeSpeed = 0.0; + + + public RunSpindexerWithStop(Spindexer spindexer, Turret turret, Hood hood, Intake intake) { + this.spindexer = spindexer; + this.turret = turret; + this.hood = hood; + this.intake = intake; + + addRequirements(spindexer); + } + + // public RunSpindexer(Spindexer spindexer) { + // this.spindexer = spindexer; + // addRequirements(spindexer); + // } + + @Override + public void initialize() { + wasHoodForcedDown = hood.getHoodForcedDown(); + timer.reset(); + timer.start(); + } + + @Override + public void execute() { + boolean hoodForcedDown = hood.getHoodForcedDown(); + + if (wasHoodForcedDown && !hoodForcedDown) { + spindexer.maxSpindexer(); + } + wasHoodForcedDown = hoodForcedDown; + + if (!turret.atSetpoint() || hoodForcedDown || spindexer.noIndexing) { + spindexer.stopSpindexer(); + reversing = false; + return; // this is so the balls don't fly out when unaligned + } + boolean jammed = spindexer.getSubsystemStatorCurrent() / 2 > SpindexerConstants.JAM_CURRENT_THRESHOLD; + Logger.recordOutput("SpindexerJammed", jammed); + if (jam_debouncer.calculate(jammed)) { + Logger.recordOutput("SpindexerJammedDebounced", jammed); + + reversing = true; + reverseTimer.reset(); + reverseTimer.start(); + storedIntakeSpeed = intake.getSpeed(); + } + if (!reversing) { + spindexer.maxSpindexer(); + } else { + spindexer.reverseSpindexer(); + + if (intake.getPosition() > IntakeConstants.INTERMEDIATE_EXTENSION + 1.0) { + intake.spinReverse(); + } else { + intake.extend(); + } + + if (reverseTimer.hasElapsed(SpindexerConstants.REVERSE_DEBOUNCE_TIME)) { + reversing = false; + intake.spin(storedIntakeSpeed); + } + } + if (!Constants.DISABLE_SMART_DASHBOARD) { + SmartDashboard.putBoolean("Spindexer Jamming", reversing); + } + } + + @Override + public void end(boolean interrupted) { + spindexer.stopSpindexer(); + reversing = false; + } + + private Timer timer = new Timer(); + + @Override + public boolean isFinished() { + return spindexer.spinningAir() && timer.hasElapsed(1.0); + } +} diff --git a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java index 6455f15..08c4492 100644 --- a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java +++ b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java @@ -82,6 +82,9 @@ public class Spindexer extends SubsystemBase implements SpindexerIO { if (!Constants.DISABLE_SMART_DASHBOARD) { SmartDashboard.putBoolean("Spindexer Reversing", state == SpindexerState.REVERSE); } + + Logger.recordOutput("HasBalls", spinningAir()); + } public void setMotorVoltages(double voltage) { @@ -120,10 +123,22 @@ public class Spindexer extends SubsystemBase implements SpindexerIO { return inputs.spindexerOneStatorCurrent + inputs.spindexerTwoStatorCurrent; } + public double getMotorOneStatorCurrent() { + return inputs.spindexerOneStatorCurrent; + } + + public double getMotorTwoStatorCurrent() { + return inputs.spindexerTwoStatorCurrent; + } + public double getSubsystemSupplyCurrent() { return inputs.spindexerOneSupplyCurrent + inputs.spindexerTwoSupplyCurrent; } + public boolean spinningAir() { + return getMotorOneStatorCurrent() < 16.0 && getMotorTwoStatorCurrent() < 28.0; + } + @Override public void updateInputs() { inputs.spindexerOneVelocity = motorOne.getVelocity().getValueAsDouble(); diff --git a/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java b/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java index 631a8f2..e4d5708 100644 --- a/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java +++ b/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java @@ -10,7 +10,7 @@ public class SpindexerConstants { public static final double CURRENT_FORWARD_STATOR_LIMIT = 150.0; public static final double CURRENT_REVERSE_STATOR_LIMIT = 20.0; public static final double CURRENT_TIME_LIMIT = 1.0; //s - public static final double JAM_CURRENT_THRESHOLD = 75.0; // A + public static final double JAM_CURRENT_THRESHOLD = 40.0; // A public static final double JAM_DEBOUNCE_TIME = 0.3; // seconds public static final double REVERSE_DEBOUNCE_TIME = 0.25; // seconds } -- 2.39.5