001/************************* PROJECT RON *************************/
002/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */
003/* Use of this source code is governed by an MIT-style license */
004/* that can be found in the repository LICENSE file.           */
005/***************************************************************/
006package com.stuypulse.robot.subsystems.handoff;
007
008import java.util.function.BooleanSupplier;
009
010import com.ctre.phoenix6.controls.VoltageOut;
011import com.ctre.phoenix6.hardware.TalonFX;
012import com.stuypulse.robot.constants.Field;
013import com.stuypulse.robot.constants.Motors;
014import com.stuypulse.robot.constants.Ports;
015import com.stuypulse.robot.constants.Settings;
016import com.stuypulse.robot.subsystems.shooter.Shooter;
017import com.stuypulse.robot.subsystems.shooter.Shooter.ShooterState;
018import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain;
019import com.stuypulse.robot.util.LoggedSignals;
020
021import edu.wpi.first.math.filter.Debouncer;
022import edu.wpi.first.math.filter.Debouncer.DebounceType;
023import edu.wpi.first.units.measure.Voltage;
024
025public class HandoffImpl extends Handoff {
026
027    private final TalonFX handoffMotor;
028
029    private final VoltageOut handoffController;
030
031    private final LoggedSignals signals;
032
033    private final BooleanSupplier handoffStalling;
034    private final Debouncer handoffDebouncer;
035
036    public HandoffImpl() {
037        handoffMotor = new TalonFX(Ports.Handoff.HANDOFF_MOTOR, Settings.CANBUS);
038        handoffController = new VoltageOut(getState().getTargetVoltage()).withEnableFOC(true);
039        signals = new LoggedSignals(LoggedSignals.SignalLocation.CANIVORE, "Handoff/",
040                handoffMotor.getSupplyCurrent(),
041                handoffMotor.getStatorCurrent(),
042                handoffMotor.getVelocity());
043        // Configuring
044        Motors.Handoff.HANDOFF_MOTOR_CONFIG.configure(handoffMotor);
045        this.handoffStalling = () -> Math.abs(handoffMotor.getStatorCurrent().getValueAsDouble()) > Settings.Handoff.STALL_CURRENT;
046        this.handoffDebouncer = new Debouncer(Settings.Handoff.STALL_DEBOUNCE, DebounceType.kRising);
047    }
048
049    @Override
050    protected void stopMotors() {
051        handoffMotor.stopMotor();
052    }
053
054    @Override
055    protected boolean handoffStalling() {
056        return handoffDebouncer.calculate(this.handoffStalling.getAsBoolean());
057    }
058
059    @Override
060    public void periodic() {
061        if (!Settings.EnabledSubsystems.HANDOFF.get()) {
062            stopMotors();
063            return;
064        }
065
066        // States
067        final Shooter shooter = Shooter.getInstance();
068        final CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance();
069        if (!(swerve.isAlignedToTarget(Field.getHubPose()))
070                && shooter.getState() == ShooterState.SHOOT) {
071            setState(HandoffState.IDLE);
072        }
073        // TODO: consider relaxing tolerances for ferrying
074        if (!(swerve.isAlignedToTarget(Field.getFerryZonePose(swerve.getPose().getTranslation())))
075                && shooter.getState() == ShooterState.FERRY) {
076            setState(HandoffState.IDLE);
077        }
078
079        // Control
080        final Voltage voltage = handoffStalling()
081                ? Handoff.HandoffState.REVERSE.getTargetVoltage()
082                : getState().getTargetVoltage();
083        final VoltageOut handoffControl = handoffController.withOutput(voltage);
084        // Apply
085        handoffMotor.setControl(handoffControl);
086        // Logging
087        this.signals.logAll();
088        super.periodic();
089    }
090}