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.shooter;
007
008import static edu.wpi.first.units.Units.Hertz;
009import static edu.wpi.first.units.Units.Volts;
010
011import com.ctre.phoenix6.controls.Follower;
012import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC;
013import com.ctre.phoenix6.hardware.TalonFX;
014import com.ctre.phoenix6.signals.MotorAlignmentValue;
015import com.stuypulse.robot.constants.Motors;
016import com.stuypulse.robot.constants.Ports;
017import com.stuypulse.robot.constants.Settings;
018import com.stuypulse.robot.util.LoggedSignals;
019import com.stuypulse.robot.util.SysId;
020import edu.wpi.first.units.measure.AngularVelocity;
021import edu.wpi.first.units.measure.Voltage;
022import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
023import java.util.Optional;
024
025public class ShooterImpl extends Shooter {
026
027    private final TalonFX shooterMotorLeft;
028
029    private final TalonFX shooterMotorCenter;
030
031    private final TalonFX shooterMotorRight;
032
033    private final VelocityTorqueCurrentFOC shooterController;
034
035    private final Follower shooterFollowerController;
036
037    private final LoggedSignals signals;
038
039    private Optional<Voltage> voltageOverride;
040
041    public ShooterImpl() {
042        // leader
043        shooterMotorRight = new TalonFX(Ports.Shooter.SHOOTER_MOTOR_RIGHT, Settings.CANBUS);
044        shooterMotorCenter = new TalonFX(Ports.Shooter.SHOOTER_MOTOR_CENTER, Settings.CANBUS);
045        shooterMotorLeft = new TalonFX(Ports.Shooter.SHOOTER_MOTOR_LEFT, Settings.CANBUS);
046        shooterController = new VelocityTorqueCurrentFOC(getState().getTargetAngularVelocity());
047
048        shooterMotorRight.getTorqueCurrent().setUpdateFrequency(Hertz.of(1000)); // TODO: test effect on CANBus usage (SystemStats/CANBus/Utilization)
049        signals = new LoggedSignals(LoggedSignals.SignalLocation.CANIVORE, "Shooter/")
050                .withMotor(
051                        "Right Motor",
052                        shooterMotorRight.getSupplyCurrent(),
053                        shooterMotorRight.getStatorCurrent(),
054                        shooterMotorRight.getVelocity())
055                .withMotor(
056                        "Center Motor",
057                        shooterMotorCenter.getSupplyCurrent(),
058                        shooterMotorCenter.getStatorCurrent(),
059                        shooterMotorCenter.getVelocity())
060                .withMotor(
061                        "Left Motor",
062                        shooterMotorLeft.getSupplyCurrent(),
063                        shooterMotorLeft.getStatorCurrent(),
064                        shooterMotorLeft.getVelocity());
065        // configure
066        Motors.Shooter.SHOOTER_MOTOR_RIGHT.configure(shooterMotorRight);
067        Motors.Shooter.SHOOTER_MOTOR_CENTER.configure(shooterMotorCenter);
068        Motors.Shooter.SHOOTER_MOTOR_LEFT.configure(shooterMotorLeft);
069        // Set center and left to follow right
070        shooterFollowerController = new Follower(shooterMotorRight.getDeviceID(), MotorAlignmentValue.Opposed);
071        shooterMotorCenter.setControl(shooterFollowerController);
072        shooterMotorLeft.setControl(shooterFollowerController);
073        voltageOverride = Optional.empty();
074    }
075
076    @Override
077    public void setVoltageOverride(Voltage voltage) {
078        this.voltageOverride = Optional.of(voltage);
079    }
080
081    @Override
082    public AngularVelocity getCurrentAngularVelocity() {
083        return shooterMotorRight.getVelocity().getValue();
084    }
085
086    @Override
087    protected void stopMotors() {
088        shooterMotorRight.stopMotor();
089        shooterMotorCenter.stopMotor();
090        shooterMotorLeft.stopMotor();
091        shooterMotorCenter.setControl(shooterFollowerController);
092        shooterMotorLeft.setControl(shooterFollowerController);
093    }
094
095    @Override
096    public void periodic() {
097        if (!Settings.EnabledSubsystems.SHOOTER.get()) {
098            stopMotors();
099            return;
100        }
101        if (voltageOverride.isPresent()) {
102            shooterMotorRight.setVoltage(voltageOverride.get().in(Volts));
103            return;
104        }
105
106        final AngularVelocity targetAngularVelocity = getState().getTargetAngularVelocity();
107        final VelocityTorqueCurrentFOC shooterControl = shooterController.withVelocity(targetAngularVelocity);
108        shooterMotorRight.setControl(shooterControl);
109        this.signals.logAll();
110        super.periodic();
111    }
112
113    @Override
114    public SysIdRoutine getShooterSysIdRoutine() {
115        return SysId.getRoutine(
116                Settings.Shooter.RAMP_RATE,
117                Settings.Shooter.STEP_VOLTAGE,
118                "Shooter",
119                this::setVoltageOverride,
120                () -> shooterMotorRight.getPosition().getValue(),
121                () -> shooterMotorRight.getVelocity().getValue(),
122                () -> shooterMotorRight.getMotorVoltage().getValue(),
123                getInstance());
124    }
125}