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.feeder;
007
008import com.ctre.phoenix6.controls.VoltageOut;
009import com.ctre.phoenix6.hardware.TalonFX;
010import com.stuypulse.robot.constants.Field;
011import com.stuypulse.robot.constants.Motors;
012import com.stuypulse.robot.constants.Ports;
013import com.stuypulse.robot.constants.Settings;
014import com.stuypulse.robot.constants.Settings.EnabledSubsystems;
015import com.stuypulse.robot.subsystems.shooter.Shooter;
016import com.stuypulse.robot.subsystems.shooter.Shooter.ShooterState;
017import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain;
018import com.stuypulse.robot.util.LoggedSignals;
019import edu.wpi.first.units.measure.*;
020
021public class FeederImpl extends Feeder {
022
023    private final TalonFX feederMotor;
024
025    private final VoltageOut controller;
026
027    private final LoggedSignals signals;
028
029    public FeederImpl() {
030        feederMotor = new TalonFX(Ports.Feeder.FEEDER_MOTOR, Settings.CANBUS);
031        Motors.Feeder.LEADER_CONFIG.configure(feederMotor);
032        controller = new VoltageOut(getState().getTargetVoltage()).withEnableFOC(true);
033        this.signals = new LoggedSignals(LoggedSignals.SignalLocation.CANIVORE, "Feeder/",
034                feederMotor.getSupplyCurrent(),
035                feederMotor.getStatorCurrent(),
036                feederMotor.getVelocity());
037    }
038
039    @Override
040    public AngularVelocity getCurrentAngularVelocity() {
041        return feederMotor.getVelocity().getValue();
042    }
043
044    @Override
045    protected void stopMotors() {
046        feederMotor.stopMotor();
047    }
048
049    @Override
050    public void periodic() {
051        if (!EnabledSubsystems.FEEDER.get()) {
052            stopMotors();
053            return;
054        }
055        // Stop shooting if not aligned
056        CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance();
057        Shooter shooter = Shooter.getInstance();
058        if (!(swerve.isAlignedToTarget(Field.getHubPose()))
059                && shooter.getState() == ShooterState.SHOOT) {
060            setState(FeederState.IDLE);
061        }
062        if (!(swerve.isAlignedToTarget(Field.getFerryZonePose(swerve.getPose().getTranslation())))
063                && shooter.getState() == ShooterState.FERRY) {
064            setState(FeederState.IDLE);
065        }
066
067        // Apply
068        feederMotor.setControl(controller.withOutput(getState().getTargetVoltage()));
069        // Logging
070        this.signals.logAll();
071        super.periodic();
072    }
073}