001package com.stuypulse.robot.commands.shooter;
002
003import static edu.wpi.first.units.Units.RPM;
004import static edu.wpi.first.units.Units.Seconds;
005
006import com.stuypulse.robot.constants.Settings;
007import com.stuypulse.robot.subsystems.shooter.Shooter;
008
009import dev.doglog.DogLog;
010import edu.wpi.first.math.filter.Debouncer;
011import edu.wpi.first.math.filter.LinearFilter;
012import edu.wpi.first.wpilibj.Timer;
013import edu.wpi.first.math.filter.Debouncer.DebounceType;
014import edu.wpi.first.wpilibj2.command.Command;
015
016public class ShooterFirstShotIncrease extends Command {
017    private final Shooter shooter;
018    private final LinearFilter currentFilter;
019    private final Debouncer shotFinished;
020    private double previousCurrent;
021
022    private final Timer timer;
023
024    public ShooterFirstShotIncrease() {
025        shooter = Shooter.getInstance();
026        currentFilter = LinearFilter.singlePoleIIR(0.1, Settings.DT.in(Seconds));
027        shotFinished = new Debouncer(Settings.Shooter.FIRST_SHOT_DEBOUNCE.in(Seconds), DebounceType.kRising);
028        previousCurrent = 0;
029
030        timer = new Timer();
031    }
032
033    @Override
034    public void initialize() {
035        timer.restart();
036
037        shooter.addToBonusVelocity(Settings.Shooter.FIRST_SHOT_BONUS.get());
038        shooter.setGainSlot(1);
039    }
040
041    private boolean currentDecreasing() {
042        final double rawCurrent = Shooter.getInstance().getCurrentAngularVelocity().in(RPM);
043        final double filteredCurrent = currentFilter.calculate(rawCurrent);
044
045        final boolean decreasing = filteredCurrent < this.previousCurrent;
046
047        this.previousCurrent = filteredCurrent;
048
049        DogLog.log("Shooter/First Shot/Raw Current", rawCurrent);
050        DogLog.log("Shooter/First Shot/Filtered Current", filteredCurrent);
051        DogLog.log("Shooter/First Shot/Is Decreasing", decreasing);
052
053        return decreasing;
054    }
055
056    @Override
057    public boolean isFinished() {
058        return timer.hasElapsed(1);
059        // return shotFinished.calculate(this.currentDecreasing());
060    }
061
062    @Override
063    public void end(boolean interrupted) {
064        shooter.resetBonusVelocity();
065        shooter.setGainSlot(0);
066    }
067}