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.intake;
007
008import java.util.Optional;
009
010import com.ctre.phoenix6.controls.DutyCycleOut;
011import com.ctre.phoenix6.controls.Follower;
012import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC;
013import com.ctre.phoenix6.controls.VoltageOut;
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.SysId;
019import com.stuypulse.robot.util.simulation.RobotVisualizer;
020import com.stuypulse.robot.util.simulation.TalonFXSimulation;
021
022import dev.doglog.DogLog;
023import edu.wpi.first.math.geometry.Rotation2d;
024import edu.wpi.first.math.system.plant.DCMotor;
025import edu.wpi.first.math.system.plant.LinearSystemId;
026import static edu.wpi.first.units.Units.KilogramSquareMeters;
027import static edu.wpi.first.units.Units.Meters;
028import static edu.wpi.first.units.Units.Radians;
029import static edu.wpi.first.units.Units.RadiansPerSecond;
030import static edu.wpi.first.units.Units.Volts;
031import edu.wpi.first.units.measure.Angle;
032import edu.wpi.first.units.measure.AngularVelocity;
033import edu.wpi.first.units.measure.Voltage;
034import edu.wpi.first.wpilibj.simulation.DCMotorSim;
035import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
036import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
037
038public class IntakeSim extends Intake {
039
040    private final SingleJointedArmSim pivotSim;
041
042    private final TalonFXSimulation pivotMotor;
043
044    private final PositionTorqueCurrentFOC pivotController;
045
046    private final TalonFXSimulation rollerMotorLeft;
047
048    private final TalonFXSimulation rollerMotorRight;
049
050    private final DCMotorSim rollerSim;
051
052    private final DutyCycleOut rollerController;
053
054    private final Follower followerController;
055
056    private Optional<Voltage> pivotVoltageOverride;
057
058    private Rotation2d zeroOffset;
059
060    public IntakeSim() {
061        pivotSim = new SingleJointedArmSim(
062                LinearSystemId.createDCMotorSystem(
063                        DCMotor.getKrakenX60(1),
064                        Settings.Intake.Pivot.MOI.in(KilogramSquareMeters),
065                        Settings.Intake.Pivot.GEAR_RATIO),
066                DCMotor.getKrakenX60(1),
067                Settings.Intake.Pivot.GEAR_RATIO,
068                Settings.Intake.Pivot.PIVOT_ARM_LENGTH.in(Meters),
069                Settings.Intake.Pivot.MAX_ANGLE.in(Radians),
070                Settings.Intake.Pivot.MIN_ANGLE.in(Radians), // reversed because negative?
071                true,
072                Settings.Intake.Pivot.INITIAL_ANGLE.in(Radians));
073        pivotMotor = new TalonFXSimulation(Ports.Intake.INTAKE_PIVOT_MOTOR, pivotSim);
074        pivotMotor.configure(Motors.Intake.PIVOT_CONFIG);
075        pivotController = new PositionTorqueCurrentFOC(getState().getTargetAngle());
076        rollerSim = new DCMotorSim(
077                LinearSystemId.createDCMotorSystem(
078                        DCMotor.getKrakenX60(2),
079                        Settings.Intake.Roller.J.in(KilogramSquareMeters),
080                        Settings.Intake.Roller.GEAR_RATIO),
081                DCMotor.getKrakenX60(2));
082        rollerMotorLeft = new TalonFXSimulation(Ports.Intake.INTAKE_ROLLER_MOTOR_LEFT, rollerSim);
083        rollerMotorRight = new TalonFXSimulation(Ports.Intake.INTAKE_ROLLER_MOTOR_RIGHT, rollerSim);
084        rollerMotorLeft.configure(Motors.Intake.LEFT_ROLLER_CONFIG);
085        rollerMotorRight.configure(Motors.Intake.RIGHT_ROLLER_CONFIG);
086        rollerController = new DutyCycleOut(0).withEnableFOC(true);
087        followerController = new Follower(rollerMotorLeft.getDeviceID(), MotorAlignmentValue.Opposed);
088        rollerMotorRight.setControl(followerController);
089        pivotVoltageOverride = Optional.empty();
090        zeroOffset = new Rotation2d();
091    }
092
093    @Override
094    public boolean limitSwitchHit() {
095        return true;
096    }
097    
098    @Override
099    public Angle getRelativePosition() {
100        return Radians.of(pivotSim.getAngleRads() + zeroOffset.getRadians());
101    }
102
103    @Override
104    public void seedPivotAngle(Angle angle) {
105        zeroOffset = new Rotation2d(-pivotSim.getAngleRads()).plus(new Rotation2d(angle));
106    }
107
108    @Override
109    public AngularVelocity getRollerVelocity() {
110        return rollerMotorRight.getVelocity().getValue();
111    }
112
113    @Override
114    protected void stopRollerMotors() {
115        rollerMotorLeft.stopMotor();
116        rollerMotorRight.stopMotor();
117        // re-add the follow control after stopMotor removes it
118        rollerMotorRight.setControl(followerController);
119    }
120
121    @Override
122    protected void stopPivotMotor() {
123        pivotMotor.stopMotor();
124    }
125
126    @Override
127    public void setPivotVoltageOverride(Voltage voltage) {
128        this.pivotVoltageOverride = Optional.of(voltage);
129    }
130
131    @Override
132    public void periodic() {
133        if (!Settings.EnabledSubsystems.INTAKE.get()) {
134            stopAllMotors();
135
136            return;
137        }
138
139        if (pivotVoltageOverride.isPresent()) {
140            pivotMotor.setControl(new VoltageOut(pivotVoltageOverride.get()));
141            return;
142        }
143        
144        pivotMotor.setControl(pivotController.withPosition(getState().getTargetAngle().times(-1)).withSlot(0)); // cooked inversion
145
146
147        rollerMotorLeft.setControl(rollerController.withOutput(getState().getTargetDutyCycle()));
148        
149        
150        // all current measured in amps
151        DogLog.log("Intake/Pivot/Stator Current", pivotMotor.getStatorCurrent().getValueAsDouble());
152        DogLog.log("Intake/Pivot/Supply Current", pivotMotor.getSupplyCurrent().getValueAsDouble());
153        DogLog.log("Intake/Pivot/Voltage", pivotMotor.getMotorVoltage().getValueAsDouble());
154        DogLog.log("Intake/Rollers/Left Current", rollerMotorLeft.getStatorCurrent().getValueAsDouble());
155        DogLog.log("Intake/Rollers/Right Current", rollerMotorRight.getStatorCurrent().getValueAsDouble());
156        DogLog.log("Intake/Rollers/Left Voltage", rollerMotorLeft.getMotorVoltage().getValueAsDouble());
157        DogLog.log("Intake/Rollers/Right Voltage", rollerMotorRight.getMotorVoltage().getValueAsDouble());
158        DogLog.log("Intake/Rollers/Left Stalling", false);
159        // TODO: implement
160        DogLog.log("Intake/Rollers/Right Stalling", false);
161            
162        rollerMotorLeft.update(Settings.DT);
163        rollerMotorRight.update(Settings.DT);
164        pivotMotor.update(Settings.DT);
165        RobotVisualizer.getInstance().updateIntake(Radians.of(getRelativePosition().in(Radians)), getRollerVelocity());
166        super.periodic();
167    }
168
169    @Override
170    public SysIdRoutine getIntakeSysIdRoutine() {
171        return SysId.getRoutine(
172                Settings.Intake.Pivot.RAMP_RATE,
173                Settings.Intake.Pivot.STEP_VOLTAGE,
174                "Intake",
175                voltage -> setPivotVoltageOverride(voltage),
176                () -> Radians.of(pivotSim.getAngleRads()),
177                () -> RadiansPerSecond.of(pivotSim.getVelocityRadPerSec()),
178                () -> Volts.of(pivotSim.getInput(0)),
179                getInstance());
180    }
181}