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 static edu.wpi.first.units.Units.*;
009
010import com.ctre.phoenix6.controls.VoltageOut;
011import com.stuypulse.robot.constants.Motors;
012import com.stuypulse.robot.constants.Ports;
013import com.stuypulse.robot.constants.Settings;
014import com.stuypulse.robot.util.simulation.RobotVisualizer;
015import com.stuypulse.robot.util.simulation.TalonFXSimulation;
016import edu.wpi.first.math.system.plant.DCMotor;
017import edu.wpi.first.math.system.plant.LinearSystemId;
018import edu.wpi.first.units.measure.*;
019import edu.wpi.first.wpilibj.simulation.DCMotorSim;
020
021public class FeederSim extends Feeder {
022
023    private final TalonFXSimulation feederMotor;
024
025    private final DCMotorSim feederSim;
026
027    private final VoltageOut feederController;
028
029    public FeederSim() {
030        feederSim = new DCMotorSim(
031                LinearSystemId.createDCMotorSystem(
032                        DCMotor.getKrakenX60(2),
033                        Settings.Feeder.J.in(KilogramSquareMeters),
034                        Settings.Feeder.GEAR_RATIO),
035                DCMotor.getKrakenX60(2));
036        feederMotor = new TalonFXSimulation(Ports.Feeder.FEEDER_MOTOR, feederSim);
037        feederMotor.configure(Motors.Feeder.LEADER_CONFIG);
038        feederController = new VoltageOut(0).withEnableFOC(true);
039    }
040
041    @Override
042    public AngularVelocity getCurrentAngularVelocity() {
043        return feederMotor.getVelocity().getValue();
044    }
045
046    @Override
047    protected void stopMotors() {
048        feederMotor.stopMotor();
049    }
050
051    @Override
052    public void periodic() {
053        if (Settings.EnabledSubsystems.FEEDER.get()) {
054            // apply control to leader before grabbing state to update other motors
055            feederMotor.setControl(feederController.withOutput(getState().getTargetVoltage()));
056            // Logging
057        } else {
058            stopMotors();
059        }
060        
061        feederMotor.update(Settings.DT);
062        RobotVisualizer.getInstance().updateFeeder(getCurrentAngularVelocity());
063        super.periodic();
064    }
065}